GCC Code Coverage Report

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