GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/actions/contact-fwddyn.hxx
Date: 2025-01-30 11:01:55
Exec Total Coverage
Lines: 191 228 83.8%
Branches: 175 552 31.7%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2024, LAAS-CNRS, University of Edinburgh, CTU, INRIA,
5 // University of Oxford, Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include <pinocchio/algorithm/centroidal.hpp>
11 #include <pinocchio/algorithm/compute-all-terms.hpp>
12 #include <pinocchio/algorithm/contact-dynamics.hpp>
13 #include <pinocchio/algorithm/frames.hpp>
14 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
15 #include <pinocchio/algorithm/rnea-derivatives.hpp>
16 #include <pinocchio/algorithm/rnea.hpp>
17
18 #include "crocoddyl/core/utils/exception.hpp"
19 #include "crocoddyl/core/utils/math.hpp"
20 #include "crocoddyl/multibody/actions/contact-fwddyn.hpp"
21
22 namespace crocoddyl {
23
24 template <typename Scalar>
25 449 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::
26 DifferentialActionModelContactFwdDynamicsTpl(
27 std::shared_ptr<StateMultibody> state,
28 std::shared_ptr<ActuationModelAbstract> actuation,
29 std::shared_ptr<ContactModelMultiple> contacts,
30 std::shared_ptr<CostModelSum> costs, const Scalar JMinvJt_damping,
31 const bool enable_force)
32 : Base(state, actuation->get_nu(), costs->get_nr(), 0, 0),
33 449 actuation_(actuation),
34 449 contacts_(contacts),
35 449 costs_(costs),
36 449 constraints_(nullptr),
37 449 pinocchio_(*state->get_pinocchio().get()),
38 449 with_armature_(true),
39
2/4
✓ Branch 3 taken 449 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 449 times.
✗ Branch 7 not taken.
 449 armature_(VectorXs::Zero(state->get_nv())),
40 449 JMinvJt_damping_(fabs(JMinvJt_damping)),
41
1/2
✓ Branch 6 taken 449 times.
✗ Branch 7 not taken.
 449 enable_force_(enable_force) {
42
1/2
✓ Branch 1 taken 449 times.
✗ Branch 2 not taken.
 449 init();
43 449 }
44
45 template <typename Scalar>
46 13 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::
47 DifferentialActionModelContactFwdDynamicsTpl(
48 std::shared_ptr<StateMultibody> state,
49 std::shared_ptr<ActuationModelAbstract> actuation,
50 std::shared_ptr<ContactModelMultiple> contacts,
51 std::shared_ptr<CostModelSum> costs,
52 std::shared_ptr<ConstraintModelManager> constraints,
53 const Scalar JMinvJt_damping, const bool enable_force)
54 : Base(state, actuation->get_nu(), costs->get_nr(), constraints->get_ng(),
55 constraints->get_nh(), constraints->get_ng_T(),
56 constraints->get_nh_T()),
57 13 actuation_(actuation),
58 13 contacts_(contacts),
59 13 costs_(costs),
60 13 constraints_(constraints),
61 13 pinocchio_(*state->get_pinocchio().get()),
62 13 with_armature_(true),
63
2/4
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 13 armature_(VectorXs::Zero(state->get_nv())),
64 13 JMinvJt_damping_(fabs(JMinvJt_damping)),
65
1/2
✓ Branch 14 taken 13 times.
✗ Branch 15 not taken.
 13 enable_force_(enable_force) {
66
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 init();
67 13 }
68
69 template <typename Scalar>
70 928 DifferentialActionModelContactFwdDynamicsTpl<
71 928 Scalar>::~DifferentialActionModelContactFwdDynamicsTpl() {}
72
73 template <typename Scalar>
74 462 void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::init() {
75
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 462 times.
 462 if (JMinvJt_damping_ < Scalar(0.)) {
76 ✗ JMinvJt_damping_ = Scalar(0.);
77 ✗ throw_pretty("Invalid argument: "
78 << "The damping factor has to be positive, set to 0");
79 }
80
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 462 times.
 462 if (contacts_->get_nu() != nu_) {
81 ✗ throw_pretty(
82 "Invalid argument: "
83 << "Contacts doesn't have the same control dimension (it should be " +
84 std::to_string(nu_) + ")");
85 }
86
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 462 times.
 462 if (costs_->get_nu() != nu_) {
87 ✗ throw_pretty(
88 "Invalid argument: "
89 << "Costs doesn't have the same control dimension (it should be " +
90 std::to_string(nu_) + ")");
91 }
92
93
3/6
✓ Branch 2 taken 462 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 462 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 462 times.
✗ Branch 9 not taken.
 462 Base::set_u_lb(Scalar(-1.) * pinocchio_.effortLimit.tail(nu_));
94
3/6
✓ Branch 2 taken 462 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 462 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 462 times.
✗ Branch 9 not taken.
 462 Base::set_u_ub(Scalar(+1.) * pinocchio_.effortLimit.tail(nu_));
95 462 }
96
97 template <typename Scalar>
98 32220 void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::calc(
99 const std::shared_ptr<DifferentialActionDataAbstract>& data,
100 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
101
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 32220 times.
 32220 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
102 ✗ throw_pretty(
103 "Invalid argument: " << "x has wrong dimension (it should be " +
104 std::to_string(state_->get_nx()) + ")");
105 }
106
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 32220 times.
 32220 if (static_cast<std::size_t>(u.size()) != nu_) {
107 ✗ throw_pretty(
108 "Invalid argument: " << "u has wrong dimension (it should be " +
109 std::to_string(nu_) + ")");
110 }
111
112 32220 const std::size_t nc = contacts_->get_nc();
113 32220 Data* d = static_cast<Data*>(data.get());
114 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
115
1/2
✓ Branch 3 taken 32220 times.
✗ Branch 4 not taken.
 32220 x.head(state_->get_nq());
116 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
117
1/2
✓ Branch 3 taken 32220 times.
✗ Branch 4 not taken.
 32220 x.tail(state_->get_nv());
118
119 // Computing the forward dynamics with the holonomic constraints defined by
120 // the contact model
121
1/2
✓ Branch 1 taken 32220 times.
✗ Branch 2 not taken.
 32220 pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
122
1/2
✓ Branch 1 taken 32220 times.
✗ Branch 2 not taken.
 32220 pinocchio::computeCentroidalMomentum(pinocchio_, d->pinocchio);
123
124
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 32220 times.
 32220 if (!with_armature_) {
125 ✗ d->pinocchio.M.diagonal() += armature_;
126 }
127
1/2
✓ Branch 2 taken 32220 times.
✗ Branch 3 not taken.
 32220 actuation_->calc(d->multibody.actuation, x, u);
128
1/2
✓ Branch 2 taken 32220 times.
✗ Branch 3 not taken.
 32220 contacts_->calc(d->multibody.contacts, x);
129
130 #ifndef NDEBUG
131
2/4
✓ Branch 2 taken 32220 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32220 times.
✗ Branch 6 not taken.
 32220 Eigen::FullPivLU<MatrixXs> Jc_lu(d->multibody.contacts->Jc.topRows(nc));
132
133
4/8
✓ Branch 1 taken 32220 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32220 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 32220 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 32220 times.
 32220 if (Jc_lu.rank() < d->multibody.contacts->Jc.topRows(nc).rows() &&
134 ✗ JMinvJt_damping_ == Scalar(0.)) {
135 ✗ throw_pretty(
136 "A damping factor is needed as the contact Jacobian is not full-rank");
137 }
138 #endif
139
140 64440 pinocchio::forwardDynamics(
141
1/2
✓ Branch 2 taken 32220 times.
✗ Branch 3 not taken.
 32220 pinocchio_, d->pinocchio, d->multibody.actuation->tau,
142
2/4
✓ Branch 2 taken 32220 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 32220 times.
✗ Branch 7 not taken.
 32220 d->multibody.contacts->Jc.topRows(nc), d->multibody.contacts->a0.head(nc),
143 JMinvJt_damping_);
144
1/2
✓ Branch 1 taken 32220 times.
✗ Branch 2 not taken.
 32220 d->xout = d->pinocchio.ddq;
145
1/2
✓ Branch 2 taken 32220 times.
✗ Branch 3 not taken.
 32220 contacts_->updateAcceleration(d->multibody.contacts, d->pinocchio.ddq);
146
1/2
✓ Branch 2 taken 32220 times.
✗ Branch 3 not taken.
 32220 contacts_->updateForce(d->multibody.contacts, d->pinocchio.lambda_c);
147
1/2
✓ Branch 2 taken 32220 times.
✗ Branch 3 not taken.
 32220 d->multibody.joint->a = d->pinocchio.ddq;
148
1/2
✓ Branch 2 taken 32220 times.
✗ Branch 3 not taken.
 32220 d->multibody.joint->tau = u;
149
1/2
✓ Branch 2 taken 32220 times.
✗ Branch 3 not taken.
 32220 costs_->calc(d->costs, x, u);
150 32220 d->cost = d->costs->cost;
151
2/2
✓ Branch 1 taken 1160 times.
✓ Branch 2 taken 31060 times.
 32220 if (constraints_ != nullptr) {
152
1/2
✓ Branch 2 taken 1160 times.
✗ Branch 3 not taken.
 1160 d->constraints->resize(this, d);
153
1/2
✓ Branch 2 taken 1160 times.
✗ Branch 3 not taken.
 1160 constraints_->calc(d->constraints, x, u);
154 }
155 32220 }
156
157 template <typename Scalar>
158 7114 void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::calc(
159 const std::shared_ptr<DifferentialActionDataAbstract>& data,
160 const Eigen::Ref<const VectorXs>& x) {
161
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 7114 times.
 7114 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
162 ✗ throw_pretty(
163 "Invalid argument: " << "x has wrong dimension (it should be " +
164 std::to_string(state_->get_nx()) + ")");
165 }
166
167 7114 Data* d = static_cast<Data*>(data.get());
168 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
169
1/2
✓ Branch 3 taken 7114 times.
✗ Branch 4 not taken.
 7114 x.head(state_->get_nq());
170 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
171
1/2
✓ Branch 3 taken 7114 times.
✗ Branch 4 not taken.
 7114 x.tail(state_->get_nv());
172
173
1/2
✓ Branch 1 taken 7114 times.
✗ Branch 2 not taken.
 7114 pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
174
1/2
✓ Branch 1 taken 7114 times.
✗ Branch 2 not taken.
 7114 pinocchio::computeCentroidalMomentum(pinocchio_, d->pinocchio);
175
1/2
✓ Branch 2 taken 7114 times.
✗ Branch 3 not taken.
 7114 costs_->calc(d->costs, x);
176 7114 d->cost = d->costs->cost;
177
2/2
✓ Branch 1 taken 834 times.
✓ Branch 2 taken 6280 times.
 7114 if (constraints_ != nullptr) {
178
1/2
✓ Branch 2 taken 834 times.
✗ Branch 3 not taken.
 834 d->constraints->resize(this, d, false);
179
1/2
✓ Branch 2 taken 834 times.
✗ Branch 3 not taken.
 834 constraints_->calc(d->constraints, x);
180 }
181 7114 }
182
183 template <typename Scalar>
184 5077 void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::calcDiff(
185 const std::shared_ptr<DifferentialActionDataAbstract>& data,
186 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
187
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 5077 times.
 5077 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
188 ✗ throw_pretty(
189 "Invalid argument: " << "x has wrong dimension (it should be " +
190 std::to_string(state_->get_nx()) + ")");
191 }
192
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5077 times.
 5077 if (static_cast<std::size_t>(u.size()) != nu_) {
193 ✗ throw_pretty(
194 "Invalid argument: " << "u has wrong dimension (it should be " +
195 std::to_string(nu_) + ")");
196 }
197
198 5077 const std::size_t nv = state_->get_nv();
199 5077 const std::size_t nc = contacts_->get_nc();
200 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
201
1/2
✓ Branch 3 taken 5077 times.
✗ Branch 4 not taken.
 5077 x.head(state_->get_nq());
202 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
203
1/2
✓ Branch 1 taken 5077 times.
✗ Branch 2 not taken.
 5077 x.tail(nv);
204
205 5077 Data* d = static_cast<Data*>(data.get());
206
207 // Computing the dynamics derivatives
208 // We resize the Kinv matrix because Eigen cannot call block operations
209 // recursively: https://eigen.tuxfamily.org/bz/show_bug.cgi?id=408. Therefore,
210 // it is not possible to pass d->Kinv.topLeftCorner(nv + nc, nv + nc)
211
1/2
✓ Branch 1 taken 5077 times.
✗ Branch 2 not taken.
 5077 d->Kinv.resize(nv + nc, nv + nc);
212 5077 pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, v, d->xout,
213
1/2
✓ Branch 2 taken 5077 times.
✗ Branch 3 not taken.
 5077 d->multibody.contacts->fext);
214
1/2
✓ Branch 2 taken 5077 times.
✗ Branch 3 not taken.
 5077 contacts_->updateRneaDiff(d->multibody.contacts, d->pinocchio);
215 5077 pinocchio::getKKTContactDynamicMatrixInverse(
216
2/4
✓ Branch 2 taken 5077 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5077 times.
✗ Branch 6 not taken.
 5077 pinocchio_, d->pinocchio, d->multibody.contacts->Jc.topRows(nc), d->Kinv);
217
218
1/2
✓ Branch 2 taken 5077 times.
✗ Branch 3 not taken.
 5077 actuation_->calcDiff(d->multibody.actuation, x, u);
219
1/2
✓ Branch 2 taken 5077 times.
✗ Branch 3 not taken.
 5077 contacts_->calcDiff(d->multibody.contacts, x);
220
221
1/2
✓ Branch 1 taken 5077 times.
✗ Branch 2 not taken.
 5077 const Eigen::Block<MatrixXs> a_partial_dtau = d->Kinv.topLeftCorner(nv, nv);
222
1/2
✓ Branch 1 taken 5077 times.
✗ Branch 2 not taken.
 5077 const Eigen::Block<MatrixXs> a_partial_da = d->Kinv.topRightCorner(nv, nc);
223 const Eigen::Block<MatrixXs> f_partial_dtau =
224
1/2
✓ Branch 1 taken 5077 times.
✗ Branch 2 not taken.
 5077 d->Kinv.bottomLeftCorner(nc, nv);
225
1/2
✓ Branch 1 taken 5077 times.
✗ Branch 2 not taken.
 5077 const Eigen::Block<MatrixXs> f_partial_da = d->Kinv.bottomRightCorner(nc, nc);
226
227
5/10
✓ Branch 1 taken 5077 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5077 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5077 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5077 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5077 times.
✗ Branch 14 not taken.
 5077 d->Fx.leftCols(nv).noalias() = -a_partial_dtau * d->pinocchio.dtau_dq;
228
5/10
✓ Branch 1 taken 5077 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5077 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5077 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5077 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5077 times.
✗ Branch 14 not taken.
 5077 d->Fx.rightCols(nv).noalias() = -a_partial_dtau * d->pinocchio.dtau_dv;
229
4/8
✓ Branch 2 taken 5077 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5077 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5077 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5077 times.
✗ Branch 12 not taken.
 5077 d->Fx.noalias() -= a_partial_da * d->multibody.contacts->da0_dx.topRows(nc);
230
3/6
✓ Branch 2 taken 5077 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5077 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5077 times.
✗ Branch 9 not taken.
 5077 d->Fx.noalias() += a_partial_dtau * d->multibody.actuation->dtau_dx;
231
3/6
✓ Branch 2 taken 5077 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5077 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5077 times.
✗ Branch 9 not taken.
 5077 d->Fu.noalias() = a_partial_dtau * d->multibody.actuation->dtau_du;
232
1/2
✓ Branch 2 taken 5077 times.
✗ Branch 3 not taken.
 5077 d->multibody.joint->da_dx = d->Fx;
233
1/2
✓ Branch 2 taken 5077 times.
✗ Branch 3 not taken.
 5077 d->multibody.joint->da_du = d->Fu;
234
235 // Computing the cost derivatives
236
1/2
✓ Branch 0 taken 5077 times.
✗ Branch 1 not taken.
 5077 if (enable_force_) {
237
3/6
✓ Branch 1 taken 5077 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5077 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5077 times.
✗ Branch 8 not taken.
 5077 d->df_dx.topLeftCorner(nc, nv).noalias() =
238
1/2
✓ Branch 1 taken 5077 times.
✗ Branch 2 not taken.
 5077 f_partial_dtau * d->pinocchio.dtau_dq;
239
3/6
✓ Branch 1 taken 5077 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5077 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5077 times.
✗ Branch 8 not taken.
 5077 d->df_dx.topRightCorner(nc, nv).noalias() =
240
1/2
✓ Branch 1 taken 5077 times.
✗ Branch 2 not taken.
 5077 f_partial_dtau * d->pinocchio.dtau_dv;
241
3/6
✓ Branch 1 taken 5077 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5077 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5077 times.
✗ Branch 8 not taken.
 5077 d->df_dx.topRows(nc).noalias() +=
242
2/4
✓ Branch 2 taken 5077 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5077 times.
✗ Branch 6 not taken.
 5077 f_partial_da * d->multibody.contacts->da0_dx.topRows(nc);
243
3/6
✓ Branch 1 taken 5077 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5077 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5077 times.
✗ Branch 8 not taken.
 5077 d->df_dx.topRows(nc).noalias() -=
244
1/2
✓ Branch 2 taken 5077 times.
✗ Branch 3 not taken.
 5077 f_partial_dtau * d->multibody.actuation->dtau_dx;
245
4/8
✓ Branch 1 taken 5077 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5077 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5077 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5077 times.
✗ Branch 11 not taken.
 5077 d->df_du.topRows(nc).noalias() =
246
1/2
✓ Branch 2 taken 5077 times.
✗ Branch 3 not taken.
 5077 -f_partial_dtau * d->multibody.actuation->dtau_du;
247
2/4
✓ Branch 2 taken 5077 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5077 times.
✗ Branch 6 not taken.
 10154 contacts_->updateAccelerationDiff(d->multibody.contacts,
248
1/2
✓ Branch 1 taken 5077 times.
✗ Branch 2 not taken.
 5077 d->Fx.bottomRows(nv));
249
4/8
✓ Branch 2 taken 5077 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5077 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5077 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5077 times.
✗ Branch 12 not taken.
 10154 contacts_->updateForceDiff(d->multibody.contacts, d->df_dx.topRows(nc),
250
1/2
✓ Branch 1 taken 5077 times.
✗ Branch 2 not taken.
 10154 d->df_du.topRows(nc));
251 }
252
1/2
✓ Branch 2 taken 5077 times.
✗ Branch 3 not taken.
 5077 costs_->calcDiff(d->costs, x, u);
253
2/2
✓ Branch 1 taken 13 times.
✓ Branch 2 taken 5064 times.
 5077 if (constraints_ != nullptr) {
254
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 13 constraints_->calcDiff(d->constraints, x, u);
255 }
256 5077 }
257
258 template <typename Scalar>
259 237 void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::calcDiff(
260 const std::shared_ptr<DifferentialActionDataAbstract>& data,
261 const Eigen::Ref<const VectorXs>& x) {
262
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 237 times.
 237 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
263 ✗ throw_pretty(
264 "Invalid argument: " << "x has wrong dimension (it should be " +
265 std::to_string(state_->get_nx()) + ")");
266 }
267 237 Data* d = static_cast<Data*>(data.get());
268 237 costs_->calcDiff(d->costs, x);
269
2/2
✓ Branch 1 taken 13 times.
✓ Branch 2 taken 224 times.
 237 if (constraints_ != nullptr) {
270 13 constraints_->calcDiff(d->constraints, x);
271 }
272 237 }
273
274 template <typename Scalar>
275 std::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
276 39554 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::createData() {
277
1/2
✓ Branch 2 taken 39554 times.
✗ Branch 3 not taken.
 39554 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
278 }
279
280 template <typename Scalar>
281 2572 void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::quasiStatic(
282 const std::shared_ptr<DifferentialActionDataAbstract>& data,
283 Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x, std::size_t,
284 Scalar) {
285
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2572 times.
 2572 if (static_cast<std::size_t>(u.size()) != nu_) {
286 ✗ throw_pretty(
287 "Invalid argument: " << "u has wrong dimension (it should be " +
288 std::to_string(nu_) + ")");
289 }
290
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 2572 times.
 2572 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
291 ✗ throw_pretty(
292 "Invalid argument: " << "x has wrong dimension (it should be " +
293 std::to_string(state_->get_nx()) + ")");
294 }
295 // Static casting the data
296 DifferentialActionDataContactFwdDynamicsTpl<Scalar>* d =
297 static_cast<DifferentialActionDataContactFwdDynamicsTpl<Scalar>*>(
298 2572 data.get());
299 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
300
1/2
✓ Branch 3 taken 2572 times.
✗ Branch 4 not taken.
 2572 x.head(state_->get_nq());
301
302 2572 const std::size_t nq = state_->get_nq();
303 2572 const std::size_t nv = state_->get_nv();
304 2572 const std::size_t nc = contacts_->get_nc();
305
306
2/4
✓ Branch 1 taken 2572 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2572 times.
✗ Branch 5 not taken.
 2572 d->tmp_xstatic.head(nq) = q;
307
2/4
✓ Branch 1 taken 2572 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2572 times.
✗ Branch 5 not taken.
 2572 d->tmp_xstatic.tail(nv).setZero();
308
1/2
✓ Branch 1 taken 2572 times.
✗ Branch 2 not taken.
 2572 u.setZero();
309
310
1/2
✓ Branch 1 taken 2572 times.
✗ Branch 2 not taken.
 2572 pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q,
311
1/2
✓ Branch 1 taken 2572 times.
✗ Branch 2 not taken.
 2572 d->tmp_xstatic.tail(nv));
312
1/2
✓ Branch 1 taken 2572 times.
✗ Branch 2 not taken.
 2572 pinocchio::computeJointJacobians(pinocchio_, d->pinocchio, q);
313
2/4
✓ Branch 1 taken 2572 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2572 times.
✗ Branch 5 not taken.
 2572 pinocchio::rnea(pinocchio_, d->pinocchio, q, d->tmp_xstatic.tail(nv),
314
1/2
✓ Branch 1 taken 2572 times.
✗ Branch 2 not taken.
 2572 d->tmp_xstatic.tail(nv));
315
3/6
✓ Branch 2 taken 2572 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2572 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2572 times.
✗ Branch 9 not taken.
 2572 actuation_->calc(d->multibody.actuation, d->tmp_xstatic, u);
316
3/6
✓ Branch 2 taken 2572 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2572 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2572 times.
✗ Branch 9 not taken.
 2572 actuation_->calcDiff(d->multibody.actuation, d->tmp_xstatic, u);
317
2/4
✓ Branch 2 taken 2572 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2572 times.
✗ Branch 6 not taken.
 2572 contacts_->calc(d->multibody.contacts, d->tmp_xstatic);
318
319 // Allocates memory
320
1/2
✓ Branch 1 taken 2572 times.
✗ Branch 2 not taken.
 2572 d->tmp_Jstatic.conservativeResize(nv, nu_ + nc);
321
2/4
✓ Branch 2 taken 2572 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2572 times.
✗ Branch 6 not taken.
 2572 d->tmp_Jstatic.leftCols(nu_) = d->multibody.actuation->dtau_du;
322
2/4
✓ Branch 1 taken 2572 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2572 times.
✗ Branch 5 not taken.
 2572 d->tmp_Jstatic.rightCols(nc) =
323
2/4
✓ Branch 2 taken 2572 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2572 times.
✗ Branch 6 not taken.
 2572 d->multibody.contacts->Jc.topRows(nc).transpose();
324
5/10
✓ Branch 2 taken 2572 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2572 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2572 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2572 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2572 times.
✗ Branch 15 not taken.
 2572 u.noalias() = (pseudoInverse(d->tmp_Jstatic) * d->pinocchio.tau).head(nu_);
325
1/2
✓ Branch 1 taken 2572 times.
✗ Branch 2 not taken.
 2572 d->pinocchio.tau.setZero();
326 2572 }
327
328 template <typename Scalar>
329 5256 bool DifferentialActionModelContactFwdDynamicsTpl<Scalar>::checkData(
330 const std::shared_ptr<DifferentialActionDataAbstract>& data) {
331 5256 std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
332
1/2
✓ Branch 1 taken 5256 times.
✗ Branch 2 not taken.
 5256 if (d != NULL) {
333 5256 return true;
334 } else {
335 ✗ return false;
336 }
337 5256 }
338
339 template <typename Scalar>
340 80 void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::print(
341 std::ostream& os) const {
342 80 os << "DifferentialActionModelContactFwdDynamics {nx=" << state_->get_nx()
343 80 << ", ndx=" << state_->get_ndx() << ", nu=" << nu_
344 80 << ", nc=" << contacts_->get_nc() << "}";
345 80 }
346
347 template <typename Scalar>
348 160464 std::size_t DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_ng()
349 const {
350
2/2
✓ Branch 1 taken 4666 times.
✓ Branch 2 taken 155798 times.
 160464 if (constraints_ != nullptr) {
351 4666 return constraints_->get_ng();
352 } else {
353 155798 return Base::get_ng();
354 }
355 }
356
357 template <typename Scalar>
358 160464 std::size_t DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_nh()
359 const {
360
2/2
✓ Branch 1 taken 4666 times.
✓ Branch 2 taken 155798 times.
 160464 if (constraints_ != nullptr) {
361 4666 return constraints_->get_nh();
362 } else {
363 155798 return Base::get_nh();
364 }
365 }
366
367 template <typename Scalar>
368 314424 std::size_t DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_ng_T()
369 const {
370
2/2
✓ Branch 1 taken 7820 times.
✓ Branch 2 taken 306604 times.
 314424 if (constraints_ != nullptr) {
371 7820 return constraints_->get_ng_T();
372 } else {
373 306604 return Base::get_ng_T();
374 }
375 }
376
377 template <typename Scalar>
378 314424 std::size_t DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_nh_T()
379 const {
380
2/2
✓ Branch 1 taken 7820 times.
✓ Branch 2 taken 306604 times.
 314424 if (constraints_ != nullptr) {
381 7820 return constraints_->get_nh_T();
382 } else {
383 306604 return Base::get_nh_T();
384 }
385 }
386
387 template <typename Scalar>
388 const typename MathBaseTpl<Scalar>::VectorXs&
389 ✗ DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_g_lb() const {
390 ✗ if (constraints_ != nullptr) {
391 ✗ return constraints_->get_lb();
392 } else {
393 ✗ return g_lb_;
394 }
395 }
396
397 template <typename Scalar>
398 const typename MathBaseTpl<Scalar>::VectorXs&
399 ✗ DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_g_ub() const {
400 ✗ if (constraints_ != nullptr) {
401 ✗ return constraints_->get_ub();
402 } else {
403 ✗ return g_lb_;
404 }
405 }
406
407 template <typename Scalar>
408 pinocchio::ModelTpl<Scalar>&
409 39554 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_pinocchio() const {
410 39554 return pinocchio_;
411 }
412
413 template <typename Scalar>
414 const std::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
415 79156 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_actuation() const {
416 79156 return actuation_;
417 }
418
419 template <typename Scalar>
420 const std::shared_ptr<ContactModelMultipleTpl<Scalar> >&
421 276886 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_contacts() const {
422 276886 return contacts_;
423 }
424
425 template <typename Scalar>
426 const std::shared_ptr<CostModelSumTpl<Scalar> >&
427 39782 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_costs() const {
428 39782 return costs_;
429 }
430
431 template <typename Scalar>
432 const std::shared_ptr<ConstraintModelManagerTpl<Scalar> >&
433 40714 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_constraints() const {
434 40714 return constraints_;
435 }
436
437 template <typename Scalar>
438 const typename MathBaseTpl<Scalar>::VectorXs&
439 ✗ DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_armature() const {
440 ✗ return armature_;
441 }
442
443 template <typename Scalar>
444 ✗ const Scalar DifferentialActionModelContactFwdDynamicsTpl<
445 Scalar>::get_damping_factor() const {
446 ✗ return JMinvJt_damping_;
447 }
448
449 template <typename Scalar>
450 ✗ void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::set_armature(
451 const VectorXs& armature) {
452 ✗ if (static_cast<std::size_t>(armature.size()) != state_->get_nv()) {
453 ✗ throw_pretty("Invalid argument: "
454 << "The armature dimension is wrong (it should be " +
455 std::to_string(state_->get_nv()) + ")");
456 }
457 ✗ armature_ = armature;
458 ✗ with_armature_ = false;
459 }
460
461 template <typename Scalar>
462 ✗ void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::set_damping_factor(
463 const Scalar damping) {
464 ✗ if (damping < 0.) {
465 ✗ throw_pretty(
466 "Invalid argument: " << "The damping factor has to be positive");
467 }
468 ✗ JMinvJt_damping_ = damping;
469 }
470
471 } // namespace crocoddyl
472