GCC Code Coverage Report

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