GCC Code Coverage Report

Directory: ./
File: unittest/factory/cost.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 109 124 87.9%
Branches: 65 132 49.2%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include "cost.hpp"
11
12 #include "crocoddyl/core/costs/residual.hpp"
13 #include "crocoddyl/core/residuals/control.hpp"
14 #include "crocoddyl/multibody/residuals/com-position.hpp"
15 #include "crocoddyl/multibody/residuals/control-gravity.hpp"
16 #include "crocoddyl/multibody/residuals/state.hpp"
17 // #include "crocoddyl/multibody/residuals/centroidal-momentum.hpp"
18 #include "crocoddyl/core/activations/quadratic.hpp"
19 #include "crocoddyl/core/costs/cost-sum.hpp"
20 #include "crocoddyl/multibody/residuals/contact-friction-cone.hpp"
21 #include "crocoddyl/multibody/residuals/contact-wrench-cone.hpp"
22 #include "crocoddyl/multibody/residuals/frame-placement.hpp"
23 #include "crocoddyl/multibody/residuals/frame-rotation.hpp"
24 #include "crocoddyl/multibody/residuals/frame-translation.hpp"
25 #include "crocoddyl/multibody/residuals/frame-velocity.hpp"
26 #ifdef CROCODDYL_WITH_PAIR_COLLISION
27 #include "crocoddyl/multibody/residuals/pair-collision.hpp"
28 #endif // CROCODDYL_WITH_PAIR_COLLISION
29
30 namespace crocoddyl {
31 namespace unittest {
32
33 const std::vector<CostModelTypes::Type> CostModelTypes::all(
34 CostModelTypes::init_all());
35 const std::vector<CostModelNoFFTypes::Type> CostModelNoFFTypes::all(
36 CostModelNoFFTypes::init_all());
37 #ifdef PINOCCHIO_WITH_HPP_FCL
38 #ifdef CROCODDYL_WITH_PAIR_COLLISION
39 const std::vector<CostModelCollisionTypes::Type> CostModelCollisionTypes::all(
40 CostModelCollisionTypes::init_all());
41 #endif // CROCODDYL_WITH_PAIR_COLLISION
42 #endif // PINOCCHIO_WITH_HPP_FCL
43
44 315 std::ostream& operator<<(std::ostream& os, CostModelTypes::Type type) {
45
7/9
✓ Branch 0 taken 45 times.
✓ Branch 1 taken 45 times.
✓ Branch 2 taken 45 times.
✓ Branch 3 taken 45 times.
✓ Branch 4 taken 45 times.
✓ Branch 5 taken 45 times.
✓ Branch 6 taken 45 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 315 switch (type) {
46 45 case CostModelTypes::CostModelResidualState:
47 45 os << "CostModelResidualState";
48 45 break;
49 45 case CostModelTypes::CostModelResidualControl:
50 45 os << "CostModelResidualControl";
51 45 break;
52 45 case CostModelTypes::CostModelResidualCoMPosition:
53 45 os << "CostModelResidualCoMPosition";
54 45 break;
55 // case CostModelTypes::CostModelResidualCentroidalMomentum:
56 // os << "CostModelResidualCentroidalMomentum";
57 // break;
58 45 case CostModelTypes::CostModelResidualFramePlacement:
59 45 os << "CostModelResidualFramePlacement";
60 45 break;
61 45 case CostModelTypes::CostModelResidualFrameRotation:
62 45 os << "CostModelResidualFrameRotation";
63 45 break;
64 45 case CostModelTypes::CostModelResidualFrameTranslation:
65 45 os << "CostModelResidualFrameTranslation";
66 45 break;
67 45 case CostModelTypes::CostModelResidualFrameVelocity:
68 45 os << "CostModelResidualFrameVelocity";
69 45 break;
70 ✗ case CostModelTypes::NbCostModelTypes:
71 ✗ os << "NbCostModelTypes";
72 ✗ break;
73 ✗ default:
74 ✗ break;
75 }
76 315 return os;
77 }
78
79 9 std::ostream& operator<<(std::ostream& os, CostModelNoFFTypes::Type type) {
80
1/3
✓ Branch 0 taken 9 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 9 switch (type) {
81 9 case CostModelNoFFTypes::CostModelResidualControlGrav:
82 9 os << "CostModelResidualControlGrav";
83 9 break;
84 ✗ case CostModelNoFFTypes::NbCostModelNoFFTypes:
85 ✗ os << "NbCostModelNoFFTypes";
86 ✗ break;
87 ✗ default:
88 ✗ break;
89 }
90 9 return os;
91 }
92
93 #ifdef PINOCCHIO_WITH_HPP_FCL
94 #ifdef CROCODDYL_WITH_PAIR_COLLISION
95 std::ostream& operator<<(std::ostream& os, CostModelCollisionTypes::Type type) {
96 switch (type) {
97 case CostModelCollisionTypes::CostModelResidualPairCollision:
98 os << "CostModelResidualPairCollision";
99 break;
100 case CostModelCollisionTypes::NbCostModelCollisionTypes:
101 os << "NbCostModelCollisionTypes";
102 break;
103 default:
104 break;
105 }
106 return os;
107 }
108 #endif // CROCODDYL_WITH_PAIR_COLLISION
109 #endif // PINOCCHIO_WITH_HPP_FCL
110
111 4191 CostModelFactory::CostModelFactory() {}
112 4191 CostModelFactory::~CostModelFactory() {}
113
114 4146 std::shared_ptr<crocoddyl::CostModelAbstract> CostModelFactory::create(
115 CostModelTypes::Type cost_type, StateModelTypes::Type state_type,
116 ActivationModelTypes::Type activation_type, std::size_t nu) const {
117
1/2
✓ Branch 1 taken 4146 times.
✗ Branch 2 not taken.
 4146 StateModelFactory state_factory;
118
1/2
✓ Branch 1 taken 4146 times.
✗ Branch 2 not taken.
 4146 ActivationModelFactory activation_factory;
119 4146 std::shared_ptr<crocoddyl::CostModelAbstract> cost;
120 std::shared_ptr<crocoddyl::StateMultibody> state =
121 std::static_pointer_cast<crocoddyl::StateMultibody>(
122
1/2
✓ Branch 1 taken 4146 times.
✗ Branch 2 not taken.
 4146 state_factory.create(state_type));
123
124
1/2
✓ Branch 2 taken 4146 times.
✗ Branch 3 not taken.
 4146 pinocchio::FrameIndex frame_index = state->get_pinocchio()->frames.size() - 1;
125
1/2
✓ Branch 1 taken 4146 times.
✗ Branch 2 not taken.
 4146 pinocchio::SE3 frame_SE3 = pinocchio::SE3::Random();
126
127
2/2
✓ Branch 1 taken 1647 times.
✓ Branch 2 taken 2499 times.
 4146 if (nu == std::numeric_limits<std::size_t>::max()) {
128
1/2
✓ Branch 2 taken 1647 times.
✗ Branch 3 not taken.
 1647 nu = state->get_nv();
129 }
130
7/8
✓ Branch 0 taken 1371 times.
✓ Branch 1 taken 1295 times.
✓ Branch 2 taken 239 times.
✓ Branch 3 taken 533 times.
✓ Branch 4 taken 236 times.
✓ Branch 5 taken 235 times.
✓ Branch 6 taken 237 times.
✗ Branch 7 not taken.
 4146 switch (cost_type) {
131 1371 case CostModelTypes::CostModelResidualState:
132
1/2
✓ Branch 1 taken 1371 times.
✗ Branch 2 not taken.
 2742 cost = std::make_shared<crocoddyl::CostModelResidual>(
133
2/4
✓ Branch 2 taken 1371 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1371 times.
✗ Branch 6 not taken.
 2742 state, activation_factory.create(activation_type, state->get_ndx()),
134
2/4
✓ Branch 2 taken 1371 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1371 times.
✗ Branch 6 not taken.
 2742 std::make_shared<crocoddyl::ResidualModelState>(state, state->zero(),
135 1371 nu));
136 1371 break;
137 1295 case CostModelTypes::CostModelResidualControl:
138
1/2
✓ Branch 1 taken 1295 times.
✗ Branch 2 not taken.
 2590 cost = std::make_shared<crocoddyl::CostModelResidual>(
139
1/2
✓ Branch 1 taken 1295 times.
✗ Branch 2 not taken.
 2590 state, activation_factory.create(activation_type, nu),
140
1/2
✓ Branch 1 taken 1295 times.
✗ Branch 2 not taken.
 2590 std::make_shared<crocoddyl::ResidualModelControl>(
141
1/2
✓ Branch 1 taken 1295 times.
✗ Branch 2 not taken.
 2590 state, Eigen::VectorXd::Random(nu)));
142 1295 break;
143 239 case CostModelTypes::CostModelResidualCoMPosition:
144
1/2
✓ Branch 1 taken 239 times.
✗ Branch 2 not taken.
 478 cost = std::make_shared<crocoddyl::CostModelResidual>(
145
1/2
✓ Branch 1 taken 239 times.
✗ Branch 2 not taken.
 478 state, activation_factory.create(activation_type, 3),
146
1/2
✓ Branch 1 taken 239 times.
✗ Branch 2 not taken.
 478 std::make_shared<crocoddyl::ResidualModelCoMPosition>(
147
1/2
✓ Branch 1 taken 239 times.
✗ Branch 2 not taken.
 478 state, Eigen::Vector3d::Random(), nu));
148 239 break;
149 // case CostModelTypes::CostModelResidualCentroidalMomentum:
150 // cost = std::make_shared<crocoddyl::CostModelResidual>(
151 // state,
152 // std::make_shared<crocoddyl::ResidualModelCentroidalMomentum>(state,
153 // Vector6d::Random(), nu), activation_factory.create(activation_type,
154 // 6));
155 // break;
156 533 case CostModelTypes::CostModelResidualFramePlacement:
157
1/2
✓ Branch 1 taken 533 times.
✗ Branch 2 not taken.
 1066 cost = std::make_shared<crocoddyl::CostModelResidual>(
158
1/2
✓ Branch 1 taken 533 times.
✗ Branch 2 not taken.
 1066 state, activation_factory.create(activation_type, 6),
159
1/2
✓ Branch 1 taken 533 times.
✗ Branch 2 not taken.
 1066 std::make_shared<crocoddyl::ResidualModelFramePlacement>(
160 533 state, frame_index, frame_SE3, nu));
161 533 break;
162 236 case CostModelTypes::CostModelResidualFrameRotation:
163
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 472 cost = std::make_shared<crocoddyl::CostModelResidual>(
164
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 472 state, activation_factory.create(activation_type, 3),
165 236 std::make_shared<crocoddyl::ResidualModelFrameRotation>(
166
2/4
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 236 times.
✗ Branch 5 not taken.
 472 state, frame_index, frame_SE3.rotation(), nu));
167 236 break;
168 235 case CostModelTypes::CostModelResidualFrameTranslation:
169
1/2
✓ Branch 1 taken 235 times.
✗ Branch 2 not taken.
 470 cost = std::make_shared<crocoddyl::CostModelResidual>(
170
1/2
✓ Branch 1 taken 235 times.
✗ Branch 2 not taken.
 470 state, activation_factory.create(activation_type, 3),
171 235 std::make_shared<crocoddyl::ResidualModelFrameTranslation>(
172
2/4
✓ Branch 1 taken 235 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 235 times.
✗ Branch 5 not taken.
 470 state, frame_index, frame_SE3.translation(), nu));
173 235 break;
174 237 case CostModelTypes::CostModelResidualFrameVelocity:
175
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 474 cost = std::make_shared<crocoddyl::CostModelResidual>(
176
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 474 state, activation_factory.create(activation_type, 6),
177
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 474 std::make_shared<crocoddyl::ResidualModelFrameVelocity>(
178 ✗ state, frame_index, pinocchio::Motion::Random(),
179
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 474 pinocchio::ReferenceFrame::LOCAL, nu));
180 237 break;
181 ✗ default:
182 ✗ throw_pretty(__FILE__ ": Wrong CostModelTypes::Type given");
183 break;
184 }
185 8292 return cost;
186 4146 }
187
188 45 std::shared_ptr<crocoddyl::CostModelAbstract> CostModelFactory::create(
189 CostModelNoFFTypes::Type cost_type,
190 ActivationModelTypes::Type activation_type, std::size_t nu) const {
191
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 StateModelFactory state_factory;
192
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 ActivationModelFactory activation_factory;
193 45 std::shared_ptr<crocoddyl::CostModelAbstract> cost;
194 std::shared_ptr<crocoddyl::StateMultibody> state =
195 std::static_pointer_cast<crocoddyl::StateMultibody>(
196
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 state_factory.create(StateModelTypes::StateMultibody_TalosArm));
197
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 if (nu == std::numeric_limits<std::size_t>::max()) {
198
1/2
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
 45 nu = state->get_nv();
199 }
200
201
1/2
✓ Branch 0 taken 45 times.
✗ Branch 1 not taken.
 45 switch (cost_type) {
202 45 case CostModelNoFFTypes::CostModelResidualControlGrav:
203
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 90 cost = std::make_shared<crocoddyl::CostModelResidual>(
204
2/4
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
 90 state, activation_factory.create(activation_type, state->get_nv()),
205
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 135 std::make_shared<ResidualModelControlGrav>(state, nu));
206 45 break;
207 ✗ default:
208 ✗ throw_pretty(__FILE__ ": Wrong CostModelTypes::Type given");
209 break;
210 }
211 90 return cost;
212 45 }
213
214 #ifdef PINOCCHIO_WITH_HPP_FCL
215 #ifdef CROCODDYL_WITH_PAIR_COLLISION
216 std::shared_ptr<crocoddyl::CostModelAbstract> CostModelFactory::create(
217 CostModelCollisionTypes::Type cost_type, StateModelTypes::Type state_type,
218 std::size_t nu) const {
219 StateModelFactory state_factory;
220 std::shared_ptr<crocoddyl::CostModelAbstract> cost;
221 std::shared_ptr<crocoddyl::StateMultibody> state =
222 std::static_pointer_cast<crocoddyl::StateMultibody>(
223 state_factory.create(state_type));
224 if (nu == std::numeric_limits<std::size_t>::max()) {
225 nu = state->get_nv();
226 }
227 pinocchio::FrameIndex frame_index = state->get_pinocchio()->frames.size() - 1;
228 pinocchio::SE3 frame_SE3 = pinocchio::SE3::Random();
229 pinocchio::SE3 frame_SE3_obstacle = pinocchio::SE3::Random();
230 double alpha = fabs(Eigen::VectorXd::Random(1)[0]);
231 double beta = fabs(Eigen::VectorXd::Random(1)[0]);
232
233 std::shared_ptr<pinocchio::GeometryModel> geometry =
234 std::make_shared<pinocchio::GeometryModel>(pinocchio::GeometryModel());
235 #if PINOCCHIO_VERSION_AT_LEAST(3, 0, 0)
236 pinocchio::GeomIndex ig_frame =
237 geometry->addGeometryObject(pinocchio::GeometryObject(
238 "frame", frame_index,
239 state->get_pinocchio()->frames[frame_index].parentJoint,
240 CollisionGeometryPtr(new hpp::fcl::Capsule(0, alpha)), frame_SE3));
241 pinocchio::GeomIndex ig_obs =
242 geometry->addGeometryObject(pinocchio::GeometryObject(
243 "obs", state->get_pinocchio()->getFrameId("universe"),
244 state->get_pinocchio()
245 ->frames[state->get_pinocchio()->getFrameId("universe")]
246 .parentJoint,
247 CollisionGeometryPtr(new hpp::fcl::Capsule(0, beta)),
248 frame_SE3_obstacle));
249 #else
250 pinocchio::GeomIndex ig_frame =
251 geometry->addGeometryObject(pinocchio::GeometryObject(
252 "frame", frame_index,
253 state->get_pinocchio()->frames[frame_index].parent,
254 CollisionGeometryPtr(new hpp::fcl::Capsule(0, alpha)), frame_SE3));
255 pinocchio::GeomIndex ig_obs =
256 geometry->addGeometryObject(pinocchio::GeometryObject(
257 "obs", state->get_pinocchio()->getFrameId("universe"),
258 state->get_pinocchio()
259 ->frames[state->get_pinocchio()->getFrameId("universe")]
260 .parent,
261 CollisionGeometryPtr(new hpp::fcl::Capsule(0, beta)),
262 frame_SE3_obstacle));
263 #endif
264 geometry->addCollisionPair(pinocchio::CollisionPair(ig_frame, ig_obs));
265
266 switch (cost_type) {
267 case CostModelCollisionTypes::CostModelResidualPairCollision:
268 #if PINOCCHIO_VERSION_AT_LEAST(3, 0, 0)
269 cost = std::make_shared<crocoddyl::CostModelResidual>(
270 state, std::make_shared<crocoddyl::ActivationModelQuad>(3),
271 std::make_shared<crocoddyl::ResidualModelPairCollision>(
272 state, nu, geometry, 0,
273 state->get_pinocchio()->frames[frame_index].parentJoint));
274 #else
275 cost = std::make_shared<crocoddyl::CostModelResidual>(
276 state, std::make_shared<crocoddyl::ActivationModelQuad>(3),
277 std::make_shared<crocoddyl::ResidualModelPairCollision>(
278 state, nu, geometry, 0,
279 state->get_pinocchio()->frames[frame_index].parent));
280 #endif
281 break;
282 default:
283 throw_pretty(__FILE__ ": Wrong CostModelTypes::Type given");
284 break;
285 }
286 return cost;
287 }
288 #endif // CROCODDYL_WITH_PAIR_COLLISION
289 #endif // PINOCCHIO_WITH_HPP_FCL
290
291 72 std::shared_ptr<crocoddyl::CostModelAbstract> create_random_cost(
292 StateModelTypes::Type state_type, std::size_t nu) {
293 static bool once = true;
294
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 71 times.
 72 if (once) {
295 1 srand((unsigned)time(NULL));
296 1 once = false;
297 }
298
299
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 CostModelFactory factory;
300 CostModelTypes::Type rand_type = static_cast<CostModelTypes::Type>(
301 72 rand() % CostModelTypes::NbCostModelTypes);
302 return factory.create(rand_type, state_type,
303
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 144 ActivationModelTypes::ActivationModelQuad, nu);
304 72 }
305
306 } // namespace unittest
307 } // namespace crocoddyl
308