GCC Code Coverage Report

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