GCC Code Coverage Report

Directory: ./
File: unittest/factory/action.cpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 192 0.0%
Branches: 0 357 0.0%
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