GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/actions/free-fwddyn.hxx
Date: 2025-01-30 11:01:55
Exec Total Coverage
Lines: 147 168 87.5%
Branches: 130 396 32.8%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2024, LAAS-CNRS, University of Edinburgh,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include <pinocchio/algorithm/aba-derivatives.hpp>
11 #include <pinocchio/algorithm/aba.hpp>
12 #include <pinocchio/algorithm/cholesky.hpp>
13 #include <pinocchio/algorithm/compute-all-terms.hpp>
14 #include <pinocchio/algorithm/frames.hpp>
15 #include <pinocchio/algorithm/jacobian.hpp>
16 #include <pinocchio/algorithm/kinematics.hpp>
17 #include <pinocchio/algorithm/rnea-derivatives.hpp>
18 #include <pinocchio/algorithm/rnea.hpp>
19
20 #include "crocoddyl/core/utils/exception.hpp"
21 #include "crocoddyl/multibody/actions/free-fwddyn.hpp"
22
23 namespace crocoddyl {
24
25 template <typename Scalar>
26 160 DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::
27 DifferentialActionModelFreeFwdDynamicsTpl(
28 std::shared_ptr<StateMultibody> state,
29 std::shared_ptr<ActuationModelAbstract> actuation,
30 std::shared_ptr<CostModelSum> costs,
31 std::shared_ptr<ConstraintModelManager> constraints)
32 : Base(state, actuation->get_nu(), costs->get_nr(),
33 259 constraints != nullptr ? constraints->get_ng() : 0,
34 259 constraints != nullptr ? constraints->get_nh() : 0,
35 259 constraints != nullptr ? constraints->get_ng_T() : 0,
36 99 constraints != nullptr ? constraints->get_nh_T() : 0),
37 160 actuation_(actuation),
38 160 costs_(costs),
39 160 constraints_(constraints),
40 160 pinocchio_(*state->get_pinocchio().get()),
41 160 without_armature_(true),
42
11/14
✓ Branch 1 taken 99 times.
✓ Branch 2 taken 61 times.
✓ Branch 3 taken 99 times.
✓ Branch 4 taken 61 times.
✓ Branch 5 taken 99 times.
✓ Branch 6 taken 61 times.
✓ Branch 7 taken 99 times.
✓ Branch 8 taken 61 times.
✓ Branch 15 taken 160 times.
✗ Branch 16 not taken.
✓ Branch 22 taken 160 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 160 times.
✗ Branch 26 not taken.
 899 armature_(VectorXs::Zero(state->get_nv())) {
43
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 160 times.
 160 if (costs_->get_nu() != nu_) {
44 ✗ throw_pretty(
45 "Invalid argument: "
46 << "Costs doesn't have the same control dimension (it should be " +
47 std::to_string(nu_) + ")");
48 }
49
4/8
✓ Branch 1 taken 160 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 160 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 160 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 160 times.
✗ Branch 11 not taken.
 160 Base::set_u_lb(Scalar(-1.) * pinocchio_.effortLimit.tail(nu_));
50
4/8
✓ Branch 1 taken 160 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 160 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 160 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 160 times.
✗ Branch 11 not taken.
 160 Base::set_u_ub(Scalar(+1.) * pinocchio_.effortLimit.tail(nu_));
51 160 }
52
53 template <typename Scalar>
54 324 DifferentialActionModelFreeFwdDynamicsTpl<
55 324 Scalar>::~DifferentialActionModelFreeFwdDynamicsTpl() {}
56
57 template <typename Scalar>
58 8573 void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::calc(
59 const std::shared_ptr<DifferentialActionDataAbstract>& data,
60 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
61
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 8573 times.
 8573 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
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 8573 times.
 8573 if (static_cast<std::size_t>(u.size()) != nu_) {
67 ✗ throw_pretty(
68 "Invalid argument: " << "u has wrong dimension (it should be " +
69 std::to_string(nu_) + ")");
70 }
71
72 8573 Data* d = static_cast<Data*>(data.get());
73 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
74
1/2
✓ Branch 3 taken 8573 times.
✗ Branch 4 not taken.
 8573 x.head(state_->get_nq());
75 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
76
1/2
✓ Branch 3 taken 8573 times.
✗ Branch 4 not taken.
 8573 x.tail(state_->get_nv());
77
78
1/2
✓ Branch 2 taken 8573 times.
✗ Branch 3 not taken.
 8573 actuation_->calc(d->multibody.actuation, x, u);
79
80 // Computing the dynamics using ABA or manually for armature case
81
2/2
✓ Branch 0 taken 8571 times.
✓ Branch 1 taken 2 times.
 8573 if (without_armature_) {
82
1/2
✓ Branch 1 taken 8571 times.
✗ Branch 2 not taken.
 8571 d->xout = pinocchio::aba(pinocchio_, d->pinocchio, q, v,
83
1/2
✓ Branch 2 taken 8571 times.
✗ Branch 3 not taken.
 8571 d->multibody.actuation->tau);
84
1/2
✓ Branch 1 taken 8571 times.
✗ Branch 2 not taken.
 8571 pinocchio::updateGlobalPlacements(pinocchio_, d->pinocchio);
85 } else {
86
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
87
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 d->pinocchio.M.diagonal() += armature_;
88
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 pinocchio::cholesky::decompose(pinocchio_, d->pinocchio);
89
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 d->Minv.setZero();
90
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 pinocchio::cholesky::computeMinv(pinocchio_, d->pinocchio, d->Minv);
91
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 d->u_drift = d->multibody.actuation->tau - d->pinocchio.nle;
92
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 d->xout.noalias() = d->Minv * d->u_drift;
93 }
94
1/2
✓ Branch 2 taken 8573 times.
✗ Branch 3 not taken.
 8573 d->multibody.joint->a = d->xout;
95
1/2
✓ Branch 2 taken 8573 times.
✗ Branch 3 not taken.
 8573 d->multibody.joint->tau = u;
96
1/2
✓ Branch 2 taken 8573 times.
✗ Branch 3 not taken.
 8573 costs_->calc(d->costs, x, u);
97 8573 d->cost = d->costs->cost;
98
2/2
✓ Branch 1 taken 4829 times.
✓ Branch 2 taken 3744 times.
 8573 if (constraints_ != nullptr) {
99
1/2
✓ Branch 2 taken 4829 times.
✗ Branch 3 not taken.
 4829 d->constraints->resize(this, d);
100
1/2
✓ Branch 2 taken 4829 times.
✗ Branch 3 not taken.
 4829 constraints_->calc(d->constraints, x, u);
101 }
102 8573 }
103
104 template <typename Scalar>
105 694 void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::calc(
106 const std::shared_ptr<DifferentialActionDataAbstract>& data,
107 const Eigen::Ref<const VectorXs>& x) {
108
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 694 times.
 694 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
109 ✗ throw_pretty(
110 "Invalid argument: " << "x has wrong dimension (it should be " +
111 std::to_string(state_->get_nx()) + ")");
112 }
113
114 694 Data* d = static_cast<Data*>(data.get());
115 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
116
1/2
✓ Branch 3 taken 694 times.
✗ Branch 4 not taken.
 694 x.head(state_->get_nq());
117 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
118
1/2
✓ Branch 3 taken 694 times.
✗ Branch 4 not taken.
 694 x.tail(state_->get_nv());
119
120
1/2
✓ Branch 1 taken 694 times.
✗ Branch 2 not taken.
 694 pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
121
122
1/2
✓ Branch 2 taken 694 times.
✗ Branch 3 not taken.
 694 costs_->calc(d->costs, x);
123 694 d->cost = d->costs->cost;
124
2/2
✓ Branch 1 taken 394 times.
✓ Branch 2 taken 300 times.
 694 if (constraints_ != nullptr) {
125
1/2
✓ Branch 2 taken 394 times.
✗ Branch 3 not taken.
 394 d->constraints->resize(this, d, false);
126
1/2
✓ Branch 2 taken 394 times.
✗ Branch 3 not taken.
 394 constraints_->calc(d->constraints, x);
127 }
128 694 }
129
130 template <typename Scalar>
131 2894 void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::calcDiff(
132 const std::shared_ptr<DifferentialActionDataAbstract>& data,
133 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
134
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 2894 times.
 2894 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
135 ✗ throw_pretty(
136 "Invalid argument: " << "x has wrong dimension (it should be " +
137 std::to_string(state_->get_nx()) + ")");
138 }
139
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2894 times.
 2894 if (static_cast<std::size_t>(u.size()) != nu_) {
140 ✗ throw_pretty(
141 "Invalid argument: " << "u has wrong dimension (it should be " +
142 std::to_string(nu_) + ")");
143 }
144
145 2894 const std::size_t nv = state_->get_nv();
146 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
147
1/2
✓ Branch 3 taken 2894 times.
✗ Branch 4 not taken.
 2894 x.head(state_->get_nq());
148 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
149
1/2
✓ Branch 1 taken 2894 times.
✗ Branch 2 not taken.
 2894 x.tail(nv);
150
151 2894 Data* d = static_cast<Data*>(data.get());
152
153
1/2
✓ Branch 2 taken 2894 times.
✗ Branch 3 not taken.
 2894 actuation_->calcDiff(d->multibody.actuation, x, u);
154
155 // Computing the dynamics derivatives
156
2/2
✓ Branch 0 taken 2893 times.
✓ Branch 1 taken 1 times.
 2894 if (without_armature_) {
157 2893 pinocchio::computeABADerivatives(
158
1/2
✓ Branch 2 taken 2893 times.
✗ Branch 3 not taken.
 2893 pinocchio_, d->pinocchio, q, v, d->multibody.actuation->tau,
159
2/4
✓ Branch 1 taken 2893 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2893 times.
✗ Branch 5 not taken.
 2893 d->Fx.leftCols(nv), d->Fx.rightCols(nv), d->pinocchio.Minv);
160
3/6
✓ Branch 2 taken 2893 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2893 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2893 times.
✗ Branch 9 not taken.
 2893 d->Fx.noalias() += d->pinocchio.Minv * d->multibody.actuation->dtau_dx;
161
3/6
✓ Branch 2 taken 2893 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2893 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2893 times.
✗ Branch 9 not taken.
 2893 d->Fu.noalias() = d->pinocchio.Minv * d->multibody.actuation->dtau_du;
162 } else {
163
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, v, d->xout);
164
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 d->dtau_dx.leftCols(nv) =
165
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 d->multibody.actuation->dtau_dx.leftCols(nv) - d->pinocchio.dtau_dq;
166
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 d->dtau_dx.rightCols(nv) =
167
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 d->multibody.actuation->dtau_dx.rightCols(nv) - d->pinocchio.dtau_dv;
168
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 d->Fx.noalias() = d->Minv * d->dtau_dx;
169
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 d->Fu.noalias() = d->Minv * d->multibody.actuation->dtau_du;
170 }
171
1/2
✓ Branch 2 taken 2894 times.
✗ Branch 3 not taken.
 2894 d->multibody.joint->da_dx = d->Fx;
172
1/2
✓ Branch 2 taken 2894 times.
✗ Branch 3 not taken.
 2894 d->multibody.joint->da_du = d->Fu;
173
1/2
✓ Branch 2 taken 2894 times.
✗ Branch 3 not taken.
 2894 costs_->calcDiff(d->costs, x, u);
174
2/2
✓ Branch 1 taken 1254 times.
✓ Branch 2 taken 1640 times.
 2894 if (constraints_ != nullptr) {
175
1/2
✓ Branch 2 taken 1254 times.
✗ Branch 3 not taken.
 1254 constraints_->calcDiff(d->constraints, x, u);
176 }
177 2894 }
178
179 template <typename Scalar>
180 142 void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::calcDiff(
181 const std::shared_ptr<DifferentialActionDataAbstract>& data,
182 const Eigen::Ref<const VectorXs>& x) {
183
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 142 times.
 142 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
184 ✗ throw_pretty(
185 "Invalid argument: " << "x has wrong dimension (it should be " +
186 std::to_string(state_->get_nx()) + ")");
187 }
188 142 Data* d = static_cast<Data*>(data.get());
189
190 142 costs_->calcDiff(d->costs, x);
191
2/2
✓ Branch 1 taken 44 times.
✓ Branch 2 taken 98 times.
 142 if (constraints_ != nullptr) {
192 44 constraints_->calcDiff(d->constraints, x);
193 }
194 142 }
195
196 template <typename Scalar>
197 std::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
198 10406 DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::createData() {
199
1/2
✓ Branch 2 taken 10406 times.
✗ Branch 3 not taken.
 10406 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
200 }
201
202 template <typename Scalar>
203 1971 bool DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::checkData(
204 const std::shared_ptr<DifferentialActionDataAbstract>& data) {
205 1971 std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
206
1/2
✓ Branch 1 taken 1971 times.
✗ Branch 2 not taken.
 1971 if (d != NULL) {
207 1971 return true;
208 } else {
209 ✗ return false;
210 }
211 1971 }
212
213 template <typename Scalar>
214 962 void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::quasiStatic(
215 const std::shared_ptr<DifferentialActionDataAbstract>& data,
216 Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
217 const std::size_t, const Scalar) {
218
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 962 times.
 962 if (static_cast<std::size_t>(u.size()) != nu_) {
219 ✗ throw_pretty(
220 "Invalid argument: " << "u has wrong dimension (it should be " +
221 std::to_string(nu_) + ")");
222 }
223
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 962 times.
 962 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
224 ✗ throw_pretty(
225 "Invalid argument: " << "x has wrong dimension (it should be " +
226 std::to_string(state_->get_nx()) + ")");
227 }
228 // Static casting the data
229 962 Data* d = static_cast<Data*>(data.get());
230 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
231
1/2
✓ Branch 3 taken 962 times.
✗ Branch 4 not taken.
 962 x.head(state_->get_nq());
232
233 962 const std::size_t nq = state_->get_nq();
234 962 const std::size_t nv = state_->get_nv();
235
236
2/4
✓ Branch 1 taken 962 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 962 times.
✗ Branch 5 not taken.
 962 d->tmp_xstatic.head(nq) = q;
237
2/4
✓ Branch 1 taken 962 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 962 times.
✗ Branch 5 not taken.
 962 d->tmp_xstatic.tail(nv).setZero();
238
1/2
✓ Branch 1 taken 962 times.
✗ Branch 2 not taken.
 962 u.setZero();
239
240
2/4
✓ Branch 1 taken 962 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 962 times.
✗ Branch 5 not taken.
 962 pinocchio::rnea(pinocchio_, d->pinocchio, q, d->tmp_xstatic.tail(nv),
241
1/2
✓ Branch 1 taken 962 times.
✗ Branch 2 not taken.
 962 d->tmp_xstatic.tail(nv));
242
3/6
✓ Branch 2 taken 962 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 962 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 962 times.
✗ Branch 9 not taken.
 962 actuation_->calc(d->multibody.actuation, d->tmp_xstatic, u);
243
3/6
✓ Branch 2 taken 962 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 962 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 962 times.
✗ Branch 9 not taken.
 962 actuation_->calcDiff(d->multibody.actuation, d->tmp_xstatic, u);
244
245
2/4
✓ Branch 1 taken 962 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 962 times.
✗ Branch 5 not taken.
 962 u.noalias() =
246
2/4
✓ Branch 3 taken 962 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 962 times.
✗ Branch 7 not taken.
 1924 pseudoInverse(d->multibody.actuation->dtau_du) * d->pinocchio.tau;
247
1/2
✓ Branch 1 taken 962 times.
✗ Branch 2 not taken.
 962 d->pinocchio.tau.setZero();
248 962 }
249
250 template <typename Scalar>
251 51526 std::size_t DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_ng() const {
252
2/2
✓ Branch 1 taken 33094 times.
✓ Branch 2 taken 18432 times.
 51526 if (constraints_ != nullptr) {
253 33094 return constraints_->get_ng();
254 } else {
255 18432 return Base::get_ng();
256 }
257 }
258
259 template <typename Scalar>
260 51262 std::size_t DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_nh() const {
261
2/2
✓ Branch 1 taken 33094 times.
✓ Branch 2 taken 18168 times.
 51262 if (constraints_ != nullptr) {
262 33094 return constraints_->get_nh();
263 } else {
264 18168 return Base::get_nh();
265 }
266 }
267
268 template <typename Scalar>
269 88560 std::size_t DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_ng_T()
270 const {
271
2/2
✓ Branch 1 taken 55700 times.
✓ Branch 2 taken 32860 times.
 88560 if (constraints_ != nullptr) {
272 55700 return constraints_->get_ng_T();
273 } else {
274 32860 return Base::get_ng_T();
275 }
276 }
277
278 template <typename Scalar>
279 88536 std::size_t DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_nh_T()
280 const {
281
2/2
✓ Branch 1 taken 55700 times.
✓ Branch 2 taken 32836 times.
 88536 if (constraints_ != nullptr) {
282 55700 return constraints_->get_nh_T();
283 } else {
284 32836 return Base::get_nh_T();
285 }
286 }
287
288 template <typename Scalar>
289 const typename MathBaseTpl<Scalar>::VectorXs&
290 ✗ DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_g_lb() const {
291 ✗ if (constraints_ != nullptr) {
292 ✗ return constraints_->get_lb();
293 } else {
294 ✗ return g_lb_;
295 }
296 }
297
298 template <typename Scalar>
299 const typename MathBaseTpl<Scalar>::VectorXs&
300 ✗ DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_g_ub() const {
301 ✗ if (constraints_ != nullptr) {
302 ✗ return constraints_->get_ub();
303 } else {
304 ✗ return g_lb_;
305 }
306 }
307
308 template <typename Scalar>
309 30 void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::print(
310 std::ostream& os) const {
311 30 os << "DifferentialActionModelFreeFwdDynamics {nx=" << state_->get_nx()
312 30 << ", ndx=" << state_->get_ndx() << ", nu=" << nu_ << "}";
313 30 }
314
315 template <typename Scalar>
316 pinocchio::ModelTpl<Scalar>&
317 10406 DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_pinocchio() const {
318 10406 return pinocchio_;
319 }
320
321 template <typename Scalar>
322 const std::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
323 20812 DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_actuation() const {
324 20812 return actuation_;
325 }
326
327 template <typename Scalar>
328 const std::shared_ptr<CostModelSumTpl<Scalar> >&
329 10406 DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_costs() const {
330 10406 return costs_;
331 }
332
333 template <typename Scalar>
334 const std::shared_ptr<ConstraintModelManagerTpl<Scalar> >&
335 17069 DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_constraints() const {
336 17069 return constraints_;
337 }
338
339 template <typename Scalar>
340 const typename MathBaseTpl<Scalar>::VectorXs&
341 ✗ DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_armature() const {
342 ✗ return armature_;
343 }
344
345 template <typename Scalar>
346 1 void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::set_armature(
347 const VectorXs& armature) {
348
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (static_cast<std::size_t>(armature.size()) != state_->get_nv()) {
349 ✗ throw_pretty("Invalid argument: "
350 << "The armature dimension is wrong (it should be " +
351 std::to_string(state_->get_nv()) + ")");
352 }
353
354 1 armature_ = armature;
355 1 without_armature_ = false;
356 1 }
357
358 } // namespace crocoddyl
359