GCC Code Coverage Report

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