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

pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:04