GCC Code Coverage Report

Directory: ./
File: unittest/factory/residual.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 78 94 83.0%
Branches: 39 79 49.4%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, 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 "residual.hpp"
11
12 #include "crocoddyl/core/residuals/control.hpp"
13 #include "crocoddyl/multibody/residuals/centroidal-momentum.hpp"
14 #include "crocoddyl/multibody/residuals/com-position.hpp"
15 #include "crocoddyl/multibody/residuals/control-gravity.hpp"
16 #include "crocoddyl/multibody/residuals/frame-placement.hpp"
17 #include "crocoddyl/multibody/residuals/frame-rotation.hpp"
18 #include "crocoddyl/multibody/residuals/frame-translation.hpp"
19 #include "crocoddyl/multibody/residuals/frame-velocity.hpp"
20 #ifdef CROCODDYL_WITH_PAIR_COLLISION
21 #include "crocoddyl/multibody/residuals/pair-collision.hpp"
22 #endif // CROCODDYL_WITH_PAIR_COLLISION
23 #include "crocoddyl/multibody/residuals/state.hpp"
24
25 namespace crocoddyl {
26 namespace unittest {
27
28 const std::vector<ResidualModelTypes::Type> ResidualModelTypes::all(
29 ResidualModelTypes::init_all());
30
31 162 std::ostream& operator<<(std::ostream& os, ResidualModelTypes::Type type) {
32
9/11
✓ Branch 0 taken 18 times.
✓ Branch 1 taken 18 times.
✓ Branch 2 taken 18 times.
✓ Branch 3 taken 18 times.
✓ Branch 4 taken 18 times.
✓ Branch 5 taken 18 times.
✓ Branch 6 taken 18 times.
✓ Branch 7 taken 18 times.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 162 switch (type) {
33 18 case ResidualModelTypes::ResidualModelState:
34 18 os << "ResidualModelState";
35 18 break;
36 18 case ResidualModelTypes::ResidualModelControl:
37 18 os << "ResidualModelControl";
38 18 break;
39 18 case ResidualModelTypes::ResidualModelCoMPosition:
40 18 os << "ResidualModelCoMPosition";
41 18 break;
42 18 case ResidualModelTypes::ResidualModelCentroidalMomentum:
43 18 os << "ResidualModelCentroidalMomentum";
44 18 break;
45 18 case ResidualModelTypes::ResidualModelFramePlacement:
46 18 os << "ResidualModelFramePlacement";
47 18 break;
48 18 case ResidualModelTypes::ResidualModelFrameRotation:
49 18 os << "ResidualModelFrameRotation";
50 18 break;
51 18 case ResidualModelTypes::ResidualModelFrameTranslation:
52 18 os << "ResidualModelFrameTranslation";
53 18 break;
54 18 case ResidualModelTypes::ResidualModelFrameVelocity:
55 18 os << "ResidualModelFrameVelocity";
56 18 break;
57 18 case ResidualModelTypes::ResidualModelControlGrav:
58 18 os << "ResidualModelControlGrav";
59 18 break;
60 #ifdef PINOCCHIO_WITH_HPP_FCL
61 #ifdef CROCODDYL_WITH_PAIR_COLLISION
62 case ResidualModelTypes::ResidualModelPairCollision:
63 os << "ResidualModelPairCollision";
64 break;
65 #endif // CROCODDYL_WITH_PAIR_COLLISION
66 #endif // PINOCCHIO_WITH_HPP_FCL
67 ✗ case ResidualModelTypes::NbResidualModelTypes:
68 ✗ os << "NbResidualModelTypes";
69 ✗ break;
70 ✗ default:
71 ✗ break;
72 }
73 162 return os;
74 }
75
76 487 ResidualModelFactory::ResidualModelFactory() {}
77 487 ResidualModelFactory::~ResidualModelFactory() {}
78
79 486 std::shared_ptr<crocoddyl::ResidualModelAbstract> ResidualModelFactory::create(
80 ResidualModelTypes::Type residual_type, StateModelTypes::Type state_type,
81 std::size_t nu) const {
82
1/2
✓ Branch 1 taken 486 times.
✗ Branch 2 not taken.
 486 StateModelFactory state_factory;
83 486 std::shared_ptr<crocoddyl::ResidualModelAbstract> residual;
84 std::shared_ptr<crocoddyl::StateMultibody> state =
85 std::static_pointer_cast<crocoddyl::StateMultibody>(
86
1/2
✓ Branch 1 taken 486 times.
✗ Branch 2 not taken.
 486 state_factory.create(state_type));
87
1/2
✓ Branch 2 taken 486 times.
✗ Branch 3 not taken.
 486 pinocchio::FrameIndex frame_index = state->get_pinocchio()->frames.size() - 1;
88
1/2
✓ Branch 1 taken 486 times.
✗ Branch 2 not taken.
 486 pinocchio::SE3 frame_SE3 = pinocchio::SE3::Random();
89
90 #ifdef PINOCCHIO_WITH_HPP_FCL
91 #ifdef CROCODDYL_WITH_PAIR_COLLISION
92 pinocchio::SE3 frame_SE3_obstacle = pinocchio::SE3::Random();
93 std::shared_ptr<pinocchio::GeometryModel> geometry =
94 std::make_shared<pinocchio::GeometryModel>(pinocchio::GeometryModel());
95 #if PINOCCHIO_VERSION_AT_LEAST(3, 0, 0)
96 pinocchio::GeomIndex ig_frame =
97 geometry->addGeometryObject(pinocchio::GeometryObject(
98 "frame", frame_index,
99 state->get_pinocchio()->frames[frame_index].parentJoint,
100 std::make_shared<hpp::fcl::Sphere>(0), frame_SE3));
101 pinocchio::GeomIndex ig_obs =
102 geometry->addGeometryObject(pinocchio::GeometryObject(
103 "obs", state->get_pinocchio()->getFrameId("universe"),
104 state->get_pinocchio()
105 ->frames[state->get_pinocchio()->getFrameId("universe")]
106 .parentJoint,
107 std::make_shared<hpp::fcl::Sphere>(0), frame_SE3_obstacle));
108 geometry->addCollisionPair(pinocchio::CollisionPair(ig_frame, ig_obs));
109 #else
110 pinocchio::GeomIndex ig_frame =
111 geometry->addGeometryObject(pinocchio::GeometryObject(
112 "frame", frame_index,
113 state->get_pinocchio()->frames[frame_index].parent,
114 std::make_shared<hpp::fcl::Sphere>(0), frame_SE3));
115 pinocchio::GeomIndex ig_obs =
116 geometry->addGeometryObject(pinocchio::GeometryObject(
117 "obs", state->get_pinocchio()->getFrameId("universe"),
118 state->get_pinocchio()
119 ->frames[state->get_pinocchio()->getFrameId("universe")]
120 .parent,
121 std::make_shared<hpp::fcl::Sphere>(0), frame_SE3_obstacle));
122 geometry->addCollisionPair(pinocchio::CollisionPair(ig_frame, ig_obs));
123 #endif
124 #endif // CROCODDYL_WITH_PAIR_COLLISION
125 #endif // PINOCCHIO_WITH_HPP_FCL
126
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 486 times.
 486 if (nu == std::numeric_limits<std::size_t>::max()) {
127 ✗ nu = state->get_nv();
128 }
129
9/10
✓ Branch 0 taken 54 times.
✓ Branch 1 taken 54 times.
✓ Branch 2 taken 54 times.
✓ Branch 3 taken 54 times.
✓ Branch 4 taken 54 times.
✓ Branch 5 taken 54 times.
✓ Branch 6 taken 54 times.
✓ Branch 7 taken 54 times.
✓ Branch 8 taken 54 times.
✗ Branch 9 not taken.
 486 switch (residual_type) {
130 54 case ResidualModelTypes::ResidualModelState:
131
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 residual = std::make_shared<crocoddyl::ResidualModelState>(
132
1/2
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
 162 state, state->rand(), nu);
133 54 break;
134 54 case ResidualModelTypes::ResidualModelControl:
135
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 residual = std::make_shared<crocoddyl::ResidualModelControl>(
136
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 state, Eigen::VectorXd::Random(nu));
137 54 break;
138 54 case ResidualModelTypes::ResidualModelCoMPosition:
139
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 residual = std::make_shared<crocoddyl::ResidualModelCoMPosition>(
140
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 state, Eigen::Vector3d::Random(), nu);
141 54 break;
142 54 case ResidualModelTypes::ResidualModelCentroidalMomentum:
143
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 residual = std::make_shared<crocoddyl::ResidualModelCentroidalMomentum>(
144
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 state, Vector6d::Random(), nu);
145 54 break;
146 54 case ResidualModelTypes::ResidualModelFramePlacement:
147
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 residual = std::make_shared<crocoddyl::ResidualModelFramePlacement>(
148 54 state, frame_index, frame_SE3, nu);
149 54 break;
150 54 case ResidualModelTypes::ResidualModelFrameRotation:
151 54 residual = std::make_shared<crocoddyl::ResidualModelFrameRotation>(
152
2/4
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
 54 state, frame_index, frame_SE3.rotation(), nu);
153 54 break;
154 54 case ResidualModelTypes::ResidualModelFrameTranslation:
155 54 residual = std::make_shared<crocoddyl::ResidualModelFrameTranslation>(
156
2/4
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
 54 state, frame_index, frame_SE3.translation(), nu);
157 54 break;
158 54 case ResidualModelTypes::ResidualModelFrameVelocity:
159
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 residual = std::make_shared<crocoddyl::ResidualModelFrameVelocity>(
160 ✗ state, frame_index, pinocchio::Motion::Random(),
161
1/2
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
 54 static_cast<pinocchio::ReferenceFrame>(rand() % 2),
162 54 nu); // the code cannot test LOCAL_WORLD_ALIGNED
163 54 break;
164 54 case ResidualModelTypes::ResidualModelControlGrav:
165 residual =
166
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 54 std::make_shared<crocoddyl::ResidualModelControlGrav>(state, nu);
167 54 break;
168 #ifdef PINOCCHIO_WITH_HPP_FCL
169 #ifdef CROCODDYL_WITH_PAIR_COLLISION
170 case ResidualModelTypes::ResidualModelPairCollision:
171 #if PINOCCHIO_VERSION_AT_LEAST(3, 0, 0)
172 residual = std::make_shared<crocoddyl::ResidualModelPairCollision>(
173 state, nu, geometry, 0,
174 state->get_pinocchio()->frames[frame_index].parentJoint);
175 #else
176 residual = std::make_shared<crocoddyl::ResidualModelPairCollision>(
177 state, nu, geometry, 0,
178 state->get_pinocchio()->frames[frame_index].parent);
179 #endif
180 break;
181 #endif // CROCODDYL_WITH_PAIR_COLLISION
182 #endif // PINOCCHIO_WITH_HPP_FCL
183 ✗ default:
184 ✗ throw_pretty(__FILE__ ": Wrong ResidualModelTypes::Type given");
185 break;
186 }
187 972 return residual;
188 486 }
189
190 ✗ std::shared_ptr<crocoddyl::ResidualModelAbstract> create_random_residual(
191 StateModelTypes::Type state_type) {
192 static bool once = true;
193 ✗ if (once) {
194 ✗ srand((unsigned)time(NULL));
195 ✗ once = false;
196 }
197
198 ✗ ResidualModelFactory factory;
199 ResidualModelTypes::Type rand_type = static_cast<ResidualModelTypes::Type>(
200 ✗ rand() % ResidualModelTypes::NbResidualModelTypes);
201 ✗ return factory.create(rand_type, state_type);
202 }
203
204 } // namespace unittest
205 } // namespace crocoddyl
206