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