GCC Code Coverage Report

Directory: ./
File: unittest/factory/residual.cpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 93 0.0%
Branches: 0 79 0.0%
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 ✗ std::ostream& operator<<(std::ostream& os, ResidualModelTypes::Type type) {
32 ✗ switch (type) {
33 ✗ case ResidualModelTypes::ResidualModelState:
34 ✗ os << "ResidualModelState";
35 ✗ break;
36 ✗ case ResidualModelTypes::ResidualModelControl:
37 ✗ os << "ResidualModelControl";
38 ✗ break;
39 ✗ case ResidualModelTypes::ResidualModelCoMPosition:
40 ✗ os << "ResidualModelCoMPosition";
41 ✗ break;
42 ✗ case ResidualModelTypes::ResidualModelCentroidalMomentum:
43 ✗ os << "ResidualModelCentroidalMomentum";
44 ✗ break;
45 ✗ case ResidualModelTypes::ResidualModelFramePlacement:
46 ✗ os << "ResidualModelFramePlacement";
47 ✗ break;
48 ✗ case ResidualModelTypes::ResidualModelFrameRotation:
49 ✗ os << "ResidualModelFrameRotation";
50 ✗ break;
51 ✗ case ResidualModelTypes::ResidualModelFrameTranslation:
52 ✗ os << "ResidualModelFrameTranslation";
53 ✗ break;
54 ✗ case ResidualModelTypes::ResidualModelFrameVelocity:
55 ✗ os << "ResidualModelFrameVelocity";
56 ✗ break;
57 ✗ case ResidualModelTypes::ResidualModelControlGrav:
58 ✗ os << "ResidualModelControlGrav";
59 ✗ 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 ✗ return os;
74 }
75
76 ✗ ResidualModelFactory::ResidualModelFactory() {}
77 ✗ ResidualModelFactory::~ResidualModelFactory() {}
78
79 ✗ std::shared_ptr<crocoddyl::ResidualModelAbstract> ResidualModelFactory::create(
80 ResidualModelTypes::Type residual_type, StateModelTypes::Type state_type,
81 std::size_t nu) const {
82 ✗ StateModelFactory state_factory;
83 ✗ std::shared_ptr<crocoddyl::ResidualModelAbstract> residual;
84 std::shared_ptr<crocoddyl::StateMultibody> state =
85 std::static_pointer_cast<crocoddyl::StateMultibody>(
86 ✗ state_factory.create(state_type));
87 ✗ pinocchio::FrameIndex frame_index = state->get_pinocchio()->frames.size() - 1;
88 ✗ 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 pinocchio::GeomIndex ig_frame =
96 geometry->addGeometryObject(pinocchio::GeometryObject(
97 "frame", frame_index,
98 state->get_pinocchio()->frames[frame_index].parentJoint,
99 std::make_shared<hpp::fcl::Sphere>(0), frame_SE3));
100 pinocchio::GeomIndex ig_obs =
101 geometry->addGeometryObject(pinocchio::GeometryObject(
102 "obs", state->get_pinocchio()->getFrameId("universe"),
103 state->get_pinocchio()
104 ->frames[state->get_pinocchio()->getFrameId("universe")]
105 .parentJoint,
106 std::make_shared<hpp::fcl::Sphere>(0), frame_SE3_obstacle));
107 geometry->addCollisionPair(pinocchio::CollisionPair(ig_frame, ig_obs));
108 #endif // CROCODDYL_WITH_PAIR_COLLISION
109 #endif // PINOCCHIO_WITH_HPP_FCL
110 ✗ if (nu == std::numeric_limits<std::size_t>::max()) {
111 ✗ nu = state->get_nv();
112 }
113 ✗ switch (residual_type) {
114 ✗ case ResidualModelTypes::ResidualModelState:
115 ✗ residual = std::make_shared<crocoddyl::ResidualModelState>(
116 ✗ state, state->rand(), nu);
117 ✗ break;
118 ✗ case ResidualModelTypes::ResidualModelControl:
119 ✗ residual = std::make_shared<crocoddyl::ResidualModelControl>(
120 ✗ state, Eigen::VectorXd::Random(nu));
121 ✗ break;
122 ✗ case ResidualModelTypes::ResidualModelCoMPosition:
123 ✗ residual = std::make_shared<crocoddyl::ResidualModelCoMPosition>(
124 ✗ state, Eigen::Vector3d::Random(), nu);
125 ✗ break;
126 ✗ case ResidualModelTypes::ResidualModelCentroidalMomentum:
127 ✗ residual = std::make_shared<crocoddyl::ResidualModelCentroidalMomentum>(
128 ✗ state, Vector6d::Random(), nu);
129 ✗ break;
130 ✗ case ResidualModelTypes::ResidualModelFramePlacement:
131 ✗ residual = std::make_shared<crocoddyl::ResidualModelFramePlacement>(
132 ✗ state, frame_index, frame_SE3, nu);
133 ✗ break;
134 ✗ case ResidualModelTypes::ResidualModelFrameRotation:
135 ✗ residual = std::make_shared<crocoddyl::ResidualModelFrameRotation>(
136 ✗ state, frame_index, frame_SE3.rotation(), nu);
137 ✗ break;
138 ✗ case ResidualModelTypes::ResidualModelFrameTranslation:
139 ✗ residual = std::make_shared<crocoddyl::ResidualModelFrameTranslation>(
140 ✗ state, frame_index, frame_SE3.translation(), nu);
141 ✗ break;
142 ✗ case ResidualModelTypes::ResidualModelFrameVelocity:
143 ✗ residual = std::make_shared<crocoddyl::ResidualModelFrameVelocity>(
144 ✗ state, frame_index, pinocchio::Motion::Random(),
145 ✗ static_cast<pinocchio::ReferenceFrame>(rand() % 2),
146 ✗ nu); // the code cannot test LOCAL_WORLD_ALIGNED
147 ✗ break;
148 ✗ case ResidualModelTypes::ResidualModelControlGrav:
149 residual =
150 ✗ std::make_shared<crocoddyl::ResidualModelControlGrav>(state, nu);
151 ✗ break;
152 #ifdef PINOCCHIO_WITH_HPP_FCL
153 #ifdef CROCODDYL_WITH_PAIR_COLLISION
154 case ResidualModelTypes::ResidualModelPairCollision:
155 residual = std::make_shared<crocoddyl::ResidualModelPairCollision>(
156 state, nu, geometry, 0,
157 state->get_pinocchio()->frames[frame_index].parentJoint);
158 break;
159 #endif // CROCODDYL_WITH_PAIR_COLLISION
160 #endif // PINOCCHIO_WITH_HPP_FCL
161 ✗ default:
162 ✗ throw_pretty(__FILE__ ": Wrong ResidualModelTypes::Type given");
163 break;
164 }
165 ✗ return residual;
166 }
167
168 ✗ std::shared_ptr<crocoddyl::ResidualModelAbstract> create_random_residual(
169 StateModelTypes::Type state_type) {
170 static bool once = true;
171 ✗ if (once) {
172 ✗ srand((unsigned)time(NULL));
173 ✗ once = false;
174 }
175
176 ✗ ResidualModelFactory factory;
177 ResidualModelTypes::Type rand_type = static_cast<ResidualModelTypes::Type>(
178 ✗ rand() % ResidualModelTypes::NbResidualModelTypes);
179 ✗ return factory.create(rand_type, state_type);
180 }
181
182 } // namespace unittest
183 } // namespace crocoddyl
184