GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 |
✓✓✓✓ ✓✓✓✓ ✓✓✓✓ ✓✓✗✗ |
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 |
boost::shared_ptr<crocoddyl::ConstraintModelAbstract> |
||
85 |
905 |
ConstraintModelFactory::create(ConstraintModelTypes::Type constraint_type, |
|
86 |
StateModelTypes::Type state_type, |
||
87 |
std::size_t nu) const { |
||
88 |
✓✗ | 1810 |
StateModelFactory state_factory; |
89 |
905 |
boost::shared_ptr<crocoddyl::ConstraintModelAbstract> constraint; |
|
90 |
boost::shared_ptr<crocoddyl::StateMultibody> state = |
||
91 |
boost::static_pointer_cast<crocoddyl::StateMultibody>( |
||
92 |
✓✗ | 1810 |
state_factory.create(state_type)); |
93 |
905 |
pinocchio::FrameIndex frame_index = state->get_pinocchio()->frames.size() - 1; |
|
94 |
✓✗ | 905 |
pinocchio::SE3 frame_SE3 = pinocchio::SE3::Random(); |
95 |
✓✗ | 905 |
pinocchio::Motion frame_motion = pinocchio::Motion::Random(); |
96 |
✓✓ | 905 |
if (nu == std::numeric_limits<std::size_t>::max()) { |
97 |
407 |
nu = state->get_nv(); |
|
98 |
} |
||
99 |
✓✗✓✗ |
1810 |
Eigen::VectorXd lb, ub; |
100 |
✓✓✓✓ ✓✓✓✓ ✓✓✓✓ ✓✓✗ |
905 |
switch (constraint_type) { |
101 |
28 |
case ConstraintModelTypes::ConstraintModelResidualStateEquality: |
|
102 |
✓✗ | 56 |
constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>( |
103 |
✓✗ | 56 |
state, boost::make_shared<crocoddyl::ResidualModelState>( |
104 |
✓✗ | 84 |
state, state->rand(), nu)); |
105 |
28 |
break; |
|
106 |
32 |
case ConstraintModelTypes::ConstraintModelResidualStateInequality: |
|
107 |
✓✗✓✗ |
32 |
lb = Eigen::VectorXd::Zero(state->get_ndx()); |
108 |
✓✗✓✗ |
32 |
ub = Eigen::VectorXd::Zero(state->get_ndx()); |
109 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
32 |
state->diff(state->zero(), state->zero(), lb); |
110 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
32 |
state->diff(state->zero(), state->rand().cwiseAbs(), ub); |
111 |
✓✗ | 64 |
constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>( |
112 |
state, |
||
113 |
✓✗ | 64 |
boost::make_shared<crocoddyl::ResidualModelState>(state, |
114 |
✓✗ | 64 |
state->rand(), nu), |
115 |
✓✗ | 64 |
lb, ub.cwiseAbs()); |
116 |
32 |
break; |
|
117 |
31 |
case ConstraintModelTypes::ConstraintModelResidualControlEquality: |
|
118 |
✓✗ | 62 |
constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>( |
119 |
✓✗ | 62 |
state, boost::make_shared<crocoddyl::ResidualModelControl>( |
120 |
✓✗ | 62 |
state, Eigen::VectorXd::Random(nu))); |
121 |
31 |
break; |
|
122 |
27 |
case ConstraintModelTypes::ConstraintModelResidualControlInequality: |
|
123 |
✓✗✓✗ |
27 |
lb = Eigen::VectorXd::Zero(nu); |
124 |
✓✗ | 27 |
lb(0) = -INFINITY; |
125 |
✓✗✓✗ ✓✗ |
27 |
ub = Eigen::VectorXd::Random(nu).cwiseAbs(); |
126 |
✓✗ | 54 |
constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>( |
127 |
state, |
||
128 |
✓✗ | 54 |
boost::make_shared<crocoddyl::ResidualModelControl>( |
129 |
✓✗ | 27 |
state, Eigen::VectorXd::Random(nu)), |
130 |
27 |
lb, ub); |
|
131 |
27 |
break; |
|
132 |
31 |
case ConstraintModelTypes::ConstraintModelResidualCoMPositionEquality: |
|
133 |
✓✗ | 62 |
constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>( |
134 |
✓✗ | 62 |
state, boost::make_shared<crocoddyl::ResidualModelCoMPosition>( |
135 |
✓✗ | 62 |
state, Eigen::Vector3d::Random(), nu)); |
136 |
31 |
break; |
|
137 |
32 |
case ConstraintModelTypes::ConstraintModelResidualCoMPositionInequality: |
|
138 |
✓✗✓✗ |
32 |
lb = Eigen::Vector3d(0., -INFINITY, 0.); |
139 |
✓✗✓✗ ✓✗ |
32 |
ub = Eigen::Vector3d::Random().cwiseAbs(); |
140 |
✓✗ | 32 |
ub(2) = INFINITY; |
141 |
✓✗ | 64 |
constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>( |
142 |
state, |
||
143 |
✓✗ | 64 |
boost::make_shared<crocoddyl::ResidualModelCoMPosition>( |
144 |
✓✗ | 32 |
state, Eigen::Vector3d::Random(), nu), |
145 |
32 |
lb, ub); |
|
146 |
32 |
break; |
|
147 |
281 |
case ConstraintModelTypes::ConstraintModelResidualFramePlacementEquality: |
|
148 |
✓✗ | 562 |
constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>( |
149 |
✓✗ | 562 |
state, boost::make_shared<crocoddyl::ResidualModelFramePlacement>( |
150 |
281 |
state, frame_index, frame_SE3, nu)); |
|
151 |
281 |
break; |
|
152 |
28 |
case ConstraintModelTypes::ConstraintModelResidualFramePlacementInequality: |
|
153 |
✓✗✓✗ |
28 |
lb = MathBaseTpl<double>::Vector6s::Zero(); |
154 |
✓✗✓✗ ✓✗✓✗ |
28 |
lb.tail(3) << -INFINITY, -INFINITY, -INFINITY; |
155 |
✓✗✓✗ ✓✗ |
28 |
ub = MathBaseTpl<double>::Vector6s::Random().cwiseAbs(); |
156 |
✓✗✓✗ ✓✗✓✗ |
28 |
ub.head(3) << INFINITY, INFINITY, INFINITY; |
157 |
✓✗ | 56 |
constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>( |
158 |
state, |
||
159 |
✓✗ | 56 |
boost::make_shared<crocoddyl::ResidualModelFramePlacement>( |
160 |
state, frame_index, frame_SE3, nu), |
||
161 |
28 |
lb, ub); |
|
162 |
28 |
break; |
|
163 |
26 |
case ConstraintModelTypes::ConstraintModelResidualFrameRotationEquality: |
|
164 |
✓✗ | 52 |
constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>( |
165 |
26 |
state, boost::make_shared<crocoddyl::ResidualModelFrameRotation>( |
|
166 |
✓✗✓✗ |
52 |
state, frame_index, frame_SE3.rotation(), nu)); |
167 |
26 |
break; |
|
168 |
30 |
case ConstraintModelTypes::ConstraintModelResidualFrameRotationInequality: |
|
169 |
✓✗✓✗ |
30 |
lb = Eigen::Vector3d::Zero(); |
170 |
✓✗ | 30 |
lb(1) = -INFINITY; |
171 |
✓✗✓✗ ✓✗ |
30 |
ub = Eigen::Vector3d::Random().cwiseAbs(); |
172 |
✓✗ | 30 |
ub(2) = INFINITY; |
173 |
✓✗ | 60 |
constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>( |
174 |
state, |
||
175 |
30 |
boost::make_shared<crocoddyl::ResidualModelFrameRotation>( |
|
176 |
✓✗✓✗ |
30 |
state, frame_index, frame_SE3.rotation(), nu), |
177 |
30 |
lb, ub); |
|
178 |
30 |
break; |
|
179 |
25 |
case ConstraintModelTypes::ConstraintModelResidualFrameTranslationEquality: |
|
180 |
✓✗ | 50 |
constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>( |
181 |
25 |
state, boost::make_shared<crocoddyl::ResidualModelFrameTranslation>( |
|
182 |
✓✗✓✗ |
50 |
state, frame_index, frame_SE3.translation(), nu)); |
183 |
25 |
break; |
|
184 |
27 |
case ConstraintModelTypes:: |
|
185 |
ConstraintModelResidualFrameTranslationInequality: |
||
186 |
✓✗✓✗ ✓✗✓✗ |
27 |
lb = -1 * Eigen::Vector3d::Random().cwiseAbs(); |
187 |
✓✗✓✗ ✓✗ |
27 |
ub = Eigen::Vector3d::Random().cwiseAbs(); |
188 |
✓✗ | 54 |
constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>( |
189 |
state, |
||
190 |
27 |
boost::make_shared<crocoddyl::ResidualModelFrameTranslation>( |
|
191 |
✓✗✓✗ |
27 |
state, frame_index, frame_SE3.translation(), nu), |
192 |
27 |
lb, ub); |
|
193 |
27 |
break; |
|
194 |
278 |
case ConstraintModelTypes::ConstraintModelResidualFrameVelocityEquality: |
|
195 |
✓✗ | 556 |
constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>( |
196 |
278 |
state, boost::make_shared<crocoddyl::ResidualModelFrameVelocity>( |
|
197 |
✓✗ | 556 |
state, frame_index, frame_motion, pinocchio::LOCAL, nu)); |
198 |
278 |
break; |
|
199 |
29 |
case ConstraintModelTypes::ConstraintModelResidualFrameVelocityInequality: |
|
200 |
✓✗✓✗ ✓✗✓✗ |
29 |
lb = -1 * MathBaseTpl<double>::Vector6s::Random().cwiseAbs(); |
201 |
✓✗ | 29 |
lb(0) = -INFINITY; |
202 |
✓✗✓✗ ✓✗ |
29 |
ub = MathBaseTpl<double>::Vector6s::Random().cwiseAbs(); |
203 |
✓✗ | 29 |
ub(0) = INFINITY; |
204 |
✓✗ | 58 |
constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>( |
205 |
state, |
||
206 |
29 |
boost::make_shared<crocoddyl::ResidualModelFrameVelocity>( |
|
207 |
✓✗ | 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 |
} |
||
216 |
|||
217 |
57 |
boost::shared_ptr<crocoddyl::ConstraintModelAbstract> create_random_constraint( |
|
218 |
StateModelTypes::Type state_type) { |
||
219 |
static bool once = true; |
||
220 |
✓✓ | 57 |
if (once) { |
221 |
1 |
srand((unsigned)time(NULL)); |
|
222 |
1 |
once = false; |
|
223 |
} |
||
224 |
|||
225 |
✓✗ | 114 |
ConstraintModelFactory factory; |
226 |
ConstraintModelTypes::Type rand_type = |
||
227 |
static_cast<ConstraintModelTypes::Type>( |
||
228 |
57 |
rand() % ConstraintModelTypes::NbConstraintModelTypes); |
|
229 |
✓✗ | 114 |
return factory.create(rand_type, state_type); |
230 |
} |
||
231 |
|||
232 |
} // namespace unittest |
||
233 |
} // namespace crocoddyl |
Generated by: GCOVR (Version 4.2) |