GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/actions/contact-invdyn.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 216 281 76.9%
Branches: 218 678 32.2%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, Heriot-Watt University, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include <pinocchio/algorithm/center-of-mass.hpp>
10 #include <pinocchio/algorithm/centroidal.hpp>
11 #include <pinocchio/algorithm/compute-all-terms.hpp>
12 #include <pinocchio/algorithm/jacobian.hpp>
13 #include <pinocchio/algorithm/kinematics.hpp>
14 #include <pinocchio/algorithm/rnea-derivatives.hpp>
15 #include <pinocchio/algorithm/rnea.hpp>
16
17 #include "crocoddyl/core/constraints/residual.hpp"
18 #include "crocoddyl/multibody/residuals/contact-force.hpp"
19
20 namespace crocoddyl {
21
22 template <typename Scalar>
23 301 DifferentialActionModelContactInvDynamicsTpl<Scalar>::
24 DifferentialActionModelContactInvDynamicsTpl(
25 std::shared_ptr<StateMultibody> state,
26 std::shared_ptr<ActuationModelAbstract> actuation,
27 std::shared_ptr<ContactModelMultiple> contacts,
28 std::shared_ptr<CostModelSum> costs)
29 301 : Base(state, state->get_nv() + contacts->get_nc_total(), costs->get_nr(),
30 301 0, state->get_nv() - actuation->get_nu() + contacts->get_nc_total()),
31 301 actuation_(actuation),
32 301 contacts_(contacts),
33 301 costs_(costs),
34 301 constraints_(std::make_shared<ConstraintModelManager>(
35
3/6
✓ Branch 2 taken 301 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 301 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 301 times.
✗ Branch 10 not taken.
 301 state, state->get_nv() + contacts->get_nc_total())),
36
2/4
✓ Branch 6 taken 301 times.
✗ Branch 7 not taken.
✓ Branch 12 taken 301 times.
✗ Branch 13 not taken.
 1204 pinocchio_(state->get_pinocchio().get()) {
37
1/2
✓ Branch 1 taken 301 times.
✗ Branch 2 not taken.
 301 init(state);
38 301 }
39
40 template <typename Scalar>
41 ✗ DifferentialActionModelContactInvDynamicsTpl<Scalar>::
42 DifferentialActionModelContactInvDynamicsTpl(
43 std::shared_ptr<StateMultibody> state,
44 std::shared_ptr<ActuationModelAbstract> actuation,
45 std::shared_ptr<ContactModelMultiple> contacts,
46 std::shared_ptr<CostModelSum> costs,
47 std::shared_ptr<ConstraintModelManager> constraints)
48 ✗ : Base(state, state->get_nv() + contacts->get_nc_total(), costs->get_nr(),
49 constraints->get_ng(),
50 ✗ state->get_nv() - actuation->get_nu() + contacts->get_nc_total() +
51 ✗ constraints->get_nh(),
52 constraints->get_ng_T(), constraints->get_nh_T()),
53 ✗ actuation_(actuation),
54 ✗ contacts_(contacts),
55 ✗ costs_(costs),
56 ✗ constraints_(constraints),
57 ✗ pinocchio_(state->get_pinocchio().get()) {
58 ✗ init(state);
59 }
60
61 template <typename Scalar>
62 301 void DifferentialActionModelContactInvDynamicsTpl<Scalar>::init(
63 const std::shared_ptr<StateMultibody>& state) {
64
2/4
✓ Branch 2 taken 301 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 301 times.
 301 if (contacts_->get_nu() != nu_) {
65 ✗ throw_pretty(
66 "Invalid argument: "
67 << "Contacts doesn't have the same control dimension (it should be " +
68 std::to_string(nu_) + ")");
69 }
70
2/4
✓ Branch 2 taken 301 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 301 times.
 301 if (costs_->get_nu() != nu_) {
71 ✗ throw_pretty(
72 "Invalid argument: "
73 << "Costs doesn't have the same control dimension (it should be " +
74 std::to_string(nu_) + ")");
75 }
76
1/2
✓ Branch 2 taken 301 times.
✗ Branch 3 not taken.
 301 const std::size_t nu = actuation_->get_nu();
77
1/2
✓ Branch 2 taken 301 times.
✗ Branch 3 not taken.
 301 const std::size_t nc = contacts_->get_nc_total();
78
1/2
✓ Branch 1 taken 301 times.
✗ Branch 2 not taken.
 301 VectorXs lb =
79
1/2
✓ Branch 2 taken 301 times.
✗ Branch 3 not taken.
 301 VectorXs::Constant(nu_, -std::numeric_limits<Scalar>::infinity());
80
1/2
✓ Branch 1 taken 301 times.
✗ Branch 2 not taken.
 301 VectorXs ub =
81
1/2
✓ Branch 2 taken 301 times.
✗ Branch 3 not taken.
 301 VectorXs::Constant(nu_, std::numeric_limits<Scalar>::infinity());
82
1/2
✓ Branch 1 taken 301 times.
✗ Branch 2 not taken.
 301 Base::set_u_lb(lb);
83
1/2
✓ Branch 1 taken 301 times.
✗ Branch 2 not taken.
 301 Base::set_u_ub(ub);
84
1/2
✓ Branch 2 taken 301 times.
✗ Branch 3 not taken.
 301 contacts_->setComputeAllContacts(true);
85
86
4/6
✓ Branch 2 taken 301 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 301 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 251 times.
✓ Branch 9 taken 50 times.
 301 if (state_->get_nv() - actuation_->get_nu() > 0) {
87
2/4
✓ Branch 4 taken 251 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 251 times.
✗ Branch 8 not taken.
 502 constraints_->addConstraint(
88 "tau", std::make_shared<ConstraintModelResidualTpl<Scalar>>(
89
1/2
✓ Branch 1 taken 251 times.
✗ Branch 2 not taken.
 251 state_,
90 std::make_shared<
91 typename DifferentialActionModelContactInvDynamicsTpl<
92 Scalar>::ResidualModelActuation>(state, nu, nc),
93
1/2
✓ Branch 1 taken 251 times.
✗ Branch 2 not taken.
 502 false));
94 }
95
3/4
✓ Branch 2 taken 301 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 300 times.
✓ Branch 5 taken 1 times.
 301 if (contacts_->get_nc_total() != 0) {
96 300 typename ContactModelMultiple::ContactModelContainer contact_list;
97
2/4
✓ Branch 2 taken 300 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 300 times.
✗ Branch 6 not taken.
 300 contact_list = contacts_->get_contacts();
98 300 typename ContactModelMultiple::ContactModelContainer::iterator it_m, end_m;
99
2/2
✓ Branch 3 taken 700 times.
✓ Branch 4 taken 300 times.
 1000 for (it_m = contact_list.begin(), end_m = contact_list.end(); it_m != end_m;
100 700 ++it_m) {
101 700 const std::shared_ptr<ContactItem>& contact = it_m->second;
102
1/2
✓ Branch 2 taken 700 times.
✗ Branch 3 not taken.
 700 const std::string name = contact->name;
103
1/2
✓ Branch 3 taken 700 times.
✗ Branch 4 not taken.
 700 const pinocchio::FrameIndex id = contact->contact->get_id();
104
1/2
✓ Branch 3 taken 700 times.
✗ Branch 4 not taken.
 700 const std::size_t nc_i = contact->contact->get_nc();
105 700 const bool active = contact->active;
106
2/4
✓ Branch 3 taken 700 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 700 times.
✗ Branch 7 not taken.
 1400 constraints_->addConstraint(
107 name + "_acc",
108 std::make_shared<ConstraintModelResidualTpl<Scalar>>(
109
1/2
✓ Branch 1 taken 700 times.
✗ Branch 2 not taken.
 700 state_,
110 std::make_shared<
111 typename DifferentialActionModelContactInvDynamicsTpl<
112 Scalar>::ResidualModelContact>(state, id, nc_i, nc),
113
1/2
✓ Branch 1 taken 700 times.
✗ Branch 2 not taken.
 700 false),
114 active);
115
3/6
✓ Branch 2 taken 700 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 700 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 700 times.
✗ Branch 10 not taken.
 2100 constraints_->addConstraint(
116 name + "_force",
117 std::make_shared<ConstraintModelResidualTpl<Scalar>>(
118
1/2
✓ Branch 1 taken 700 times.
✗ Branch 2 not taken.
 700 state_,
119 std::make_shared<ResidualModelContactForceTpl<Scalar>>(
120 700 state, id, pinocchio::ForceTpl<Scalar>::Zero(), nc_i, nu_,
121 ✗ false),
122 700 false),
123
1/2
✓ Branch 1 taken 700 times.
✗ Branch 2 not taken.
 700 !active);
124 }
125 300 }
126 301 }
127
128 template <typename Scalar>
129 25930 void DifferentialActionModelContactInvDynamicsTpl<Scalar>::calc(
130 const std::shared_ptr<DifferentialActionDataAbstract>& data,
131 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
132
2/4
✓ Branch 3 taken 25930 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 25930 times.
 25930 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
133 ✗ throw_pretty(
134 "Invalid argument: " << "x has wrong dimension (it should be " +
135 std::to_string(state_->get_nx()) + ")");
136 }
137
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 25930 times.
 25930 if (static_cast<std::size_t>(u.size()) != nu_) {
138 ✗ throw_pretty(
139 "Invalid argument: " << "u has wrong dimension (it should be " +
140 std::to_string(nu_) + ")");
141 }
142 25930 Data* d = static_cast<Data*>(data.get());
143
1/2
✓ Branch 2 taken 25930 times.
✗ Branch 3 not taken.
 25930 const std::size_t nv = state_->get_nv();
144
1/2
✓ Branch 2 taken 25930 times.
✗ Branch 3 not taken.
 25930 const std::size_t nc = contacts_->get_nc_total();
145 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
146
2/4
✓ Branch 2 taken 25930 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25930 times.
✗ Branch 6 not taken.
 25930 x.head(state_->get_nq());
147 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
148
1/2
✓ Branch 1 taken 25930 times.
✗ Branch 2 not taken.
 25930 x.tail(nv);
149 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> a =
150
1/2
✓ Branch 1 taken 25930 times.
✗ Branch 2 not taken.
 25930 u.head(nv);
151 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic>
152
1/2
✓ Branch 1 taken 25930 times.
✗ Branch 2 not taken.
 25930 f_ext = u.tail(nc);
153
154
1/2
✓ Branch 1 taken 25930 times.
✗ Branch 2 not taken.
 25930 d->xout = a;
155
1/2
✓ Branch 1 taken 25930 times.
✗ Branch 2 not taken.
 25930 pinocchio::forwardKinematics(*pinocchio_, d->pinocchio, q, v, a);
156
1/2
✓ Branch 1 taken 25930 times.
✗ Branch 2 not taken.
 25930 pinocchio::computeJointJacobians(*pinocchio_, d->pinocchio);
157
1/2
✓ Branch 2 taken 25930 times.
✗ Branch 3 not taken.
 25930 contacts_->calc(d->multibody.contacts, x);
158
2/4
✓ Branch 2 taken 25930 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25930 times.
✗ Branch 6 not taken.
 25930 contacts_->updateForce(d->multibody.contacts, f_ext);
159 25930 pinocchio::rnea(*pinocchio_, d->pinocchio, q, v, a,
160
1/2
✓ Branch 2 taken 25930 times.
✗ Branch 3 not taken.
 25930 d->multibody.contacts->fext);
161
1/2
✓ Branch 1 taken 25930 times.
✗ Branch 2 not taken.
 25930 pinocchio::updateGlobalPlacements(*pinocchio_, d->pinocchio);
162
1/2
✓ Branch 1 taken 25930 times.
✗ Branch 2 not taken.
 25930 pinocchio::centerOfMass(*pinocchio_, d->pinocchio, q, v, a);
163
2/4
✓ Branch 2 taken 25930 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25930 times.
✗ Branch 6 not taken.
 25930 actuation_->commands(d->multibody.actuation, x, d->pinocchio.tau);
164
1/2
✓ Branch 2 taken 25930 times.
✗ Branch 3 not taken.
 25930 d->multibody.joint->a = a;
165
1/2
✓ Branch 3 taken 25930 times.
✗ Branch 4 not taken.
 25930 d->multibody.joint->tau = d->multibody.actuation->u;
166
2/4
✓ Branch 3 taken 25930 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 25930 times.
✗ Branch 7 not taken.
 25930 actuation_->calc(d->multibody.actuation, x, d->multibody.joint->tau);
167
1/2
✓ Branch 2 taken 25930 times.
✗ Branch 3 not taken.
 25930 costs_->calc(d->costs, x, u);
168 25930 d->cost = d->costs->cost;
169
4/6
✓ Branch 2 taken 25930 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 63366 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 63366 times.
✓ Branch 13 taken 25930 times.
 89296 for (std::string name : contacts_->get_active_set()) {
170
2/4
✓ Branch 2 taken 63366 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 63366 times.
✗ Branch 6 not taken.
 63366 constraints_->changeConstraintStatus(name + "_acc", true);
171
2/4
✓ Branch 2 taken 63366 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 63366 times.
✗ Branch 6 not taken.
 63366 constraints_->changeConstraintStatus(name + "_force", false);
172 }
173
4/6
✓ Branch 2 taken 25930 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 4 times.
✓ Branch 13 taken 25930 times.
 25934 for (std::string name : contacts_->get_inactive_set()) {
174
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 4 constraints_->changeConstraintStatus(name + "_acc", false);
175
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 4 constraints_->changeConstraintStatus(name + "_force", true);
176 }
177
1/2
✓ Branch 2 taken 25930 times.
✗ Branch 3 not taken.
 25930 d->constraints->resize(this, d);
178
1/2
✓ Branch 2 taken 25930 times.
✗ Branch 3 not taken.
 25930 constraints_->calc(d->constraints, x, u);
179 25930 }
180
181 template <typename Scalar>
182 2862 void DifferentialActionModelContactInvDynamicsTpl<Scalar>::calc(
183 const std::shared_ptr<DifferentialActionDataAbstract>& data,
184 const Eigen::Ref<const VectorXs>& x) {
185
2/4
✓ Branch 3 taken 2862 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 2862 times.
 2862 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
186 ✗ throw_pretty(
187 "Invalid argument: " << "x has wrong dimension (it should be " +
188 std::to_string(state_->get_nx()) + ")");
189 }
190
191 2862 Data* d = static_cast<Data*>(data.get());
192 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
193
2/4
✓ Branch 2 taken 2862 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2862 times.
✗ Branch 6 not taken.
 2862 x.head(state_->get_nq());
194 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
195
2/4
✓ Branch 2 taken 2862 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2862 times.
✗ Branch 6 not taken.
 2862 x.tail(state_->get_nv());
196
197
1/2
✓ Branch 1 taken 2862 times.
✗ Branch 2 not taken.
 2862 pinocchio::computeAllTerms(*pinocchio_, d->pinocchio, q, v);
198
1/2
✓ Branch 1 taken 2862 times.
✗ Branch 2 not taken.
 2862 pinocchio::computeCentroidalMomentum(*pinocchio_, d->pinocchio);
199
1/2
✓ Branch 2 taken 2862 times.
✗ Branch 3 not taken.
 2862 costs_->calc(d->costs, x);
200 2862 d->cost = d->costs->cost;
201
1/2
✓ Branch 2 taken 2862 times.
✗ Branch 3 not taken.
 2862 d->constraints->resize(this, d, false);
202
1/2
✓ Branch 2 taken 2862 times.
✗ Branch 3 not taken.
 2862 constraints_->calc(d->constraints, x);
203 2862 }
204
205 template <typename Scalar>
206 3762 void DifferentialActionModelContactInvDynamicsTpl<Scalar>::calcDiff(
207 const std::shared_ptr<DifferentialActionDataAbstract>& data,
208 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
209
2/4
✓ Branch 3 taken 3762 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 3762 times.
 3762 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
210 ✗ throw_pretty(
211 "Invalid argument: " << "x has wrong dimension (it should be " +
212 std::to_string(state_->get_nx()) + ")");
213 }
214
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3762 times.
 3762 if (static_cast<std::size_t>(u.size()) != nu_) {
215 ✗ throw_pretty(
216 "Invalid argument: " << "u has wrong dimension (it should be " +
217 std::to_string(nu_) + ")");
218 }
219 3762 Data* d = static_cast<Data*>(data.get());
220
1/2
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
 3762 const std::size_t nv = state_->get_nv();
221
1/2
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
 3762 const std::size_t nc = contacts_->get_nc_total();
222 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
223
2/4
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3762 times.
✗ Branch 6 not taken.
 3762 x.head(state_->get_nq());
224 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
225
1/2
✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
 3762 x.tail(nv);
226 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> a =
227
1/2
✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
 3762 u.head(nv);
228
229 3762 pinocchio::computeRNEADerivatives(*pinocchio_, d->pinocchio, q, v, a,
230
1/2
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
 3762 d->multibody.contacts->fext);
231
1/2
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
 3762 contacts_->updateRneaDiff(d->multibody.contacts, d->pinocchio);
232
2/4
✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3762 times.
✗ Branch 5 not taken.
 3762 d->pinocchio.M.template triangularView<Eigen::StrictlyLower>() =
233
1/2
✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
 3762 d->pinocchio.M.template triangularView<Eigen::StrictlyUpper>()
234
1/2
✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
 3762 .transpose();
235
1/2
✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
 3762 pinocchio::jacobianCenterOfMass(*pinocchio_, d->pinocchio, false);
236
2/4
✓ Branch 3 taken 3762 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3762 times.
✗ Branch 7 not taken.
 3762 actuation_->calcDiff(d->multibody.actuation, x, d->multibody.joint->tau);
237
1/2
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
 7524 actuation_->torqueTransform(d->multibody.actuation, x,
238
1/2
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
 3762 d->multibody.joint->tau);
239
3/6
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3762 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3762 times.
✗ Branch 9 not taken.
 3762 d->multibody.joint->dtau_dx.leftCols(nv).noalias() =
240
1/2
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
 3762 d->multibody.actuation->Mtau * d->pinocchio.dtau_dq;
241
3/6
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3762 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3762 times.
✗ Branch 9 not taken.
 3762 d->multibody.joint->dtau_dx.rightCols(nv).noalias() =
242
1/2
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
 3762 d->multibody.actuation->Mtau * d->pinocchio.dtau_dv;
243
3/6
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3762 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3762 times.
✗ Branch 9 not taken.
 3762 d->multibody.joint->dtau_du.leftCols(nv).noalias() =
244
1/2
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
 3762 d->multibody.actuation->Mtau * d->pinocchio.M;
245
4/8
✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3762 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3762 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3762 times.
✗ Branch 12 not taken.
 3762 d->multibody.joint->dtau_du.rightCols(nc).noalias() =
246
1/2
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
 3762 -d->multibody.actuation->Mtau *
247
2/4
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3762 times.
✗ Branch 6 not taken.
 3762 d->multibody.contacts->Jc.topRows(nc).transpose();
248
1/2
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
 3762 contacts_->calcDiff(d->multibody.contacts, x);
249
1/2
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
 3762 costs_->calcDiff(d->costs, x, u);
250
4/6
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 8778 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 8778 times.
✓ Branch 13 taken 3762 times.
 12540 for (std::string name : contacts_->get_active_set()) {
251
2/4
✓ Branch 2 taken 8778 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8778 times.
✗ Branch 6 not taken.
 8778 constraints_->changeConstraintStatus(name + "_acc", true);
252
2/4
✓ Branch 2 taken 8778 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8778 times.
✗ Branch 6 not taken.
 8778 constraints_->changeConstraintStatus(name + "_force", false);
253 }
254
2/6
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 3762 times.
 3762 for (std::string name : contacts_->get_inactive_set()) {
255 ✗ constraints_->changeConstraintStatus(name + "_acc", false);
256 ✗ constraints_->changeConstraintStatus(name + "_force", true);
257 }
258
1/2
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
 3762 d->constraints->resize(this, d);
259
1/2
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
 3762 constraints_->calcDiff(d->constraints, x, u);
260 3762 }
261
262 template <typename Scalar>
263 132 void DifferentialActionModelContactInvDynamicsTpl<Scalar>::calcDiff(
264 const std::shared_ptr<DifferentialActionDataAbstract>& data,
265 const Eigen::Ref<const VectorXs>& x) {
266
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 132 times.
 132 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
267 ✗ throw_pretty(
268 "Invalid argument: " << "x has wrong dimension (it should be " +
269 std::to_string(state_->get_nx()) + ")");
270 }
271 132 Data* d = static_cast<Data*>(data.get());
272 132 costs_->calcDiff(d->costs, x);
273
1/2
✓ Branch 1 taken 132 times.
✗ Branch 2 not taken.
 132 if (constraints_ != nullptr) {
274 132 constraints_->calcDiff(d->constraints, x);
275 }
276 132 }
277
278 template <typename Scalar>
279 std::shared_ptr<DifferentialActionDataAbstractTpl<Scalar>>
280 31430 DifferentialActionModelContactInvDynamicsTpl<Scalar>::createData() {
281
1/2
✓ Branch 2 taken 31430 times.
✗ Branch 3 not taken.
 31430 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
282 }
283
284 template <typename Scalar>
285 1930 void DifferentialActionModelContactInvDynamicsTpl<Scalar>::quasiStatic(
286 const std::shared_ptr<DifferentialActionDataAbstract>& data,
287 Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x, std::size_t,
288 Scalar) {
289
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1930 times.
 1930 if (static_cast<std::size_t>(u.size()) != nu_) {
290 ✗ throw_pretty(
291 "Invalid argument: " << "u has wrong dimension (it should be " +
292 std::to_string(nu_) + ")");
293 }
294
2/4
✓ Branch 3 taken 1930 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1930 times.
 1930 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
295 ✗ throw_pretty(
296 "Invalid argument: " << "x has wrong dimension (it should be " +
297 std::to_string(state_->get_nx()) + ")");
298 }
299 1930 Data* d = static_cast<Data*>(data.get());
300
1/2
✓ Branch 2 taken 1930 times.
✗ Branch 3 not taken.
 1930 const std::size_t nq = state_->get_nq();
301
1/2
✓ Branch 2 taken 1930 times.
✗ Branch 3 not taken.
 1930 const std::size_t nv = state_->get_nv();
302
1/2
✓ Branch 2 taken 1930 times.
✗ Branch 3 not taken.
 1930 const std::size_t nu = actuation_->get_nu();
303
1/2
✓ Branch 2 taken 1930 times.
✗ Branch 3 not taken.
 1930 std::size_t nc = contacts_->get_nc_total();
304 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
305
1/2
✓ Branch 1 taken 1930 times.
✗ Branch 2 not taken.
 1930 x.head(nq);
306
2/4
✓ Branch 1 taken 1930 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1930 times.
✗ Branch 5 not taken.
 1930 d->tmp_xstatic.head(nq) = q;
307
2/4
✓ Branch 1 taken 1930 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1930 times.
✗ Branch 5 not taken.
 1930 d->tmp_xstatic.tail(nv).setZero();
308
1/2
✓ Branch 1 taken 1930 times.
✗ Branch 2 not taken.
 1930 u.setZero();
309
310
1/2
✓ Branch 1 taken 1930 times.
✗ Branch 2 not taken.
 1930 pinocchio::computeAllTerms(*pinocchio_, d->pinocchio, q,
311
1/2
✓ Branch 1 taken 1930 times.
✗ Branch 2 not taken.
 1930 d->tmp_xstatic.tail(nv));
312
1/2
✓ Branch 1 taken 1930 times.
✗ Branch 2 not taken.
 1930 pinocchio::computeJointJacobians(*pinocchio_, d->pinocchio, q);
313
2/4
✓ Branch 1 taken 1930 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1930 times.
✗ Branch 5 not taken.
 1930 pinocchio::rnea(*pinocchio_, d->pinocchio, q, d->tmp_xstatic.tail(nv),
314
1/2
✓ Branch 1 taken 1930 times.
✗ Branch 2 not taken.
 1930 d->tmp_xstatic.tail(nv));
315
3/6
✓ Branch 2 taken 1930 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1930 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1930 times.
✗ Branch 9 not taken.
 3860 actuation_->calc(d->multibody.actuation, d->tmp_xstatic,
316
1/2
✓ Branch 1 taken 1930 times.
✗ Branch 2 not taken.
 1930 d->tmp_xstatic.tail(nu));
317
3/6
✓ Branch 2 taken 1930 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1930 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1930 times.
✗ Branch 9 not taken.
 3860 actuation_->calcDiff(d->multibody.actuation, d->tmp_xstatic,
318
1/2
✓ Branch 1 taken 1930 times.
✗ Branch 2 not taken.
 1930 d->tmp_xstatic.tail(nu));
319
1/2
✓ Branch 2 taken 1930 times.
✗ Branch 3 not taken.
 1930 contacts_->setComputeAllContacts(false);
320
2/4
✓ Branch 2 taken 1930 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1930 times.
✗ Branch 6 not taken.
 1930 contacts_->calc(d->multibody.contacts, d->tmp_xstatic);
321
1/2
✓ Branch 2 taken 1930 times.
✗ Branch 3 not taken.
 1930 contacts_->setComputeAllContacts(true);
322
323
1/2
✓ Branch 1 taken 1930 times.
✗ Branch 2 not taken.
 1930 d->tmp_Jstatic.conservativeResize(nv, nu + nc);
324
2/4
✓ Branch 2 taken 1930 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1930 times.
✗ Branch 6 not taken.
 1930 d->tmp_Jstatic.leftCols(nu) = d->multibody.actuation->dtau_du;
325
2/4
✓ Branch 1 taken 1930 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1930 times.
✗ Branch 5 not taken.
 1930 d->tmp_Jstatic.rightCols(nc) =
326
2/4
✓ Branch 2 taken 1930 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1930 times.
✗ Branch 6 not taken.
 1930 d->multibody.contacts->Jc.topRows(nc).transpose();
327
4/8
✓ Branch 2 taken 1930 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1930 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1930 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1930 times.
✗ Branch 12 not taken.
 1930 d->tmp_rstatic.noalias() = pseudoInverse(d->tmp_Jstatic) * d->pinocchio.tau;
328
1/2
✓ Branch 0 taken 1930 times.
✗ Branch 1 not taken.
 1930 if (nc != 0) {
329 1930 nc = 0;
330 1930 std::size_t nc_r = 0;
331 1930 for (typename ContactModelMultiple::ContactModelContainer::const_iterator
332
1/2
✓ Branch 2 taken 1930 times.
✗ Branch 3 not taken.
 1930 it_m = contacts_->get_contacts().begin();
333
3/4
✓ Branch 2 taken 6436 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4506 times.
✓ Branch 7 taken 1930 times.
 6436 it_m != contacts_->get_contacts().end(); ++it_m) {
334 4506 const std::shared_ptr<ContactItem>& m_i = it_m->second;
335
1/2
✓ Branch 3 taken 4506 times.
✗ Branch 4 not taken.
 4506 const std::size_t nc_i = m_i->contact->get_nc();
336
2/2
✓ Branch 1 taken 4502 times.
✓ Branch 2 taken 4 times.
 4506 if (m_i->active) {
337
3/6
✓ Branch 1 taken 4502 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4502 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4502 times.
✗ Branch 8 not taken.
 4502 u.segment(nv + nc, nc_i) = d->tmp_rstatic.segment(nu + nc_r, nc_i);
338 4502 nc_r += nc_i;
339 } else {
340
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 u.segment(nv + nc, nc_i).setZero();
341 }
342 4506 nc += nc_i;
343 }
344 }
345
1/2
✓ Branch 1 taken 1930 times.
✗ Branch 2 not taken.
 1930 d->pinocchio.tau.setZero();
346 1930 }
347
348 template <typename Scalar>
349 template <typename NewScalar>
350 DifferentialActionModelContactInvDynamicsTpl<NewScalar>
351 ✗ DifferentialActionModelContactInvDynamicsTpl<Scalar>::cast() const {
352 typedef DifferentialActionModelContactInvDynamicsTpl<NewScalar> ReturnType;
353 typedef StateMultibodyTpl<NewScalar> StateType;
354 typedef ContactModelMultipleTpl<NewScalar> ContactType;
355 typedef CostModelSumTpl<NewScalar> CostType;
356 typedef ConstraintModelManagerTpl<NewScalar> ConstraintType;
357 ✗ if (constraints_) {
358 ✗ const std::shared_ptr<ConstraintType>& constraints =
359 std::make_shared<ConstraintType>(
360 ✗ constraints_->template cast<NewScalar>());
361 ✗ if (state_->get_nv() - actuation_->get_nu() > 0) {
362 ✗ constraints->removeConstraint("tau");
363 }
364 ✗ if (contacts_->get_nc_total() != 0) {
365 ✗ typename ContactModelMultiple::ContactModelContainer contact_list;
366 ✗ contact_list = contacts_->get_contacts();
367 ✗ typename ContactModelMultiple::ContactModelContainer::iterator it_m,
368 ✗ end_m;
369 ✗ for (it_m = contact_list.begin(), end_m = contact_list.end();
370 ✗ it_m != end_m; ++it_m) {
371 ✗ const std::string name = it_m->second->name;
372 ✗ constraints->removeConstraint(name + "_acc");
373 ✗ constraints->removeConstraint(name + "_force");
374 }
375 }
376 ✗ ReturnType ret(
377 ✗ std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
378 ✗ actuation_->template cast<NewScalar>(),
379 ✗ std::make_shared<ContactType>(contacts_->template cast<NewScalar>()),
380 ✗ std::make_shared<CostType>(costs_->template cast<NewScalar>()),
381 constraints);
382 ✗ return ret;
383 ✗ } else {
384 ✗ ReturnType ret(
385 ✗ std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
386 ✗ actuation_->template cast<NewScalar>(),
387 ✗ std::make_shared<ContactType>(contacts_->template cast<NewScalar>()),
388 ✗ std::make_shared<CostType>(costs_->template cast<NewScalar>()));
389 ✗ return ret;
390 }
391 }
392
393 template <typename Scalar>
394 3942 bool DifferentialActionModelContactInvDynamicsTpl<Scalar>::checkData(
395 const std::shared_ptr<DifferentialActionDataAbstract>& data) {
396 3942 std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
397
1/2
✓ Branch 1 taken 3942 times.
✗ Branch 2 not taken.
 3942 if (d != NULL) {
398 3942 return true;
399 } else {
400 ✗ return false;
401 }
402 3942 }
403
404 template <typename Scalar>
405 60 void DifferentialActionModelContactInvDynamicsTpl<Scalar>::print(
406 std::ostream& os) const {
407 60 os << "DifferentialActionModelContactInvDynamics {nx=" << state_->get_nx()
408 60 << ", ndx=" << state_->get_ndx() << ", nu=" << nu_
409 60 << ", nc=" << contacts_->get_nc_total() << "}";
410 60 }
411
412 template <typename Scalar>
413 159970 std::size_t DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_ng()
414 const {
415
1/2
✓ Branch 1 taken 159970 times.
✗ Branch 2 not taken.
 159970 if (constraints_ != nullptr) {
416 159970 return constraints_->get_ng();
417 } else {
418 ✗ return Base::get_ng();
419 }
420 }
421
422 template <typename Scalar>
423 288352 std::size_t DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_nh()
424 const {
425
1/2
✓ Branch 1 taken 288352 times.
✗ Branch 2 not taken.
 288352 if (constraints_ != nullptr) {
426 288352 return constraints_->get_nh();
427 } else {
428 ✗ return Base::get_nh();
429 }
430 }
431
432 template <typename Scalar>
433 259746 std::size_t DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_ng_T()
434 const {
435
1/2
✓ Branch 1 taken 259746 times.
✗ Branch 2 not taken.
 259746 if (constraints_ != nullptr) {
436 259746 return constraints_->get_ng_T();
437 } else {
438 ✗ return Base::get_ng_T();
439 }
440 }
441
442 template <typename Scalar>
443 131364 std::size_t DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_nh_T()
444 const {
445
1/2
✓ Branch 1 taken 131364 times.
✗ Branch 2 not taken.
 131364 if (constraints_ != nullptr) {
446 131364 return constraints_->get_nh_T();
447 } else {
448 ✗ return Base::get_nh_T();
449 }
450 }
451
452 template <typename Scalar>
453 const typename MathBaseTpl<Scalar>::VectorXs&
454 ✗ DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_g_lb() const {
455 ✗ if (constraints_ != nullptr) {
456 ✗ return constraints_->get_lb();
457 } else {
458 ✗ return g_lb_;
459 }
460 }
461
462 template <typename Scalar>
463 const typename MathBaseTpl<Scalar>::VectorXs&
464 ✗ DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_g_ub() const {
465 ✗ if (constraints_ != nullptr) {
466 ✗ return constraints_->get_ub();
467 } else {
468 ✗ return g_lb_;
469 }
470 }
471
472 template <typename Scalar>
473 pinocchio::ModelTpl<Scalar>&
474 31430 DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_pinocchio() const {
475 31430 return *pinocchio_;
476 }
477
478 template <typename Scalar>
479 const std::shared_ptr<ActuationModelAbstractTpl<Scalar>>&
480 125720 DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_actuation() const {
481 125720 return actuation_;
482 }
483
484 template <typename Scalar>
485 const std::shared_ptr<ContactModelMultipleTpl<Scalar>>&
486 574318 DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_contacts() const {
487 574318 return contacts_;
488 }
489
490 template <typename Scalar>
491 const std::shared_ptr<CostModelSumTpl<Scalar>>&
492 31430 DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_costs() const {
493 31430 return costs_;
494 }
495
496 template <typename Scalar>
497 const std::shared_ptr<ConstraintModelManagerTpl<Scalar>>&
498 31430 DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_constraints() const {
499 31430 return constraints_;
500 }
501
502 } // namespace crocoddyl
503