GCC Code Coverage Report

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