GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/actions/impulse-fwddyn.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 162 221 73.3%
Branches: 123 386 31.9%
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 namespace crocoddyl {
11
12 template <typename Scalar>
13 69 ActionModelImpulseFwdDynamicsTpl<Scalar>::ActionModelImpulseFwdDynamicsTpl(
14 std::shared_ptr<StateMultibody> state,
15 std::shared_ptr<ImpulseModelMultiple> impulses,
16 std::shared_ptr<CostModelSum> costs, const Scalar r_coeff,
17 const Scalar JMinvJt_damping, const bool enable_force)
18 : Base(state, 0, costs->get_nr(), 0, 0),
19 69 impulses_(impulses),
20 69 costs_(costs),
21 69 constraints_(nullptr),
22
1/2
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
 69 pinocchio_(state->get_pinocchio().get()),
23 69 with_armature_(true),
24
3/6
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 69 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 69 times.
✗ Branch 9 not taken.
 69 armature_(VectorXs::Zero(state->get_nv())),
25 69 r_coeff_(r_coeff),
26 69 JMinvJt_damping_(JMinvJt_damping),
27 69 enable_force_(enable_force),
28
3/6
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
✓ Branch 10 taken 69 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 69 times.
✗ Branch 15 not taken.
 138 gravity_(state->get_pinocchio()->gravity) {
29
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 init();
30 69 }
31
32 template <typename Scalar>
33 12 ActionModelImpulseFwdDynamicsTpl<Scalar>::ActionModelImpulseFwdDynamicsTpl(
34 std::shared_ptr<StateMultibody> state,
35 std::shared_ptr<ImpulseModelMultiple> impulses,
36 std::shared_ptr<CostModelSum> costs,
37 std::shared_ptr<ConstraintModelManager> constraints, const Scalar r_coeff,
38 const Scalar JMinvJt_damping, const bool enable_force)
39 : Base(state, 0, costs->get_nr(), constraints->get_ng(),
40 constraints->get_nh(), constraints->get_ng_T(),
41 constraints->get_nh_T()),
42 12 impulses_(impulses),
43 12 costs_(costs),
44 12 constraints_(constraints),
45
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 pinocchio_(state->get_pinocchio().get()),
46 12 with_armature_(true),
47
3/6
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12 times.
✗ Branch 9 not taken.
 12 armature_(VectorXs::Zero(state->get_nv())),
48 12 r_coeff_(r_coeff),
49 12 JMinvJt_damping_(JMinvJt_damping),
50 12 enable_force_(enable_force),
51
3/6
✓ Branch 12 taken 12 times.
✗ Branch 13 not taken.
✓ Branch 18 taken 12 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 12 times.
✗ Branch 23 not taken.
 24 gravity_(state->get_pinocchio()->gravity) {
52
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 init();
53 12 }
54
55 template <typename Scalar>
56 81 void ActionModelImpulseFwdDynamicsTpl<Scalar>::init() {
57
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 81 times.
 81 if (r_coeff_ < Scalar(0.)) {
58 ✗ r_coeff_ = Scalar(0.);
59 ✗ throw_pretty("Invalid argument: "
60 << "The restitution coefficient has to be positive, set to 0");
61 }
62
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 81 times.
 81 if (JMinvJt_damping_ < Scalar(0.)) {
63 ✗ JMinvJt_damping_ = Scalar(0.);
64 ✗ throw_pretty("Invalid argument: "
65 << "The damping factor has to be positive, set to 0");
66 }
67 81 }
68
69 template <typename Scalar>
70 4050 void ActionModelImpulseFwdDynamicsTpl<Scalar>::calc(
71 const std::shared_ptr<ActionDataAbstract>& data,
72 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
73 4050 Data* d = static_cast<Data*>(data.get());
74
75 // Computing impulse dynamics and forces
76 4050 initCalc(d, x);
77
78 // Computing the cost and constraints
79 4050 costs_->calc(d->costs, x, u);
80 4050 d->cost = d->costs->cost;
81
2/2
✓ Branch 1 taken 796 times.
✓ Branch 2 taken 3254 times.
 4050 if (constraints_ != nullptr) {
82 796 d->constraints->resize(this, d);
83 796 constraints_->calc(d->constraints, x, u);
84 }
85 4050 }
86
87 template <typename Scalar>
88 2930 void ActionModelImpulseFwdDynamicsTpl<Scalar>::calc(
89 const std::shared_ptr<ActionDataAbstract>& data,
90 const Eigen::Ref<const VectorXs>& x) {
91 2930 Data* d = static_cast<Data*>(data.get());
92
93 // Computing impulse dynamics and forces
94 2930 initCalc(d, x);
95
96 // Computing the cost and constraints
97 2930 costs_->calc(d->costs, x);
98 2930 d->cost = d->costs->cost;
99
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2930 times.
 2930 if (constraints_ != nullptr) {
100 ✗ d->constraints->resize(this, d, false);
101 ✗ constraints_->calc(d->constraints, x);
102 }
103 2930 }
104
105 template <typename Scalar>
106 196 void ActionModelImpulseFwdDynamicsTpl<Scalar>::calcDiff(
107 const std::shared_ptr<ActionDataAbstract>& data,
108 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
109 196 Data* d = static_cast<Data*>(data.get());
110
111 // Computing derivatives of impulse dynamics and forces
112 196 initCalcDiff(d, x);
113
114 // Computing derivatives of cost and constraints
115 196 costs_->calcDiff(d->costs, x, u);
116
2/2
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 184 times.
 196 if (constraints_ != nullptr) {
117 12 constraints_->calcDiff(d->constraints, x, u);
118 }
119 196 }
120
121 template <typename Scalar>
122 52 void ActionModelImpulseFwdDynamicsTpl<Scalar>::calcDiff(
123 const std::shared_ptr<ActionDataAbstract>& data,
124 const Eigen::Ref<const VectorXs>& x) {
125 52 Data* d = static_cast<Data*>(data.get());
126
127 // Computing derivatives of impulse dynamics and forces
128 52 initCalcDiff(d, x);
129
130 // Computing derivatives of cost and constraints
131 52 costs_->calcDiff(d->costs, x);
132
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 52 times.
 52 if (constraints_ != nullptr) {
133 ✗ constraints_->calcDiff(d->constraints, x);
134 }
135 52 }
136
137 template <typename Scalar>
138 std::shared_ptr<ActionDataAbstractTpl<Scalar> >
139 4256 ActionModelImpulseFwdDynamicsTpl<Scalar>::createData() {
140
1/2
✓ Branch 2 taken 4256 times.
✗ Branch 3 not taken.
 4256 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
141 }
142
143 template <typename Scalar>
144 template <typename NewScalar>
145 ActionModelImpulseFwdDynamicsTpl<NewScalar>
146 ✗ ActionModelImpulseFwdDynamicsTpl<Scalar>::cast() const {
147 typedef ActionModelImpulseFwdDynamicsTpl<NewScalar> ReturnType;
148 typedef StateMultibodyTpl<NewScalar> StateType;
149 typedef ImpulseModelMultipleTpl<NewScalar> ImpulseType;
150 typedef CostModelSumTpl<NewScalar> CostType;
151 typedef ConstraintModelManagerTpl<NewScalar> ConstraintType;
152 ✗ if (constraints_) {
153 ✗ ReturnType ret(
154 ✗ std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
155 ✗ std::make_shared<ImpulseType>(impulses_->template cast<NewScalar>()),
156 ✗ std::make_shared<CostType>(costs_->template cast<NewScalar>()),
157 std::make_shared<ConstraintType>(
158 ✗ constraints_->template cast<NewScalar>()),
159 ✗ scalar_cast<NewScalar>(r_coeff_),
160 ✗ scalar_cast<NewScalar>(JMinvJt_damping_), enable_force_);
161 ✗ return ret;
162 ✗ } else {
163 ✗ ReturnType ret(
164 ✗ std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
165 ✗ std::make_shared<ImpulseType>(impulses_->template cast<NewScalar>()),
166 ✗ std::make_shared<CostType>(costs_->template cast<NewScalar>()),
167 ✗ scalar_cast<NewScalar>(r_coeff_),
168 ✗ scalar_cast<NewScalar>(JMinvJt_damping_), enable_force_);
169 ✗ return ret;
170 }
171 }
172
173 template <typename Scalar>
174 128 bool ActionModelImpulseFwdDynamicsTpl<Scalar>::checkData(
175 const std::shared_ptr<ActionDataAbstract>& data) {
176 128 std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
177
1/2
✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
 128 if (d != NULL) {
178 128 return true;
179 } else {
180 ✗ return false;
181 }
182 128 }
183
184 template <typename Scalar>
185 160 void ActionModelImpulseFwdDynamicsTpl<Scalar>::quasiStatic(
186 const std::shared_ptr<ActionDataAbstract>&, Eigen::Ref<VectorXs>,
187 const Eigen::Ref<const VectorXs>&, const std::size_t, const Scalar) {
188 // do nothing
189 160 }
190
191 template <typename Scalar>
192 6980 void ActionModelImpulseFwdDynamicsTpl<Scalar>::initCalc(
193 Data* data, const Eigen::Ref<const VectorXs>& x) {
194
2/4
✓ Branch 3 taken 6980 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 6980 times.
 6980 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
195 ✗ throw_pretty(
196 "Invalid argument: " << "x has wrong dimension (it should be " +
197 std::to_string(state_->get_nx()) + ")");
198 }
199
200
1/2
✓ Branch 2 taken 6980 times.
✗ Branch 3 not taken.
 6980 const std::size_t nq = state_->get_nq();
201
1/2
✓ Branch 2 taken 6980 times.
✗ Branch 3 not taken.
 6980 const std::size_t nv = state_->get_nv();
202
1/2
✓ Branch 2 taken 6980 times.
✗ Branch 3 not taken.
 6980 const std::size_t nc = impulses_->get_nc();
203 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
204
1/2
✓ Branch 1 taken 6980 times.
✗ Branch 2 not taken.
 6980 x.head(nq);
205 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
206
1/2
✓ Branch 1 taken 6980 times.
✗ Branch 2 not taken.
 6980 x.tail(nv);
207
208 // Computing the forward dynamics with the holonomic constraints defined by
209 // the contact model
210
1/2
✓ Branch 1 taken 6980 times.
✗ Branch 2 not taken.
 6980 pinocchio::computeAllTerms(*pinocchio_, data->pinocchio, q, v);
211
1/2
✓ Branch 1 taken 6980 times.
✗ Branch 2 not taken.
 6980 pinocchio::computeCentroidalMomentum(*pinocchio_, data->pinocchio);
212
213
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 6980 times.
 6980 if (!with_armature_) {
214 ✗ data->pinocchio.M.diagonal() += armature_;
215 }
216
1/2
✓ Branch 2 taken 6980 times.
✗ Branch 3 not taken.
 6980 impulses_->calc(data->multibody.impulses, x);
217
218 #ifndef NDEBUG
219
2/4
✓ Branch 2 taken 6980 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6980 times.
✗ Branch 6 not taken.
 6980 Eigen::FullPivLU<MatrixXs> Jc_lu(data->multibody.impulses->Jc.topRows(nc));
220
221
4/8
✓ Branch 1 taken 6980 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6980 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 6980 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 6980 times.
 6980 if (Jc_lu.rank() < data->multibody.impulses->Jc.topRows(nc).rows() &&
222 ✗ JMinvJt_damping_ == Scalar(0.)) {
223 ✗ throw_pretty(
224 "It is needed a damping factor since the contact Jacobian is not "
225 "full-rank");
226 }
227 #endif
228
229
1/2
✓ Branch 1 taken 6980 times.
✗ Branch 2 not taken.
 6980 pinocchio::impulseDynamics(*pinocchio_, data->pinocchio, v,
230
1/2
✓ Branch 2 taken 6980 times.
✗ Branch 3 not taken.
 6980 data->multibody.impulses->Jc.topRows(nc), r_coeff_,
231 JMinvJt_damping_);
232
2/4
✓ Branch 1 taken 6980 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6980 times.
✗ Branch 5 not taken.
 6980 data->xnext.head(nq) = q;
233
2/4
✓ Branch 1 taken 6980 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6980 times.
✗ Branch 5 not taken.
 6980 data->xnext.tail(nv) = data->pinocchio.dq_after;
234
1/2
✓ Branch 2 taken 6980 times.
✗ Branch 3 not taken.
 6980 impulses_->updateVelocity(data->multibody.impulses, data->pinocchio.dq_after);
235
1/2
✓ Branch 2 taken 6980 times.
✗ Branch 3 not taken.
 6980 impulses_->updateForce(data->multibody.impulses, data->pinocchio.impulse_c);
236 6980 }
237
238 template <typename Scalar>
239 248 void ActionModelImpulseFwdDynamicsTpl<Scalar>::initCalcDiff(
240 Data* data, const Eigen::Ref<const VectorXs>& x) {
241
2/4
✓ Branch 3 taken 248 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 248 times.
 248 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
242 ✗ throw_pretty(
243 "Invalid argument: " << "x has wrong dimension (it should be " +
244 std::to_string(state_->get_nx()) + ")");
245 }
246
247
1/2
✓ Branch 2 taken 248 times.
✗ Branch 3 not taken.
 248 const std::size_t nv = state_->get_nv();
248
1/2
✓ Branch 2 taken 248 times.
✗ Branch 3 not taken.
 248 const std::size_t nc = impulses_->get_nc();
249 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
250
2/4
✓ Branch 2 taken 248 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 248 times.
✗ Branch 6 not taken.
 248 x.head(state_->get_nq());
251 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
252
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 x.tail(nv);
253
254 // Computing the dynamics derivatives
255 // We resize the Kinv matrix because Eigen cannot call block operations
256 // recursively: https://eigen.tuxfamily.org/bz/show_bug.cgi?id=408. Therefore,
257 // it is not possible to pass d->Kinv.topLeftCorner(nv + nc, nv + nc)
258
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 data->Kinv.resize(nv + nc, nv + nc);
259 ✗ pinocchio::computeRNEADerivatives(*pinocchio_, data->pinocchio, q,
260
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 data->vnone, data->pinocchio.dq_after - v,
261
1/2
✓ Branch 2 taken 248 times.
✗ Branch 3 not taken.
 248 data->multibody.impulses->fext);
262 248 pinocchio::computeGeneralizedGravityDerivatives(*pinocchio_, data->pinocchio,
263
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 q, data->dgrav_dq);
264 248 pinocchio::getKKTContactDynamicMatrixInverse(
265
2/4
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 248 times.
✗ Branch 5 not taken.
 248 *pinocchio_, data->pinocchio, data->multibody.impulses->Jc.topRows(nc),
266 248 data->Kinv);
267
268 248 pinocchio::computeForwardKinematicsDerivatives(
269
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 *pinocchio_, data->pinocchio, q, data->pinocchio.dq_after, data->vnone);
270
1/2
✓ Branch 2 taken 248 times.
✗ Branch 3 not taken.
 248 impulses_->calcDiff(data->multibody.impulses, x);
271
1/2
✓ Branch 2 taken 248 times.
✗ Branch 3 not taken.
 248 impulses_->updateRneaDiff(data->multibody.impulses, data->pinocchio);
272
273
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 Eigen::Block<MatrixXs> a_partial_dtau = data->Kinv.topLeftCorner(nv, nv);
274
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 Eigen::Block<MatrixXs> a_partial_da = data->Kinv.topRightCorner(nv, nc);
275
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 Eigen::Block<MatrixXs> f_partial_dtau = data->Kinv.bottomLeftCorner(nc, nv);
276
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 Eigen::Block<MatrixXs> f_partial_da = data->Kinv.bottomRightCorner(nc, nc);
277
278
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 data->pinocchio.dtau_dq -= data->dgrav_dq;
279
2/4
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 248 times.
✗ Branch 5 not taken.
 248 data->pinocchio.M.template triangularView<Eigen::StrictlyLower>() =
280
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 data->pinocchio.M.transpose()
281
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 .template triangularView<Eigen::StrictlyLower>();
282
2/4
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 248 times.
✗ Branch 5 not taken.
 248 data->Fx.topLeftCorner(nv, nv).setIdentity();
283
2/4
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 248 times.
✗ Branch 5 not taken.
 248 data->Fx.topRightCorner(nv, nv).setZero();
284
4/8
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 248 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 248 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 248 times.
✗ Branch 11 not taken.
 248 data->Fx.bottomLeftCorner(nv, nv).noalias() =
285
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 -a_partial_dtau * data->pinocchio.dtau_dq;
286
3/6
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 248 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 248 times.
✗ Branch 8 not taken.
 248 data->Fx.bottomLeftCorner(nv, nv).noalias() -=
287
2/4
✓ Branch 2 taken 248 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 248 times.
✗ Branch 6 not taken.
 248 a_partial_da * data->multibody.impulses->dv0_dq.topRows(nc);
288
3/6
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 248 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 248 times.
✗ Branch 8 not taken.
 248 data->Fx.bottomRightCorner(nv, nv).noalias() =
289
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 a_partial_dtau * data->pinocchio.M;
290
291 // Computing the cost derivatives
292
1/2
✓ Branch 0 taken 248 times.
✗ Branch 1 not taken.
 248 if (enable_force_) {
293
3/6
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 248 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 248 times.
✗ Branch 8 not taken.
 248 data->df_dx.topLeftCorner(nc, nv).noalias() =
294
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 f_partial_dtau * data->pinocchio.dtau_dq;
295
3/6
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 248 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 248 times.
✗ Branch 8 not taken.
 248 data->df_dx.topLeftCorner(nc, nv).noalias() +=
296
2/4
✓ Branch 2 taken 248 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 248 times.
✗ Branch 6 not taken.
 248 f_partial_da * data->multibody.impulses->dv0_dq.topRows(nc);
297
3/6
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 248 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 248 times.
✗ Branch 8 not taken.
 248 data->df_dx.topRightCorner(nc, nv).noalias() =
298
2/4
✓ Branch 2 taken 248 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 248 times.
✗ Branch 6 not taken.
 248 f_partial_da * data->multibody.impulses->Jc.topRows(nc);
299
2/4
✓ Branch 2 taken 248 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 248 times.
✗ Branch 6 not taken.
 496 impulses_->updateVelocityDiff(data->multibody.impulses,
300
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 data->Fx.bottomRows(nv));
301
2/4
✓ Branch 2 taken 248 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 248 times.
✗ Branch 6 not taken.
 496 impulses_->updateForceDiff(data->multibody.impulses,
302
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 496 data->df_dx.topRows(nc));
303 }
304 248 }
305
306 template <typename Scalar>
307 13716 std::size_t ActionModelImpulseFwdDynamicsTpl<Scalar>::get_ng() const {
308
2/2
✓ Branch 1 taken 3208 times.
✓ Branch 2 taken 10508 times.
 13716 if (constraints_ != nullptr) {
309 3208 return constraints_->get_ng();
310 } else {
311 10508 return Base::get_ng();
312 }
313 }
314
315 template <typename Scalar>
316 13676 std::size_t ActionModelImpulseFwdDynamicsTpl<Scalar>::get_nh() const {
317
2/2
✓ Branch 1 taken 3208 times.
✓ Branch 2 taken 10468 times.
 13676 if (constraints_ != nullptr) {
318 3208 return constraints_->get_nh();
319 } else {
320 10468 return Base::get_nh();
321 }
322 }
323
324 template <typename Scalar>
325 25640 std::size_t ActionModelImpulseFwdDynamicsTpl<Scalar>::get_ng_T() const {
326
2/2
✓ Branch 1 taken 4788 times.
✓ Branch 2 taken 20852 times.
 25640 if (constraints_ != nullptr) {
327 4788 return constraints_->get_ng_T();
328 } else {
329 20852 return Base::get_ng_T();
330 }
331 }
332
333 template <typename Scalar>
334 25636 std::size_t ActionModelImpulseFwdDynamicsTpl<Scalar>::get_nh_T() const {
335
2/2
✓ Branch 1 taken 4788 times.
✓ Branch 2 taken 20848 times.
 25636 if (constraints_ != nullptr) {
336 4788 return constraints_->get_nh_T();
337 } else {
338 20848 return Base::get_nh_T();
339 }
340 }
341
342 template <typename Scalar>
343 const typename MathBaseTpl<Scalar>::VectorXs&
344 ✗ ActionModelImpulseFwdDynamicsTpl<Scalar>::get_g_lb() const {
345 ✗ if (constraints_ != nullptr) {
346 ✗ return constraints_->get_lb();
347 } else {
348 ✗ return g_lb_;
349 }
350 }
351
352 template <typename Scalar>
353 const typename MathBaseTpl<Scalar>::VectorXs&
354 ✗ ActionModelImpulseFwdDynamicsTpl<Scalar>::get_g_ub() const {
355 ✗ if (constraints_ != nullptr) {
356 ✗ return constraints_->get_ub();
357 } else {
358 ✗ return g_lb_;
359 }
360 }
361
362 template <typename Scalar>
363 44 void ActionModelImpulseFwdDynamicsTpl<Scalar>::print(std::ostream& os) const {
364 44 os << "ActionModelImpulseFwdDynamics {nx=" << state_->get_nx()
365 44 << ", ndx=" << state_->get_ndx() << ", nc=" << impulses_->get_nc() << "}";
366 44 }
367
368 template <typename Scalar>
369 pinocchio::ModelTpl<Scalar>&
370 4256 ActionModelImpulseFwdDynamicsTpl<Scalar>::get_pinocchio() const {
371 4256 return *pinocchio_;
372 }
373
374 template <typename Scalar>
375 const std::shared_ptr<ImpulseModelMultipleTpl<Scalar> >&
376 17024 ActionModelImpulseFwdDynamicsTpl<Scalar>::get_impulses() const {
377 17024 return impulses_;
378 }
379
380 template <typename Scalar>
381 const std::shared_ptr<CostModelSumTpl<Scalar> >&
382 4442 ActionModelImpulseFwdDynamicsTpl<Scalar>::get_costs() const {
383 4442 return costs_;
384 }
385
386 template <typename Scalar>
387 const std::shared_ptr<ConstraintModelManagerTpl<Scalar> >&
388 5052 ActionModelImpulseFwdDynamicsTpl<Scalar>::get_constraints() const {
389 5052 return constraints_;
390 }
391
392 template <typename Scalar>
393 const typename MathBaseTpl<Scalar>::VectorXs&
394 ✗ ActionModelImpulseFwdDynamicsTpl<Scalar>::get_armature() const {
395 ✗ return armature_;
396 }
397
398 template <typename Scalar>
399 const Scalar
400 ✗ ActionModelImpulseFwdDynamicsTpl<Scalar>::get_restitution_coefficient() const {
401 ✗ return r_coeff_;
402 }
403
404 template <typename Scalar>
405 ✗ const Scalar ActionModelImpulseFwdDynamicsTpl<Scalar>::get_damping_factor()
406 const {
407 ✗ return JMinvJt_damping_;
408 }
409
410 template <typename Scalar>
411 ✗ void ActionModelImpulseFwdDynamicsTpl<Scalar>::set_armature(
412 const VectorXs& armature) {
413 ✗ if (static_cast<std::size_t>(armature.size()) != state_->get_nv()) {
414 ✗ throw_pretty("Invalid argument: "
415 << "The armature dimension is wrong (it should be " +
416 std::to_string(state_->get_nv()) + ")");
417 }
418 ✗ armature_ = armature;
419 ✗ with_armature_ = false;
420 }
421
422 template <typename Scalar>
423 ✗ void ActionModelImpulseFwdDynamicsTpl<Scalar>::set_restitution_coefficient(
424 const Scalar r_coeff) {
425 ✗ if (r_coeff < 0.) {
426 ✗ throw_pretty("Invalid argument: "
427 << "The restitution coefficient has to be positive");
428 }
429 ✗ r_coeff_ = r_coeff;
430 }
431
432 template <typename Scalar>
433 ✗ void ActionModelImpulseFwdDynamicsTpl<Scalar>::set_damping_factor(
434 const Scalar damping) {
435 ✗ if (damping < 0.) {
436 ✗ throw_pretty(
437 "Invalid argument: " << "The damping factor has to be positive");
438 }
439 ✗ JMinvJt_damping_ = damping;
440 }
441
442 } // namespace crocoddyl
443