GCC Code Coverage Report

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