GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/actions/free-invdyn.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 127 165 77.0%
Branches: 92 362 25.4%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, Heriot-Watt University, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include <pinocchio/algorithm/compute-all-terms.hpp>
10 #include <pinocchio/algorithm/jacobian.hpp>
11 #include <pinocchio/algorithm/kinematics.hpp>
12 #include <pinocchio/algorithm/rnea-derivatives.hpp>
13 #include <pinocchio/algorithm/rnea.hpp>
14
15 #include "crocoddyl/core/constraints/residual.hpp"
16
17 namespace crocoddyl {
18
19 template <typename Scalar>
20 1 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::
21 DifferentialActionModelFreeInvDynamicsTpl(
22 std::shared_ptr<StateMultibody> state,
23 std::shared_ptr<ActuationModelAbstract> actuation,
24 std::shared_ptr<CostModelSum> costs)
25 1 : Base(state, state->get_nv(), costs->get_nr(), 0,
26 1 state->get_nv() - actuation->get_nu()),
27 1 actuation_(actuation),
28 1 costs_(costs),
29 1 constraints_(
30
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 std::make_shared<ConstraintModelManager>(state, state->get_nv())),
31
2/4
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 3 pinocchio_(state->get_pinocchio().get()) {
32
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 init(state);
33 1 }
34
35 template <typename Scalar>
36 150 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::
37 DifferentialActionModelFreeInvDynamicsTpl(
38 std::shared_ptr<StateMultibody> state,
39 std::shared_ptr<ActuationModelAbstract> actuation,
40 std::shared_ptr<CostModelSum> costs,
41 std::shared_ptr<ConstraintModelManager> constraints)
42 150 : Base(state, state->get_nv(), costs->get_nr(), constraints->get_ng(),
43 150 constraints->get_nh() + state->get_nv() - actuation->get_nu(),
44 constraints->get_ng_T(), constraints->get_nh_T()),
45 150 actuation_(actuation),
46 150 costs_(costs),
47 150 constraints_(constraints),
48
2/4
✓ Branch 13 taken 150 times.
✗ Branch 14 not taken.
✓ Branch 19 taken 150 times.
✗ Branch 20 not taken.
 450 pinocchio_(state->get_pinocchio().get()) {
49
1/2
✓ Branch 1 taken 150 times.
✗ Branch 2 not taken.
 150 init(state);
50 150 }
51
52 template <typename Scalar>
53 151 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::init(
54 const std::shared_ptr<StateMultibody>& state) {
55
2/4
✓ Branch 2 taken 151 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 151 times.
 151 if (costs_->get_nu() != nu_) {
56 ✗ throw_pretty(
57 "Invalid argument: "
58 << "Costs doesn't have the same control dimension (it should be " +
59 std::to_string(nu_) + ")");
60 }
61
2/4
✓ Branch 2 taken 151 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 151 times.
 151 if (constraints_->get_nu() != nu_) {
62 ✗ throw_pretty("Invalid argument: "
63 << "Constraints doesn't have the same control dimension (it "
64 "should be " +
65 std::to_string(nu_) + ")");
66 }
67
1/2
✓ Branch 1 taken 151 times.
✗ Branch 2 not taken.
 151 VectorXs lb =
68
1/2
✓ Branch 2 taken 151 times.
✗ Branch 3 not taken.
 151 VectorXs::Constant(nu_, -std::numeric_limits<Scalar>::infinity());
69
1/2
✓ Branch 1 taken 151 times.
✗ Branch 2 not taken.
 151 VectorXs ub =
70
1/2
✓ Branch 2 taken 151 times.
✗ Branch 3 not taken.
 151 VectorXs::Constant(nu_, std::numeric_limits<Scalar>::infinity());
71
1/2
✓ Branch 1 taken 151 times.
✗ Branch 2 not taken.
 151 Base::set_u_lb(lb);
72
1/2
✓ Branch 1 taken 151 times.
✗ Branch 2 not taken.
 151 Base::set_u_ub(ub);
73
74
4/6
✓ Branch 2 taken 151 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 151 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 51 times.
✓ Branch 9 taken 100 times.
 151 if (state->get_nv() - actuation_->get_nu() > 0) {
75
2/4
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
 102 constraints_->addConstraint(
76 "tau",
77 std::make_shared<ConstraintModelResidual>(
78
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 state_,
79 std::make_shared<typename DifferentialActionModelFreeInvDynamicsTpl<
80
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 51 Scalar>::ResidualModelActuation>(state, actuation_->get_nu()),
81 102 false));
82 }
83 151 }
84
85 template <typename Scalar>
86 7141 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calc(
87 const std::shared_ptr<DifferentialActionDataAbstract>& data,
88 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
89
2/4
✓ Branch 3 taken 7141 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 7141 times.
 7141 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
90 ✗ throw_pretty(
91 "Invalid argument: " << "x has wrong dimension (it should be " +
92 std::to_string(state_->get_nx()) + ")");
93 }
94
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7141 times.
 7141 if (static_cast<std::size_t>(u.size()) != nu_) {
95 ✗ throw_pretty(
96 "Invalid argument: " << "u has wrong dimension (it should be " +
97 std::to_string(nu_) + ")");
98 }
99 7141 Data* d = static_cast<Data*>(data.get());
100
1/2
✓ Branch 2 taken 7141 times.
✗ Branch 3 not taken.
 7141 const std::size_t nv = state_->get_nv();
101 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
102
2/4
✓ Branch 2 taken 7141 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7141 times.
✗ Branch 6 not taken.
 7141 x.head(state_->get_nq());
103 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
104
1/2
✓ Branch 1 taken 7141 times.
✗ Branch 2 not taken.
 7141 x.tail(nv);
105
106
1/2
✓ Branch 1 taken 7141 times.
✗ Branch 2 not taken.
 7141 d->xout = u;
107
1/2
✓ Branch 1 taken 7141 times.
✗ Branch 2 not taken.
 7141 pinocchio::rnea(*pinocchio_, d->pinocchio, q, v, u);
108
1/2
✓ Branch 1 taken 7141 times.
✗ Branch 2 not taken.
 7141 pinocchio::updateGlobalPlacements(*pinocchio_, d->pinocchio);
109
2/4
✓ Branch 2 taken 7141 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7141 times.
✗ Branch 6 not taken.
 7141 actuation_->commands(d->multibody.actuation, x, d->pinocchio.tau);
110
1/2
✓ Branch 2 taken 7141 times.
✗ Branch 3 not taken.
 7141 d->multibody.joint->a = u;
111
1/2
✓ Branch 3 taken 7141 times.
✗ Branch 4 not taken.
 7141 d->multibody.joint->tau = d->multibody.actuation->u;
112
2/4
✓ Branch 3 taken 7141 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7141 times.
✗ Branch 7 not taken.
 7141 actuation_->calc(d->multibody.actuation, x, d->multibody.joint->tau);
113
1/2
✓ Branch 2 taken 7141 times.
✗ Branch 3 not taken.
 7141 costs_->calc(d->costs, x, u);
114 7141 d->cost = d->costs->cost;
115
1/2
✓ Branch 2 taken 7141 times.
✗ Branch 3 not taken.
 7141 d->constraints->resize(this, d);
116
1/2
✓ Branch 2 taken 7141 times.
✗ Branch 3 not taken.
 7141 constraints_->calc(d->constraints, x, u);
117 7141 }
118
119 template <typename Scalar>
120 571 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calc(
121 const std::shared_ptr<DifferentialActionDataAbstract>& data,
122 const Eigen::Ref<const VectorXs>& x) {
123
2/4
✓ Branch 3 taken 571 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 571 times.
 571 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
124 ✗ throw_pretty(
125 "Invalid argument: " << "x has wrong dimension (it should be " +
126 std::to_string(state_->get_nx()) + ")");
127 }
128
129 571 Data* d = static_cast<Data*>(data.get());
130 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
131
2/4
✓ Branch 2 taken 571 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 571 times.
✗ Branch 6 not taken.
 571 x.head(state_->get_nq());
132 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
133
2/4
✓ Branch 2 taken 571 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 571 times.
✗ Branch 6 not taken.
 571 x.tail(state_->get_nv());
134
135
1/2
✓ Branch 1 taken 571 times.
✗ Branch 2 not taken.
 571 pinocchio::computeAllTerms(*pinocchio_, d->pinocchio, q, v);
136
137
1/2
✓ Branch 2 taken 571 times.
✗ Branch 3 not taken.
 571 costs_->calc(d->costs, x);
138 571 d->cost = d->costs->cost;
139
1/2
✓ Branch 2 taken 571 times.
✗ Branch 3 not taken.
 571 d->constraints->resize(this, d, false);
140
1/2
✓ Branch 2 taken 571 times.
✗ Branch 3 not taken.
 571 constraints_->calc(d->constraints, x);
141 571 }
142
143 template <typename Scalar>
144 1881 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calcDiff(
145 const std::shared_ptr<DifferentialActionDataAbstract>& data,
146 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
147
2/4
✓ Branch 3 taken 1881 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1881 times.
 1881 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
148 ✗ throw_pretty(
149 "Invalid argument: " << "x has wrong dimension (it should be " +
150 std::to_string(state_->get_nx()) + ")");
151 }
152
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1881 times.
 1881 if (static_cast<std::size_t>(u.size()) != nu_) {
153 ✗ throw_pretty(
154 "Invalid argument: " << "u has wrong dimension (it should be " +
155 std::to_string(nu_) + ")");
156 }
157 1881 Data* d = static_cast<Data*>(data.get());
158
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 const std::size_t nv = state_->get_nv();
159 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
160
2/4
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1881 times.
✗ Branch 6 not taken.
 1881 x.head(state_->get_nq());
161 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
162
1/2
✓ Branch 1 taken 1881 times.
✗ Branch 2 not taken.
 1881 x.tail(nv);
163
164
1/2
✓ Branch 1 taken 1881 times.
✗ Branch 2 not taken.
 1881 pinocchio::computeRNEADerivatives(*pinocchio_, d->pinocchio, q, v, u);
165
2/4
✓ Branch 1 taken 1881 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1881 times.
✗ Branch 5 not taken.
 1881 d->pinocchio.M.template triangularView<Eigen::StrictlyLower>() =
166
1/2
✓ Branch 1 taken 1881 times.
✗ Branch 2 not taken.
 1881 d->pinocchio.M.template triangularView<Eigen::StrictlyUpper>()
167
1/2
✓ Branch 1 taken 1881 times.
✗ Branch 2 not taken.
 1881 .transpose();
168
2/4
✓ Branch 3 taken 1881 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1881 times.
✗ Branch 7 not taken.
 1881 actuation_->calcDiff(d->multibody.actuation, x, d->multibody.joint->tau);
169
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 3762 actuation_->torqueTransform(d->multibody.actuation, x,
170
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 d->multibody.joint->tau);
171
3/6
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1881 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1881 times.
✗ Branch 9 not taken.
 1881 d->multibody.joint->dtau_dx.leftCols(nv).noalias() =
172
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 d->multibody.actuation->Mtau * d->pinocchio.dtau_dq;
173
3/6
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1881 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1881 times.
✗ Branch 9 not taken.
 1881 d->multibody.joint->dtau_dx.rightCols(nv).noalias() =
174
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 d->multibody.actuation->Mtau * d->pinocchio.dtau_dv;
175
2/4
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1881 times.
✗ Branch 6 not taken.
 1881 d->multibody.joint->dtau_du.noalias() =
176
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 d->multibody.actuation->Mtau * d->pinocchio.M;
177
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 costs_->calcDiff(d->costs, x, u);
178
1/2
✓ Branch 2 taken 1881 times.
✗ Branch 3 not taken.
 1881 constraints_->calcDiff(d->constraints, x, u);
179 1881 }
180
181 template <typename Scalar>
182 66 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calcDiff(
183 const std::shared_ptr<DifferentialActionDataAbstract>& data,
184 const Eigen::Ref<const VectorXs>& x) {
185
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 66 times.
 66 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
186 ✗ throw_pretty(
187 "Invalid argument: " << "x has wrong dimension (it should be " +
188 std::to_string(state_->get_nx()) + ")");
189 }
190 66 Data* d = static_cast<Data*>(data.get());
191
192 66 costs_->calcDiff(d->costs, x);
193 66 constraints_->calcDiff(d->constraints, x);
194 66 }
195
196 template <typename Scalar>
197 std::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
198 9894 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::createData() {
199
1/2
✓ Branch 2 taken 9894 times.
✗ Branch 3 not taken.
 9894 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
200 }
201
202 template <typename Scalar>
203 template <typename NewScalar>
204 DifferentialActionModelFreeInvDynamicsTpl<NewScalar>
205 ✗ DifferentialActionModelFreeInvDynamicsTpl<Scalar>::cast() const {
206 typedef DifferentialActionModelFreeInvDynamicsTpl<NewScalar> ReturnType;
207 typedef StateMultibodyTpl<NewScalar> StateType;
208 typedef CostModelSumTpl<NewScalar> CostType;
209 typedef ConstraintModelManagerTpl<NewScalar> ConstraintType;
210 ✗ if (constraints_) {
211 ✗ const std::shared_ptr<ConstraintType>& constraints =
212 std::make_shared<ConstraintType>(
213 ✗ constraints_->template cast<NewScalar>());
214 ✗ if (state_->get_nv() - actuation_->get_nu() > 0) {
215 ✗ constraints->removeConstraint("tau");
216 }
217 ✗ ReturnType ret(
218 ✗ std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
219 ✗ actuation_->template cast<NewScalar>(),
220 ✗ std::make_shared<CostType>(costs_->template cast<NewScalar>()),
221 constraints);
222 ✗ return ret;
223 ✗ } else {
224 ✗ ReturnType ret(
225 ✗ std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
226 ✗ actuation_->template cast<NewScalar>(),
227 ✗ std::make_shared<CostType>(costs_->template cast<NewScalar>()));
228 ✗ return ret;
229 }
230 }
231
232 template <typename Scalar>
233 1971 bool DifferentialActionModelFreeInvDynamicsTpl<Scalar>::checkData(
234 const std::shared_ptr<DifferentialActionDataAbstract>& data) {
235 1971 std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
236
1/2
✓ Branch 1 taken 1971 times.
✗ Branch 2 not taken.
 1971 if (d != NULL) {
237 1971 return true;
238 } else {
239 ✗ return false;
240 }
241 1971 }
242
243 template <typename Scalar>
244 963 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::quasiStatic(
245 const std::shared_ptr<DifferentialActionDataAbstract>&,
246 Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>&,
247 const std::size_t, const Scalar) {
248 963 u.setZero();
249 963 }
250
251 template <typename Scalar>
252 30 void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::print(
253 std::ostream& os) const {
254 30 os << "DifferentialActionModelFreeInvDynamics {nx=" << state_->get_nx()
255 30 << ", ndx=" << state_->get_ndx() << ", nu=" << nu_ << "}";
256 30 }
257
258 template <typename Scalar>
259 49135 std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_ng() const {
260
1/2
✓ Branch 1 taken 49135 times.
✗ Branch 2 not taken.
 49135 if (constraints_ != nullptr) {
261 49135 return constraints_->get_ng();
262 } else {
263 ✗ return Base::get_ng();
264 }
265 }
266
267 template <typename Scalar>
268 62545 std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_nh() const {
269
1/2
✓ Branch 1 taken 62545 times.
✗ Branch 2 not taken.
 62545 if (constraints_ != nullptr) {
270 62545 return constraints_->get_nh();
271 } else {
272 ✗ return Base::get_nh();
273 }
274 }
275
276 template <typename Scalar>
277 82723 std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_ng_T()
278 const {
279
1/2
✓ Branch 1 taken 82723 times.
✗ Branch 2 not taken.
 82723 if (constraints_ != nullptr) {
280 82723 return constraints_->get_ng_T();
281 } else {
282 ✗ return Base::get_ng_T();
283 }
284 }
285
286 template <typename Scalar>
287 69313 std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_nh_T()
288 const {
289
1/2
✓ Branch 1 taken 69313 times.
✗ Branch 2 not taken.
 69313 if (constraints_ != nullptr) {
290 69313 return constraints_->get_nh_T();
291 } else {
292 ✗ return Base::get_nh_T();
293 }
294 }
295
296 template <typename Scalar>
297 const typename MathBaseTpl<Scalar>::VectorXs&
298 ✗ DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_g_lb() const {
299 ✗ if (constraints_ != nullptr) {
300 ✗ return constraints_->get_lb();
301 } else {
302 ✗ return g_lb_;
303 }
304 }
305
306 template <typename Scalar>
307 const typename MathBaseTpl<Scalar>::VectorXs&
308 ✗ DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_g_ub() const {
309 ✗ if (constraints_ != nullptr) {
310 ✗ return constraints_->get_ub();
311 } else {
312 ✗ return g_lb_;
313 }
314 }
315
316 template <typename Scalar>
317 const std::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
318 19788 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_actuation() const {
319 19788 return actuation_;
320 }
321
322 template <typename Scalar>
323 const std::shared_ptr<CostModelSumTpl<Scalar> >&
324 9894 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_costs() const {
325 9894 return costs_;
326 }
327
328 template <typename Scalar>
329 const std::shared_ptr<ConstraintModelManagerTpl<Scalar> >&
330 9894 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_constraints() const {
331 9894 return constraints_;
332 }
333
334 template <typename Scalar>
335 pinocchio::ModelTpl<Scalar>&
336 9894 DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_pinocchio() const {
337 9894 return *pinocchio_;
338 }
339
340 } // namespace crocoddyl
341