GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/actions/unicycle.hxx
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 72 0.0%
Branches: 0 230 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 namespace crocoddyl {
11 template <typename Scalar>
12 ✗ ActionModelUnicycleTpl<Scalar>::ActionModelUnicycleTpl()
13 : ActionModelAbstractTpl<Scalar>(
14 ✗ std::make_shared<StateVectorTpl<Scalar> >(3), 2, 5),
15 ✗ dt_(Scalar(0.1)) {
16 ✗ cost_weights_ << Scalar(10.), Scalar(1.);
17 }
18
19 template <typename Scalar>
20 ✗ void ActionModelUnicycleTpl<Scalar>::calc(
21 const std::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
22 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
23 ✗ if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
24 ✗ throw_pretty(
25 "Invalid argument: " << "x has wrong dimension (it should be " +
26 std::to_string(state_->get_nx()) + ")");
27 }
28 ✗ if (static_cast<std::size_t>(u.size()) != nu_) {
29 ✗ throw_pretty(
30 "Invalid argument: " << "u has wrong dimension (it should be " +
31 std::to_string(nu_) + ")");
32 }
33 ✗ Data* d = static_cast<Data*>(data.get());
34
35 ✗ const Scalar c = cos(x[2]);
36 ✗ const Scalar s = sin(x[2]);
37 ✗ d->xnext << x[0] + c * u[0] * dt_, x[1] + s * u[0] * dt_, x[2] + u[1] * dt_;
38 ✗ d->r.template head<3>() = cost_weights_[0] * x;
39 ✗ d->r.template tail<2>() = cost_weights_[1] * u;
40 ✗ d->cost = Scalar(0.5) * d->r.dot(d->r);
41 }
42
43 template <typename Scalar>
44 ✗ void ActionModelUnicycleTpl<Scalar>::calc(
45 const std::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
46 const Eigen::Ref<const VectorXs>& x) {
47 ✗ if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
48 ✗ throw_pretty(
49 "Invalid argument: " << "x has wrong dimension (it should be " +
50 std::to_string(state_->get_nx()) + ")");
51 }
52 ✗ Data* d = static_cast<Data*>(data.get());
53
54 ✗ d->xnext = x;
55 ✗ d->r.template head<3>() = cost_weights_[0] * x;
56 ✗ d->r.template tail<2>().setZero();
57 ✗ d->cost = Scalar(0.5) * d->r.template head<3>().dot(d->r.template head<3>());
58 }
59
60 template <typename Scalar>
61 ✗ void ActionModelUnicycleTpl<Scalar>::calcDiff(
62 const std::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
63 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
64 ✗ if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
65 ✗ throw_pretty(
66 "Invalid argument: " << "x has wrong dimension (it should be " +
67 std::to_string(state_->get_nx()) + ")");
68 }
69 ✗ if (static_cast<std::size_t>(u.size()) != nu_) {
70 ✗ throw_pretty(
71 "Invalid argument: " << "u has wrong dimension (it should be " +
72 std::to_string(nu_) + ")");
73 }
74 ✗ Data* d = static_cast<Data*>(data.get());
75
76 ✗ const Scalar c = static_cast<Scalar>(cos(x[2]));
77 ✗ const Scalar s = static_cast<Scalar>(sin(x[2]));
78 ✗ const Scalar w_x = cost_weights_[0] * cost_weights_[0];
79 ✗ const Scalar w_u = cost_weights_[1] * cost_weights_[1];
80 ✗ d->Lx = x * w_x;
81 ✗ d->Lu = u * w_u;
82 ✗ d->Lxx.diagonal().setConstant(w_x);
83 ✗ d->Luu.diagonal().setConstant(w_u);
84 ✗ d->Fx(0, 2) = -s * u[0] * dt_;
85 ✗ d->Fx(1, 2) = c * u[0] * dt_;
86 ✗ d->Fu(0, 0) = c * dt_;
87 ✗ d->Fu(1, 0) = s * dt_;
88 ✗ d->Fu(2, 1) = dt_;
89 }
90
91 template <typename Scalar>
92 ✗ void ActionModelUnicycleTpl<Scalar>::calcDiff(
93 const std::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
94 const Eigen::Ref<const VectorXs>& x) {
95 ✗ if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
96 ✗ throw_pretty(
97 "Invalid argument: " << "x has wrong dimension (it should be " +
98 std::to_string(state_->get_nx()) + ")");
99 }
100 ✗ Data* d = static_cast<Data*>(data.get());
101
102 ✗ const Scalar w_x = cost_weights_[0] * cost_weights_[0];
103 ✗ d->Lx = x * w_x;
104 ✗ d->Lxx.diagonal().setConstant(w_x);
105 }
106
107 template <typename Scalar>
108 std::shared_ptr<ActionDataAbstractTpl<Scalar> >
109 ✗ ActionModelUnicycleTpl<Scalar>::createData() {
110 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
111 }
112
113 template <typename Scalar>
114 template <typename NewScalar>
115 ✗ ActionModelUnicycleTpl<NewScalar> ActionModelUnicycleTpl<Scalar>::cast() const {
116 typedef ActionModelUnicycleTpl<NewScalar> ReturnType;
117 ✗ ReturnType ret;
118 ✗ return ret;
119 }
120
121 template <typename Scalar>
122 ✗ bool ActionModelUnicycleTpl<Scalar>::checkData(
123 const std::shared_ptr<ActionDataAbstract>& data) {
124 ✗ std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
125 ✗ if (d != NULL) {
126 ✗ return true;
127 } else {
128 ✗ return false;
129 }
130 }
131
132 template <typename Scalar>
133 ✗ void ActionModelUnicycleTpl<Scalar>::print(std::ostream& os) const {
134 ✗ os << "ActionModelUnicycle {dt=" << dt_ << "}";
135 }
136
137 template <typename Scalar>
138 const typename MathBaseTpl<Scalar>::Vector2s&
139 ✗ ActionModelUnicycleTpl<Scalar>::get_cost_weights() const {
140 ✗ return cost_weights_;
141 }
142
143 template <typename Scalar>
144 ✗ void ActionModelUnicycleTpl<Scalar>::set_cost_weights(
145 const typename MathBase::Vector2s& weights) {
146 ✗ cost_weights_ = weights;
147 }
148
149 template <typename Scalar>
150 ✗ Scalar ActionModelUnicycleTpl<Scalar>::get_dt() const {
151 ✗ return dt_;
152 }
153
154 template <typename Scalar>
155 ✗ void ActionModelUnicycleTpl<Scalar>::set_dt(const Scalar dt) {
156 ✗ if (dt <= 0)
157 ✗ throw_pretty("Invalid argument: dt should be strictly positive.");
158 ✗ dt_ = dt;
159 }
160
161 } // namespace crocoddyl
162