se3.hpp 10.9 KB
Newer Older
1
//
2
// Copyright (c) 2015-2016 CNRS
3
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
19
20
21
22
23
24
25
#ifndef __se3_se3_hpp__
#define __se3_se3_hpp__

#include <Eigen/Geometry>
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/skew.hpp"

26

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
27
28
29
namespace se3
{

30
  /* Type returned by the "se3Action" and "se3ActionInverse" functions. */
31
32
33
34
35
36
  namespace internal 
  {
    template<typename D>
    struct ActionReturn    { typedef D Type; };
  }

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
37
38
39
40
41
42
43
44
45
46
47
48
  /** The rigid transform aMb can be seen in two ways: 
   *
   * - given a point p expressed in frame B by its coordinate vector Bp, aMb
   * computes its coordinates in frame A by Ap = aMb Bp.
   * - aMb displaces a solid S centered at frame A into the solid centered in
   * B. In particular, the origin of A is displaced at the origin of B: $^aM_b
   * ^aA = ^aB$.

   * The rigid displacement is stored as a rotation matrix and translation vector by:
   * aMb (x) =  aRb*x + aAB
   * where aAB is the vector from origin A to origin B expressed in coordinates A.
   */
49
  template<class Derived>
50
  class SE3Base
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
51
  {
52
  protected:
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
53

54
    typedef Derived  Derived_t;
55
    SPATIAL_TYPEDEF_TEMPLATE(Derived_t);
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
56
57

  public:
58
59
60
      Derived_t & derived() { return *static_cast<Derived_t*>(this); }
      const Derived_t& derived() const { return *static_cast<const Derived_t*>(this); }

61
62
      ConstAngular_t & rotation() const  { return derived().rotation_impl(); }
      ConstLinear_t & translation() const  { return derived().translation_impl(); }
63
64
65
66
      Angular_t & rotation()  { return derived().rotation_impl(); }
      Linear_t & translation()   { return derived().translation_impl(); }
      void rotation(const Angular_t & R) { derived().rotation_impl(R); }
      void translation(const Linear_t & R) { derived().translation_impl(R); }
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
67

68

69
70
71
72
73
      Matrix4 toHomogeneousMatrix() const
      {
        return derived().toHomogeneousMatrix_impl();
      }
      operator Matrix4() const { return toHomogeneousMatrix(); }
74

75
76
77
78
79
      Matrix6 toActionMatrix() const
      {
        return derived().toActionMatrix_impl();
      }
      operator Matrix6() const { return toActionMatrix(); }
80
81
    
    Matrix6 toDualActionMatrix() const { return derived().toDualActionMatrix_impl(); }
82
83


84

85
86
87
88
89
90
91
      void disp(std::ostream & os) const
      {
        static_cast<const Derived_t*>(this)->disp_impl(os);
      }

      Derived_t operator*(const Derived_t & m2) const    { return derived().__mult__(m2); }

Valenza Florian's avatar
Valenza Florian committed
92
      /// ay = aXb.act(by)
93
94
95
96
97
      template<typename D>
      typename internal::ActionReturn<D>::Type act   (const D & d) const 
      { 
        return derived().act_impl(d);
      }
Valenza Florian's avatar
Valenza Florian committed
98
99
      
      /// by = aXb.actInv(ay)
100
101
102
103
104
105
106
107
108
      template<typename D> typename internal::ActionReturn<D>::Type actInv(const D & d) const
      {
        return derived().actInv_impl(d);
      }


      Derived_t act   (const Derived_t& m2) const { return derived().act_impl(m2); }
      Derived_t actInv(const Derived_t& m2) const { return derived().actInv_impl(m2); }

109

jcarpent's avatar
jcarpent committed
110
111
    bool operator==(const Derived_t & other) const { return derived().__equal__(other); }
    bool operator!=(const Derived_t & other) const { return !(*this == other); }
112

113
      bool isApprox (const Derived_t & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
114
      {
115
        return derived().isApprox_impl(other, prec);
116
117
      }

118
119
120
121
122
      friend std::ostream & operator << (std::ostream & os,const SE3Base<Derived> & X)
      { 
        X.disp(os);
        return os;
      }
123
124
125
126
127
128
129
130
    
    ///
    /// \returns true if *this is approximately equal to the identity placement, within the precision given by prec.
    ///
    bool isIdentity(const typename traits<Derived>::Scalar & prec = Eigen::NumTraits<typename traits<Derived>::Scalar>::dummy_precision()) const
    {
      return derived().isIdentity(prec);
    }
131

Valenza Florian's avatar
Valenza Florian committed
132
  }; // class SE3Base
133
134


Valenza Florian's avatar
Valenza Florian committed
135
136
137
  template<typename T, int U>
  struct traits< SE3Tpl<T, U> >
  {
138
    typedef T Scalar;
Valenza Florian's avatar
Valenza Florian committed
139
140
141
142
143
144
145
    typedef Eigen::Matrix<T,3,1,U> Vector3;
    typedef Eigen::Matrix<T,4,1,U> Vector4;
    typedef Eigen::Matrix<T,6,1,U> Vector6;
    typedef Eigen::Matrix<T,3,3,U> Matrix3;
    typedef Eigen::Matrix<T,4,4,U> Matrix4;
    typedef Eigen::Matrix<T,6,6,U> Matrix6;
    typedef Matrix3 Angular_t;
146
    typedef const Matrix3 ConstAngular_t;
Valenza Florian's avatar
Valenza Florian committed
147
    typedef Vector3 Linear_t;
148
    typedef const Vector3 ConstLinear_t;
Valenza Florian's avatar
Valenza Florian committed
149
150
151
152
153
154
155
156
157
    typedef Matrix6 ActionMatrix_t;
    typedef Eigen::Quaternion<T,U> Quaternion_t;
    typedef SE3Tpl<T,U> SE3;
    typedef ForceTpl<T,U> Force;
    typedef MotionTpl<T,U> Motion;
    typedef Symmetric3Tpl<T,U> Symmetric3;
    enum {
      LINEAR = 0,
      ANGULAR = 3
158
    };
Valenza Florian's avatar
Valenza Florian committed
159
  }; // traits SE3Tpl
160

Valenza Florian's avatar
Valenza Florian committed
161
162
163
  template<typename _Scalar, int _Options>
  class SE3Tpl : public SE3Base< SE3Tpl< _Scalar, _Options > >
  {
164

Valenza Florian's avatar
Valenza Florian committed
165
166
  public:
    friend class SE3Base< SE3Tpl< _Scalar, _Options > >;
167
    SPATIAL_TYPEDEF_TEMPLATE(SE3Tpl);
168
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
169
170


Valenza Florian's avatar
Valenza Florian committed
171
    SE3Tpl(): rot(), trans() {};
172
173


Valenza Florian's avatar
Valenza Florian committed
174
175
176
177
    template<typename M3,typename v3>
    SE3Tpl(const Eigen::MatrixBase<M3> & R, const Eigen::MatrixBase<v3> & p) 
    : rot(R), trans(p)
    {
178
179
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(v3,3)
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M3,3,3)
Valenza Florian's avatar
Valenza Florian committed
180
    }
181

182
    template<typename M4>
183
    explicit SE3Tpl(const Eigen::MatrixBase<M4> & m) 
184
185
    : rot(m.template block<3,3>(LINEAR,LINEAR)), trans(m.template block<3,1>(LINEAR,ANGULAR))
    {
186
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M4,4,4);
187
188
    }

Valenza Florian's avatar
Valenza Florian committed
189
    SE3Tpl(int) : rot(Matrix3::Identity()), trans(Vector3::Zero()) {}
190

Valenza Florian's avatar
Valenza Florian committed
191
192
193
    template<typename S2, int O2>
    SE3Tpl( const SE3Tpl<S2,O2> & clone )
    : rot(clone.rotation()),trans(clone.translation()) {}
194

195

Valenza Florian's avatar
Valenza Florian committed
196
197
198
199
200
201
202
    template<typename S2, int O2>
    SE3Tpl & operator= (const SE3Tpl<S2,O2> & other)
    {
      rot = other.rotation ();
      trans = other.translation ();
      return *this;
    }
203

Valenza Florian's avatar
Valenza Florian committed
204
205
206
207
    static SE3Tpl Identity()
    {
      return SE3Tpl(1);
    }
208

Valenza Florian's avatar
Valenza Florian committed
209
    SE3Tpl & setIdentity () { rot.setIdentity (); trans.setZero (); return *this;}
210

Valenza Florian's avatar
Valenza Florian committed
211
212
213
214
215
    /// aXb = bXa.inverse()
    SE3Tpl inverse() const
    {
      return SE3Tpl(rot.transpose(), -rot.transpose()*trans);
    }
216

Valenza Florian's avatar
Valenza Florian committed
217
218
219
220
221
222
    static SE3Tpl Random()
    {
      Quaternion_t q(Vector4::Random());
      q.normalize();
      return SE3Tpl(q.matrix(),Vector3::Random());
    }
223

Valenza Florian's avatar
Valenza Florian committed
224
225
226
227
228
229
    SE3Tpl & setRandom ()
    {
      Quaternion_t q(Vector4::Random());
      q.normalize ();
      rot = q.matrix ();
      trans.setRandom ();
230

Valenza Florian's avatar
Valenza Florian committed
231
232
      return *this;
    }
jcarpent's avatar
jcarpent committed
233
    
Valenza Florian's avatar
Valenza Florian committed
234
235
236
237
238
239
240
241
242
243
244
245
246
    Matrix4 toHomogeneousMatrix_impl() const
    {
      Matrix4 M;
      M.template block<3,3>(LINEAR,LINEAR) = rot;
      M.template block<3,1>(LINEAR,ANGULAR) = trans;
      M.template block<1,3>(ANGULAR,LINEAR).setZero();
      M(3,3) = 1;
      return M;
    }

    /// Vb.toVector() = bXa.toMatrix() * Va.toVector()
    Matrix6 toActionMatrix_impl() const
    {
247
      typedef Eigen::Block<Matrix6,3,3> Block3;
Valenza Florian's avatar
Valenza Florian committed
248
249
250
251
      Matrix6 M;
      M.template block<3,3>(ANGULAR,ANGULAR)
      = M.template block<3,3>(LINEAR,LINEAR) = rot;
      M.template block<3,3>(ANGULAR,LINEAR).setZero();
252
253
254
255
256
      Block3 B = M.template block<3,3>(LINEAR,ANGULAR);
      
      B.col(0) = trans.cross(rot.col(0));
      B.col(1) = trans.cross(rot.col(1));
      B.col(2) = trans.cross(rot.col(2));
Valenza Florian's avatar
Valenza Florian committed
257
258
      return M;
    }
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
    
    Matrix6 toDualActionMatrix_impl() const
    {
      typedef Eigen::Block<Matrix6,3,3> Block3;
      Matrix6 M;
      M.template block<3,3>(ANGULAR,ANGULAR)
      = M.template block<3,3>(LINEAR,LINEAR) = rot;
      M.template block<3,3>(LINEAR,ANGULAR).setZero();
      Block3 B = M.template block<3,3>(ANGULAR,LINEAR);
      
      B.col(0) = trans.cross(rot.col(0));
      B.col(1) = trans.cross(rot.col(1));
      B.col(2) = trans.cross(rot.col(2));
      return M;
    }
Valenza Florian's avatar
Valenza Florian committed
274
275
276
277
278
279

    void disp_impl(std::ostream & os) const
    {
      os << "  R =\n" << rot << std::endl
      << "  p = " << trans.transpose() << std::endl;
    }
280

Valenza Florian's avatar
Valenza Florian committed
281
    /// --- GROUP ACTIONS ON M6, F6 and I6 --- 
282

Valenza Florian's avatar
Valenza Florian committed
283
284
285
286
287
288
289
290
291
292
293
    /// ay = aXb.act(by)
    template<typename D>
    typename internal::ActionReturn<D>::Type act_impl   (const D & d) const 
    { 
      return d.se3Action(*this);
    }
    /// by = aXb.actInv(ay)
    template<typename D> typename internal::ActionReturn<D>::Type actInv_impl(const D & d) const
    {
      return d.se3ActionInverse(*this);
    }
294

295
    template<typename EigenDerived>
296
    typename EigenDerived::PlainObject actOnEigenObject(const Eigen::MatrixBase<EigenDerived> & p) const
297
    { return (rot*p+trans).eval(); }
298
299

    template<typename MapDerived>
300
    Vector3 actOnEigenObject(const Eigen::MapBase<MapDerived> & p) const
301
302
303
    { return Vector3(rot*p+trans); }

    template<typename EigenDerived>
304
    typename EigenDerived::PlainObject actInvOnEigenObject(const Eigen::MatrixBase<EigenDerived> & p) const
305
    { return (rot.transpose()*(p-trans)).eval(); }
306
307

    template<typename MapDerived>
308
    Vector3 actInvOnEigenObject(const Eigen::MapBase<MapDerived> & p) const
309
310
    { return Vector3(rot.transpose()*(p-trans)); }

311
312
    Vector3 act_impl   (const Vector3& p) const { return Vector3(rot*p+trans); }
    Vector3 actInv_impl(const Vector3& p) const { return Vector3(rot.transpose()*(p-trans)); }
313

Valenza Florian's avatar
Valenza Florian committed
314
315
    SE3Tpl act_impl    (const SE3Tpl& m2) const { return SE3Tpl( rot*m2.rot,trans+rot*m2.trans);}
    SE3Tpl actInv_impl (const SE3Tpl& m2) const { return SE3Tpl( rot.transpose()*m2.rot, rot.transpose()*(m2.trans-trans));}
316
317


Valenza Florian's avatar
Valenza Florian committed
318
    SE3Tpl __mult__(const SE3Tpl & m2) const { return this->act(m2);}
319

320
321
322
323
324
    bool __equal__( const SE3Tpl & m2 ) const
    {
      return (rotation_impl() == m2.rotation() && translation_impl() == m2.translation());
    }

325
    bool isApprox_impl (const SE3Tpl & m2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
326
    {
327
      return rot.isApprox(m2.rot, prec) && trans.isApprox(m2.trans, prec);
328
    }
329
330
331
332
333
    
    bool isIdentity(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    {
      return rot.isIdentity(prec) && trans.isZero(prec);
    }
334

335
    ConstAngular_t & rotation_impl() const { return rot; }
Valenza Florian's avatar
Valenza Florian committed
336
337
    Angular_t & rotation_impl() { return rot; }
    void rotation_impl(const Angular_t & R) { rot = R; }
338
    ConstLinear_t & translation_impl() const { return trans;}
Valenza Florian's avatar
Valenza Florian committed
339
340
    Linear_t & translation_impl() { return trans;}
    void translation_impl(const Linear_t & p) { trans=p; }
341

Valenza Florian's avatar
Valenza Florian committed
342
343
344
  protected:
    Angular_t rot;
    Linear_t trans;
Valenza Florian's avatar
Valenza Florian committed
345
346
    
  }; // class SE3Tpl
347

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
348
349
350
351
} // namespace se3

#endif // ifndef __se3_se3_hpp__