pinocchio  2.7.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
se3-tpl.hpp
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_se3_tpl_hpp__
7 #define __pinocchio_se3_tpl_hpp__
8 
9 #include "pinocchio/spatial/fwd.hpp"
10 #include "pinocchio/spatial/se3-base.hpp"
11 
12 #include "pinocchio/math/quaternion.hpp"
13 #include "pinocchio/math/rotation.hpp"
14 #include "pinocchio/spatial/cartesian-axis.hpp"
15 
16 #include <Eigen/Geometry>
17 
18 namespace pinocchio
19 {
20  template<typename _Scalar, int _Options>
21  struct traits< SE3Tpl<_Scalar,_Options> >
22  {
23  enum {
24  Options = _Options,
25  LINEAR = 0,
26  ANGULAR = 3
27  };
28  typedef _Scalar Scalar;
29  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
30  typedef Eigen::Matrix<Scalar,4,1,Options> Vector4;
31  typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
32  typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
33  typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
34  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
35  typedef Matrix3 AngularType;
36  typedef typename PINOCCHIO_EIGEN_REF_TYPE(Matrix3) AngularRef;
37  typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix3) ConstAngularRef;
38  typedef Vector3 LinearType;
39  typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector3) LinearRef;
40  typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector3) ConstLinearRef;
41  typedef Matrix6 ActionMatrixType;
42  typedef Matrix4 HomogeneousMatrixType;
43  typedef SE3Tpl<Scalar,Options> PlainType;
44  }; // traits SE3Tpl
45 
46  template<typename _Scalar, int _Options>
47  struct SE3Tpl : public SE3Base< SE3Tpl<_Scalar,_Options> >
48  {
49  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
50  PINOCCHIO_SE3_TYPEDEF_TPL(SE3Tpl);
51  typedef SE3Base< SE3Tpl<_Scalar,_Options> > Base;
52  typedef Eigen::Quaternion<Scalar,Options> Quaternion;
53  typedef typename traits<SE3Tpl>::Vector3 Vector3;
54  typedef typename traits<SE3Tpl>::Matrix3 Matrix3;
55  typedef typename traits<SE3Tpl>::Matrix4 Matrix4;
56  typedef typename traits<SE3Tpl>::Vector4 Vector4;
57  typedef typename traits<SE3Tpl>::Matrix6 Matrix6;
58 
59  using Base::rotation;
60  using Base::translation;
61 
62  SE3Tpl(): rot(), trans() {};
63 
64  template<typename QuaternionLike,typename Vector3Like>
65  SE3Tpl(const Eigen::QuaternionBase<QuaternionLike> & quat,
66  const Eigen::MatrixBase<Vector3Like> & trans)
67  : rot(quat.matrix()), trans(trans)
68  {
69  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
70  }
71 
72  template<typename Matrix3Like,typename Vector3Like>
73  SE3Tpl(const Eigen::MatrixBase<Matrix3Like> & R,
74  const Eigen::MatrixBase<Vector3Like> & trans)
75  : rot(R), trans(trans)
76  {
77  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
78  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like,3,3)
79  }
80 
81  template<typename Matrix4Like>
82  explicit SE3Tpl(const Eigen::MatrixBase<Matrix4Like> & m)
83  : rot(m.template block<3,3>(LINEAR,LINEAR))
84  , trans(m.template block<3,1>(LINEAR,ANGULAR))
85  {
86  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like,4,4);
87  }
88 
89  SE3Tpl(int)
90  : rot(AngularType::Identity())
91  , trans(LinearType::Zero())
92  {}
93 
94  template<int O2>
95  SE3Tpl(const SE3Tpl<Scalar,O2> & clone)
96  : rot(clone.rotation()),trans(clone.translation()) {}
97 
98  template<int O2>
99  SE3Tpl & operator=(const SE3Tpl<Scalar,O2> & other)
100  {
101  rot = other.rotation();
102  trans = other.translation();
103  return *this;
104  }
105 
106  static SE3Tpl Identity()
107  {
108  return SE3Tpl(1);
109  }
110 
111  SE3Tpl & setIdentity()
112  { rot.setIdentity (); trans.setZero (); return *this;}
113 
115  SE3Tpl inverse() const
116  {
117  return SE3Tpl(rot.transpose(), -rot.transpose()*trans);
118  }
119 
120  static SE3Tpl Random()
121  {
122  return SE3Tpl().setRandom();
123  }
124 
125  SE3Tpl & setRandom()
126  {
127  Quaternion q; quaternion::uniformRandom(q);
128  rot = q.matrix();
129  trans.setRandom();
130 
131  return *this;
132  }
133 
134  HomogeneousMatrixType toHomogeneousMatrix_impl() const
135  {
136  HomogeneousMatrixType M;
137  M.template block<3,3>(LINEAR,LINEAR) = rot;
138  M.template block<3,1>(LINEAR,ANGULAR) = trans;
139  M.template block<1,3>(ANGULAR,LINEAR).setZero();
140  M(3,3) = 1;
141  return M;
142  }
143 
145  ActionMatrixType toActionMatrix_impl() const
146  {
147  typedef Eigen::Block<ActionMatrixType,3,3> Block3;
148  ActionMatrixType M;
149  M.template block<3,3>(ANGULAR,ANGULAR)
150  = M.template block<3,3>(LINEAR,LINEAR) = rot;
151  M.template block<3,3>(ANGULAR,LINEAR).setZero();
152  Block3 B = M.template block<3,3>(LINEAR,ANGULAR);
153 
154  B.col(0) = trans.cross(rot.col(0));
155  B.col(1) = trans.cross(rot.col(1));
156  B.col(2) = trans.cross(rot.col(2));
157  return M;
158  }
159 
160  ActionMatrixType toActionMatrixInverse_impl() const
161  {
162  typedef Eigen::Block<ActionMatrixType,3,3> Block3;
163  ActionMatrixType M;
164  M.template block<3,3>(ANGULAR,ANGULAR)
165  = M.template block<3,3>(LINEAR,LINEAR) = rot.transpose();
166  Block3 C = M.template block<3,3>(ANGULAR,LINEAR); // used as temporary
167  Block3 B = M.template block<3,3>(LINEAR,ANGULAR);
168 
169 #define PINOCCHIO_INTERNAL_COMPUTATION(axis_id,v3_in,v3_out,R,res) \
170  CartesianAxis<axis_id>::cross(v3_in,v3_out); \
171  res.col(axis_id).noalias() = R.transpose() * v3_out;
172 
173  PINOCCHIO_INTERNAL_COMPUTATION(0,trans,C.col(0),rot,B);
174  PINOCCHIO_INTERNAL_COMPUTATION(1,trans,C.col(0),rot,B);
175  PINOCCHIO_INTERNAL_COMPUTATION(2,trans,C.col(0),rot,B);
176 
177 #undef PINOCCHIO_INTERNAL_COMPUTATION
178 
179  C.setZero();
180  return M;
181  }
182 
183  ActionMatrixType toDualActionMatrix_impl() const
184  {
185  typedef Eigen::Block<ActionMatrixType,3,3> Block3;
186  ActionMatrixType M;
187  M.template block<3,3>(ANGULAR,ANGULAR)
188  = M.template block<3,3>(LINEAR,LINEAR) = rot;
189  M.template block<3,3>(LINEAR,ANGULAR).setZero();
190  Block3 B = M.template block<3,3>(ANGULAR,LINEAR);
191 
192  B.col(0) = trans.cross(rot.col(0));
193  B.col(1) = trans.cross(rot.col(1));
194  B.col(2) = trans.cross(rot.col(2));
195  return M;
196  }
197 
198  void disp_impl(std::ostream & os) const
199  {
200  os
201  << " R =\n" << rot << std::endl
202  << " p = " << trans.transpose() << std::endl;
203  }
204 
206 
208  template<typename D>
209  typename SE3GroupAction<D>::ReturnType
210  act_impl(const D & d) const
211  {
212  return d.se3Action(*this);
213  }
214 
216  template<typename D> typename SE3GroupAction<D>::ReturnType
217  actInv_impl(const D & d) const
218  {
219  return d.se3ActionInverse(*this);
220  }
221 
222  template<typename EigenDerived>
223  typename EigenDerived::PlainObject
224  actOnEigenObject(const Eigen::MatrixBase<EigenDerived> & p) const
225  { return (rotation()*p+translation()).eval(); }
226 
227  template<typename MapDerived>
228  Vector3 actOnEigenObject(const Eigen::MapBase<MapDerived> & p) const
229  { return Vector3(rotation()*p+translation()); }
230 
231  template<typename EigenDerived>
232  typename EigenDerived::PlainObject
233  actInvOnEigenObject(const Eigen::MatrixBase<EigenDerived> & p) const
234  { return (rotation().transpose()*(p-translation())).eval(); }
235 
236  template<typename MapDerived>
237  Vector3 actInvOnEigenObject(const Eigen::MapBase<MapDerived> & p) const
238  { return Vector3(rotation().transpose()*(p-translation())); }
239 
240  Vector3 act_impl(const Vector3 & p) const
241  { return Vector3(rotation()*p+translation()); }
242 
243  Vector3 actInv_impl(const Vector3 & p) const
244  { return Vector3(rotation().transpose()*(p-translation())); }
245 
246  template<int O2>
247  SE3Tpl act_impl(const SE3Tpl<Scalar,O2> & m2) const
248  { return SE3Tpl(rot*m2.rotation()
249  ,translation()+rotation()*m2.translation());}
250 
251  template<int O2>
252  SE3Tpl actInv_impl(const SE3Tpl<Scalar,O2> & m2) const
253  { return SE3Tpl(rot.transpose()*m2.rotation(),
254  rot.transpose()*(m2.translation()-translation()));}
255 
256  template<int O2>
257  SE3Tpl __mult__(const SE3Tpl<Scalar,O2> & m2) const
258  { return this->act_impl(m2);}
259 
260  template<int O2>
261  bool isEqual(const SE3Tpl<Scalar,O2> & m2) const
262  {
263  return (rotation() == m2.rotation() && translation() == m2.translation());
264  }
265 
266  template<int O2>
267  bool isApprox_impl(const SE3Tpl<Scalar,O2> & m2,
268  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
269  {
270  return rotation().isApprox(m2.rotation(), prec)
271  && translation().isApprox(m2.translation(), prec);
272  }
273 
274  bool isIdentity(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
275  {
276  return rotation().isIdentity(prec) && translation().isZero(prec);
277  }
278 
279  ConstAngularRef rotation_impl() const { return rot; }
280  AngularRef rotation_impl() { return rot; }
281  void rotation_impl(const AngularType & R) { rot = R; }
282  ConstLinearRef translation_impl() const { return trans;}
283  LinearRef translation_impl() { return trans;}
284  void translation_impl(const LinearType & p) { trans = p; }
285 
287  template<typename NewScalar>
288  SE3Tpl<NewScalar,Options> cast() const
289  {
290  typedef SE3Tpl<NewScalar,Options> ReturnType;
291  ReturnType res(rot.template cast<NewScalar>(),
292  trans.template cast<NewScalar>());
293 
294  // During the cast, it may appear that the matrix is not normalized correctly.
295  // Force the normalization of the rotation part of the matrix.
296  internal::cast_call_normalize_method<SE3Tpl,NewScalar,Scalar>::run(res);
297  return res;
298  }
299 
300  bool isNormalized(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
301  {
302  return isUnitary(rot,prec);
303  }
304 
305  void normalize()
306  {
307  rot = orthogonalProjection(rot);
308  }
309 
310  PlainType normalized() const
311  {
312  PlainType res(*this); res.normalize();
313  return res;
314  }
315 
327  template<typename OtherScalar>
328  static SE3Tpl Interpolate(const SE3Tpl & A, const SE3Tpl & B, const OtherScalar & alpha);
329 
330  protected:
331  AngularType rot;
332  LinearType trans;
333 
334  }; // class SE3Tpl
335 
336  namespace internal
337  {
338  template<typename Scalar, int Options>
339  struct cast_call_normalize_method<SE3Tpl<Scalar,Options>,Scalar,Scalar>
340  {
341  template<typename T>
342  static void run(T &) {}
343  };
344 
345  template<typename Scalar, int Options, typename NewScalar>
346  struct cast_call_normalize_method<SE3Tpl<Scalar,Options>,NewScalar,Scalar>
347  {
348  template<typename T>
349  static void run(T & self)
350  {
351  if(pinocchio::cast<NewScalar>(Eigen::NumTraits<Scalar>::epsilon()) > Eigen::NumTraits<NewScalar>::epsilon())
352  self.normalize();
353  }
354  };
355 
356  }
357 
358 } // namespace pinocchio
359 
360 #endif // ifndef __pinocchio_se3_tpl_hpp__
361 
pinocchio::SE3Tpl::toActionMatrix_impl
ActionMatrixType toActionMatrix_impl() const
Vb.toVector() = bXa.toMatrix() * Va.toVector()
Definition: se3-tpl.hpp:145
pinocchio::quaternion::uniformRandom
void uniformRandom(Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Definition: quaternion.hpp:108
pinocchio::isUnitary
bool isUnitary(const Eigen::MatrixBase< MatrixLike > &mat, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Check whether the input matrix is Unitary within the given precision.
Definition: matrix.hpp:140
pinocchio::SE3Tpl
Definition: fwd.hpp:38
pinocchio::orthogonalProjection
Matrix3 orthogonalProjection(const Eigen::MatrixBase< Matrix3 > &mat)
Orthogonal projection of a matrix on the SO(3) manifold.
Definition: rotation.hpp:80
pinocchio::SE3Tpl::Interpolate
static SE3Tpl Interpolate(const SE3Tpl &A, const SE3Tpl &B, const OtherScalar &alpha)
Linear interpolation on the SE3 manifold.
pinocchio::SE3Tpl::inverse
SE3Tpl inverse() const
aXb = bXa.inverse()
Definition: se3-tpl.hpp:115
pinocchio::SE3Tpl::act_impl
SE3GroupAction< D >::ReturnType act_impl(const D &d) const
— GROUP ACTIONS ON M6, F6 and I6 —
Definition: se3-tpl.hpp:210
pinocchio::SE3Base
Base class for rigid transformation.
Definition: se3-base.hpp:30
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio::SE3Tpl::actInv_impl
SE3GroupAction< D >::ReturnType actInv_impl(const D &d) const
by = aXb.actInv(ay)
Definition: se3-tpl.hpp:217
pinocchio::SE3Tpl::cast
SE3Tpl< NewScalar, Options > cast() const
Definition: se3-tpl.hpp:288
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11