GCC Code Coverage Report

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