GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// Copyright (c) 2017-2020, CNRS INRIA |
||
2 |
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) |
||
3 |
// |
||
4 |
|||
5 |
#include "pinocchio/multibody/liegroup/liegroup.hpp" |
||
6 |
#include "pinocchio/multibody/liegroup/liegroup-collection.hpp" |
||
7 |
#include "pinocchio/multibody/liegroup/liegroup-generic.hpp" |
||
8 |
#include "pinocchio/multibody/liegroup/cartesian-product-variant.hpp" |
||
9 |
|||
10 |
#include "pinocchio/multibody/joint/joint-generic.hpp" |
||
11 |
|||
12 |
#include <boost/test/unit_test.hpp> |
||
13 |
#include <boost/utility/binary.hpp> |
||
14 |
#include <boost/algorithm/string.hpp> |
||
15 |
#include <boost/mpl/vector.hpp> |
||
16 |
|||
17 |
#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision) \ |
||
18 |
BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), \ |
||
19 |
"check " #Va ".isApprox(" #Vb ") failed " \ |
||
20 |
"[\n" << (Va).transpose() << "\n!=\n" << (Vb).transpose() << "\n]") |
||
21 |
#define EIGEN_MATRIX_IS_APPROX(Va, Vb, precision) \ |
||
22 |
BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), \ |
||
23 |
"check " #Va ".isApprox(" #Vb ") failed " \ |
||
24 |
"[\n" << (Va) << "\n!=\n" << (Vb) << "\n]") |
||
25 |
|||
26 |
using namespace pinocchio; |
||
27 |
|||
28 |
#define VERBOSE false |
||
29 |
#define IFVERBOSE if(VERBOSE) |
||
30 |
|||
31 |
namespace pinocchio { |
||
32 |
template<typename Derived> |
||
33 |
std::ostream& operator<< (std::ostream& os, const LieGroupBase<Derived>& lg) |
||
34 |
{ |
||
35 |
return os << lg.name(); |
||
36 |
} |
||
37 |
template<typename LieGroupCollection> |
||
38 |
std::ostream& operator<< (std::ostream& os, const LieGroupGenericTpl<LieGroupCollection>& lg) |
||
39 |
{ |
||
40 |
return os << lg.name(); |
||
41 |
} |
||
42 |
} // namespace pinocchio |
||
43 |
|||
44 |
template <typename T> |
||
45 |
28640 |
void test_lie_group_methods (T & jmodel, typename T::JointDataDerived &) |
|
46 |
{ |
||
47 |
typedef typename LieGroup<T>::type LieGroupType; |
||
48 |
typedef double Scalar; |
||
49 |
|||
50 |
28640 |
const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision(); |
|
51 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
28640 |
BOOST_TEST_MESSAGE ("Testing Joint over " << jmodel.shortname()); |
52 |
typedef typename T::ConfigVector_t ConfigVector_t; |
||
53 |
typedef typename T::TangentVector_t TangentVector_t; |
||
54 |
|||
55 |
✓✗✓✗ ✓✗ |
28640 |
ConfigVector_t q1(ConfigVector_t::Random (jmodel.nq())); |
56 |
✓✗✓✗ ✓✗ |
28640 |
TangentVector_t q1_dot(TangentVector_t::Random (jmodel.nv())); |
57 |
✓✗✓✗ ✓✗ |
28640 |
ConfigVector_t q2(ConfigVector_t::Random (jmodel.nq())); |
58 |
|||
59 |
✓✓✓✗ ✓✗✓✗ ✓✗✗✗ |
28640 |
static ConfigVector_t Ones(ConfigVector_t::Ones(jmodel.nq())); |
60 |
28640 |
const Scalar u = 0.3; |
|
61 |
// pinocchio::Inertia::Matrix6 Ia(pinocchio::Inertia::Random().matrix()); |
||
62 |
// bool update_I = false; |
||
63 |
|||
64 |
✓✗✓✗ ✓✗ |
28640 |
q1 = LieGroupType().randomConfiguration(-Ones, Ones); |
65 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
28640 |
BOOST_CHECK(LieGroupType().isNormalized(q1)); |
66 |
|||
67 |
✓✗ | 28640 |
typename T::JointDataDerived jdata = jmodel.createData(); |
68 |
|||
69 |
// Check integrate |
||
70 |
✓✗ | 28640 |
jmodel.calc(jdata, q1, q1_dot); |
71 |
✓✗ | 28640 |
SE3 M1 = jdata.M; |
72 |
✓✗ | 28640 |
Motion v1(jdata.v); |
73 |
|||
74 |
✓✗✓✗ |
28640 |
q2 = LieGroupType().integrate(q1,q1_dot); |
75 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
28640 |
BOOST_CHECK(LieGroupType().isNormalized(q2)); |
76 |
✓✗ | 28640 |
jmodel.calc(jdata,q2); |
77 |
✓✗ | 28640 |
SE3 M2 = jdata.M; |
78 |
|||
79 |
28640 |
double tol_test = 1e2; |
|
80 |
✓✗✓✓ |
28640 |
if(jmodel.shortname() == "JointModelPlanar") |
81 |
2040 |
tol_test = 5e4; |
|
82 |
|||
83 |
✓✗✓✗ |
28640 |
const SE3 M2_exp = M1*exp6(v1); |
84 |
|||
85 |
✓✗✓✓ |
28640 |
if(jmodel.shortname() != "JointModelSphericalZYX") |
86 |
{ |
||
87 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
26600 |
BOOST_CHECK_MESSAGE(M2.isApprox(M2_exp), std::string("Error when integrating1 " + jmodel.shortname())); |
88 |
} |
||
89 |
|||
90 |
// Check integrate when the same vector is passed as input and output |
||
91 |
✓✗✓✗ ✓✗ |
28640 |
ConfigVector_t qTest(ConfigVector_t::Random (jmodel.nq())); |
92 |
✓✗✓✗ ✓✗ |
28640 |
TangentVector_t qTest_dot(TangentVector_t::Random (jmodel.nv())); |
93 |
✓✗✓✗ ✓✗ |
28640 |
ConfigVector_t qResult(ConfigVector_t::Random (jmodel.nq())); |
94 |
✓✗✓✗ ✓✗ |
28640 |
qTest = LieGroupType().randomConfiguration(-Ones, Ones); |
95 |
✓✗✓✗ |
28640 |
qResult = LieGroupType().integrate(qTest, qTest_dot); |
96 |
✓✗✓✗ |
28640 |
LieGroupType().integrate(qTest, qTest_dot, qTest); |
97 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
28640 |
BOOST_CHECK_MESSAGE(LieGroupType().isNormalized(qTest), |
98 |
std::string("Normalization error when integrating with same input and output " + jmodel.shortname())); |
||
99 |
|||
100 |
✓✗✓✗ |
28640 |
SE3 MTest, MResult; |
101 |
{ |
||
102 |
✓✗ | 28640 |
typename T::JointDataDerived jdata = jmodel.createData(); |
103 |
✓✗ | 28640 |
jmodel.calc(jdata, qTest); |
104 |
✓✗ | 28640 |
MTest = jdata.M; |
105 |
} |
||
106 |
{ |
||
107 |
✓✗ | 28640 |
typename T::JointDataDerived jdata = jmodel.createData(); |
108 |
✓✗ | 28640 |
jmodel.calc(jdata, qResult); |
109 |
✓✗ | 28640 |
MResult = jdata.M; |
110 |
} |
||
111 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
28640 |
BOOST_CHECK_MESSAGE(MTest.isApprox(MResult), |
112 |
std::string("Inconsistent value when integrating with same input and output " + jmodel.shortname())); |
||
113 |
|||
114 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
28640 |
BOOST_CHECK_MESSAGE(qTest.isApprox(qResult,prec), |
115 |
std::string("Inconsistent value when integrating with same input and output " + jmodel.shortname())); |
||
116 |
|||
117 |
// Check the reversability of integrate |
||
118 |
✓✗✓✗ ✓✗ |
28640 |
ConfigVector_t q3 = LieGroupType().integrate(q2,-q1_dot); |
119 |
✓✗ | 28640 |
jmodel.calc(jdata,q3); |
120 |
✓✗ | 28640 |
SE3 M3 = jdata.M; |
121 |
|||
122 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
28640 |
BOOST_CHECK_MESSAGE(M3.isApprox(M1), std::string("Error when integrating back " + jmodel.shortname())); |
123 |
|||
124 |
// Check interpolate |
||
125 |
✓✗✓✗ |
28640 |
ConfigVector_t q_interpolate = LieGroupType().interpolate(q1,q2,0.); |
126 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
28640 |
BOOST_CHECK_MESSAGE(q_interpolate.isApprox(q1), std::string("Error when interpolating " + jmodel.shortname())); |
127 |
|||
128 |
✓✗✓✗ |
28640 |
q_interpolate = LieGroupType().interpolate(q1,q2,1.); |
129 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
28640 |
BOOST_CHECK_MESSAGE(q_interpolate.isApprox(q2,tol_test*prec), std::string("Error when interpolating " + jmodel.shortname())); |
130 |
|||
131 |
✓✗✓✓ |
28640 |
if(jmodel.shortname() != "JointModelSphericalZYX") |
132 |
{ |
||
133 |
✓✗✓✗ |
26600 |
q_interpolate = LieGroupType().interpolate(q1,q2,u); |
134 |
✓✗ | 26600 |
jmodel.calc(jdata,q_interpolate); |
135 |
✓✗ | 26600 |
SE3 M_interpolate = jdata.M; |
136 |
|||
137 |
✓✗✓✗ ✓✗ |
26600 |
SE3 M_interpolate_expected = M1*exp6(u*v1); |
138 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
26600 |
BOOST_CHECK_MESSAGE(M_interpolate_expected.isApprox(M_interpolate,1e4*prec), std::string("Error when interpolating " + jmodel.shortname())); |
139 |
} |
||
140 |
|||
141 |
// Check that difference between two equal configuration is exactly 0 |
||
142 |
✓✗✓✗ |
28640 |
TangentVector_t zero = LieGroupType().difference(q1,q1); |
143 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
28640 |
BOOST_CHECK_MESSAGE (zero.isZero(), std::string ("Error: difference between two equal configurations is not 0.")); |
144 |
✓✗✓✗ |
28640 |
zero = LieGroupType().difference(q2,q2); |
145 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
28640 |
BOOST_CHECK_MESSAGE (zero.isZero(), std::string ("Error: difference between two equal configurations is not 0.")); |
146 |
|||
147 |
// Check difference |
||
148 |
// TODO(jcarpent): check the increase of tolerance. |
||
149 |
|||
150 |
|||
151 |
✓✗✓✗ |
28640 |
TangentVector_t vdiff = LieGroupType().difference(q1,q2); |
152 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
28640 |
BOOST_CHECK_MESSAGE(vdiff.isApprox(q1_dot,tol_test*prec), std::string("Error when differentiating " + jmodel.shortname())); |
153 |
|||
154 |
// Check distance |
||
155 |
✓✗✓✗ |
28640 |
Scalar dist = LieGroupType().distance(q1,q2); |
156 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
28640 |
BOOST_CHECK_MESSAGE(dist > 0., "distance - wrong results"); |
157 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
28640 |
BOOST_CHECK_SMALL(math::fabs(dist-q1_dot.norm()), tol_test*prec); |
158 |
|||
159 |
✓✗ | 57280 |
std::string error_prefix("LieGroup"); |
160 |
✓✗✓✗ ✓✗ |
28640 |
error_prefix += " on joint " + jmodel.shortname(); |
161 |
|||
162 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
28640 |
BOOST_CHECK_MESSAGE(jmodel.nq() == LieGroupType::NQ, std::string(error_prefix + " - nq ")); |
163 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
28640 |
BOOST_CHECK_MESSAGE(jmodel.nv() == LieGroupType::NV, std::string(error_prefix + " - nv ")); |
164 |
|||
165 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
28640 |
BOOST_CHECK_MESSAGE |
166 |
(jmodel.nq() == |
||
167 |
LieGroupType().randomConfiguration(-1 * Ones, Ones).size(), |
||
168 |
std::string(error_prefix + " - RandomConfiguration dimensions ")); |
||
169 |
|||
170 |
✓✗✓✗ |
28640 |
ConfigVector_t q_normalize(ConfigVector_t::Random()); |
171 |
✓✗ | 57280 |
Eigen::VectorXd q_normalize_ref(q_normalize); |
172 |
✓✗✓✓ |
28640 |
if(jmodel.shortname() == "JointModelSpherical") |
173 |
{ |
||
174 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2040 |
BOOST_CHECK_MESSAGE(!LieGroupType().isNormalized(q_normalize_ref), std::string(error_prefix + " - !isNormalized ")); |
175 |
✓✗✓✗ |
2040 |
q_normalize_ref /= q_normalize_ref.norm(); |
176 |
} |
||
177 |
✓✗✓✓ |
26600 |
else if(jmodel.shortname() == "JointModelFreeFlyer") |
178 |
{ |
||
179 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2040 |
BOOST_CHECK_MESSAGE(!LieGroupType().isNormalized(q_normalize_ref), std::string(error_prefix + " - !isNormalized ")); |
180 |
✓✗✓✗ ✓✗✓✗ |
2040 |
q_normalize_ref.template tail<4>() /= q_normalize_ref.template tail<4>().norm(); |
181 |
} |
||
182 |
✓✗✓✗ ✓✓ |
24560 |
else if(boost::algorithm::istarts_with(jmodel.shortname(),"JointModelRUB")) |
183 |
{ |
||
184 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
6120 |
BOOST_CHECK_MESSAGE(!LieGroupType().isNormalized(q_normalize_ref), std::string(error_prefix + " - !isNormalized ")); |
185 |
✓✗✓✗ |
6120 |
q_normalize_ref /= q_normalize_ref.norm(); |
186 |
} |
||
187 |
✓✗✓✓ |
18440 |
else if(jmodel.shortname() == "JointModelPlanar") |
188 |
{ |
||
189 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2040 |
BOOST_CHECK_MESSAGE(!LieGroupType().isNormalized(q_normalize_ref), std::string(error_prefix + " - !isNormalized ")); |
190 |
✓✗✓✗ ✓✗✓✗ |
2040 |
q_normalize_ref.template tail<2>() /= q_normalize_ref.template tail<2>().norm(); |
191 |
} |
||
192 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
28640 |
BOOST_CHECK_MESSAGE(LieGroupType().isNormalized(q_normalize_ref), std::string(error_prefix + " - isNormalized ")); |
193 |
✓✗✓✗ |
28640 |
LieGroupType().normalize(q_normalize); |
194 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
28640 |
BOOST_CHECK_MESSAGE(q_normalize.isApprox(q_normalize_ref), std::string(error_prefix + " - normalize ")); |
195 |
28640 |
} |
|
196 |
|||
197 |
struct TestJoint{ |
||
198 |
|||
199 |
template <typename T> |
||
200 |
560 |
void operator()(const T ) const |
|
201 |
{ |
||
202 |
✓✗ | 560 |
T jmodel; |
203 |
✓✗ | 560 |
jmodel.setIndexes(0,0,0); |
204 |
✓✗ | 560 |
typename T::JointDataDerived jdata = jmodel.createData(); |
205 |
|||
206 |
✓✓ | 29120 |
for (int i = 0; i <= 50; ++i) |
207 |
{ |
||
208 |
✓✗ | 28560 |
test_lie_group_methods(jmodel, jdata); |
209 |
} |
||
210 |
560 |
} |
|
211 |
|||
212 |
20 |
void operator()(const pinocchio::JointModelRevoluteUnaligned & ) const |
|
213 |
{ |
||
214 |
✓✗ | 20 |
pinocchio::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); |
215 |
✓✗ | 20 |
jmodel.setIndexes(0,0,0); |
216 |
✓✗ | 20 |
pinocchio::JointModelRevoluteUnaligned::JointDataDerived jdata = jmodel.createData(); |
217 |
|||
218 |
✓✗ | 20 |
test_lie_group_methods(jmodel, jdata); |
219 |
20 |
} |
|
220 |
|||
221 |
20 |
void operator()(const pinocchio::JointModelPrismaticUnaligned & ) const |
|
222 |
{ |
||
223 |
✓✗ | 20 |
pinocchio::JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); |
224 |
✓✗ | 20 |
jmodel.setIndexes(0,0,0); |
225 |
✓✗ | 20 |
pinocchio::JointModelPrismaticUnaligned::JointDataDerived jdata = jmodel.createData(); |
226 |
|||
227 |
✓✗ | 20 |
test_lie_group_methods(jmodel, jdata); |
228 |
20 |
} |
|
229 |
|||
230 |
}; |
||
231 |
|||
232 |
struct LieGroup_Jdifference{ |
||
233 |
template <typename T> |
||
234 |
320 |
void operator()(const T ) const |
|
235 |
{ |
||
236 |
typedef typename T::ConfigVector_t ConfigVector_t; |
||
237 |
typedef typename T::TangentVector_t TangentVector_t; |
||
238 |
typedef typename T::JacobianMatrix_t JacobianMatrix_t; |
||
239 |
typedef typename T::Scalar Scalar; |
||
240 |
|||
241 |
✓✗ | 320 |
T lg; |
242 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
320 |
BOOST_TEST_MESSAGE (lg.name()); |
243 |
✓✓✓✗ ✓✓✓✗ |
1600 |
ConfigVector_t q[2], q_dv[2]; |
244 |
✓✗ | 320 |
q[0] = lg.random(); |
245 |
✓✗ | 320 |
q[1] = lg.random(); |
246 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
247 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
248 |
✓✗✓✗ ✓✗ |
320 |
TangentVector_t va, vb, dv; |
249 |
✓✓✓✗ |
960 |
JacobianMatrix_t J[2]; |
250 |
✓✗ | 320 |
dv.setZero(); |
251 |
|||
252 |
✓✗ | 320 |
lg.difference (q[0], q[1], va); |
253 |
✓✗ | 320 |
lg.template dDifference<ARG0> (q[0], q[1], J[0]); |
254 |
✓✗ | 320 |
lg.template dDifference<ARG1> (q[0], q[1], J[1]); |
255 |
|||
256 |
320 |
const Scalar eps = 1e-6; |
|
257 |
✓✓ | 960 |
for (int k = 0; k < 2; ++k) { |
258 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
640 |
BOOST_TEST_MESSAGE ("Checking J" << k << '\n' << J[k]); |
259 |
✓✗ | 640 |
q_dv[0] = q[0]; |
260 |
✓✗ | 640 |
q_dv[1] = q[1]; |
261 |
// Check J[k] |
||
262 |
✓✗✓✓ |
2640 |
for (int i = 0; i < dv.size(); ++i) |
263 |
{ |
||
264 |
✓✗ | 2000 |
dv[i] = eps; |
265 |
✓✗ | 2000 |
lg.integrate (q[k], dv, q_dv[k]); |
266 |
✓✗ | 2000 |
lg.difference (q_dv[0], q_dv[1], vb); |
267 |
|||
268 |
// vb - va ~ J[k] * dv |
||
269 |
✓✗✓✗ |
2000 |
TangentVector_t J_dv = J[k].col(i); |
270 |
✓✗✓✗ ✓✗ |
2000 |
TangentVector_t vb_va = (vb - va) / eps; |
271 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2000 |
EIGEN_VECTOR_IS_APPROX (vb_va, J_dv, 1e-2); |
272 |
✓✗ | 2000 |
dv[i] = 0; |
273 |
} |
||
274 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
275 |
} |
||
276 |
|||
277 |
✓✗✓✗ |
320 |
specificTests(lg); |
278 |
320 |
} |
|
279 |
|||
280 |
template <typename T> |
||
281 |
240 |
void specificTests(const T ) const |
|
282 |
240 |
{} |
|
283 |
|||
284 |
template <typename Scalar, int Options> |
||
285 |
20 |
void specificTests(const SpecialEuclideanOperationTpl<3,Scalar,Options>) const |
|
286 |
{ |
||
287 |
typedef SE3Tpl<Scalar> SE3; |
||
288 |
typedef SpecialEuclideanOperationTpl<3,Scalar,Options> LG_t; |
||
289 |
typedef typename LG_t::ConfigVector_t ConfigVector_t; |
||
290 |
typedef typename LG_t::JacobianMatrix_t JacobianMatrix_t; |
||
291 |
typedef typename LG_t::ConstQuaternionMap_t ConstQuaternionMap_t; |
||
292 |
|||
293 |
✓✗ | 20 |
LG_t lg; |
294 |
|||
295 |
✓✓✓✗ |
60 |
ConfigVector_t q[2]; |
296 |
✓✗ | 20 |
q[0] = lg.random(); |
297 |
✓✗ | 20 |
q[1] = lg.random(); |
298 |
|||
299 |
✓✗✓✗ ✓✗✓✗ |
20 |
ConstQuaternionMap_t quat0(q[0].template tail<4>().data()), quat1(q[1].template tail<4>().data()); |
300 |
✓✓✓✗ |
60 |
JacobianMatrix_t J[2]; |
301 |
|||
302 |
✓✗ | 20 |
lg.template dDifference<ARG0> (q[0], q[1], J[0]); |
303 |
✓✗ | 20 |
lg.template dDifference<ARG1> (q[0], q[1], J[1]); |
304 |
|||
305 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
20 |
SE3 om0 (typename SE3::Quaternion (q[0].template tail<4>()).matrix(), q[0].template head<3>()), |
306 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
20 |
om1 (typename SE3::Quaternion (q[1].template tail<4>()).matrix(), q[1].template head<3>()), |
307 |
✓✗ | 20 |
_1m2 (om1.actInv (om0)) ; |
308 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
EIGEN_MATRIX_IS_APPROX (J[1] * _1m2.toActionMatrix(), - J[0], 1e-8); |
309 |
|||
310 |
// Test against SE3::Interpolate |
||
311 |
20 |
const Scalar u = 0.3; |
|
312 |
✓✗ | 20 |
ConfigVector_t q_interp = lg.interpolate(q[0],q[1],u); |
313 |
✓✗✓✗ |
20 |
ConstQuaternionMap_t quat_interp(q_interp.template tail<4>().data()); |
314 |
|||
315 |
✓✗✓✗ |
20 |
SE3 M0(quat0,q[0].template head<3>()); |
316 |
✓✗✓✗ |
20 |
SE3 M1(quat1,q[1].template head<3>()); |
317 |
|||
318 |
✓✗ | 20 |
SE3 M_u = SE3::Interpolate(M0,M1,u); |
319 |
✓✗✓✗ |
20 |
SE3 M_interp(quat_interp,q_interp.template head<3>()); |
320 |
|||
321 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK(M_u.isApprox(M_interp)); |
322 |
20 |
} |
|
323 |
|||
324 |
template <typename Scalar, int Options> |
||
325 |
20 |
void specificTests(const CartesianProductOperation< |
|
326 |
VectorSpaceOperationTpl<3,Scalar,Options>, |
||
327 |
SpecialOrthogonalOperationTpl<3,Scalar,Options> |
||
328 |
>) const |
||
329 |
{ |
||
330 |
typedef SE3Tpl<Scalar> SE3; |
||
331 |
typedef CartesianProductOperation< |
||
332 |
VectorSpaceOperationTpl<3,Scalar,Options>, |
||
333 |
SpecialOrthogonalOperationTpl<3,Scalar,Options> |
||
334 |
> LG_t; |
||
335 |
typedef typename LG_t::ConfigVector_t ConfigVector_t; |
||
336 |
typedef typename LG_t::JacobianMatrix_t JacobianMatrix_t; |
||
337 |
|||
338 |
✓✗ | 20 |
LG_t lg; |
339 |
|||
340 |
✓✓✓✗ |
60 |
ConfigVector_t q[2]; |
341 |
✓✗ | 20 |
q[0] = lg.random(); |
342 |
✓✗ | 20 |
q[1] = lg.random(); |
343 |
✓✓✓✗ |
60 |
JacobianMatrix_t J[2]; |
344 |
|||
345 |
✓✗ | 20 |
lg.template dDifference<ARG0> (q[0], q[1], J[0]); |
346 |
✓✗ | 20 |
lg.template dDifference<ARG1> (q[0], q[1], J[1]); |
347 |
|||
348 |
typename SE3::Matrix3 |
||
349 |
✓✗✓✗ ✓✗ |
20 |
oR0 (typename SE3::Quaternion (q[0].template tail<4>()).matrix()), |
350 |
✓✗✓✗ ✓✗ |
20 |
oR1 (typename SE3::Quaternion (q[1].template tail<4>()).matrix()); |
351 |
✓✗✓✗ |
20 |
JacobianMatrix_t X (JacobianMatrix_t::Identity()); |
352 |
✓✗✓✗ ✓✗✓✗ |
20 |
X.template bottomRightCorner<3,3>() = oR1.transpose() * oR0; |
353 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
EIGEN_MATRIX_IS_APPROX (J[1] * X, - J[0], 1e-8); |
354 |
20 |
} |
|
355 |
}; |
||
356 |
|||
357 |
template<bool around_identity> |
||
358 |
struct LieGroup_Jintegrate{ |
||
359 |
template <typename T> |
||
360 |
336 |
void operator()(const T ) const |
|
361 |
{ |
||
362 |
typedef typename T::ConfigVector_t ConfigVector_t; |
||
363 |
typedef typename T::TangentVector_t TangentVector_t; |
||
364 |
typedef typename T::JacobianMatrix_t JacobianMatrix_t; |
||
365 |
typedef typename T::Scalar Scalar; |
||
366 |
|||
367 |
✓✗ | 336 |
T lg; |
368 |
✓✗ | 336 |
ConfigVector_t q = lg.random(); |
369 |
✓✗✓✗ ✓✗ |
336 |
TangentVector_t v, dq, dv; |
370 |
if(around_identity) |
||
371 |
✓✗ | 16 |
v.setZero(); |
372 |
else |
||
373 |
✓✗ | 320 |
v.setRandom(); |
374 |
|||
375 |
✓✗ | 336 |
dq.setZero(); |
376 |
✓✗ | 336 |
dv.setZero(); |
377 |
|||
378 |
✓✗ | 336 |
ConfigVector_t q_v = lg.integrate (q, v); |
379 |
|||
380 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
381 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
382 |
✓✗✓✗ |
336 |
JacobianMatrix_t Jq, Jv; |
383 |
✓✗ | 336 |
lg.dIntegrate_dq (q, v, Jq); |
384 |
✓✗ | 336 |
lg.dIntegrate_dv (q, v, Jv); |
385 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
386 |
|||
387 |
336 |
const Scalar eps = 1e-6; |
|
388 |
✓✗✓✓ |
1386 |
for (int i = 0; i < v.size(); ++i) |
389 |
{ |
||
390 |
✓✗✓✗ |
1050 |
dq[i] = dv[i] = eps; |
391 |
✓✗ | 1050 |
ConfigVector_t q_dq = lg.integrate (q, dq); |
392 |
|||
393 |
✓✗ | 1050 |
ConfigVector_t q_dq_v = lg.integrate (q_dq, v); |
394 |
✓✗✓✗ |
1050 |
TangentVector_t Jq_dq = Jq.col(i); |
395 |
// q_dv_v - q_v ~ Jq dv |
||
396 |
✓✗✓✗ ✓✗ |
1050 |
TangentVector_t dI_dq = lg.difference (q_v, q_dq_v) / eps; |
397 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
1050 |
EIGEN_VECTOR_IS_APPROX (dI_dq, Jq_dq, 1e-2); |
398 |
|||
399 |
✓✗✓✗ ✓✗ |
1050 |
ConfigVector_t q_v_dv = lg.integrate (q, (v+dv).eval()); |
400 |
✓✗✓✗ |
1050 |
TangentVector_t Jv_dv = Jv.col(i); |
401 |
// q_v_dv - q_v ~ Jv dv |
||
402 |
✓✗✓✗ ✓✗ |
1050 |
TangentVector_t dI_dv = lg.difference (q_v, q_v_dv) / eps; |
403 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
1050 |
EIGEN_VECTOR_IS_APPROX (dI_dv, Jv_dv, 1e-2); |
404 |
|||
405 |
✓✗✓✗ |
1050 |
dq[i] = dv[i] = 0; |
406 |
} |
||
407 |
336 |
} |
|
408 |
}; |
||
409 |
|||
410 |
struct LieGroup_JintegrateJdifference{ |
||
411 |
template <typename T> |
||
412 |
320 |
void operator()(const T ) const |
|
413 |
{ |
||
414 |
typedef typename T::ConfigVector_t ConfigVector_t; |
||
415 |
typedef typename T::TangentVector_t TangentVector_t; |
||
416 |
typedef typename T::JacobianMatrix_t JacobianMatrix_t; |
||
417 |
|||
418 |
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
||
419 |
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
||
420 |
✓✗ | 320 |
T lg; |
421 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
320 |
BOOST_TEST_MESSAGE (lg.name()); |
422 |
✓✗✓✓ ✗✓✗ |
320 |
ConfigVector_t qa, qb (lg.nq()); |
423 |
✓✗ | 320 |
qa = lg.random(); |
424 |
✓✓✗✓ ✗ |
320 |
TangentVector_t v (lg.nv()); |
425 |
✓✗ | 320 |
v.setRandom (); |
426 |
✓✗ | 320 |
lg.integrate(qa, v, qb); |
427 |
|||
428 |
✓✗✓✗ |
320 |
JacobianMatrix_t Jd_qb, Ji_v; |
429 |
|||
430 |
✓✗ | 320 |
lg.template dDifference<ARG1> (qa, qb, Jd_qb); |
431 |
✓✗ | 320 |
lg.template dIntegrate <ARG1> (qa, v , Ji_v ); |
432 |
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
||
433 |
|||
434 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
320 |
BOOST_CHECK_MESSAGE ((Jd_qb * Ji_v).isIdentity(), |
435 |
"Jd_qb\n" << |
||
436 |
Jd_qb << '\n' << |
||
437 |
"* Ji_v\n" << |
||
438 |
Ji_v << '\n' << |
||
439 |
"!= Identity\n" << |
||
440 |
Jd_qb * Ji_v << '\n'); |
||
441 |
320 |
} |
|
442 |
}; |
||
443 |
|||
444 |
struct LieGroup_JintegrateCoeffWise |
||
445 |
{ |
||
446 |
template <typename T> |
||
447 |
320 |
void operator()(const T ) const |
|
448 |
{ |
||
449 |
typedef typename T::ConfigVector_t ConfigVector_t; |
||
450 |
typedef typename T::TangentVector_t TangentVector_t; |
||
451 |
typedef typename T::Scalar Scalar; |
||
452 |
|||
453 |
✓✗ | 320 |
T lg; |
454 |
✓✗ | 320 |
ConfigVector_t q = lg.random(); |
455 |
✓✓✗✓ ✓✗✓✗ |
320 |
TangentVector_t dv(TangentVector_t::Zero(lg.nv())); |
456 |
|||
457 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
320 |
BOOST_TEST_MESSAGE (lg.name()); |
458 |
typedef Eigen::Matrix<Scalar,T::NQ,T::NV> JacobianCoeffs; |
||
459 |
✓✗✓✓ ✗✓✓✗ ✓✗ |
320 |
JacobianCoeffs Jintegrate(JacobianCoeffs::Zero(lg.nq(),lg.nv())); |
460 |
✓✗ | 320 |
lg.integrateCoeffWiseJacobian(q,Jintegrate); |
461 |
✓✗✓✓ ✗✓✓✗ ✓✗ |
320 |
JacobianCoeffs Jintegrate_fd(JacobianCoeffs::Zero(lg.nq(),lg.nv())); |
462 |
|||
463 |
320 |
const Scalar eps = 1e-8; |
|
464 |
✓✓✓✓ |
1320 |
for (int i = 0; i < lg.nv(); ++i) |
465 |
{ |
||
466 |
✓✗ | 1000 |
dv[i] = eps; |
467 |
✓✓✗✓ ✓✗✓✗ |
1000 |
ConfigVector_t q_next(ConfigVector_t::Zero(lg.nq())); |
468 |
✓✗ | 1000 |
lg.integrate(q, dv,q_next); |
469 |
✓✗✓✗ ✓✗✓✗ |
1000 |
Jintegrate_fd.col(i) = (q_next - q)/eps; |
470 |
|||
471 |
✓✗ | 1000 |
dv[i] = 0; |
472 |
} |
||
473 |
|||
474 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
320 |
EIGEN_MATRIX_IS_APPROX(Jintegrate, Jintegrate_fd, sqrt(eps)); |
475 |
320 |
} |
|
476 |
}; |
||
477 |
|||
478 |
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) |
||
479 |
|||
480 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_all ) |
481 |
{ |
||
482 |
typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned |
||
483 |
, JointModelSpherical, JointModelSphericalZYX |
||
484 |
, JointModelPX, JointModelPY, JointModelPZ |
||
485 |
, JointModelPrismaticUnaligned |
||
486 |
, JointModelFreeFlyer |
||
487 |
, JointModelPlanar |
||
488 |
, JointModelTranslation |
||
489 |
, JointModelRUBX, JointModelRUBY, JointModelRUBZ |
||
490 |
> Variant; |
||
491 |
✓✓ | 42 |
for (int i = 0; i < 20; ++i) |
492 |
✓✗ | 40 |
boost::mpl::for_each<Variant::types>(TestJoint()); |
493 |
|||
494 |
// FIXME JointModelComposite does not work. |
||
495 |
// boost::mpl::for_each<JointModelVariant::types>(TestJoint()); |
||
496 |
|||
497 |
2 |
} |
|
498 |
|||
499 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( Jdifference ) |
500 |
{ |
||
501 |
typedef double Scalar; |
||
502 |
enum { Options = 0 }; |
||
503 |
|||
504 |
typedef boost::mpl::vector< VectorSpaceOperationTpl<1,Scalar,Options> |
||
505 |
, VectorSpaceOperationTpl<2,Scalar,Options> |
||
506 |
, SpecialOrthogonalOperationTpl<2,Scalar,Options> |
||
507 |
, SpecialOrthogonalOperationTpl<3,Scalar,Options> |
||
508 |
, SpecialEuclideanOperationTpl<2,Scalar,Options> |
||
509 |
, SpecialEuclideanOperationTpl<3,Scalar,Options> |
||
510 |
, CartesianProductOperation< |
||
511 |
VectorSpaceOperationTpl<2,Scalar,Options>, |
||
512 |
SpecialOrthogonalOperationTpl<2,Scalar,Options> |
||
513 |
> |
||
514 |
, CartesianProductOperation< |
||
515 |
VectorSpaceOperationTpl<3,Scalar,Options>, |
||
516 |
SpecialOrthogonalOperationTpl<3,Scalar,Options> |
||
517 |
> |
||
518 |
> Types; |
||
519 |
✓✓ | 42 |
for (int i = 0; i < 20; ++i) |
520 |
✓✗ | 40 |
boost::mpl::for_each<Types>(LieGroup_Jdifference()); |
521 |
2 |
} |
|
522 |
|||
523 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( Jintegrate ) |
524 |
{ |
||
525 |
typedef double Scalar; |
||
526 |
enum { Options = 0 }; |
||
527 |
|||
528 |
typedef boost::mpl::vector< VectorSpaceOperationTpl<1,Scalar,Options> |
||
529 |
, VectorSpaceOperationTpl<2,Scalar,Options> |
||
530 |
, SpecialOrthogonalOperationTpl<2,Scalar,Options> |
||
531 |
, SpecialOrthogonalOperationTpl<3,Scalar,Options> |
||
532 |
, SpecialEuclideanOperationTpl<2,Scalar,Options> |
||
533 |
, SpecialEuclideanOperationTpl<3,Scalar,Options> |
||
534 |
, CartesianProductOperation< |
||
535 |
VectorSpaceOperationTpl<2,Scalar,Options>, |
||
536 |
SpecialOrthogonalOperationTpl<2,Scalar,Options> |
||
537 |
> |
||
538 |
, CartesianProductOperation< |
||
539 |
VectorSpaceOperationTpl<3,Scalar,Options>, |
||
540 |
SpecialOrthogonalOperationTpl<3,Scalar,Options> |
||
541 |
> |
||
542 |
> Types; |
||
543 |
✓✓ | 42 |
for (int i = 0; i < 20; ++i) |
544 |
✓✗ | 40 |
boost::mpl::for_each<Types>(LieGroup_Jintegrate<false>()); |
545 |
|||
546 |
// Around identity |
||
547 |
✓✗ | 2 |
boost::mpl::for_each<Types>(LieGroup_Jintegrate<true>()); |
548 |
2 |
} |
|
549 |
|||
550 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( Jintegrate_Jdifference ) |
551 |
{ |
||
552 |
typedef double Scalar; |
||
553 |
enum { Options = 0 }; |
||
554 |
|||
555 |
typedef boost::mpl::vector< VectorSpaceOperationTpl<1,Scalar,Options> |
||
556 |
, VectorSpaceOperationTpl<2,Scalar,Options> |
||
557 |
, SpecialOrthogonalOperationTpl<2,Scalar,Options> |
||
558 |
, SpecialOrthogonalOperationTpl<3,Scalar,Options> |
||
559 |
, SpecialEuclideanOperationTpl<2,Scalar,Options> |
||
560 |
, SpecialEuclideanOperationTpl<3,Scalar,Options> |
||
561 |
, CartesianProductOperation< |
||
562 |
VectorSpaceOperationTpl<2,Scalar,Options>, |
||
563 |
SpecialOrthogonalOperationTpl<2,Scalar,Options> |
||
564 |
> |
||
565 |
, CartesianProductOperation< |
||
566 |
VectorSpaceOperationTpl<3,Scalar,Options>, |
||
567 |
SpecialOrthogonalOperationTpl<3,Scalar,Options> |
||
568 |
> |
||
569 |
> Types; |
||
570 |
✓✓ | 42 |
for (int i = 0; i < 20; ++i) |
571 |
✓✗ | 40 |
boost::mpl::for_each<Types>(LieGroup_JintegrateJdifference()); |
572 |
2 |
} |
|
573 |
|||
574 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(JintegrateCoeffWise) |
575 |
{ |
||
576 |
typedef double Scalar; |
||
577 |
enum { Options = 0 }; |
||
578 |
|||
579 |
typedef boost::mpl::vector< VectorSpaceOperationTpl<1,Scalar,Options> |
||
580 |
, VectorSpaceOperationTpl<2,Scalar,Options> |
||
581 |
, SpecialOrthogonalOperationTpl<2,Scalar,Options> |
||
582 |
, SpecialOrthogonalOperationTpl<3,Scalar,Options> |
||
583 |
, SpecialEuclideanOperationTpl<2,Scalar,Options> |
||
584 |
, SpecialEuclideanOperationTpl<3,Scalar,Options> |
||
585 |
, CartesianProductOperation< |
||
586 |
VectorSpaceOperationTpl<2,Scalar,Options>, |
||
587 |
SpecialOrthogonalOperationTpl<2,Scalar,Options> |
||
588 |
> |
||
589 |
, CartesianProductOperation< |
||
590 |
VectorSpaceOperationTpl<3,Scalar,Options>, |
||
591 |
SpecialOrthogonalOperationTpl<3,Scalar,Options> |
||
592 |
> |
||
593 |
> Types; |
||
594 |
✓✓ | 42 |
for (int i = 0; i < 20; ++i) |
595 |
✓✗ | 40 |
boost::mpl::for_each<Types>(LieGroup_JintegrateCoeffWise()); |
596 |
|||
597 |
{ |
||
598 |
typedef SpecialEuclideanOperationTpl<3,Scalar,Options> LieGroup; |
||
599 |
typedef LieGroup::ConfigVector_t ConfigVector_t; |
||
600 |
✓✗ | 2 |
LieGroup lg; |
601 |
|||
602 |
✓✗ | 2 |
ConfigVector_t q = lg.random(); |
603 |
// TangentVector_t dv(TangentVector_t::Zero(lg.nv())); |
||
604 |
|||
605 |
typedef Eigen::Matrix<Scalar,LieGroup::NQ,LieGroup::NV> JacobianCoeffs; |
||
606 |
✓✗✓✗ |
2 |
JacobianCoeffs Jintegrate(JacobianCoeffs::Zero(lg.nq(),lg.nv())); |
607 |
✓✗ | 2 |
lg.integrateCoeffWiseJacobian(q,Jintegrate); |
608 |
|||
609 |
|||
610 |
|||
611 |
} |
||
612 |
2 |
} |
|
613 |
|||
614 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_vector_space ) |
615 |
{ |
||
616 |
typedef VectorSpaceOperationTpl<3,double> VSO_t; |
||
617 |
✓✗ | 2 |
VSO_t::ConfigVector_t q, |
618 |
✓✗✓✗ |
2 |
lo(VSO_t::ConfigVector_t::Constant(-std::numeric_limits<double>::infinity())), |
619 |
// lo(VSO_t::ConfigVector_t::Constant( 0)), |
||
620 |
// up(VSO_t::ConfigVector_t::Constant( std::numeric_limits<double>::infinity())); |
||
621 |
✓✗✓✗ |
2 |
up(VSO_t::ConfigVector_t::Constant( 0)); |
622 |
|||
623 |
2 |
bool error = false; |
|
624 |
try { |
||
625 |
✓✗✗✓ |
2 |
VSO_t ().randomConfiguration(lo, up, q); |
626 |
2 |
} catch (const std::runtime_error&) { |
|
627 |
2 |
error = true; |
|
628 |
} |
||
629 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_MESSAGE(error, "Random configuration between infinite bounds should return an error"); |
630 |
2 |
} |
|
631 |
|||
632 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_size ) |
633 |
{ |
||
634 |
// R^1: neutral = [0] |
||
635 |
✓✗ | 2 |
VectorSpaceOperationTpl <1,double> vs1; |
636 |
✓✗ | 4 |
Eigen::VectorXd neutral; |
637 |
✓✗ | 2 |
neutral.resize (1); |
638 |
✓✗ | 2 |
neutral.setZero (); |
639 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (vs1.nq () == 1); |
640 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (vs1.nv () == 1); |
641 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (vs1.name () == "R^1"); |
642 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK (vs1.neutral () == neutral); |
643 |
// R^2: neutral = [0, 0] |
||
644 |
✓✗ | 2 |
VectorSpaceOperationTpl <2,double> vs2; |
645 |
✓✗ | 2 |
neutral.resize (2); |
646 |
✓✗ | 2 |
neutral.setZero (); |
647 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (vs2.nq () == 2); |
648 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (vs2.nv () == 2); |
649 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (vs2.name () == "R^2"); |
650 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK (vs2.neutral () == neutral); |
651 |
// R^3: neutral = [0, 0, 0] |
||
652 |
✓✗ | 2 |
VectorSpaceOperationTpl <3,double> vs3; |
653 |
✓✗ | 2 |
neutral.resize (3); |
654 |
✓✗ | 2 |
neutral.setZero (); |
655 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (vs3.nq () == 3); |
656 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (vs3.nv () == 3); |
657 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (vs3.name () == "R^3"); |
658 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK (vs3.neutral () == neutral); |
659 |
// SO(2): neutral = [1, 0] |
||
660 |
✓✗ | 2 |
SpecialOrthogonalOperationTpl<2,double> so2; |
661 |
✓✗✓✗ ✓✗ |
2 |
neutral.resize (2); neutral [0] = 1; neutral [1] = 0; |
662 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK (so2.nq () == 2); |
663 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK (so2.nv () == 1); |
664 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (so2.name () == "SO(2)"); |
665 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK (so2.neutral () == neutral); |
666 |
// SO(3): neutral = [0, 0, 0, 1] |
||
667 |
✓✗ | 2 |
SpecialOrthogonalOperationTpl<3,double> so3; |
668 |
✓✗✓✗ |
2 |
neutral.resize (4); neutral.setZero (); |
669 |
✓✗ | 2 |
neutral [3] = 1; |
670 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK (so3.nq () == 4); |
671 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK (so3.nv () == 3); |
672 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (so3.name () == "SO(3)"); |
673 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK (so3.neutral () == neutral); |
674 |
// SE(2): neutral = [0, 0, 1, 0] |
||
675 |
✓✗ | 2 |
SpecialEuclideanOperationTpl <2,double> se2; |
676 |
✓✗✓✗ |
2 |
neutral.resize (4); neutral.setZero (); |
677 |
✓✗ | 2 |
neutral [2] = 1; |
678 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK (se2.nq () == 4); |
679 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK (se2.nv () == 3); |
680 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (se2.name () == "SE(2)"); |
681 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK (se2.neutral () == neutral); |
682 |
// SE(3): neutral = [0, 0, 0, 0, 0, 0, 1] |
||
683 |
✓✗ | 2 |
SpecialEuclideanOperationTpl <3,double> se3; |
684 |
✓✗✓✗ |
2 |
neutral.resize (7); neutral.setZero (); |
685 |
✓✗ | 2 |
neutral [6] = 1; |
686 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK (se3.nq () == 7); |
687 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK (se3.nv () == 6); |
688 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (se3.name () == "SE(3)"); |
689 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK (se3.neutral () == neutral); |
690 |
// R^2 x SO(2): neutral = [0, 0, 1, 0] |
||
691 |
CartesianProductOperation <VectorSpaceOperationTpl <2,double>, |
||
692 |
✓✗ | 2 |
SpecialOrthogonalOperationTpl <2,double> > r2xso2; |
693 |
✓✗✓✗ |
2 |
neutral.resize (4); neutral.setZero (); |
694 |
✓✗ | 2 |
neutral [2] = 1; |
695 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (r2xso2.nq () == 4); |
696 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (r2xso2.nv () == 3); |
697 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (r2xso2.name () == "R^2*SO(2)"); |
698 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK (r2xso2.neutral () == neutral); |
699 |
// R^3 x SO(3): neutral = [0, 0, 0, 0, 0, 0, 1] |
||
700 |
CartesianProductOperation <VectorSpaceOperationTpl <3,double>, |
||
701 |
✓✗ | 2 |
SpecialOrthogonalOperationTpl <3,double> > r3xso3; |
702 |
✓✗✓✗ |
2 |
neutral.resize (7); neutral.setZero (); |
703 |
✓✗ | 2 |
neutral [6] = 1; |
704 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (r3xso3.nq () == 7); |
705 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (r3xso3.nv () == 6); |
706 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK (r3xso3.name () == "R^3*SO(3)"); |
707 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK (r3xso3.neutral () == neutral); |
708 |
2 |
} |
|
709 |
|||
710 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_dim_computation) |
711 |
{ |
||
712 |
2 |
int dim = eval_set_dim<1,1>::value ; |
|
713 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dim == 2); |
714 |
2 |
dim = eval_set_dim<Eigen::Dynamic,1>::value; |
|
715 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dim == Eigen::Dynamic); |
716 |
2 |
dim = eval_set_dim<1,Eigen::Dynamic>::value; |
|
717 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dim == Eigen::Dynamic); |
718 |
2 |
} |
|
719 |
|||
720 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE (small_distance_test) |
721 |
{ |
||
722 |
✓✗ | 2 |
SpecialOrthogonalOperationTpl <3,double> so3; |
723 |
✓✗ | 4 |
Eigen::VectorXd q1(so3.nq()); |
724 |
✓✗ | 4 |
Eigen::VectorXd q2(so3.nq()); |
725 |
✓✗✓✗ ✓✗✓✗ |
2 |
q1 << 0,0,-0.1953711450011105244,0.9807293794421349169; |
726 |
✓✗✓✗ ✓✗✓✗ |
2 |
q2 << 0,0,-0.19537114500111049664,0.98072937944213492244; |
727 |
|||
728 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_MESSAGE (so3.distance(q1,q2) > 0., |
729 |
"SO3 small distance - wrong results"); |
||
730 |
2 |
} |
|
731 |
|||
732 |
template<typename LieGroupCollection> |
||
733 |
struct TestLieGroupVariantVisitor |
||
734 |
{ |
||
735 |
|||
736 |
typedef LieGroupGenericTpl<LieGroupCollection> LieGroupGeneric; |
||
737 |
typedef typename LieGroupGeneric::ConfigVector_t ConfigVector_t; |
||
738 |
typedef typename LieGroupGeneric::TangentVector_t TangentVector_t; |
||
739 |
|||
740 |
template<typename Derived> |
||
741 |
16 |
void operator() (const LieGroupBase<Derived> & lg) const |
|
742 |
{ |
||
743 |
✓✗ | 32 |
LieGroupGenericTpl<LieGroupCollection> lg_generic(lg.derived()); |
744 |
✓✗ | 16 |
test(lg,lg_generic); |
745 |
16 |
} |
|
746 |
|||
747 |
template<typename Derived> |
||
748 |
16 |
static void test(const LieGroupBase<Derived> & lg, |
|
749 |
const LieGroupGenericTpl<LieGroupCollection> & lg_generic) |
||
750 |
{ |
||
751 |
✓✗✓✗ ✓✗✓✓ ✗✓✓✗ ✓✓✗✓ ✗✗✓✗ ✓ |
16 |
BOOST_CHECK(lg.nq() == nq(lg_generic)); |
752 |
✓✗✓✗ ✓✗✓✓ ✗✓✓✗ ✓✓✗✓ ✗✗✓✗ ✓ |
16 |
BOOST_CHECK(lg.nv() == nv(lg_generic)); |
753 |
|||
754 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
16 |
BOOST_CHECK(lg.name() == name(lg_generic)); |
755 |
|||
756 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓ |
16 |
BOOST_CHECK(lg.neutral() == neutral(lg_generic)); |
757 |
|||
758 |
typedef typename LieGroupGeneric::ConfigVector_t ConfigVectorGeneric; |
||
759 |
typedef typename LieGroupGeneric::TangentVector_t TangentVectorGeneric; |
||
760 |
|||
761 |
✓✗✓✗ |
32 |
ConfigVector_t q0 = lg.random(); |
762 |
✓✓✗✓ ✓✗✓✗ |
32 |
TangentVector_t v = TangentVector_t::Random(lg.nv()); |
763 |
✓✓✗✓ ✗ |
32 |
ConfigVector_t qout_ref(lg.nq()); |
764 |
✓✗ | 16 |
lg.integrate(q0, v, qout_ref); |
765 |
|||
766 |
✓✓✗✓ ✗ |
32 |
ConfigVectorGeneric qout(lg.nq()); |
767 |
✓✗✓✗ ✓✗ |
16 |
integrate(lg_generic, ConfigVectorGeneric(q0), TangentVectorGeneric(v), qout); |
768 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
16 |
BOOST_CHECK(qout.isApprox(qout_ref)); |
769 |
|||
770 |
✓✗✓✗ |
32 |
ConfigVector_t q1 (nq(lg_generic)); |
771 |
✓✗ | 16 |
random (lg_generic, q1); |
772 |
✓✗ | 16 |
difference(lg_generic, q0, q1, v); |
773 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
16 |
BOOST_CHECK_EQUAL(lg.distance(q0, q1), distance (lg_generic, q0, q1)); |
774 |
|||
775 |
✓✗✓✗ |
32 |
ConfigVector_t q2(nq(lg_generic)); |
776 |
✓✗ | 16 |
random(lg_generic, q2); |
777 |
✓✗ | 16 |
normalize(lg_generic, q2); |
778 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
16 |
BOOST_CHECK(isNormalized(lg_generic, q2)); |
779 |
16 |
} |
|
780 |
}; |
||
781 |
|||
782 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_liegroup_variant) |
783 |
{ |
||
784 |
✓✗ | 2 |
boost::mpl::for_each<LieGroupCollectionDefault::LieGroupVariant::types>(TestLieGroupVariantVisitor<LieGroupCollectionDefault>()); |
785 |
2 |
} |
|
786 |
|||
787 |
template<typename Lg1, typename Lg2> |
||
788 |
1 |
void test_liegroup_variant_equal(Lg1 lg1, Lg2 lg2) |
|
789 |
{ |
||
790 |
typedef LieGroupGenericTpl<LieGroupCollectionDefault> LieGroupGeneric; |
||
791 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
1 |
BOOST_CHECK_EQUAL(LieGroupGeneric(lg1), LieGroupGeneric(lg2)); |
792 |
1 |
} |
|
793 |
|||
794 |
template<typename Lg1, typename Lg2> |
||
795 |
1 |
void test_liegroup_variant_not_equal(Lg1 lg1, Lg2 lg2) |
|
796 |
{ |
||
797 |
typedef LieGroupGenericTpl<LieGroupCollectionDefault> LieGroupGeneric; |
||
798 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
1 |
BOOST_CHECK_PREDICATE( std::not_equal_to<LieGroupGeneric>(), |
799 |
(LieGroupGeneric(lg1))(LieGroupGeneric(lg2)) ); |
||
800 |
1 |
} |
|
801 |
|||
802 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_liegroup_variant_comparison) |
803 |
{ |
||
804 |
✓✗ | 2 |
test_liegroup_variant_equal( |
805 |
✓✗ | 2 |
VectorSpaceOperationTpl<1, double>(), |
806 |
2 |
VectorSpaceOperationTpl<Eigen::Dynamic, double>(1)); |
|
807 |
✓✗ | 2 |
test_liegroup_variant_not_equal( |
808 |
✓✗ | 2 |
VectorSpaceOperationTpl<1, double>(), |
809 |
2 |
VectorSpaceOperationTpl<Eigen::Dynamic, double>(2)); |
|
810 |
2 |
} |
|
811 |
|||
812 |
BOOST_AUTO_TEST_SUITE_END () |
Generated by: GCOVR (Version 4.2) |