GCC Code Coverage Report

Directory: ./
File: unittest/factory/cost.cpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 121 0.0%
Branches: 0 132 0.0%
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 ✗ std::ostream& operator<<(std::ostream& os, CostModelTypes::Type type) {
45 ✗ switch (type) {
46 ✗ case CostModelTypes::CostModelResidualState:
47 ✗ os << "CostModelResidualState";
48 ✗ break;
49 ✗ case CostModelTypes::CostModelResidualControl:
50 ✗ os << "CostModelResidualControl";
51 ✗ break;
52 ✗ case CostModelTypes::CostModelResidualCoMPosition:
53 ✗ os << "CostModelResidualCoMPosition";
54 ✗ break;
55 // case CostModelTypes::CostModelResidualCentroidalMomentum:
56 // os << "CostModelResidualCentroidalMomentum";
57 // break;
58 ✗ case CostModelTypes::CostModelResidualFramePlacement:
59 ✗ os << "CostModelResidualFramePlacement";
60 ✗ break;
61 ✗ case CostModelTypes::CostModelResidualFrameRotation:
62 ✗ os << "CostModelResidualFrameRotation";
63 ✗ break;
64 ✗ case CostModelTypes::CostModelResidualFrameTranslation:
65 ✗ os << "CostModelResidualFrameTranslation";
66 ✗ break;
67 ✗ case CostModelTypes::CostModelResidualFrameVelocity:
68 ✗ os << "CostModelResidualFrameVelocity";
69 ✗ break;
70 ✗ case CostModelTypes::NbCostModelTypes:
71 ✗ os << "NbCostModelTypes";
72 ✗ break;
73 ✗ default:
74 ✗ break;
75 }
76 ✗ return os;
77 }
78
79 ✗ std::ostream& operator<<(std::ostream& os, CostModelNoFFTypes::Type type) {
80 ✗ switch (type) {
81 ✗ case CostModelNoFFTypes::CostModelResidualControlGrav:
82 ✗ os << "CostModelResidualControlGrav";
83 ✗ break;
84 ✗ case CostModelNoFFTypes::NbCostModelNoFFTypes:
85 ✗ os << "NbCostModelNoFFTypes";
86 ✗ break;
87 ✗ default:
88 ✗ break;
89 }
90 ✗ 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 ✗ CostModelFactory::CostModelFactory() {}
112 ✗ CostModelFactory::~CostModelFactory() {}
113
114 ✗ 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 ✗ StateModelFactory state_factory;
118 ✗ ActivationModelFactory activation_factory;
119 ✗ std::shared_ptr<crocoddyl::CostModelAbstract> cost;
120 std::shared_ptr<crocoddyl::StateMultibody> state =
121 std::static_pointer_cast<crocoddyl::StateMultibody>(
122 ✗ state_factory.create(state_type));
123
124 ✗ pinocchio::FrameIndex frame_index = state->get_pinocchio()->frames.size() - 1;
125 ✗ pinocchio::SE3 frame_SE3 = pinocchio::SE3::Random();
126
127 ✗ if (nu == std::numeric_limits<std::size_t>::max()) {
128 ✗ nu = state->get_nv();
129 }
130 ✗ switch (cost_type) {
131 ✗ case CostModelTypes::CostModelResidualState:
132 ✗ cost = std::make_shared<crocoddyl::CostModelResidual>(
133 ✗ state, activation_factory.create(activation_type, state->get_ndx()),
134 ✗ std::make_shared<crocoddyl::ResidualModelState>(state, state->zero(),
135 ✗ nu));
136 ✗ break;
137 ✗ case CostModelTypes::CostModelResidualControl:
138 ✗ cost = std::make_shared<crocoddyl::CostModelResidual>(
139 ✗ state, activation_factory.create(activation_type, nu),
140 ✗ std::make_shared<crocoddyl::ResidualModelControl>(
141 ✗ state, Eigen::VectorXd::Random(nu)));
142 ✗ break;
143 ✗ case CostModelTypes::CostModelResidualCoMPosition:
144 ✗ cost = std::make_shared<crocoddyl::CostModelResidual>(
145 ✗ state, activation_factory.create(activation_type, 3),
146 ✗ std::make_shared<crocoddyl::ResidualModelCoMPosition>(
147 ✗ state, Eigen::Vector3d::Random(), nu));
148 ✗ 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 ✗ case CostModelTypes::CostModelResidualFramePlacement:
157 ✗ cost = std::make_shared<crocoddyl::CostModelResidual>(
158 ✗ state, activation_factory.create(activation_type, 6),
159 ✗ std::make_shared<crocoddyl::ResidualModelFramePlacement>(
160 ✗ state, frame_index, frame_SE3, nu));
161 ✗ break;
162 ✗ case CostModelTypes::CostModelResidualFrameRotation:
163 ✗ cost = std::make_shared<crocoddyl::CostModelResidual>(
164 ✗ state, activation_factory.create(activation_type, 3),
165 ✗ std::make_shared<crocoddyl::ResidualModelFrameRotation>(
166 ✗ state, frame_index, frame_SE3.rotation(), nu));
167 ✗ break;
168 ✗ case CostModelTypes::CostModelResidualFrameTranslation:
169 ✗ cost = std::make_shared<crocoddyl::CostModelResidual>(
170 ✗ state, activation_factory.create(activation_type, 3),
171 ✗ std::make_shared<crocoddyl::ResidualModelFrameTranslation>(
172 ✗ state, frame_index, frame_SE3.translation(), nu));
173 ✗ break;
174 ✗ case CostModelTypes::CostModelResidualFrameVelocity:
175 ✗ cost = std::make_shared<crocoddyl::CostModelResidual>(
176 ✗ state, activation_factory.create(activation_type, 6),
177 ✗ std::make_shared<crocoddyl::ResidualModelFrameVelocity>(
178 ✗ state, frame_index, pinocchio::Motion::Random(),
179 ✗ pinocchio::ReferenceFrame::LOCAL, nu));
180 ✗ break;
181 ✗ default:
182 ✗ throw_pretty(__FILE__ ": Wrong CostModelTypes::Type given");
183 break;
184 }
185 ✗ return cost;
186 }
187
188 ✗ std::shared_ptr<crocoddyl::CostModelAbstract> CostModelFactory::create(
189 CostModelNoFFTypes::Type cost_type,
190 ActivationModelTypes::Type activation_type, std::size_t nu) const {
191 ✗ StateModelFactory state_factory;
192 ✗ ActivationModelFactory activation_factory;
193 ✗ std::shared_ptr<crocoddyl::CostModelAbstract> cost;
194 std::shared_ptr<crocoddyl::StateMultibody> state =
195 std::static_pointer_cast<crocoddyl::StateMultibody>(
196 ✗ state_factory.create(StateModelTypes::StateMultibody_TalosArm));
197 ✗ if (nu == std::numeric_limits<std::size_t>::max()) {
198 ✗ nu = state->get_nv();
199 }
200
201 ✗ switch (cost_type) {
202 ✗ case CostModelNoFFTypes::CostModelResidualControlGrav:
203 ✗ cost = std::make_shared<crocoddyl::CostModelResidual>(
204 ✗ state, activation_factory.create(activation_type, state->get_nv()),
205 ✗ std::make_shared<ResidualModelControlGrav>(state, nu));
206 ✗ break;
207 ✗ default:
208 ✗ throw_pretty(__FILE__ ": Wrong CostModelTypes::Type given");
209 break;
210 }
211 ✗ return cost;
212 }
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 pinocchio::GeomIndex ig_frame =
236 geometry->addGeometryObject(pinocchio::GeometryObject(
237 "frame", frame_index,
238 state->get_pinocchio()->frames[frame_index].parentJoint,
239 CollisionGeometryPtr(new hpp::fcl::Capsule(0, alpha)), frame_SE3));
240 pinocchio::GeomIndex ig_obs =
241 geometry->addGeometryObject(pinocchio::GeometryObject(
242 "obs", state->get_pinocchio()->getFrameId("universe"),
243 state->get_pinocchio()
244 ->frames[state->get_pinocchio()->getFrameId("universe")]
245 .parentJoint,
246 CollisionGeometryPtr(new hpp::fcl::Capsule(0, beta)),
247 frame_SE3_obstacle));
248 geometry->addCollisionPair(pinocchio::CollisionPair(ig_frame, ig_obs));
249
250 switch (cost_type) {
251 case CostModelCollisionTypes::CostModelResidualPairCollision:
252 cost = std::make_shared<crocoddyl::CostModelResidual>(
253 state, std::make_shared<crocoddyl::ActivationModelQuad>(3),
254 std::make_shared<crocoddyl::ResidualModelPairCollision>(
255 state, nu, geometry, 0,
256 state->get_pinocchio()->frames[frame_index].parentJoint));
257 break;
258 default:
259 throw_pretty(__FILE__ ": Wrong CostModelTypes::Type given");
260 break;
261 }
262 return cost;
263 }
264 #endif // CROCODDYL_WITH_PAIR_COLLISION
265 #endif // PINOCCHIO_WITH_HPP_FCL
266
267 ✗ std::shared_ptr<crocoddyl::CostModelAbstract> create_random_cost(
268 StateModelTypes::Type state_type, std::size_t nu) {
269 static bool once = true;
270 ✗ if (once) {
271 ✗ srand((unsigned)time(NULL));
272 ✗ once = false;
273 }
274
275 ✗ CostModelFactory factory;
276 CostModelTypes::Type rand_type = static_cast<CostModelTypes::Type>(
277 ✗ rand() % CostModelTypes::NbCostModelTypes);
278 return factory.create(rand_type, state_type,
279 ✗ ActivationModelTypes::ActivationModelQuad, nu);
280 }
281
282 } // namespace unittest
283 } // namespace crocoddyl
284