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

pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:31