GCC Code Coverage Report

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