GCC Code Coverage Report

Directory: ./
File: unittest/factory/residual.cpp
Date: 2025-02-24 23:41:29
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 540 std::shared_ptr<crocoddyl::ResidualModelAbstract> ResidualModelFactory::create(
77 ResidualModelTypes::Type residual_type, StateModelTypes::Type state_type,
78 std::size_t nu) const {
79
1/2
✓ Branch 1 taken 540 times.
✗ Branch 2 not taken.
 540 StateModelFactory state_factory;
80 540 std::shared_ptr<crocoddyl::ResidualModelAbstract> residual;
81 std::shared_ptr<crocoddyl::StateMultibody> state =
82 std::static_pointer_cast<crocoddyl::StateMultibody>(
83
1/2
✓ Branch 1 taken 540 times.
✗ Branch 2 not taken.
 540 state_factory.create(state_type));
84 540 pinocchio::FrameIndex frame_index = state->get_pinocchio()->frames.size() - 1;
85
1/2
✓ Branch 1 taken 540 times.
✗ Branch 2 not taken.
 540 pinocchio::SE3 frame_SE3 = pinocchio::SE3::Random();
86
87 #ifdef PINOCCHIO_WITH_HPP_FCL
88
1/2
✓ Branch 1 taken 540 times.
✗ Branch 2 not taken.
 540 pinocchio::SE3 frame_SE3_obstacle = pinocchio::SE3::Random();
89 std::shared_ptr<pinocchio::GeometryModel> geometry =
90
2/4
✓ Branch 1 taken 540 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 540 times.
✗ Branch 5 not taken.
 540 std::make_shared<pinocchio::GeometryModel>(pinocchio::GeometryModel());
91 #if PINOCCHIO_VERSION_AT_LEAST(3, 0, 0)
92 pinocchio::GeomIndex ig_frame =
93
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(
94 "frame", frame_index,
95 540 state->get_pinocchio()->frames[frame_index].parentJoint,
96
1/2
✓ Branch 1 taken 540 times.
✗ Branch 2 not taken.
 1080 std::make_shared<hpp::fcl::Sphere>(0), frame_SE3));
97 pinocchio::GeomIndex ig_obs =
98
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(
99 540 "obs", state->get_pinocchio()->getFrameId("universe"),
100 540 state->get_pinocchio()
101
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")]
102 .parentJoint,
103
1/2
✓ Branch 1 taken 540 times.
✗ Branch 2 not taken.
 1080 std::make_shared<hpp::fcl::Sphere>(0), frame_SE3_obstacle));
104
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));
105 #else
106 pinocchio::GeomIndex ig_frame =
107 geometry->addGeometryObject(pinocchio::GeometryObject(
108 "frame", frame_index,
109 state->get_pinocchio()->frames[frame_index].parent,
110 std::make_shared<hpp::fcl::Sphere>(0), frame_SE3));
111 pinocchio::GeomIndex ig_obs =
112 geometry->addGeometryObject(pinocchio::GeometryObject(
113 "obs", state->get_pinocchio()->getFrameId("universe"),
114 state->get_pinocchio()
115 ->frames[state->get_pinocchio()->getFrameId("universe")]
116 .parent,
117 std::make_shared<hpp::fcl::Sphere>(0), frame_SE3_obstacle));
118 geometry->addCollisionPair(pinocchio::CollisionPair(ig_frame, ig_obs));
119 #endif
120 #endif // PINOCCHIO_WITH_HPP_FCL
121
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 540 times.
 540 if (nu == std::numeric_limits<std::size_t>::max()) {
122 ✗ nu = state->get_nv();
123 }
124
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) {
125 54 case ResidualModelTypes::ResidualModelState:
126
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 residual = std::make_shared<crocoddyl::ResidualModelState>(
127
1/2
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
 162 state, state->rand(), nu);
128 54 break;
129 54 case ResidualModelTypes::ResidualModelControl:
130
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 residual = std::make_shared<crocoddyl::ResidualModelControl>(
131
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 state, Eigen::VectorXd::Random(nu));
132 54 break;
133 54 case ResidualModelTypes::ResidualModelCoMPosition:
134
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 residual = std::make_shared<crocoddyl::ResidualModelCoMPosition>(
135
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 state, Eigen::Vector3d::Random(), nu);
136 54 break;
137 54 case ResidualModelTypes::ResidualModelCentroidalMomentum:
138
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 residual = std::make_shared<crocoddyl::ResidualModelCentroidalMomentum>(
139
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 state, Vector6d::Random(), nu);
140 54 break;
141 54 case ResidualModelTypes::ResidualModelFramePlacement:
142
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 residual = std::make_shared<crocoddyl::ResidualModelFramePlacement>(
143 54 state, frame_index, frame_SE3, nu);
144 54 break;
145 54 case ResidualModelTypes::ResidualModelFrameRotation:
146 54 residual = std::make_shared<crocoddyl::ResidualModelFrameRotation>(
147
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);
148 54 break;
149 54 case ResidualModelTypes::ResidualModelFrameTranslation:
150 54 residual = std::make_shared<crocoddyl::ResidualModelFrameTranslation>(
151
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);
152 54 break;
153 54 case ResidualModelTypes::ResidualModelFrameVelocity:
154
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 108 residual = std::make_shared<crocoddyl::ResidualModelFrameVelocity>(
155 ✗ state, frame_index, pinocchio::Motion::Random(),
156
1/2
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
 54 static_cast<pinocchio::ReferenceFrame>(rand() % 2),
157 54 nu); // the code cannot test LOCAL_WORLD_ALIGNED
158 54 break;
159 54 case ResidualModelTypes::ResidualModelControlGrav:
160 residual =
161
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 54 std::make_shared<crocoddyl::ResidualModelControlGrav>(state, nu);
162 54 break;
163 #ifdef PINOCCHIO_WITH_HPP_FCL
164 54 case ResidualModelTypes::ResidualModelPairCollision:
165 #if PINOCCHIO_VERSION_AT_LEAST(3, 0, 0)
166 54 residual = std::make_shared<crocoddyl::ResidualModelPairCollision>(
167 ✗ state, nu, geometry, 0,
168
1/2
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
 54 state->get_pinocchio()->frames[frame_index].parentJoint);
169 #else
170 residual = std::make_shared<crocoddyl::ResidualModelPairCollision>(
171 state, nu, geometry, 0,
172 state->get_pinocchio()->frames[frame_index].parent);
173 #endif
174 54 break;
175 #endif // PINOCCHIO_WITH_HPP_FCL
176 ✗ default:
177 ✗ throw_pretty(__FILE__ ": Wrong ResidualModelTypes::Type given");
178 break;
179 }
180 1080 return residual;
181 540 }
182
183 ✗ std::shared_ptr<crocoddyl::ResidualModelAbstract> create_random_residual(
184 StateModelTypes::Type state_type) {
185 static bool once = true;
186 ✗ if (once) {
187 ✗ srand((unsigned)time(NULL));
188 ✗ once = false;
189 }
190
191 ✗ ResidualModelFactory factory;
192 ResidualModelTypes::Type rand_type = static_cast<ResidualModelTypes::Type>(
193 ✗ rand() % ResidualModelTypes::NbResidualModelTypes);
194 ✗ return factory.create(rand_type, state_type);
195 }
196
197 } // namespace unittest
198 } // namespace crocoddyl
199