GCC Code Coverage Report

Directory: ./
File: unittest/factory/diff_action.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 506 519 97.5%
Branches: 536 1030 52.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, University of Edinburgh, CTU, INRIA,
5 // Heriot-Watt University, University of Pisa
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include "diff_action.hpp"
11
12 #include "contact.hpp"
13 #include "cost.hpp"
14 #include "crocoddyl/core/actions/diff-lqr.hpp"
15 #include "crocoddyl/core/activations/quadratic-barrier.hpp"
16 #include "crocoddyl/core/activations/quadratic.hpp"
17 #include "crocoddyl/core/costs/residual.hpp"
18 #include "crocoddyl/core/residuals/joint-acceleration.hpp"
19 #include "crocoddyl/core/residuals/joint-effort.hpp"
20 #include "crocoddyl/multibody/actuations/floating-base.hpp"
21 #include "crocoddyl/multibody/actuations/full.hpp"
22 #include "crocoddyl/multibody/residuals/contact-force.hpp"
23 #include "crocoddyl/multibody/residuals/contact-friction-cone.hpp"
24 #include "crocoddyl/multibody/residuals/contact-wrench-cone.hpp"
25 #include "crocoddyl/multibody/residuals/frame-placement.hpp"
26 #include "crocoddyl/multibody/residuals/frame-translation.hpp"
27 #include "crocoddyl/multibody/states/multibody.hpp"
28
29 namespace crocoddyl {
30 namespace unittest {
31
32 const std::vector<DifferentialActionModelTypes::Type>
33 DifferentialActionModelTypes::all(DifferentialActionModelTypes::init_all());
34
35 368 std::ostream& operator<<(std::ostream& os,
36 DifferentialActionModelTypes::Type type) {
37
23/25
✓ Branch 0 taken 16 times.
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 16 times.
✓ Branch 3 taken 16 times.
✓ Branch 4 taken 16 times.
✓ Branch 5 taken 16 times.
✓ Branch 6 taken 16 times.
✓ Branch 7 taken 16 times.
✓ Branch 8 taken 16 times.
✓ Branch 9 taken 16 times.
✓ Branch 10 taken 16 times.
✓ Branch 11 taken 16 times.
✓ Branch 12 taken 16 times.
✓ Branch 13 taken 16 times.
✓ Branch 14 taken 16 times.
✓ Branch 15 taken 16 times.
✓ Branch 16 taken 16 times.
✓ Branch 17 taken 16 times.
✓ Branch 18 taken 16 times.
✓ Branch 19 taken 16 times.
✓ Branch 20 taken 16 times.
✓ Branch 21 taken 16 times.
✓ Branch 22 taken 16 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 368 switch (type) {
38 16 case DifferentialActionModelTypes::DifferentialActionModelLQR:
39 16 os << "DifferentialActionModelLQR";
40 16 break;
41 16 case DifferentialActionModelTypes::DifferentialActionModelLQRDriftFree:
42 16 os << "DifferentialActionModelLQRDriftFree";
43 16 break;
44 16 case DifferentialActionModelTypes::DifferentialActionModelRandomLQR:
45 16 os << "DifferentialActionModelRandomLQR";
46 16 break;
47 16 case DifferentialActionModelTypes::
48 DifferentialActionModelFreeFwdDynamics_Hector:
49 16 os << "DifferentialActionModelFreeFwdDynamics_Hector";
50 16 break;
51 16 case DifferentialActionModelTypes::
52 DifferentialActionModelFreeFwdDynamics_TalosArm:
53 16 os << "DifferentialActionModelFreeFwdDynamics_TalosArm";
54 16 break;
55 16 case DifferentialActionModelTypes::
56 DifferentialActionModelFreeFwdDynamics_TalosArm_Squashed:
57 16 os << "DifferentialActionModelFreeFwdDynamics_TalosArm_Squashed";
58 16 break;
59 16 case DifferentialActionModelTypes::
60 DifferentialActionModelFreeInvDynamics_Hector:
61 16 os << "DifferentialActionModelFreeInvDynamics_Hector";
62 16 break;
63 16 case DifferentialActionModelTypes::
64 DifferentialActionModelFreeInvDynamics_TalosArm:
65 16 os << "DifferentialActionModelFreeInvDynamics_TalosArm";
66 16 break;
67 16 case DifferentialActionModelTypes::
68 DifferentialActionModelFreeInvDynamics_TalosArm_Squashed:
69 16 os << "DifferentialActionModelFreeInvDynamics_TalosArm_Squashed";
70 16 break;
71 16 case DifferentialActionModelTypes::
72 DifferentialActionModelContactFwdDynamics_TalosArm:
73 16 os << "DifferentialActionModelContactFwdDynamics_TalosArm";
74 16 break;
75 16 case DifferentialActionModelTypes::
76 DifferentialActionModelContact2DFwdDynamics_TalosArm:
77 16 os << "DifferentialActionModelContact2DFwdDynamics_TalosArm";
78 16 break;
79 16 case DifferentialActionModelTypes::
80 DifferentialActionModelContactFwdDynamics_HyQ:
81 16 os << "DifferentialActionModelContactFwdDynamics_HyQ";
82 16 break;
83 16 case DifferentialActionModelTypes::
84 DifferentialActionModelContactFwdDynamics_Talos:
85 16 os << "DifferentialActionModelContactFwdDynamics_Talos";
86 16 break;
87 16 case DifferentialActionModelTypes::
88 DifferentialActionModelContactFwdDynamicsWithFriction_TalosArm:
89 16 os << "DifferentialActionModelContactFwdDynamicsWithFriction_TalosArm";
90 16 break;
91 16 case DifferentialActionModelTypes::
92 DifferentialActionModelContact2DFwdDynamicsWithFriction_TalosArm:
93 16 os << "DifferentialActionModelContact2DFwdDynamicsWithFriction_TalosArm";
94 16 break;
95 16 case DifferentialActionModelTypes::
96 DifferentialActionModelContactFwdDynamicsWithFriction_HyQ:
97 16 os << "DifferentialActionModelContactFwdDynamicsWithFriction_HyQ";
98 16 break;
99 16 case DifferentialActionModelTypes::
100 DifferentialActionModelContactFwdDynamicsWithFriction_Talos:
101 16 os << "DifferentialActionModelContactFwdDynamicsWithFriction_Talos";
102 16 break;
103 16 case DifferentialActionModelTypes::
104 DifferentialActionModelContactInvDynamics_TalosArm:
105 16 os << "DifferentialActionModelContactInvDynamics_TalosArm";
106 16 break;
107 16 case DifferentialActionModelTypes::
108 DifferentialActionModelContactInvDynamics_HyQ:
109 16 os << "DifferentialActionModelContactInvDynamics_HyQ";
110 16 break;
111 16 case DifferentialActionModelTypes::
112 DifferentialActionModelContactInvDynamics_Talos:
113 16 os << "DifferentialActionModelContactInvDynamics_Talos";
114 16 break;
115 16 case DifferentialActionModelTypes::
116 DifferentialActionModelContactInvDynamicsWithFriction_TalosArm:
117 16 os << "DifferentialActionModelContactInvDynamicsWithFriction_TalosArm";
118 16 break;
119 16 case DifferentialActionModelTypes::
120 DifferentialActionModelContactInvDynamicsWithFriction_HyQ:
121 16 os << "DifferentialActionModelContactInvDynamicsWithFriction_HyQ";
122 16 break;
123 16 case DifferentialActionModelTypes::
124 DifferentialActionModelContactInvDynamicsWithFriction_Talos:
125 16 os << "DifferentialActionModelContactInvDynamicsWithFriction_Talos";
126 16 break;
127 ✗ case DifferentialActionModelTypes::NbDifferentialActionModelTypes:
128 ✗ os << "NbDifferentialActionModelTypes";
129 ✗ break;
130 ✗ default:
131 ✗ break;
132 }
133 368 return os;
134 }
135
136 1197 DifferentialActionModelFactory::DifferentialActionModelFactory() {}
137 1197 DifferentialActionModelFactory::~DifferentialActionModelFactory() {}
138
139 std::shared_ptr<crocoddyl::DifferentialActionModelAbstract>
140 1149 DifferentialActionModelFactory::create(DifferentialActionModelTypes::Type type,
141 bool with_baumgarte) const {
142 1149 std::shared_ptr<crocoddyl::DifferentialActionModelAbstract> action;
143
23/24
✓ Branch 0 taken 50 times.
✓ Branch 1 taken 50 times.
✓ Branch 2 taken 50 times.
✓ Branch 3 taken 50 times.
✓ Branch 4 taken 50 times.
✓ Branch 5 taken 49 times.
✓ Branch 6 taken 50 times.
✓ Branch 7 taken 50 times.
✓ Branch 8 taken 50 times.
✓ Branch 9 taken 50 times.
✓ Branch 10 taken 50 times.
✓ Branch 11 taken 50 times.
✓ Branch 12 taken 50 times.
✓ Branch 13 taken 50 times.
✓ Branch 14 taken 50 times.
✓ Branch 15 taken 50 times.
✓ Branch 16 taken 50 times.
✓ Branch 17 taken 50 times.
✓ Branch 18 taken 50 times.
✓ Branch 19 taken 50 times.
✓ Branch 20 taken 50 times.
✓ Branch 21 taken 50 times.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
 1149 switch (type) {
144 50 case DifferentialActionModelTypes::DifferentialActionModelLQR:
145 50 action = std::make_shared<crocoddyl::DifferentialActionModelLQR>(40, 40,
146
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 false);
147 50 break;
148 50 case DifferentialActionModelTypes::DifferentialActionModelLQRDriftFree:
149 action =
150
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 std::make_shared<crocoddyl::DifferentialActionModelLQR>(40, 40, true);
151 50 break;
152 50 case DifferentialActionModelTypes::DifferentialActionModelRandomLQR:
153
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = std::make_shared<crocoddyl::DifferentialActionModelLQR>(
154
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 150 crocoddyl::DifferentialActionModelLQR::Random(40, 40));
155 50 break;
156 50 case DifferentialActionModelTypes::
157 DifferentialActionModelFreeFwdDynamics_Hector:
158
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_freeFwdDynamics(
159 StateModelTypes::StateMultibody_Hector,
160 50 ActuationModelTypes::ActuationModelFloatingBaseThrusters, false);
161 50 break;
162 50 case DifferentialActionModelTypes::
163 DifferentialActionModelFreeFwdDynamics_TalosArm:
164
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_freeFwdDynamics(StateModelTypes::StateMultibody_TalosArm,
165 50 ActuationModelTypes::ActuationModelFull);
166 50 break;
167 49 case DifferentialActionModelTypes::
168 DifferentialActionModelFreeFwdDynamics_TalosArm_Squashed:
169
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 98 action = create_freeFwdDynamics(
170 StateModelTypes::StateMultibody_TalosArm,
171 49 ActuationModelTypes::ActuationModelSquashingFull);
172 49 break;
173 50 case DifferentialActionModelTypes::
174 DifferentialActionModelFreeInvDynamics_Hector:
175
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_freeInvDynamics(
176 StateModelTypes::StateMultibody_Hector,
177 50 ActuationModelTypes::ActuationModelFloatingBaseThrusters);
178 50 break;
179 50 case DifferentialActionModelTypes::
180 DifferentialActionModelFreeInvDynamics_TalosArm:
181
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_freeInvDynamics(StateModelTypes::StateMultibody_TalosArm,
182 50 ActuationModelTypes::ActuationModelFull);
183 50 break;
184 50 case DifferentialActionModelTypes::
185 DifferentialActionModelFreeInvDynamics_TalosArm_Squashed:
186
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_freeInvDynamics(
187 StateModelTypes::StateMultibody_TalosArm,
188 50 ActuationModelTypes::ActuationModelSquashingFull);
189 50 break;
190 50 case DifferentialActionModelTypes::
191 DifferentialActionModelContactFwdDynamics_TalosArm:
192
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_contactFwdDynamics(
193 StateModelTypes::StateMultibody_TalosArm,
194 50 ActuationModelTypes::ActuationModelFull, false, with_baumgarte);
195 50 break;
196 50 case DifferentialActionModelTypes::
197 DifferentialActionModelContact2DFwdDynamics_TalosArm:
198
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_contactFwdDynamics(
199 StateModelTypes::StateMultibodyContact2D_TalosArm,
200 50 ActuationModelTypes::ActuationModelFull, false, with_baumgarte);
201 50 break;
202 50 case DifferentialActionModelTypes::
203 DifferentialActionModelContactFwdDynamics_HyQ:
204
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_contactFwdDynamics(
205 StateModelTypes::StateMultibody_HyQ,
206 ActuationModelTypes::ActuationModelFloatingBase, false,
207 50 with_baumgarte);
208 50 break;
209 50 case DifferentialActionModelTypes::
210 DifferentialActionModelContactFwdDynamics_Talos:
211
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_contactFwdDynamics(
212 StateModelTypes::StateMultibody_Talos,
213 ActuationModelTypes::ActuationModelFloatingBase, false,
214 50 with_baumgarte);
215 50 break;
216 50 case DifferentialActionModelTypes::
217 DifferentialActionModelContactFwdDynamicsWithFriction_TalosArm:
218
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_contactFwdDynamics(
219 StateModelTypes::StateMultibody_TalosArm,
220 50 ActuationModelTypes::ActuationModelFull, true, with_baumgarte);
221 50 break;
222 50 case DifferentialActionModelTypes::
223 DifferentialActionModelContact2DFwdDynamicsWithFriction_TalosArm:
224
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_contactFwdDynamics(
225 StateModelTypes::StateMultibodyContact2D_TalosArm,
226 50 ActuationModelTypes::ActuationModelFull, true, with_baumgarte);
227 50 break;
228 50 case DifferentialActionModelTypes::
229 DifferentialActionModelContactFwdDynamicsWithFriction_HyQ:
230
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_contactFwdDynamics(
231 StateModelTypes::StateMultibody_HyQ,
232 ActuationModelTypes::ActuationModelFloatingBase, true,
233 50 with_baumgarte);
234 50 break;
235 50 case DifferentialActionModelTypes::
236 DifferentialActionModelContactFwdDynamicsWithFriction_Talos:
237
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_contactFwdDynamics(
238 StateModelTypes::StateMultibody_Talos,
239 ActuationModelTypes::ActuationModelFloatingBase, true,
240 50 with_baumgarte);
241 50 break;
242 50 case DifferentialActionModelTypes::
243 DifferentialActionModelContactInvDynamics_TalosArm:
244
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_contactInvDynamics(
245 StateModelTypes::StateMultibody_TalosArm,
246 ActuationModelTypes::ActuationModelFloatingBase, false,
247 50 with_baumgarte);
248 50 break;
249 50 case DifferentialActionModelTypes::
250 DifferentialActionModelContactInvDynamics_HyQ:
251
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_contactInvDynamics(
252 StateModelTypes::StateMultibody_HyQ,
253 ActuationModelTypes::ActuationModelFloatingBase, false,
254 50 with_baumgarte);
255 50 break;
256 50 case DifferentialActionModelTypes::
257 DifferentialActionModelContactInvDynamics_Talos:
258
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_contactInvDynamics(
259 StateModelTypes::StateMultibody_Talos,
260 ActuationModelTypes::ActuationModelFloatingBase, false,
261 50 with_baumgarte);
262 50 break;
263 50 case DifferentialActionModelTypes::
264 DifferentialActionModelContactInvDynamicsWithFriction_TalosArm:
265
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_contactInvDynamics(
266 StateModelTypes::StateMultibody_TalosArm,
267 50 ActuationModelTypes::ActuationModelFull, true, with_baumgarte);
268 50 break;
269 50 case DifferentialActionModelTypes::
270 DifferentialActionModelContactInvDynamicsWithFriction_HyQ:
271
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_contactInvDynamics(
272 StateModelTypes::StateMultibody_HyQ,
273 ActuationModelTypes::ActuationModelFloatingBase, true,
274 50 with_baumgarte);
275 50 break;
276 50 case DifferentialActionModelTypes::
277 DifferentialActionModelContactInvDynamicsWithFriction_Talos:
278
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 action = create_contactInvDynamics(
279 StateModelTypes::StateMultibody_Talos,
280 ActuationModelTypes::ActuationModelFloatingBase, true,
281 50 with_baumgarte);
282 50 break;
283 ✗ default:
284 ✗ throw_pretty(__FILE__ ": Wrong DifferentialActionModelTypes::Type given");
285 break;
286 }
287 1149 return action;
288 }
289
290 std::shared_ptr<crocoddyl::DifferentialActionModelFreeFwdDynamics>
291 149 DifferentialActionModelFactory::create_freeFwdDynamics(
292 StateModelTypes::Type state_type, ActuationModelTypes::Type actuation_type,
293 bool constraints) const {
294 149 std::shared_ptr<crocoddyl::DifferentialActionModelFreeFwdDynamics> action;
295 149 std::shared_ptr<crocoddyl::StateMultibody> state;
296 149 std::shared_ptr<crocoddyl::ActuationModelAbstract> actuation;
297 149 std::shared_ptr<crocoddyl::CostModelSum> cost;
298 149 std::shared_ptr<crocoddyl::ConstraintModelManager> constraint;
299 298 state = std::static_pointer_cast<crocoddyl::StateMultibody>(
300
2/4
✓ Branch 1 taken 149 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 149 times.
✗ Branch 5 not taken.
 447 StateModelFactory().create(state_type));
301
2/4
✓ Branch 1 taken 149 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 149 times.
✗ Branch 5 not taken.
 149 actuation = ActuationModelFactory().create(actuation_type, state_type);
302
1/2
✓ Branch 2 taken 149 times.
✗ Branch 3 not taken.
 149 const std::size_t nu = actuation->get_nu();
303
1/2
✓ Branch 1 taken 149 times.
✗ Branch 2 not taken.
 149 cost = std::make_shared<crocoddyl::CostModelSum>(state, nu);
304
2/4
✓ Branch 3 taken 149 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 149 times.
✗ Branch 7 not taken.
 298 cost->addCost("state",
305
2/4
✓ Branch 1 taken 149 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 149 times.
✗ Branch 5 not taken.
 298 CostModelFactory().create(
306 CostModelTypes::CostModelResidualState, state_type,
307 ActivationModelTypes::ActivationModelQuad, nu),
308 1.);
309
2/4
✓ Branch 3 taken 149 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 149 times.
✗ Branch 7 not taken.
 298 cost->addCost("control",
310
2/4
✓ Branch 1 taken 149 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 149 times.
✗ Branch 5 not taken.
 298 CostModelFactory().create(
311 CostModelTypes::CostModelResidualControl, state_type,
312 ActivationModelTypes::ActivationModelQuad, nu),
313 1.);
314
2/4
✓ Branch 4 taken 149 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 149 times.
✗ Branch 8 not taken.
 298 cost->addCost(
315 "joint_eff",
316
1/2
✓ Branch 1 taken 149 times.
✗ Branch 2 not taken.
 298 std::make_shared<crocoddyl::CostModelResidual>(
317
1/2
✓ Branch 1 taken 149 times.
✗ Branch 2 not taken.
 298 state, std::make_shared<crocoddyl::ResidualModelJointEffort>(
318
1/2
✓ Branch 1 taken 149 times.
✗ Branch 2 not taken.
 149 state, actuation, Eigen::VectorXd::Zero(nu), nu, true)),
319 1.);
320
2/4
✓ Branch 4 taken 149 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 149 times.
✗ Branch 8 not taken.
 298 cost->addCost(
321 "joint_acc",
322
1/2
✓ Branch 1 taken 149 times.
✗ Branch 2 not taken.
 298 std::make_shared<crocoddyl::CostModelResidual>(
323
1/2
✓ Branch 1 taken 149 times.
✗ Branch 2 not taken.
 298 state, std::make_shared<crocoddyl::ResidualModelJointAcceleration>(
324 state, nu)),
325 0.01);
326
2/4
✓ Branch 3 taken 149 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 149 times.
✗ Branch 7 not taken.
 298 cost->addCost("frame",
327
2/4
✓ Branch 1 taken 149 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 149 times.
✗ Branch 5 not taken.
 298 CostModelFactory().create(
328 CostModelTypes::CostModelResidualFramePlacement, state_type,
329 ActivationModelTypes::ActivationModelQuad, nu),
330 1.);
331
2/2
✓ Branch 0 taken 99 times.
✓ Branch 1 taken 50 times.
 149 if (constraints) {
332
1/2
✓ Branch 1 taken 99 times.
✗ Branch 2 not taken.
 99 constraint = std::make_shared<crocoddyl::ConstraintModelManager>(state, nu);
333
2/4
✓ Branch 3 taken 99 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 99 times.
✗ Branch 7 not taken.
 198 constraint->addConstraint(
334 "frame",
335
2/4
✓ Branch 1 taken 99 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 99 times.
✗ Branch 5 not taken.
 198 ConstraintModelFactory().create(
336 ConstraintModelTypes::ConstraintModelResidualFramePlacementEquality,
337 state_type, nu));
338
2/4
✓ Branch 3 taken 99 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 99 times.
✗ Branch 7 not taken.
 198 constraint->addConstraint(
339 "frame-velocity",
340
2/4
✓ Branch 1 taken 99 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 99 times.
✗ Branch 5 not taken.
 198 ConstraintModelFactory().create(
341 ConstraintModelTypes::ConstraintModelResidualFrameVelocityEquality,
342 state_type, nu));
343 action =
344
1/2
✓ Branch 1 taken 99 times.
✗ Branch 2 not taken.
 198 std::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(
345 99 state, actuation, cost, constraint);
346 } else {
347 action =
348
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(
349 50 state, actuation, cost);
350 }
351 298 return action;
352 149 }
353
354 std::shared_ptr<crocoddyl::DifferentialActionModelFreeInvDynamics>
355 150 DifferentialActionModelFactory::create_freeInvDynamics(
356 StateModelTypes::Type state_type, ActuationModelTypes::Type actuation_type,
357 bool constraints) const {
358 150 std::shared_ptr<crocoddyl::DifferentialActionModelFreeInvDynamics> action;
359 150 std::shared_ptr<crocoddyl::StateMultibody> state;
360 150 std::shared_ptr<crocoddyl::ActuationModelAbstract> actuation;
361 150 std::shared_ptr<crocoddyl::CostModelSum> cost;
362 150 std::shared_ptr<crocoddyl::ConstraintModelManager> constraint;
363 300 state = std::static_pointer_cast<crocoddyl::StateMultibody>(
364
2/4
✓ Branch 1 taken 150 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 150 times.
✗ Branch 5 not taken.
 450 StateModelFactory().create(state_type));
365
2/4
✓ Branch 1 taken 150 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 150 times.
✗ Branch 5 not taken.
 150 actuation = ActuationModelFactory().create(actuation_type, state_type);
366
1/2
✓ Branch 2 taken 150 times.
✗ Branch 3 not taken.
 150 const std::size_t nu = state->get_nv();
367
1/2
✓ Branch 1 taken 150 times.
✗ Branch 2 not taken.
 150 cost = std::make_shared<crocoddyl::CostModelSum>(state, nu);
368
2/4
✓ Branch 3 taken 150 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 150 times.
✗ Branch 7 not taken.
 300 cost->addCost("state",
369
2/4
✓ Branch 1 taken 150 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 150 times.
✗ Branch 5 not taken.
 300 CostModelFactory().create(
370 CostModelTypes::CostModelResidualState, state_type,
371 ActivationModelTypes::ActivationModelQuad, nu),
372 1.);
373
2/4
✓ Branch 3 taken 150 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 150 times.
✗ Branch 7 not taken.
 300 cost->addCost("control",
374
2/4
✓ Branch 1 taken 150 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 150 times.
✗ Branch 5 not taken.
 300 CostModelFactory().create(
375 CostModelTypes::CostModelResidualControl, state_type,
376 ActivationModelTypes::ActivationModelQuad, nu),
377 1.);
378
2/4
✓ Branch 3 taken 150 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 150 times.
✗ Branch 7 not taken.
 300 cost->addCost("frame",
379
2/4
✓ Branch 1 taken 150 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 150 times.
✗ Branch 5 not taken.
 300 CostModelFactory().create(
380 CostModelTypes::CostModelResidualFramePlacement, state_type,
381 ActivationModelTypes::ActivationModelQuad, nu),
382 1.);
383
1/2
✓ Branch 0 taken 150 times.
✗ Branch 1 not taken.
 150 if (constraints) {
384
1/2
✓ Branch 1 taken 150 times.
✗ Branch 2 not taken.
 150 constraint = std::make_shared<crocoddyl::ConstraintModelManager>(state, nu);
385
2/4
✓ Branch 3 taken 150 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 150 times.
✗ Branch 7 not taken.
 300 constraint->addConstraint(
386 "frame",
387
2/4
✓ Branch 1 taken 150 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 150 times.
✗ Branch 5 not taken.
 300 ConstraintModelFactory().create(
388 ConstraintModelTypes::ConstraintModelResidualFramePlacementEquality,
389 state_type, nu));
390
2/4
✓ Branch 3 taken 150 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 150 times.
✗ Branch 7 not taken.
 300 constraint->addConstraint(
391 "frame-velocity",
392
2/4
✓ Branch 1 taken 150 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 150 times.
✗ Branch 5 not taken.
 300 ConstraintModelFactory().create(
393 ConstraintModelTypes::ConstraintModelResidualFrameVelocityEquality,
394 state_type, nu));
395 action =
396
1/2
✓ Branch 1 taken 150 times.
✗ Branch 2 not taken.
 300 std::make_shared<crocoddyl::DifferentialActionModelFreeInvDynamics>(
397 150 state, actuation, cost, constraint);
398 } else {
399 action =
400 ✗ std::make_shared<crocoddyl::DifferentialActionModelFreeInvDynamics>(
401 ✗ state, actuation, cost);
402 }
403 300 return action;
404 150 }
405
406 std::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics>
407 448 DifferentialActionModelFactory::create_contactFwdDynamics(
408 StateModelTypes::Type state_type, ActuationModelTypes::Type actuation_type,
409 bool with_friction, bool with_baumgarte) const {
410 448 std::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics> action;
411 448 std::shared_ptr<crocoddyl::StateMultibody> state;
412 448 std::shared_ptr<crocoddyl::ActuationModelAbstract> actuation;
413 448 std::shared_ptr<crocoddyl::ContactModelMultiple> contact;
414 448 std::shared_ptr<crocoddyl::CostModelSum> cost;
415 896 state = std::static_pointer_cast<crocoddyl::StateMultibody>(
416
2/4
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 448 times.
✗ Branch 5 not taken.
 1344 StateModelFactory().create(state_type));
417
2/4
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 448 times.
✗ Branch 5 not taken.
 448 actuation = ActuationModelFactory().create(actuation_type, state_type);
418
1/2
✓ Branch 2 taken 448 times.
✗ Branch 3 not taken.
 448 const std::size_t nu = actuation->get_nu();
419
1/2
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
 448 contact = std::make_shared<crocoddyl::ContactModelMultiple>(state, nu);
420
1/2
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
 448 cost = std::make_shared<crocoddyl::CostModelSum>(state, nu);
421
422
2/4
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 448 times.
✗ Branch 5 not taken.
 448 Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
423
1/2
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
 448 pinocchio::Force force = pinocchio::Force::Zero();
424
1/2
✓ Branch 2 taken 448 times.
✗ Branch 3 not taken.
 448 crocoddyl::FrictionCone friction_cone(R, 0.8, 4, false);
425
1/2
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
 448 crocoddyl::WrenchCone wrench_cone(R, 0.8, Eigen::Vector2d(0.1, 0.1), 4,
426
1/2
✓ Branch 2 taken 448 times.
✗ Branch 3 not taken.
 896 false);
427 896 crocoddyl::ActivationBounds friction_bounds(friction_cone.get_lb(),
428
3/6
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 448 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 448 times.
✗ Branch 8 not taken.
 448 friction_cone.get_ub());
429 896 crocoddyl::ActivationBounds wrench_bounds(wrench_cone.get_lb(),
430
3/6
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 448 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 448 times.
✗ Branch 8 not taken.
 448 wrench_cone.get_ub());
431 std::shared_ptr<crocoddyl::ActivationModelAbstract> friction_activation =
432
1/2
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
 448 std::make_shared<crocoddyl::ActivationModelQuadraticBarrier>(
433 448 friction_bounds);
434 std::shared_ptr<crocoddyl::ActivationModelAbstract> wrench_activation =
435
1/2
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
 448 std::make_shared<crocoddyl::ActivationModelQuadraticBarrier>(
436 448 wrench_bounds);
437
2/4
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 448 times.
✗ Branch 5 not taken.
 448 Eigen::Vector2d gains = Eigen::Vector2d::Random();
438
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 440 times.
 448 if (!with_baumgarte) {
439
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 gains.setZero();
440 }
441
442
4/5
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 100 times.
✓ Branch 2 taken 118 times.
✓ Branch 3 taken 130 times.
✗ Branch 4 not taken.
 448 switch (state_type) {
443 100 case StateModelTypes::StateMultibody_TalosArm:
444
6/12
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 100 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 100 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 100 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 100 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 100 times.
✗ Branch 20 not taken.
 100 contact->addContact("lf", ContactModelFactory().create(
445 ContactModelTypes::ContactModel3D_LOCAL,
446 PinocchioModelTypes::TalosArm, gains,
447 "gripper_left_fingertip_1_link", nu));
448
2/2
✓ Branch 0 taken 50 times.
✓ Branch 1 taken 50 times.
 100 if (with_friction) {
449 // friction cone
450
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
451 "lf_cone",
452
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
453 state, friction_activation,
454 50 std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
455 state,
456
4/8
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 50 times.
✗ Branch 14 not taken.
 100 state->get_pinocchio()->getFrameId(
457 "gripper_left_fingertip_1_link"),
458 friction_cone, nu)),
459 0.1);
460 // force regularization
461
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
462 "lf_forceReg",
463
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
464 50 state, std::make_shared<crocoddyl::ResidualModelContactForce>(
465 state,
466
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state->get_pinocchio()->getFrameId(
467 "gripper_left_fingertip_1_link"),
468 100 force, 3, nu)),
469 0.1);
470 }
471 100 break;
472 100 case StateModelTypes::StateMultibodyContact2D_TalosArm:
473
6/12
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 100 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 100 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 100 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 100 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 100 times.
✗ Branch 20 not taken.
 100 contact->addContact("lf", ContactModelFactory().create(
474 ContactModelTypes::ContactModel2D,
475 PinocchioModelTypes::TalosArm, gains,
476 "gripper_left_fingertip_1_link", nu));
477
2/2
✓ Branch 0 taken 50 times.
✓ Branch 1 taken 50 times.
 100 if (with_friction) {
478 // friction cone
479
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
480 "lf_cone",
481
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
482 state, friction_activation,
483 50 std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
484 state,
485
4/8
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 50 times.
✗ Branch 14 not taken.
 100 state->get_pinocchio()->getFrameId(
486 "gripper_left_fingertip_1_link"),
487 friction_cone, nu)),
488 0.1);
489 // TODO: enable force regularization once it would support Contact2D
490 }
491 100 break;
492 118 case StateModelTypes::StateMultibody_HyQ:
493
2/4
✓ Branch 3 taken 118 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 118 times.
✗ Branch 7 not taken.
 236 contact->addContact(
494
4/8
✓ Branch 1 taken 118 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 118 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 118 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 118 times.
✗ Branch 12 not taken.
 472 "lf", ContactModelFactory().create(
495 ContactModelTypes::ContactModel3D_LOCAL,
496
1/2
✓ Branch 1 taken 118 times.
✗ Branch 2 not taken.
 236 PinocchioModelTypes::HyQ, 0.25 * gains, "lf_foot", nu));
497
2/4
✓ Branch 3 taken 118 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 118 times.
✗ Branch 7 not taken.
 236 contact->addContact(
498
4/8
✓ Branch 1 taken 118 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 118 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 118 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 118 times.
✗ Branch 12 not taken.
 472 "rf", ContactModelFactory().create(
499 ContactModelTypes::ContactModel3D_WORLD,
500
1/2
✓ Branch 1 taken 118 times.
✗ Branch 2 not taken.
 236 PinocchioModelTypes::HyQ, 0.25 * gains, "rf_foot", nu));
501
2/4
✓ Branch 3 taken 118 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 118 times.
✗ Branch 7 not taken.
 236 contact->addContact(
502
4/8
✓ Branch 1 taken 118 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 118 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 118 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 118 times.
✗ Branch 12 not taken.
 472 "lh", ContactModelFactory().create(
503 ContactModelTypes::ContactModel3D_LWA,
504
1/2
✓ Branch 1 taken 118 times.
✗ Branch 2 not taken.
 236 PinocchioModelTypes::HyQ, 0.25 * gains, "lh_foot", nu));
505
2/4
✓ Branch 3 taken 118 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 118 times.
✗ Branch 7 not taken.
 236 contact->addContact(
506
4/8
✓ Branch 1 taken 118 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 118 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 118 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 118 times.
✗ Branch 12 not taken.
 472 "rh", ContactModelFactory().create(
507 ContactModelTypes::ContactModel3D_LOCAL,
508
1/2
✓ Branch 1 taken 118 times.
✗ Branch 2 not taken.
 236 PinocchioModelTypes::HyQ, 0.25 * gains, "rh_foot", nu));
509
2/2
✓ Branch 0 taken 50 times.
✓ Branch 1 taken 68 times.
 118 if (with_friction) {
510 // friction cone
511
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
512 "lf_cone",
513
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
514 state, friction_activation,
515 50 std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
516
4/8
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 50 times.
✗ Branch 14 not taken.
 100 state, state->get_pinocchio()->getFrameId("lf_foot"),
517 friction_cone, nu)),
518 0.1);
519
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
520 "rf_cone",
521
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
522 state, friction_activation,
523 50 std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
524
4/8
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 50 times.
✗ Branch 14 not taken.
 100 state, state->get_pinocchio()->getFrameId("rf_foot"),
525 friction_cone, nu)),
526 0.1);
527
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
528 "lh_cone",
529
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
530 state, friction_activation,
531 50 std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
532
4/8
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 50 times.
✗ Branch 14 not taken.
 100 state, state->get_pinocchio()->getFrameId("lh_foot"),
533 friction_cone, nu)),
534 0.1);
535
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
536 "rh_cone",
537
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
538 state, friction_activation,
539 50 std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
540
4/8
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 50 times.
✗ Branch 14 not taken.
 100 state, state->get_pinocchio()->getFrameId("rh_foot"),
541 friction_cone, nu)),
542 0.1);
543 // force regularization
544
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
545 "lf_forceReg",
546
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
547 50 state, std::make_shared<crocoddyl::ResidualModelContactForce>(
548
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state, state->get_pinocchio()->getFrameId("lf_foot"),
549 50 force, 3, nu)),
550 0.1);
551
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
552 "rf_forceReg",
553
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
554 50 state, std::make_shared<crocoddyl::ResidualModelContactForce>(
555
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state, state->get_pinocchio()->getFrameId("rf_foot"),
556 50 force, 3, nu)),
557 0.1);
558
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
559 "lh_forceReg",
560
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
561 50 state, std::make_shared<crocoddyl::ResidualModelContactForce>(
562
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state, state->get_pinocchio()->getFrameId("lh_foot"),
563 50 force, 3, nu)),
564 0.1);
565
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
566 "rh_forceReg",
567
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
568 50 state, std::make_shared<crocoddyl::ResidualModelContactForce>(
569
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state, state->get_pinocchio()->getFrameId("rh_foot"),
570 100 force, 3, nu)),
571 0.1);
572 }
573 118 break;
574 130 case StateModelTypes::StateMultibody_Talos:
575
2/4
✓ Branch 3 taken 130 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 130 times.
✗ Branch 7 not taken.
 260 contact->addContact(
576
4/8
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 130 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 130 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 130 times.
✗ Branch 12 not taken.
 260 "lf", ContactModelFactory().create(
577 ContactModelTypes::ContactModel6D_LOCAL,
578 PinocchioModelTypes::Talos, gains, "left_sole_link", nu));
579
2/4
✓ Branch 3 taken 130 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 130 times.
✗ Branch 7 not taken.
 260 contact->addContact(
580
4/8
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 130 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 130 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 130 times.
✗ Branch 12 not taken.
 260 "rf", ContactModelFactory().create(
581 ContactModelTypes::ContactModel6D_WORLD,
582 PinocchioModelTypes::Talos, gains, "right_sole_link", nu));
583
2/2
✓ Branch 0 taken 50 times.
✓ Branch 1 taken 80 times.
 130 if (with_friction) {
584 // friction / wrench cone
585
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
586 "lf_cone",
587
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
588 state, friction_activation,
589 50 std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
590
4/8
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 50 times.
✗ Branch 14 not taken.
 100 state, state->get_pinocchio()->getFrameId("left_sole_link"),
591 friction_cone, nu)),
592 0.01);
593
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
594 "rf_cone",
595
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
596 state, wrench_activation,
597 50 std::make_shared<crocoddyl::ResidualModelContactWrenchCone>(
598 state,
599
4/8
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 50 times.
✗ Branch 14 not taken.
 100 state->get_pinocchio()->getFrameId("right_sole_link"),
600 wrench_cone, nu)),
601 0.01);
602 // force regularization
603
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
604 "lf_forceReg",
605
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
606 state,
607 50 std::make_shared<crocoddyl::ResidualModelContactForce>(
608
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state, state->get_pinocchio()->getFrameId("left_sole_link"),
609 50 force, 6, nu)),
610 0.01);
611
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
612 "rf_forceReg",
613
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
614 state,
615 50 std::make_shared<crocoddyl::ResidualModelContactForce>(
616 state,
617
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state->get_pinocchio()->getFrameId("right_sole_link"),
618 100 force, 6, nu)),
619 0.01);
620 }
621 130 break;
622 ✗ default:
623 ✗ throw_pretty(__FILE__ ": Wrong StateModelTypes::Type given");
624 break;
625 }
626
2/4
✓ Branch 3 taken 448 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 448 times.
✗ Branch 7 not taken.
 896 cost->addCost("state",
627
2/4
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 448 times.
✗ Branch 5 not taken.
 896 CostModelFactory().create(
628 CostModelTypes::CostModelResidualState, state_type,
629 ActivationModelTypes::ActivationModelQuad, nu),
630 0.1);
631
2/4
✓ Branch 3 taken 448 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 448 times.
✗ Branch 7 not taken.
 896 cost->addCost("control",
632
2/4
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 448 times.
✗ Branch 5 not taken.
 896 CostModelFactory().create(
633 CostModelTypes::CostModelResidualControl, state_type,
634 ActivationModelTypes::ActivationModelQuad, nu),
635 0.1);
636
2/4
✓ Branch 4 taken 448 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 448 times.
✗ Branch 8 not taken.
 896 cost->addCost(
637 "joint_eff",
638
1/2
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
 896 std::make_shared<crocoddyl::CostModelResidual>(
639
1/2
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
 896 state, std::make_shared<crocoddyl::ResidualModelJointEffort>(
640
1/2
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
 448 state, actuation, Eigen::VectorXd::Zero(nu), nu, true)),
641 0.1);
642 action =
643 448 std::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>(
644
1/2
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
 448 state, actuation, contact, cost, 0., true);
645 896 return action;
646 448 }
647
648 std::shared_ptr<crocoddyl::DifferentialActionModelContactInvDynamics>
649 300 DifferentialActionModelFactory::create_contactInvDynamics(
650 StateModelTypes::Type state_type, ActuationModelTypes::Type actuation_type,
651 bool with_friction, bool with_baumgarte) const {
652 300 std::shared_ptr<crocoddyl::DifferentialActionModelContactInvDynamics> action;
653 300 std::shared_ptr<crocoddyl::StateMultibody> state;
654 300 std::shared_ptr<crocoddyl::ActuationModelAbstract> actuation;
655 300 std::shared_ptr<crocoddyl::ContactModelMultiple> contact;
656 300 std::shared_ptr<crocoddyl::CostModelSum> cost;
657 600 state = std::static_pointer_cast<crocoddyl::StateMultibody>(
658
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 900 StateModelFactory().create(state_type));
659
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 actuation = ActuationModelFactory().create(actuation_type, state_type);
660
1/2
✓ Branch 2 taken 300 times.
✗ Branch 3 not taken.
 300 std::size_t nu = state->get_nv();
661
662
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
663
1/2
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
 300 pinocchio::Force force = pinocchio::Force::Zero();
664
1/2
✓ Branch 2 taken 300 times.
✗ Branch 3 not taken.
 300 crocoddyl::FrictionCone friction_cone(R, 0.8, 4, false);
665
1/2
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
 300 crocoddyl::WrenchCone wrench_cone(R, 0.8, Eigen::Vector2d(0.1, 0.1), 4,
666
1/2
✓ Branch 2 taken 300 times.
✗ Branch 3 not taken.
 600 false);
667 600 crocoddyl::ActivationBounds friction_bounds(friction_cone.get_lb(),
668
3/6
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
 300 friction_cone.get_ub());
669 600 crocoddyl::ActivationBounds wrench_bounds(wrench_cone.get_lb(),
670
3/6
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
 300 wrench_cone.get_ub());
671 std::shared_ptr<crocoddyl::ActivationModelAbstract> friction_activation =
672
1/2
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
 300 std::make_shared<crocoddyl::ActivationModelQuadraticBarrier>(
673 300 friction_bounds);
674 std::shared_ptr<crocoddyl::ActivationModelAbstract> wrench_activation =
675
1/2
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
 300 std::make_shared<crocoddyl::ActivationModelQuadraticBarrier>(
676 300 wrench_bounds);
677
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 Eigen::Vector2d gains = Eigen::Vector2d::Random();
678
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 294 times.
 300 if (!with_baumgarte) {
679
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 gains.setZero();
680 }
681
682
3/4
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 100 times.
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
 300 switch (state_type) {
683 100 case StateModelTypes::StateMultibody_TalosArm:
684 100 nu += 3;
685
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 contact = std::make_shared<crocoddyl::ContactModelMultiple>(state, nu);
686
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 cost = std::make_shared<crocoddyl::CostModelSum>(state, nu);
687
6/12
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 100 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 100 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 100 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 100 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 100 times.
✗ Branch 20 not taken.
 100 contact->addContact("lf", ContactModelFactory().create(
688 ContactModelTypes::ContactModel3D_LOCAL,
689 PinocchioModelTypes::TalosArm, gains,
690 "gripper_left_fingertip_1_link", nu));
691
2/2
✓ Branch 0 taken 50 times.
✓ Branch 1 taken 50 times.
 100 if (with_friction) {
692 // friction cone
693
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
694 "lf_cone",
695
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
696 state, friction_activation,
697 50 std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
698 state,
699
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state->get_pinocchio()->getFrameId(
700 "gripper_left_fingertip_1_link"),
701 50 friction_cone, nu, false)),
702 0.1);
703 // force regularization
704
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
705 "lf_forceReg",
706
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
707 50 state, std::make_shared<crocoddyl::ResidualModelContactForce>(
708 state,
709
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state->get_pinocchio()->getFrameId(
710 "gripper_left_fingertip_1_link"),
711 100 force, 3, nu, false)),
712 0.1);
713 }
714 100 break;
715 100 case StateModelTypes::StateMultibody_HyQ:
716 100 nu += 12; // it includes nc
717
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 contact = std::make_shared<crocoddyl::ContactModelMultiple>(state, nu);
718
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 cost = std::make_shared<crocoddyl::CostModelSum>(state, nu);
719
2/4
✓ Branch 3 taken 100 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 100 times.
✗ Branch 7 not taken.
 200 contact->addContact("lf",
720
4/8
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
 200 ContactModelFactory().create(
721 ContactModelTypes::ContactModel3D_LOCAL,
722 PinocchioModelTypes::HyQ, gains, "lf_foot", nu));
723
2/4
✓ Branch 3 taken 100 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 100 times.
✗ Branch 7 not taken.
 200 contact->addContact("rf",
724
4/8
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
 200 ContactModelFactory().create(
725 ContactModelTypes::ContactModel3D_WORLD,
726 PinocchioModelTypes::HyQ, gains, "rf_foot", nu));
727
2/4
✓ Branch 3 taken 100 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 100 times.
✗ Branch 7 not taken.
 200 contact->addContact("lh",
728
4/8
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
 200 ContactModelFactory().create(
729 ContactModelTypes::ContactModel3D_LWA,
730 PinocchioModelTypes::HyQ, gains, "lh_foot", nu));
731
2/4
✓ Branch 3 taken 100 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 100 times.
✗ Branch 7 not taken.
 200 contact->addContact("rh",
732
4/8
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
 200 ContactModelFactory().create(
733 ContactModelTypes::ContactModel3D_LOCAL,
734 PinocchioModelTypes::HyQ, gains, "rh_foot", nu));
735
2/2
✓ Branch 0 taken 50 times.
✓ Branch 1 taken 50 times.
 100 if (with_friction) {
736 // friction cone
737
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
738 "lf_cone",
739
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
740 state, friction_activation,
741 50 std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
742
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state, state->get_pinocchio()->getFrameId("lf_foot"),
743 50 friction_cone, nu, false)),
744 0.1);
745
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
746 "rf_cone",
747
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
748 state, friction_activation,
749 50 std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
750
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state, state->get_pinocchio()->getFrameId("rf_foot"),
751 50 friction_cone, nu, false)),
752 0.1);
753
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
754 "lh_cone",
755
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
756 state, friction_activation,
757 50 std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
758
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state, state->get_pinocchio()->getFrameId("lh_foot"),
759 50 friction_cone, nu, false)),
760 0.1);
761
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
762 "rh_cone",
763
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
764 state, friction_activation,
765 50 std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
766
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state, state->get_pinocchio()->getFrameId("rh_foot"),
767 50 friction_cone, nu, false)),
768 0.1);
769 // force regularization
770
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
771 "lf_forceReg",
772
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
773 50 state, std::make_shared<crocoddyl::ResidualModelContactForce>(
774
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state, state->get_pinocchio()->getFrameId("lf_foot"),
775 50 force, 3, nu, false)),
776 0.1);
777
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
778 "rf_forceReg",
779
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
780 50 state, std::make_shared<crocoddyl::ResidualModelContactForce>(
781
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state, state->get_pinocchio()->getFrameId("rf_foot"),
782 50 force, 3, nu, false)),
783 0.1);
784
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
785 "lh_forceReg",
786
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
787 50 state, std::make_shared<crocoddyl::ResidualModelContactForce>(
788
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state, state->get_pinocchio()->getFrameId("lh_foot"),
789 50 force, 3, nu, false)),
790 0.1);
791
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
792 "rh_forceReg",
793
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
794 50 state, std::make_shared<crocoddyl::ResidualModelContactForce>(
795
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state, state->get_pinocchio()->getFrameId("rh_foot"),
796 100 force, 3, nu, false)),
797 0.1);
798 }
799 100 break;
800 100 case StateModelTypes::StateMultibody_Talos:
801 100 nu += 12; // it includes nc
802
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 contact = std::make_shared<crocoddyl::ContactModelMultiple>(state, nu);
803
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 cost = std::make_shared<crocoddyl::CostModelSum>(state, nu);
804
2/4
✓ Branch 3 taken 100 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 100 times.
✗ Branch 7 not taken.
 200 contact->addContact(
805
4/8
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
 200 "lf", ContactModelFactory().create(
806 ContactModelTypes::ContactModel6D_LOCAL,
807 PinocchioModelTypes::Talos, gains, "left_sole_link", nu));
808
2/4
✓ Branch 3 taken 100 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 100 times.
✗ Branch 7 not taken.
 200 contact->addContact(
809
4/8
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
 200 "rf", ContactModelFactory().create(
810 ContactModelTypes::ContactModel6D_WORLD,
811 PinocchioModelTypes::Talos, gains, "right_sole_link", nu));
812
2/2
✓ Branch 0 taken 50 times.
✓ Branch 1 taken 50 times.
 100 if (with_friction) {
813 // friction / wrench cone
814
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
815 "lf_cone",
816
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
817 state, friction_activation,
818 50 std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
819
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state, state->get_pinocchio()->getFrameId("left_sole_link"),
820 50 friction_cone, nu, false)),
821 0.01);
822
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
823 "rf_cone",
824
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
825 state, wrench_activation,
826 50 std::make_shared<crocoddyl::ResidualModelContactWrenchCone>(
827 state,
828
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state->get_pinocchio()->getFrameId("right_sole_link"),
829 50 wrench_cone, nu, false)),
830 0.01);
831 // force regularization
832
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
833 "lf_forceReg",
834
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
835 state,
836 50 std::make_shared<crocoddyl::ResidualModelContactForce>(
837
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state, state->get_pinocchio()->getFrameId("left_sole_link"),
838 50 force, 6, nu, false)),
839 0.01);
840
2/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 100 cost->addCost(
841 "rf_forceReg",
842
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 100 std::make_shared<crocoddyl::CostModelResidual>(
843 state,
844 50 std::make_shared<crocoddyl::ResidualModelContactForce>(
845 state,
846
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 100 state->get_pinocchio()->getFrameId("right_sole_link"),
847 100 force, 6, nu, false)),
848 0.01);
849 }
850 100 break;
851 ✗ default:
852 ✗ throw_pretty(__FILE__ ": Wrong StateModelTypes::Type given");
853 break;
854 }
855
2/4
✓ Branch 3 taken 300 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 300 times.
✗ Branch 7 not taken.
 600 cost->addCost("state",
856
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 600 CostModelFactory().create(
857 CostModelTypes::CostModelResidualState, state_type,
858 ActivationModelTypes::ActivationModelQuad, nu),
859 0.1);
860
2/4
✓ Branch 3 taken 300 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 300 times.
✗ Branch 7 not taken.
 600 cost->addCost("control",
861
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 600 CostModelFactory().create(
862 CostModelTypes::CostModelResidualControl, state_type,
863 ActivationModelTypes::ActivationModelQuad, nu),
864 0.1);
865 action =
866
1/2
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
 600 std::make_shared<crocoddyl::DifferentialActionModelContactInvDynamics>(
867 300 state, actuation, contact, cost);
868 600 return action;
869 300 }
870
871 } // namespace unittest
872 } // namespace crocoddyl
873