GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/action-base.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 93 122 76.2%
Branches: 61 242 25.2%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5 // University of Oxford, Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include <boost/core/demangle.hpp>
11 #include <iostream>
12 #include <typeinfo>
13
14 namespace crocoddyl {
15
16 template <typename Scalar>
17 1692 ActionModelAbstractTpl<Scalar>::ActionModelAbstractTpl(
18 std::shared_ptr<StateAbstractTpl<Scalar> > state, const std::size_t nu,
19 const std::size_t nr, const std::size_t ng, const std::size_t nh,
20 const std::size_t ng_T, const std::size_t nh_T)
21 1692 : nu_(nu),
22 1692 nr_(nr),
23 1692 ng_(ng),
24 1692 nh_(nh),
25 1692 ng_T_(ng_T),
26 1692 nh_T_(nh_T),
27 1692 state_(state),
28
2/4
✓ Branch 1 taken 1692 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1692 times.
✗ Branch 5 not taken.
 1692 unone_(MathBase::VectorXs::Zero(nu)),
29
3/6
✗ Branch 0 not taken.
✓ Branch 1 taken 1692 times.
✓ Branch 3 taken 1692 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1692 times.
✗ Branch 7 not taken.
 1692 g_lb_(VectorXs::Constant(ng > ng_T ? ng : ng_T,
30 1692 -std::numeric_limits<Scalar>::infinity())),
31
2/4
✓ Branch 1 taken 1692 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1692 times.
✗ Branch 5 not taken.
 1692 g_ub_(VectorXs::Constant(ng > ng_T ? ng : ng_T,
32
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1692 times.
 1692 std::numeric_limits<Scalar>::infinity())),
33
2/4
✓ Branch 1 taken 1692 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1692 times.
✗ Branch 5 not taken.
 1692 u_lb_(MathBase::VectorXs::Constant(
34 1692 nu, -std::numeric_limits<Scalar>::infinity())),
35
1/2
✓ Branch 1 taken 1692 times.
✗ Branch 2 not taken.
 1692 u_ub_(MathBase::VectorXs::Constant(
36
1/2
✓ Branch 2 taken 1692 times.
✗ Branch 3 not taken.
 1692 nu, std::numeric_limits<Scalar>::infinity())),
37 3384 has_control_limits_(false) {}
38
39 template <typename Scalar>
40 10 ActionModelAbstractTpl<Scalar>::ActionModelAbstractTpl(
41 const ActionModelAbstractTpl<Scalar>& other)
42 10 : nu_(other.nu_),
43 10 nr_(other.nr_),
44 10 ng_(other.ng_),
45 10 nh_(other.nh_),
46 10 ng_T_(other.ng_T_),
47 10 nh_T_(other.nh_T_),
48 10 state_(other.state_),
49
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 g_lb_(other.g_lb_),
50
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 g_ub_(other.g_ub_),
51
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 u_lb_(other.u_lb_),
52
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 u_ub_(other.u_ub_),
53
1/2
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
 20 has_control_limits_(other.has_control_limits_) {}
54
55 template <typename Scalar>
56 3 void ActionModelAbstractTpl<Scalar>::calc(
57 const std::shared_ptr<ActionDataAbstract>& data,
58 const Eigen::Ref<const VectorXs>& x) {
59
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 calc(data, x, unone_);
60 3 }
61
62 template <typename Scalar>
63 1 void ActionModelAbstractTpl<Scalar>::calcDiff(
64 const std::shared_ptr<ActionDataAbstract>& data,
65 const Eigen::Ref<const VectorXs>& x) {
66
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 calcDiff(data, x, unone_);
67 1 }
68
69 template <typename Scalar>
70 400 void ActionModelAbstractTpl<Scalar>::quasiStatic(
71 const std::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
72 const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter,
73 const Scalar tol) {
74
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 400 times.
 400 if (static_cast<std::size_t>(u.size()) != nu_) {
75 ✗ throw_pretty(
76 "Invalid argument: " << "u has wrong dimension (it should be " +
77 std::to_string(nu_) + ")");
78 }
79
2/4
✓ Branch 3 taken 400 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 400 times.
 400 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
80 ✗ throw_pretty(
81 "Invalid argument: " << "x has wrong dimension (it should be " +
82 std::to_string(state_->get_nx()) + ")");
83 }
84 // Check the velocity input is zero
85
4/16
✓ Branch 2 taken 400 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 400 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 400 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 400 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
 400 assert_pretty(x.tail(state_->get_nv()).isZero(),
86 "The velocity input should be zero for quasi-static to work.");
87
88
1/2
✓ Branch 2 taken 400 times.
✗ Branch 3 not taken.
 400 const std::size_t ndx = state_->get_ndx();
89
2/4
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
 400 VectorXs dx = VectorXs::Zero(ndx);
90
1/2
✓ Branch 0 taken 400 times.
✗ Branch 1 not taken.
 400 if (nu_ != 0) {
91
2/4
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
 400 VectorXs du = VectorXs::Zero(nu_);
92
1/2
✓ Branch 0 taken 640 times.
✗ Branch 1 not taken.
 640 for (std::size_t i = 0; i < maxiter; ++i) {
93
2/4
✓ Branch 1 taken 640 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 640 times.
✗ Branch 5 not taken.
 640 calc(data, x, u);
94
2/4
✓ Branch 1 taken 640 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 640 times.
✗ Branch 5 not taken.
 640 calcDiff(data, x, u);
95
3/6
✓ Branch 2 taken 640 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 640 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 640 times.
✗ Branch 10 not taken.
 640 state_->diff(x, data->xnext, dx);
96
5/10
✓ Branch 3 taken 640 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 640 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 640 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 640 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 640 times.
✗ Branch 16 not taken.
 640 du.noalias() = -pseudoInverse(data->Fu) * dx;
97
1/2
✓ Branch 1 taken 640 times.
✗ Branch 2 not taken.
 640 u += du;
98
3/4
✓ Branch 1 taken 640 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 400 times.
✓ Branch 4 taken 240 times.
 640 if (du.norm() <= tol) {
99 400 break;
100 }
101 }
102 400 }
103 400 }
104
105 template <typename Scalar>
106 typename MathBaseTpl<Scalar>::VectorXs
107 ✗ ActionModelAbstractTpl<Scalar>::quasiStatic_x(
108 const std::shared_ptr<ActionDataAbstract>& data, const VectorXs& x,
109 const std::size_t maxiter, const Scalar tol) {
110 ✗ VectorXs u(nu_);
111 ✗ u.setZero();
112 ✗ quasiStatic(data, u, x, maxiter, tol);
113 ✗ return u;
114 }
115
116 template <typename Scalar>
117 std::shared_ptr<ActionDataAbstractTpl<Scalar> >
118 ✗ ActionModelAbstractTpl<Scalar>::createData() {
119 return std::allocate_shared<ActionDataAbstract>(
120 ✗ Eigen::aligned_allocator<ActionDataAbstract>(), this);
121 }
122
123 template <typename Scalar>
124 ✗ bool ActionModelAbstractTpl<Scalar>::checkData(
125 const std::shared_ptr<ActionDataAbstract>&) {
126 ✗ return false;
127 }
128
129 template <typename Scalar>
130 ✗ void ActionModelAbstractTpl<Scalar>::print(std::ostream& os) const {
131 ✗ os << boost::core::demangle(typeid(*this).name());
132 }
133
134 template <typename Scalar>
135 732502 std::size_t ActionModelAbstractTpl<Scalar>::get_nu() const {
136 732502 return nu_;
137 }
138
139 template <typename Scalar>
140 47310 std::size_t ActionModelAbstractTpl<Scalar>::get_nr() const {
141 47310 return nr_;
142 }
143
144 template <typename Scalar>
145 21231 std::size_t ActionModelAbstractTpl<Scalar>::get_ng() const {
146 21231 return ng_;
147 }
148
149 template <typename Scalar>
150 20496 std::size_t ActionModelAbstractTpl<Scalar>::get_nh() const {
151 20496 return nh_;
152 }
153
154 template <typename Scalar>
155 39050 std::size_t ActionModelAbstractTpl<Scalar>::get_ng_T() const {
156 39050 return ng_T_;
157 }
158
159 template <typename Scalar>
160 38766 std::size_t ActionModelAbstractTpl<Scalar>::get_nh_T() const {
161 38766 return nh_T_;
162 }
163
164 template <typename Scalar>
165 const std::shared_ptr<StateAbstractTpl<Scalar> >&
166 1251431 ActionModelAbstractTpl<Scalar>::get_state() const {
167 1251431 return state_;
168 }
169
170 template <typename Scalar>
171 const typename MathBaseTpl<Scalar>::VectorXs&
172 ✗ ActionModelAbstractTpl<Scalar>::get_g_lb() const {
173 ✗ return g_lb_;
174 }
175
176 template <typename Scalar>
177 const typename MathBaseTpl<Scalar>::VectorXs&
178 ✗ ActionModelAbstractTpl<Scalar>::get_g_ub() const {
179 ✗ return g_ub_;
180 }
181
182 template <typename Scalar>
183 const typename MathBaseTpl<Scalar>::VectorXs&
184 269 ActionModelAbstractTpl<Scalar>::get_u_lb() const {
185 269 return u_lb_;
186 }
187
188 template <typename Scalar>
189 const typename MathBaseTpl<Scalar>::VectorXs&
190 269 ActionModelAbstractTpl<Scalar>::get_u_ub() const {
191 269 return u_ub_;
192 }
193
194 template <typename Scalar>
195 1000 bool ActionModelAbstractTpl<Scalar>::get_has_control_limits() const {
196 1000 return has_control_limits_;
197 }
198
199 template <typename Scalar>
200 ✗ void ActionModelAbstractTpl<Scalar>::set_g_lb(const VectorXs& g_lb) {
201 ✗ const std::size_t ng = ng_ > ng_T_ ? ng_ : ng_T_;
202 ✗ if (static_cast<std::size_t>(g_lb.size()) != ng) {
203 ✗ throw_pretty(
204 "Invalid argument: "
205 << "inequality lower bound has wrong dimension (it should be " +
206 std::to_string(ng) + ")");
207 }
208 ✗ g_lb_ = g_lb;
209 }
210
211 template <typename Scalar>
212 ✗ void ActionModelAbstractTpl<Scalar>::set_g_ub(const VectorXs& g_ub) {
213 ✗ const std::size_t ng = ng_ > ng_T_ ? ng_ : ng_T_;
214 ✗ if (static_cast<std::size_t>(g_ub.size()) != ng) {
215 ✗ throw_pretty(
216 "Invalid argument: "
217 << "inequality upper bound has wrong dimension (it should be " +
218 std::to_string(ng_) + ")");
219 }
220 ✗ g_ub_ = g_ub;
221 }
222
223 template <typename Scalar>
224 1360 void ActionModelAbstractTpl<Scalar>::set_u_lb(const VectorXs& u_lb) {
225
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1360 times.
 1360 if (static_cast<std::size_t>(u_lb.size()) != nu_) {
226 ✗ throw_pretty("Invalid argument: "
227 << "control lower bound has wrong dimension (it should be " +
228 std::to_string(nu_) + ")");
229 }
230 1360 u_lb_ = u_lb;
231 1360 update_has_control_limits();
232 1360 }
233
234 template <typename Scalar>
235 1360 void ActionModelAbstractTpl<Scalar>::set_u_ub(const VectorXs& u_ub) {
236
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1360 times.
 1360 if (static_cast<std::size_t>(u_ub.size()) != nu_) {
237 ✗ throw_pretty("Invalid argument: "
238 << "control upper bound has wrong dimension (it should be " +
239 std::to_string(nu_) + ")");
240 }
241 1360 u_ub_ = u_ub;
242 1360 update_has_control_limits();
243 1360 }
244
245 template <typename Scalar>
246 2720 void ActionModelAbstractTpl<Scalar>::update_has_control_limits() {
247 2720 has_control_limits_ =
248
5/8
✓ Branch 2 taken 2720 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2720 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2720 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 1246 times.
✓ Branch 11 taken 1474 times.
 3966 isfinite(u_lb_.template cast<ScalarType>().array()).any() &&
249
5/8
✓ Branch 2 taken 1246 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1246 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1246 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 623 times.
✓ Branch 11 taken 623 times.
 1246 isfinite(u_ub_.template cast<ScalarType>().array()).any();
250 2720 }
251
252 template <typename Scalar>
253 361 std::ostream& operator<<(std::ostream& os,
254 const ActionModelAbstractTpl<Scalar>& model) {
255 361 model.print(os);
256 361 return os;
257 }
258
259 } // namespace crocoddyl
260