GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/spatial/se3-tpl.hpp Lines: 112 113 99.1 %
Date: 2024-04-26 13:14:21 Branches: 122 238 51.3 %

Line Branch Exec Source
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
198947
    SE3Tpl(): rot(), trans() {};
63
64
    template<typename QuaternionLike,typename Vector3Like>
65
61
    SE3Tpl(const Eigen::QuaternionBase<QuaternionLike> & quat,
66
           const Eigen::MatrixBase<Vector3Like> & trans)
67
61
    : rot(quat.matrix()), trans(trans)
68
    {
69
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
70
61
    }
71
72
    template<typename Matrix3Like,typename Vector3Like>
73
2909970
    SE3Tpl(const Eigen::MatrixBase<Matrix3Like> & R,
74
           const Eigen::MatrixBase<Vector3Like> & trans)
75
2909970
    : 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
2909970
    }
80
81
    template<typename Matrix4Like>
82
1
    explicit SE3Tpl(const Eigen::MatrixBase<Matrix4Like> & m)
83
    : rot(m.template block<3,3>(LINEAR,LINEAR))
84

1
    , trans(m.template block<3,1>(LINEAR,ANGULAR))
85
    {
86
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like,4,4);
87
1
    }
88
89
542224
    SE3Tpl(int)
90
    : rot(AngularType::Identity())
91

542224
    , trans(LinearType::Zero())
92
542224
    {}
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
542216
    static SE3Tpl Identity()
107
    {
108
542216
      return SE3Tpl(1);
109
    }
110
111
183
    SE3Tpl & setIdentity()
112
183
    { rot.setIdentity (); trans.setZero (); return *this;}
113
114
    /// aXb = bXa.inverse()
115
6253
    SE3Tpl inverse() const
116
    {
117


12506
      return SE3Tpl(rot.transpose(), -rot.transpose()*trans);
118
    }
119
120
105620
    static SE3Tpl Random()
121
    {
122

105620
      return SE3Tpl().setRandom();
123
    }
124
125
105633
    SE3Tpl & setRandom()
126
    {
127

105633
      Quaternion q; quaternion::uniformRandom(q);
128
105633
      rot = q.matrix();
129
105633
      trans.setRandom();
130
131
105633
      return *this;
132
    }
133
134
5
    HomogeneousMatrixType toHomogeneousMatrix_impl() const
135
    {
136
5
      HomogeneousMatrixType M;
137
5
      M.template block<3,3>(LINEAR,LINEAR) = rot;
138
5
      M.template block<3,1>(LINEAR,ANGULAR) = trans;
139
5
      M.template block<1,3>(ANGULAR,LINEAR).setZero();
140
5
      M(3,3) = 1;
141
5
      return M;
142
    }
143
144
    /// Vb.toVector() = bXa.toMatrix() * Va.toVector()
145
3853
    ActionMatrixType toActionMatrix_impl() const
146
    {
147
      typedef Eigen::Block<ActionMatrixType,3,3> Block3;
148
3853
      ActionMatrixType M;
149
      M.template block<3,3>(ANGULAR,ANGULAR)
150


3853
      = M.template block<3,3>(LINEAR,LINEAR) = rot;
151

3853
      M.template block<3,3>(ANGULAR,LINEAR).setZero();
152
3853
      Block3 B = M.template block<3,3>(LINEAR,ANGULAR);
153
154


3853
      B.col(0) = trans.cross(rot.col(0));
155


3853
      B.col(1) = trans.cross(rot.col(1));
156


3853
      B.col(2) = trans.cross(rot.col(2));
157
7706
      return M;
158
    }
159
160
15
    ActionMatrixType toActionMatrixInverse_impl() const
161
    {
162
      typedef Eigen::Block<ActionMatrixType,3,3> Block3;
163
15
      ActionMatrixType M;
164
      M.template block<3,3>(ANGULAR,ANGULAR)
165


15
      = M.template block<3,3>(LINEAR,LINEAR) = rot.transpose();
166
15
      Block3 C = M.template block<3,3>(ANGULAR,LINEAR); // used as temporary
167
15
      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




15
      PINOCCHIO_INTERNAL_COMPUTATION(0,trans,C.col(0),rot,B);
174




15
      PINOCCHIO_INTERNAL_COMPUTATION(1,trans,C.col(0),rot,B);
175




15
      PINOCCHIO_INTERNAL_COMPUTATION(2,trans,C.col(0),rot,B);
176
177
#undef PINOCCHIO_INTERNAL_COMPUTATION
178
179
15
      C.setZero();
180
30
      return M;
181
    }
182
183
88
    ActionMatrixType toDualActionMatrix_impl() const
184
    {
185
      typedef Eigen::Block<ActionMatrixType,3,3> Block3;
186
88
      ActionMatrixType M;
187
      M.template block<3,3>(ANGULAR,ANGULAR)
188


88
      = M.template block<3,3>(LINEAR,LINEAR) = rot;
189

88
      M.template block<3,3>(LINEAR,ANGULAR).setZero();
190
88
      Block3 B = M.template block<3,3>(ANGULAR,LINEAR);
191
192


88
      B.col(0) = trans.cross(rot.col(0));
193


88
      B.col(1) = trans.cross(rot.col(1));
194


88
      B.col(2) = trans.cross(rot.col(2));
195
176
      return M;
196
    }
197
198
4
    void disp_impl(std::ostream & os) const
199
    {
200
      os
201
4
      << "  R =\n" << rot << std::endl
202

4
      << "  p = " << trans.transpose() << std::endl;
203
4
    }
204
205
    /// --- GROUP ACTIONS ON M6, F6 and I6 ---
206
207
    /// ay = aXb.act(by)
208
    template<typename D>
209
    typename SE3GroupAction<D>::ReturnType
210
283825
    act_impl(const D & d) const
211
    {
212
283825
      return d.se3Action(*this);
213
    }
214
215
    /// by = aXb.actInv(ay)
216
    template<typename D> typename SE3GroupAction<D>::ReturnType
217
188908
    actInv_impl(const D & d) const
218
    {
219
188908
      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
1355
    Vector3 act_impl(const Vector3 & p) const
241

1355
    { return Vector3(rotation()*p+translation()); }
242
243
5513
    Vector3 actInv_impl(const Vector3 & p) const
244


5513
    { return Vector3(rotation().transpose()*(p-translation())); }
245
246
    template<int O2>
247
1413723
    SE3Tpl act_impl(const SE3Tpl<Scalar,O2> & m2) const
248

1413723
    { return SE3Tpl(rot*m2.rotation()
249

4241169
                    ,translation()+rotation()*m2.translation());}
250
251
    template<int O2>
252
14495
    SE3Tpl actInv_impl(const SE3Tpl<Scalar,O2> & m2) const
253

14495
    { return SE3Tpl(rot.transpose()*m2.rotation(),
254

43485
                    rot.transpose()*(m2.translation()-translation()));}
255
256
    template<int O2>
257
1413723
    SE3Tpl __mult__(const SE3Tpl<Scalar,O2> & m2) const
258
1413723
    { return this->act_impl(m2);}
259
260
    template<int O2>
261
9764
    bool isEqual(const SE3Tpl<Scalar,O2> & m2) const
262
    {
263

9764
      return (rotation() == m2.rotation() && translation() == m2.translation());
264
    }
265
266
    template<int O2>
267
55781
    bool isApprox_impl(const SE3Tpl<Scalar,O2> & m2,
268
                       const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
269
    {
270
55781
      return rotation().isApprox(m2.rotation(), prec)
271

55781
      && translation().isApprox(m2.translation(), prec);
272
    }
273
274
2
    bool isIdentity(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
275
    {
276

2
      return rotation().isIdentity(prec) && translation().isZero(prec);
277
    }
278
279
3770010
    ConstAngularRef rotation_impl() const { return rot; }
280
568481
    AngularRef rotation_impl() { return rot; }
281
29023
    void rotation_impl(const AngularType & R) { rot = R; }
282
3324689
    ConstLinearRef translation_impl() const { return trans;}
283
97551
    LinearRef translation_impl() { return trans;}
284
22556
    void translation_impl(const LinearType & p) { trans = p; }
285
286
    /// \returns An expression of *this with the Scalar type casted to NewScalar.
287
    template<typename NewScalar>
288
1616
    SE3Tpl<NewScalar,Options> cast() const
289
    {
290
      typedef SE3Tpl<NewScalar,Options> ReturnType;
291

1616
      ReturnType res(rot.template cast<NewScalar>(),
292
1616
                     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
1616
      internal::cast_call_normalize_method<SE3Tpl,NewScalar,Scalar>::run(res);
297
1616
      return res;
298
    }
299
300
3
    bool isNormalized(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
301
    {
302
3
      return isUnitary(rot,prec);
303
    }
304
305
176
    void normalize()
306
    {
307
176
      rot = orthogonalProjection(rot);
308
176
    }
309
310
1
    PlainType normalized() const
311
    {
312
1
      PlainType res(*this); res.normalize();
313
1
      return res;
314
    }
315
316
    ///
317
    /// \brief Linear interpolation on the SE3 manifold.
318
    ///
319
    /// \param[in] A Initial transformation.
320
    /// \param[in] B Target transformation.
321
    /// \param[in] alpha Interpolation factor in [0 ... 1].
322
    ///
323
    /// \returns An interpolated transformation between A and B.
324
    ///
325
    /// \note This is similar to the SLERP operation which acts initially for rotation but applied here to rigid transformation.
326
    ///
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
1015
      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
178
      static void run(T & self)
350
      {
351

178
        if(pinocchio::cast<NewScalar>(Eigen::NumTraits<Scalar>::epsilon()) > Eigen::NumTraits<NewScalar>::epsilon())
352
172
          self.normalize();
353
178
      }
354
    };
355
356
  }
357
358
} // namespace pinocchio
359
360
#endif // ifndef __pinocchio_se3_tpl_hpp__
361