GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/actions/unicycle.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 58 79 73.4%
Branches: 58 230 25.2%
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 33 ActionModelUnicycleTpl<Scalar>::ActionModelUnicycleTpl()
13 : ActionModelAbstractTpl<Scalar>(
14 ✗ std::make_shared<StateVectorTpl<Scalar> >(3), 2, 5),
15
3/6
✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 33 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 33 times.
✗ Branch 11 not taken.
 33 dt_(Scalar(0.1)) {
16
2/4
✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33 times.
✗ Branch 5 not taken.
 33 cost_weights_ << Scalar(10.), Scalar(1.);
17 33 }
18
19 template <typename Scalar>
20 2193 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
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 2193 times.
 2193 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
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2193 times.
 2193 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 2193 Data* d = static_cast<Data*>(data.get());
34
35 2193 const Scalar c = cos(x[2]);
36 2193 const Scalar s = sin(x[2]);
37
7/14
✓ Branch 3 taken 2193 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2193 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2193 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2193 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 2193 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2193 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2193 times.
✗ Branch 22 not taken.
 2193 d->xnext << x[0] + c * u[0] * dt_, x[1] + s * u[0] * dt_, x[2] + u[1] * dt_;
38
2/4
✓ Branch 3 taken 2193 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2193 times.
✗ Branch 7 not taken.
 2193 d->r.template head<3>() = cost_weights_[0] * x;
39
2/4
✓ Branch 3 taken 2193 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2193 times.
✗ Branch 7 not taken.
 2193 d->r.template tail<2>() = cost_weights_[1] * u;
40 2193 d->cost = Scalar(0.5) * d->r.dot(d->r);
41 2193 }
42
43 template <typename Scalar>
44 187 void ActionModelUnicycleTpl<Scalar>::calc(
45 const std::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
46 const Eigen::Ref<const VectorXs>& x) {
47
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 187 times.
 187 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 187 Data* d = static_cast<Data*>(data.get());
53
54 187 d->xnext = x;
55
2/4
✓ Branch 3 taken 187 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 187 times.
✗ Branch 7 not taken.
 187 d->r.template head<3>() = cost_weights_[0] * x;
56
1/2
✓ Branch 2 taken 187 times.
✗ Branch 3 not taken.
 187 d->r.template tail<2>().setZero();
57
2/4
✓ Branch 2 taken 187 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 187 times.
✗ Branch 6 not taken.
 187 d->cost = Scalar(0.5) * d->r.template head<3>().dot(d->r.template head<3>());
58 187 }
59
60 template <typename Scalar>
61 1369 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
2/4
✓ Branch 3 taken 1369 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1369 times.
 1369 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
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1369 times.
 1369 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 1369 Data* d = static_cast<Data*>(data.get());
75
76
1/2
✓ Branch 1 taken 1369 times.
✗ Branch 2 not taken.
 1369 const Scalar c = static_cast<Scalar>(cos(x[2]));
77
1/2
✓ Branch 1 taken 1369 times.
✗ Branch 2 not taken.
 1369 const Scalar s = static_cast<Scalar>(sin(x[2]));
78
2/4
✓ Branch 1 taken 1369 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1369 times.
✗ Branch 5 not taken.
 1369 const Scalar w_x = cost_weights_[0] * cost_weights_[0];
79
2/4
✓ Branch 1 taken 1369 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1369 times.
✗ Branch 5 not taken.
 1369 const Scalar w_u = cost_weights_[1] * cost_weights_[1];
80
2/4
✓ Branch 1 taken 1369 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1369 times.
✗ Branch 5 not taken.
 1369 d->Lx = x * w_x;
81
2/4
✓ Branch 1 taken 1369 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1369 times.
✗ Branch 5 not taken.
 1369 d->Lu = u * w_u;
82
2/4
✓ Branch 1 taken 1369 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1369 times.
✗ Branch 5 not taken.
 1369 d->Lxx.diagonal().setConstant(w_x);
83
2/4
✓ Branch 1 taken 1369 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1369 times.
✗ Branch 5 not taken.
 1369 d->Luu.diagonal().setConstant(w_u);
84
2/4
✓ Branch 1 taken 1369 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1369 times.
✗ Branch 5 not taken.
 1369 d->Fx(0, 2) = -s * u[0] * dt_;
85
2/4
✓ Branch 1 taken 1369 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1369 times.
✗ Branch 5 not taken.
 1369 d->Fx(1, 2) = c * u[0] * dt_;
86
1/2
✓ Branch 1 taken 1369 times.
✗ Branch 2 not taken.
 1369 d->Fu(0, 0) = c * dt_;
87
1/2
✓ Branch 1 taken 1369 times.
✗ Branch 2 not taken.
 1369 d->Fu(1, 0) = s * dt_;
88
1/2
✓ Branch 1 taken 1369 times.
✗ Branch 2 not taken.
 1369 d->Fu(2, 1) = dt_;
89 1369 }
90
91 template <typename Scalar>
92 113 void ActionModelUnicycleTpl<Scalar>::calcDiff(
93 const std::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
94 const Eigen::Ref<const VectorXs>& x) {
95
2/4
✓ Branch 3 taken 113 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 113 times.
 113 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 113 Data* d = static_cast<Data*>(data.get());
101
102
2/4
✓ Branch 1 taken 113 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 113 times.
✗ Branch 5 not taken.
 113 const Scalar w_x = cost_weights_[0] * cost_weights_[0];
103
2/4
✓ Branch 1 taken 113 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 113 times.
✗ Branch 5 not taken.
 113 d->Lx = x * w_x;
104
2/4
✓ Branch 1 taken 113 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 113 times.
✗ Branch 5 not taken.
 113 d->Lxx.diagonal().setConstant(w_x);
105 113 }
106
107 template <typename Scalar>
108 std::shared_ptr<ActionDataAbstractTpl<Scalar> >
109 866 ActionModelUnicycleTpl<Scalar>::createData() {
110
1/2
✓ Branch 2 taken 866 times.
✗ Branch 3 not taken.
 866 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 64 bool ActionModelUnicycleTpl<Scalar>::checkData(
123 const std::shared_ptr<ActionDataAbstract>& data) {
124 64 std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
125
1/2
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
 64 if (d != NULL) {
126 64 return true;
127 } else {
128 ✗ return false;
129 }
130 64 }
131
132 template <typename Scalar>
133 22 void ActionModelUnicycleTpl<Scalar>::print(std::ostream& os) const {
134 22 os << "ActionModelUnicycle {dt=" << dt_ << "}";
135 22 }
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