GCC Code Coverage Report

Directory: ./
File: unittest/factory/constraint.cpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 172 0.0%
Branches: 0 275 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2022, University of Edinburgh, IRI: CSIC-UPC
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include "constraint.hpp"
10
11 #include "crocoddyl/core/constraints/residual.hpp"
12 #include "crocoddyl/core/residuals/control.hpp"
13 #include "crocoddyl/multibody/residuals/com-position.hpp"
14 #include "crocoddyl/multibody/residuals/frame-placement.hpp"
15 #include "crocoddyl/multibody/residuals/frame-rotation.hpp"
16 #include "crocoddyl/multibody/residuals/frame-translation.hpp"
17 #include "crocoddyl/multibody/residuals/frame-velocity.hpp"
18 #include "crocoddyl/multibody/residuals/state.hpp"
19
20 namespace crocoddyl {
21 namespace unittest {
22
23 const std::vector<ConstraintModelTypes::Type> ConstraintModelTypes::all(
24 ConstraintModelTypes::init_all());
25
26 ✗ std::ostream& operator<<(std::ostream& os, ConstraintModelTypes::Type type) {
27 ✗ switch (type) {
28 ✗ case ConstraintModelTypes::ConstraintModelResidualStateEquality:
29 ✗ os << "ConstraintModelResidualStateEquality";
30 ✗ break;
31 ✗ case ConstraintModelTypes::ConstraintModelResidualStateInequality:
32 ✗ os << "ConstraintModelResidualStateInequality";
33 ✗ break;
34 ✗ case ConstraintModelTypes::ConstraintModelResidualControlEquality:
35 ✗ os << "ConstraintModelResidualControlEquality";
36 ✗ break;
37 ✗ case ConstraintModelTypes::ConstraintModelResidualControlInequality:
38 ✗ os << "ConstraintModelResidualControlInequality";
39 ✗ break;
40 ✗ case ConstraintModelTypes::ConstraintModelResidualCoMPositionEquality:
41 ✗ os << "ConstraintModelResidualCoMPositionEquality";
42 ✗ break;
43 ✗ case ConstraintModelTypes::ConstraintModelResidualCoMPositionInequality:
44 ✗ os << "ConstraintModelResidualCoMPositionInequality";
45 ✗ break;
46 ✗ case ConstraintModelTypes::ConstraintModelResidualFramePlacementEquality:
47 ✗ os << "ConstraintModelResidualFramePlacementEquality";
48 ✗ break;
49 ✗ case ConstraintModelTypes::ConstraintModelResidualFramePlacementInequality:
50 ✗ os << "ConstraintModelResidualFramePlacementInequality";
51 ✗ break;
52 ✗ case ConstraintModelTypes::ConstraintModelResidualFrameRotationEquality:
53 ✗ os << "ConstraintModelResidualFrameRotationEquality";
54 ✗ break;
55 ✗ case ConstraintModelTypes::ConstraintModelResidualFrameRotationInequality:
56 ✗ os << "ConstraintModelResidualFrameRotationInequality";
57 ✗ break;
58 ✗ case ConstraintModelTypes::ConstraintModelResidualFrameTranslationEquality:
59 ✗ os << "ConstraintModelResidualFrameTranslationEquality";
60 ✗ break;
61 ✗ case ConstraintModelTypes::
62 ConstraintModelResidualFrameTranslationInequality:
63 ✗ os << "ConstraintModelResidualFrameTranslationInequality";
64 ✗ break;
65 ✗ case ConstraintModelTypes::ConstraintModelResidualFrameVelocityEquality:
66 ✗ os << "ConstraintModelResidualFrameVelocityEquality";
67 ✗ break;
68 ✗ case ConstraintModelTypes::ConstraintModelResidualFrameVelocityInequality:
69 ✗ os << "ConstraintModelResidualFrameVelocityInequality";
70 ✗ break;
71 ✗ case ConstraintModelTypes::NbConstraintModelTypes:
72 ✗ os << "NbConstraintModelTypes";
73 ✗ break;
74 ✗ default:
75 ✗ break;
76 }
77 ✗ return os;
78 }
79
80 ✗ ConstraintModelFactory::ConstraintModelFactory() {}
81 ✗ ConstraintModelFactory::~ConstraintModelFactory() {}
82
83 std::shared_ptr<crocoddyl::ConstraintModelAbstract>
84 ✗ ConstraintModelFactory::create(ConstraintModelTypes::Type constraint_type,
85 StateModelTypes::Type state_type,
86 std::size_t nu) const {
87 ✗ StateModelFactory state_factory;
88 ✗ std::shared_ptr<crocoddyl::ConstraintModelAbstract> constraint;
89 std::shared_ptr<crocoddyl::StateMultibody> state =
90 std::static_pointer_cast<crocoddyl::StateMultibody>(
91 ✗ state_factory.create(state_type));
92 ✗ pinocchio::FrameIndex frame_index = state->get_pinocchio()->frames.size() - 1;
93 ✗ pinocchio::SE3 frame_SE3 = pinocchio::SE3::Random();
94 ✗ pinocchio::Motion frame_motion = pinocchio::Motion::Random();
95 ✗ if (nu == std::numeric_limits<std::size_t>::max()) {
96 ✗ nu = state->get_nv();
97 }
98 ✗ Eigen::VectorXd lb, ub;
99 ✗ switch (constraint_type) {
100 ✗ case ConstraintModelTypes::ConstraintModelResidualStateEquality:
101 ✗ constraint = std::make_shared<crocoddyl::ConstraintModelResidual>(
102 ✗ state, std::make_shared<crocoddyl::ResidualModelState>(
103 ✗ state, state->rand(), nu));
104 ✗ break;
105 ✗ case ConstraintModelTypes::ConstraintModelResidualStateInequality:
106 ✗ lb = Eigen::VectorXd::Zero(state->get_ndx());
107 ✗ ub = Eigen::VectorXd::Zero(state->get_ndx());
108 ✗ state->diff(state->zero(), state->zero(), lb);
109 ✗ state->diff(state->zero(), state->rand().cwiseAbs(), ub);
110 ✗ constraint = std::make_shared<crocoddyl::ConstraintModelResidual>(
111 state,
112 ✗ std::make_shared<crocoddyl::ResidualModelState>(state, state->rand(),
113 nu),
114 ✗ lb, ub.cwiseAbs());
115 ✗ break;
116 ✗ case ConstraintModelTypes::ConstraintModelResidualControlEquality:
117 ✗ constraint = std::make_shared<crocoddyl::ConstraintModelResidual>(
118 ✗ state, std::make_shared<crocoddyl::ResidualModelControl>(
119 ✗ state, Eigen::VectorXd::Random(nu)));
120 ✗ break;
121 ✗ case ConstraintModelTypes::ConstraintModelResidualControlInequality:
122 ✗ lb = Eigen::VectorXd::Zero(nu);
123 ✗ lb(0) = -INFINITY;
124 ✗ ub = Eigen::VectorXd::Random(nu).cwiseAbs();
125 ✗ constraint = std::make_shared<crocoddyl::ConstraintModelResidual>(
126 state,
127 ✗ std::make_shared<crocoddyl::ResidualModelControl>(
128 ✗ state, Eigen::VectorXd::Random(nu)),
129 ✗ lb, ub);
130 ✗ break;
131 ✗ case ConstraintModelTypes::ConstraintModelResidualCoMPositionEquality:
132 ✗ constraint = std::make_shared<crocoddyl::ConstraintModelResidual>(
133 ✗ state, std::make_shared<crocoddyl::ResidualModelCoMPosition>(
134 ✗ state, Eigen::Vector3d::Random(), nu));
135 ✗ break;
136 ✗ case ConstraintModelTypes::ConstraintModelResidualCoMPositionInequality:
137 ✗ lb = Eigen::Vector3d(0., -INFINITY, 0.);
138 ✗ ub = Eigen::Vector3d::Random().cwiseAbs();
139 ✗ ub(2) = INFINITY;
140 ✗ constraint = std::make_shared<crocoddyl::ConstraintModelResidual>(
141 state,
142 ✗ std::make_shared<crocoddyl::ResidualModelCoMPosition>(
143 ✗ state, Eigen::Vector3d::Random(), nu),
144 ✗ lb, ub);
145 ✗ break;
146 ✗ case ConstraintModelTypes::ConstraintModelResidualFramePlacementEquality:
147 ✗ constraint = std::make_shared<crocoddyl::ConstraintModelResidual>(
148 ✗ state, std::make_shared<crocoddyl::ResidualModelFramePlacement>(
149 ✗ state, frame_index, frame_SE3, nu));
150 ✗ break;
151 ✗ case ConstraintModelTypes::ConstraintModelResidualFramePlacementInequality:
152 ✗ lb = MathBaseTpl<double>::Vector6s::Zero();
153 ✗ lb.tail<3>() << -INFINITY, -INFINITY, -INFINITY;
154 ✗ ub = MathBaseTpl<double>::Vector6s::Random().cwiseAbs();
155 ✗ ub.head<3>() << INFINITY, INFINITY, INFINITY;
156 ✗ constraint = std::make_shared<crocoddyl::ConstraintModelResidual>(
157 state,
158 ✗ std::make_shared<crocoddyl::ResidualModelFramePlacement>(
159 state, frame_index, frame_SE3, nu),
160 ✗ lb, ub);
161 ✗ break;
162 ✗ case ConstraintModelTypes::ConstraintModelResidualFrameRotationEquality:
163 ✗ constraint = std::make_shared<crocoddyl::ConstraintModelResidual>(
164 ✗ state, std::make_shared<crocoddyl::ResidualModelFrameRotation>(
165 ✗ state, frame_index, frame_SE3.rotation(), nu));
166 ✗ break;
167 ✗ case ConstraintModelTypes::ConstraintModelResidualFrameRotationInequality:
168 ✗ lb = Eigen::Vector3d::Zero();
169 ✗ lb(1) = -INFINITY;
170 ✗ ub = Eigen::Vector3d::Random().cwiseAbs();
171 ✗ ub(2) = INFINITY;
172 ✗ constraint = std::make_shared<crocoddyl::ConstraintModelResidual>(
173 state,
174 ✗ std::make_shared<crocoddyl::ResidualModelFrameRotation>(
175 ✗ state, frame_index, frame_SE3.rotation(), nu),
176 ✗ lb, ub);
177 ✗ break;
178 ✗ case ConstraintModelTypes::ConstraintModelResidualFrameTranslationEquality:
179 ✗ constraint = std::make_shared<crocoddyl::ConstraintModelResidual>(
180 ✗ state, std::make_shared<crocoddyl::ResidualModelFrameTranslation>(
181 ✗ state, frame_index, frame_SE3.translation(), nu));
182 ✗ break;
183 ✗ case ConstraintModelTypes::
184 ConstraintModelResidualFrameTranslationInequality:
185 ✗ lb = -1 * Eigen::Vector3d::Random().cwiseAbs();
186 ✗ ub = Eigen::Vector3d::Random().cwiseAbs();
187 ✗ constraint = std::make_shared<crocoddyl::ConstraintModelResidual>(
188 state,
189 ✗ std::make_shared<crocoddyl::ResidualModelFrameTranslation>(
190 ✗ state, frame_index, frame_SE3.translation(), nu),
191 ✗ lb, ub);
192 ✗ break;
193 ✗ case ConstraintModelTypes::ConstraintModelResidualFrameVelocityEquality:
194 ✗ constraint = std::make_shared<crocoddyl::ConstraintModelResidual>(
195 ✗ state, std::make_shared<crocoddyl::ResidualModelFrameVelocity>(
196 ✗ state, frame_index, frame_motion, pinocchio::LOCAL, nu));
197 ✗ break;
198 ✗ case ConstraintModelTypes::ConstraintModelResidualFrameVelocityInequality:
199 ✗ lb = -1 * MathBaseTpl<double>::Vector6s::Random().cwiseAbs();
200 ✗ lb(0) = -INFINITY;
201 ✗ ub = MathBaseTpl<double>::Vector6s::Random().cwiseAbs();
202 ✗ ub(0) = INFINITY;
203 ✗ constraint = std::make_shared<crocoddyl::ConstraintModelResidual>(
204 state,
205 ✗ std::make_shared<crocoddyl::ResidualModelFrameVelocity>(
206 ✗ state, frame_index, frame_motion, pinocchio::LOCAL, nu),
207 ✗ lb, ub);
208 ✗ break;
209 ✗ default:
210 ✗ throw_pretty(__FILE__ ": Wrong ConstraintModelType::Type given");
211 break;
212 }
213 ✗ return constraint;
214 }
215
216 ✗ std::shared_ptr<crocoddyl::ConstraintModelAbstract> create_random_constraint(
217 StateModelTypes::Type state_type) {
218 static bool once = true;
219 ✗ if (once) {
220 ✗ srand((unsigned)time(NULL));
221 ✗ once = false;
222 }
223
224 ✗ ConstraintModelFactory factory;
225 ConstraintModelTypes::Type rand_type =
226 static_cast<ConstraintModelTypes::Type>(
227 ✗ rand() % ConstraintModelTypes::NbConstraintModelTypes);
228 ✗ return factory.create(rand_type, state_type);
229 }
230
231 } // namespace unittest
232 } // namespace crocoddyl
233