GCC Code Coverage Report

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