GCC Code Coverage Report

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