GCC Code Coverage Report

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