GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2024, University of Edinburgh, LAAS-CNRS,
5			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#include "action.hpp"
11
12			#include "cost.hpp"
13			#include "crocoddyl/core/actions/lqr.hpp"
14			#include "crocoddyl/core/actions/unicycle.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/multibody/impulses/impulse-3d.hpp"
19			#include "crocoddyl/multibody/impulses/impulse-6d.hpp"
20			#include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
21			#include "crocoddyl/multibody/residuals/contact-force.hpp"
22			#include "crocoddyl/multibody/residuals/contact-friction-cone.hpp"
23			#include "crocoddyl/multibody/residuals/contact-wrench-cone.hpp"
24			#include "impulse.hpp"
25
26			namespace crocoddyl {
27			namespace unittest {
28
29			const std::vector<ActionModelTypes::Type> ActionModelTypes::all(
30			ActionModelTypes::init_all());
31
32		✗	std::ostream& operator<<(std::ostream& os, ActionModelTypes::Type type) {
33		✗	switch (type) {
34		✗	case ActionModelTypes::ActionModelUnicycle:
35		✗	os << "ActionModelUnicycle";
36		✗	break;
37		✗	case ActionModelTypes::ActionModelLQRDriftFree:
38		✗	os << "ActionModelLQRDriftFree";
39		✗	break;
40		✗	case ActionModelTypes::ActionModelLQR:
41		✗	os << "ActionModelLQR";
42		✗	break;
43		✗	case ActionModelTypes::ActionModelRandomLQR:
44		✗	os << "ActionModelRandomLQR";
45		✗	break;
46		✗	case ActionModelTypes::ActionModelRandomLQRwithTerminalConstraint:
47		✗	os << "ActionModelRandomLQRwithTerminalConstraint";
48		✗	break;
49		✗	case ActionModelTypes::ActionModelImpulseFwdDynamics_HyQ:
50		✗	os << "ActionModelImpulseFwdDynamics_HyQ";
51		✗	break;
52		✗	case ActionModelTypes::ActionModelImpulseFwdDynamics_Talos:
53		✗	os << "ActionModelImpulseFwdDynamics_Talos";
54		✗	break;
55		✗	case ActionModelTypes::NbActionModelTypes:
56		✗	os << "NbActionModelTypes";
57		✗	break;
58		✗	default:
59		✗	break;
60			}
61		✗	return os;
62			}
63
64		✗	ActionModelFactory::ActionModelFactory() {}
65		✗	ActionModelFactory::~ActionModelFactory() {}
66
67		✗	std::shared_ptr<crocoddyl::ActionModelAbstract> ActionModelFactory::create(
68			ActionModelTypes::Type type, Instance instance) const {
69		✗	std::shared_ptr<crocoddyl::ActionModelAbstract> action;
70		✗	switch (type) {
71		✗	case ActionModelTypes::ActionModelUnicycle:
72		✗	action = std::make_shared<crocoddyl::ActionModelUnicycle>();
73		✗	break;
74		✗	case ActionModelTypes::ActionModelLQRDriftFree:
75		✗	switch (instance) {
76		✗	case First:
77		✗	action = std::make_shared<crocoddyl::ActionModelLQR>(8, 2, true);
78		✗	break;
79		✗	case Second:
80			case Terminal:
81		✗	action = std::make_shared<crocoddyl::ActionModelLQR>(8, 4, true);
82		✗	break;
83			}
84			case ActionModelTypes::ActionModelLQR:
85		✗	switch (instance) {
86		✗	case First:
87		✗	action = std::make_shared<crocoddyl::ActionModelLQR>(8, 2, false);
88		✗	break;
89		✗	case Second:
90			case Terminal:
91		✗	action = std::make_shared<crocoddyl::ActionModelLQR>(8, 4, false);
92		✗	break;
93			}
94		✗	break;
95		✗	case ActionModelTypes::ActionModelRandomLQR:
96		✗	switch (instance) {
97		✗	case First:
98		✗	action = std::make_shared<crocoddyl::ActionModelLQR>(
99		✗	crocoddyl::ActionModelLQR::Random(8, 2));
100		✗	break;
101		✗	case Second:
102			case Terminal:
103		✗	action = std::make_shared<crocoddyl::ActionModelLQR>(
104		✗	crocoddyl::ActionModelLQR::Random(8, 4));
105		✗	break;
106			}
107		✗	break;
108		✗	case ActionModelTypes::ActionModelRandomLQRwithTerminalConstraint:
109		✗	switch (instance) {
110		✗	case First:
111		✗	action = std::make_shared<crocoddyl::ActionModelLQR>(
112		✗	crocoddyl::ActionModelLQR::Random(8, 2));
113		✗	break;
114		✗	case Second:
115		✗	action = std::make_shared<crocoddyl::ActionModelLQR>(
116		✗	crocoddyl::ActionModelLQR::Random(8, 4));
117		✗	break;
118		✗	case Terminal:
119		✗	action = std::make_shared<crocoddyl::ActionModelLQR>(
120		✗	crocoddyl::ActionModelLQR::Random(8, 4, 0, 2));
121		✗	break;
122			}
123		✗	break;
124		✗	case ActionModelTypes::ActionModelImpulseFwdDynamics_HyQ:
125		✗	action = create_impulseFwdDynamics(StateModelTypes::StateMultibody_HyQ);
126		✗	break;
127		✗	case ActionModelTypes::ActionModelImpulseFwdDynamics_Talos:
128		✗	action = create_impulseFwdDynamics(StateModelTypes::StateMultibody_Talos);
129		✗	break;
130		✗	default:
131		✗	throw_pretty(__FILE__ ": Wrong ActionModelTypes::Type given");
132			break;
133			}
134		✗	return action;
135		✗	}
136
137			std::shared_ptr<crocoddyl::ActionModelImpulseFwdDynamics>
138		✗	ActionModelFactory::create_impulseFwdDynamics(
139			StateModelTypes::Type state_type) const {
140		✗	std::shared_ptr<crocoddyl::ActionModelImpulseFwdDynamics> action;
141		✗	std::shared_ptr<crocoddyl::StateMultibody> state;
142		✗	std::shared_ptr<crocoddyl::ImpulseModelMultiple> impulse;
143		✗	std::shared_ptr<crocoddyl::CostModelSum> cost;
144		✗	state = std::static_pointer_cast<crocoddyl::StateMultibody>(
145		✗	StateModelFactory().create(state_type));
146		✗	impulse = std::make_shared<crocoddyl::ImpulseModelMultiple>(state);
147		✗	cost = std::make_shared<crocoddyl::CostModelSum>(state, 0);
148		✗	double r_coeff = 0.; // TODO(cmastall): random_real_in_range(1e-16, 1e-2);
149		✗	double damping = 0.; // TODO(cmastall): random_real_in_range(1e-16, 1e-2);
150
151		✗	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
152		✗	pinocchio::Force force = pinocchio::Force::Zero();
153		✗	crocoddyl::FrictionCone friction_cone(R, 0.8, 4, false);
154		✗	crocoddyl::WrenchCone wrench_cone(R, 0.8, Eigen::Vector2d(0.1, 0.1), 4,
155		✗	false);
156		✗	crocoddyl::ActivationBounds friction_bounds(friction_cone.get_lb(),
157		✗	friction_cone.get_ub());
158		✗	crocoddyl::ActivationBounds wrench_bounds(wrench_cone.get_lb(),
159		✗	wrench_cone.get_ub());
160			std::shared_ptr<crocoddyl::ActivationModelAbstract> friction_activation =
161		✗	std::make_shared<crocoddyl::ActivationModelQuadraticBarrier>(
162		✗	friction_bounds);
163			std::shared_ptr<crocoddyl::ActivationModelAbstract> wrench_activation =
164		✗	std::make_shared<crocoddyl::ActivationModelQuadraticBarrier>(
165		✗	wrench_bounds);
166
167		✗	switch (state_type) {
168		✗	case StateModelTypes::StateMultibody_HyQ:
169		✗	impulse->addImpulse("lf", ImpulseModelFactory().create(
170			ImpulseModelTypes::ImpulseModel3D_LOCAL,
171			PinocchioModelTypes::HyQ, "lf_foot"));
172		✗	impulse->addImpulse("rf", ImpulseModelFactory().create(
173			ImpulseModelTypes::ImpulseModel3D_WORLD,
174			PinocchioModelTypes::HyQ, "rf_foot"));
175		✗	impulse->addImpulse("lh", ImpulseModelFactory().create(
176			ImpulseModelTypes::ImpulseModel3D_LWA,
177			PinocchioModelTypes::HyQ, "lh_foot"));
178		✗	impulse->addImpulse("rh", ImpulseModelFactory().create(
179			ImpulseModelTypes::ImpulseModel3D_LOCAL,
180			PinocchioModelTypes::HyQ, "rh_foot"));
181
182			// friction cone
183		✗	cost->addCost(
184			"lf_cone",
185		✗	std::make_shared<crocoddyl::CostModelResidual>(
186			state, friction_activation,
187		✗	std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
188		✗	state, state->get_pinocchio()->getFrameId("lf_foot"),
189		✗	friction_cone, 0)),
190			0.1);
191		✗	cost->addCost(
192			"rf_cone",
193		✗	std::make_shared<crocoddyl::CostModelResidual>(
194			state, friction_activation,
195		✗	std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
196		✗	state, state->get_pinocchio()->getFrameId("rf_foot"),
197		✗	friction_cone, 0)),
198			0.1);
199		✗	cost->addCost(
200			"lh_cone",
201		✗	std::make_shared<crocoddyl::CostModelResidual>(
202			state, friction_activation,
203		✗	std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
204		✗	state, state->get_pinocchio()->getFrameId("lh_foot"),
205		✗	friction_cone, 0)),
206			0.1);
207		✗	cost->addCost(
208			"rh_cone",
209		✗	std::make_shared<crocoddyl::CostModelResidual>(
210			state, friction_activation,
211		✗	std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
212		✗	state, state->get_pinocchio()->getFrameId("rh_foot"),
213		✗	friction_cone, 0)),
214			0.1);
215			// force regularization
216		✗	cost->addCost(
217			"lf_forceReg",
218		✗	std::make_shared<crocoddyl::CostModelResidual>(
219		✗	state, std::make_shared<crocoddyl::ResidualModelContactForce>(
220		✗	state, state->get_pinocchio()->getFrameId("lf_foot"),
221		✗	force, 3, 0)),
222			0.1);
223		✗	cost->addCost(
224			"rf_forceReg",
225		✗	std::make_shared<crocoddyl::CostModelResidual>(
226		✗	state, std::make_shared<crocoddyl::ResidualModelContactForce>(
227		✗	state, state->get_pinocchio()->getFrameId("rf_foot"),
228		✗	force, 3, 0)),
229			0.1);
230		✗	cost->addCost(
231			"lh_forceReg",
232		✗	std::make_shared<crocoddyl::CostModelResidual>(
233		✗	state, std::make_shared<crocoddyl::ResidualModelContactForce>(
234		✗	state, state->get_pinocchio()->getFrameId("lh_foot"),
235		✗	force, 3, 0)),
236			0.1);
237		✗	cost->addCost(
238			"rh_forceReg",
239		✗	std::make_shared<crocoddyl::CostModelResidual>(
240		✗	state, std::make_shared<crocoddyl::ResidualModelContactForce>(
241		✗	state, state->get_pinocchio()->getFrameId("rh_foot"),
242		✗	force, 3, 0)),
243			0.1);
244		✗	break;
245		✗	case StateModelTypes::StateMultibody_Talos:
246		✗	impulse->addImpulse("lf",
247		✗	ImpulseModelFactory().create(
248			ImpulseModelTypes::ImpulseModel6D_LOCAL,
249			PinocchioModelTypes::Talos, "left_sole_link"));
250		✗	impulse->addImpulse("rf",
251		✗	ImpulseModelFactory().create(
252			ImpulseModelTypes::ImpulseModel6D_WORLD,
253			PinocchioModelTypes::Talos, "right_sole_link"));
254
255			// friction / wrench cone
256		✗	cost->addCost(
257			"lf_cone",
258		✗	std::make_shared<crocoddyl::CostModelResidual>(
259			state, friction_activation,
260		✗	std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
261		✗	state, state->get_pinocchio()->getFrameId("left_sole_link"),
262		✗	friction_cone, 0)),
263			0.01);
264		✗	cost->addCost(
265			"rf_cone",
266		✗	std::make_shared<crocoddyl::CostModelResidual>(
267			state, wrench_activation,
268		✗	std::make_shared<crocoddyl::ResidualModelContactWrenchCone>(
269		✗	state, state->get_pinocchio()->getFrameId("right_sole_link"),
270		✗	wrench_cone, 0)),
271			0.01);
272			// force regularization
273		✗	cost->addCost(
274			"lf_forceReg",
275		✗	std::make_shared<crocoddyl::CostModelResidual>(
276			state,
277		✗	std::make_shared<crocoddyl::ResidualModelContactForce>(
278		✗	state, state->get_pinocchio()->getFrameId("left_sole_link"),
279		✗	force, 6, 0)),
280			0.01);
281		✗	cost->addCost(
282			"rf_forceReg",
283		✗	std::make_shared<crocoddyl::CostModelResidual>(
284			state,
285		✗	std::make_shared<crocoddyl::ResidualModelContactForce>(
286		✗	state, state->get_pinocchio()->getFrameId("right_sole_link"),
287		✗	force, 6, 0)),
288			0.01);
289		✗	break;
290		✗	default:
291		✗	throw_pretty(__FILE__ ": Wrong StateModelTypes::Type given");
292			break;
293			}
294		✗	cost->addCost("state",
295		✗	CostModelFactory().create(
296			CostModelTypes::CostModelResidualState, state_type,
297			ActivationModelTypes::ActivationModelQuad, 0),
298			0.1);
299		✗	action = std::make_shared<crocoddyl::ActionModelImpulseFwdDynamics>(
300		✗	state, impulse, cost, r_coeff, damping, true);
301		✗	return action;
302		✗	}
303
304			} // namespace unittest
305			} // namespace crocoddyl
306