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