GCC Code Coverage Report

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