GCC Code Coverage Report

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