GCC Code Coverage Report

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