GCC Code Coverage Report

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