special-euclidean.hpp 11.3 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
//
// Copyright (c) 2016 CNRS
//
// 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/>.

#ifndef __se3_special_euclidean_operation_hpp__
#define __se3_special_euclidean_operation_hpp__

#include <limits>

23
24
25
26
#include <pinocchio/macros.hpp>
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/liegroup/operation-base.hpp"
27

28
29
30
#include "pinocchio/multibody/liegroup/vector-space.hpp"
#include "pinocchio/multibody/liegroup/cartesian-product.hpp"
#include "pinocchio/multibody/liegroup/special-orthogonal.hpp"
31
32
33

namespace se3
{
34
35
36
37
  template<int N> struct SpecialEuclideanOperation {};
  template<int N> struct traits<SpecialEuclideanOperation<N> > {};

  template<> struct traits<SpecialEuclideanOperation<2> > {
38
39
    typedef double Scalar;
    enum {
40
41
      NQ = 4,
      NV = 3
42
43
44
45
46
    };
    typedef Eigen::Matrix<Scalar,NQ,1> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1> TangentVector_t;
  };

47
  template<> struct traits<SpecialEuclideanOperation<3> > {
48
49
    typedef double Scalar;
    enum {
50
51
      NQ = 7,
      NV = 6
52
53
54
55
56
    };
    typedef Eigen::Matrix<Scalar,NQ,1> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1> TangentVector_t;
  };

57
58
  template<>
  struct SpecialEuclideanOperation<2> : public LieGroupOperationBase <SpecialEuclideanOperation<2> >
59
  {
60
61
    typedef CartesianProductOperation <VectorSpaceOperation<2>, SpecialOrthogonalOperation<2> > R2crossSO2_t;
    typedef SpecialEuclideanOperation LieGroupDerived;
62
63
    SE3_LIE_GROUP_TYPEDEF;

64
65
66
67
68
69
70
71
72
73
74
75
76
77
    /// Get dimension of Lie Group vector representation
    ///
    /// For instance, for SO(3), the dimension of the vector representation is
    /// 4 (quaternion) while the dimension of the tangent space is 3.
    Index nq () const
    {
      return NQ;
    }
    /// Get dimension of Lie Group tangent space
    Index nv () const
    {
      return NV;
    }

78
79
80
81
82
    std::string name () const
    {
      return std::string ("SE(2)");
    }

83
84
85
86
87
88
89
90
91
    template <class ConfigL_t, class ConfigR_t, class Tangent_t>
    static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                const Eigen::MatrixBase<ConfigR_t> & q1,
                                const Eigen::MatrixBase<Tangent_t> & d)
    {
      SE3 M0(SE3::Identity()); forwardKinematics(M0, q0);
      SE3 M1(SE3::Identity()); forwardKinematics(M1, q1);

      Motion nu(log6(M0.inverse()*M1)); // TODO: optimize implementation
92

93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
      Tangent_t& out = const_cast< Eigen::MatrixBase<Tangent_t>& >(d).derived();
      out.template head<2>() = nu.linear().head<2>();
      out(2) = nu.angular()(2);
    }

    template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
    static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
                               const Eigen::MatrixBase<Velocity_t> & vs,
                               const Eigen::MatrixBase<ConfigOut_t> & qout)
    {
      ConfigOut_t& out = (const_cast< Eigen::MatrixBase<ConfigOut_t>& >(qout)).derived();
      typedef Eigen::Matrix<Scalar, 2, 2> Matrix22;
      typedef Eigen::Matrix<Scalar, 2, 1> Vector2;

      const double& c0 = q(2), s0 = q(3);
      Matrix22 R0;
      R0 << c0, -s0, s0, c0;
110

111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
      const double& t = vs[2];
      const double theta = std::fabs(t);

      if(theta > 1e-14)
      {
        // vs = [ x, y, t ]
        // w = [ 0, 0, t ]
        // v = [ x, y, 0 ]
        // Considering only the 2x2 top left corner:
        // Sp = [ 0, -1; 1, 0],
        // if t > 0: S = Sp
        // else    : S = -Sp
        // S / t = Sp / |t|
        // S * S = - I2
        // R = I2 + ( 1 - ct) / |t| * S + ( 1 - st / |t| ) * S * S
        //   =      ( 1 - ct) / |t| * S +       st / |t|   * I2
        //
        // Ru = exp3 (w)
        // tu = R * v = (1 - ct) / |t| * S * v + st / t * v
        //
        // M0 * Mu = ( R0 * Ru, R0 * tu + t0 )

        // FIXME remove this copy.
        Vector2 v = vs.template head<2>();
        Eigen::Matrix<Scalar,2,1> cst;
        SINCOS (t, &cst[1], &cst[0]);
        const Scalar inv_theta = 1/theta;
        const Scalar c_coeff = (1.-cst[0]) * inv_theta;
        const Scalar s_coeff = std::fabs(cst[1]) * inv_theta;
        const Vector2 Sp_v (-v[1], v[0]);

        if (t > 0) out.template head<2>() = q.template head<2>() + R0 * (s_coeff * v + c_coeff * Sp_v);
        else       out.template head<2>() = q.template head<2>() + R0 * (s_coeff * v - c_coeff * Sp_v);
        out.template tail<2>() = R0 * cst;
      }
      else
      {
        // cos(t) ~ 1 - t^2 / 2
        // sin(t) ~ t
        out.template head<2>() = q.template head<2>() + R0*vs.template head<2>();
        out(2) = c0 * 1 - s0 * t;
        out(3) = s0 * 1 + c0 * t;
      }
    }

    // interpolate_impl use default implementation.
    // template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
    // static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                 // const Eigen::MatrixBase<ConfigR_t> & q1,
                                 // const Scalar& u,
                                 // const Eigen::MatrixBase<ConfigOut_t>& qout)

    // template <class ConfigL_t, class ConfigR_t>
    // static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                       // const Eigen::MatrixBase<ConfigR_t> & q1)

    template <class Config_t>
168
    void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
169
    {
jcarpent's avatar
jcarpent committed
170
      R2crossSO2_t().random(qout);
171
172
173
    }

    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
174
175
176
177
    void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
                                  const Eigen::MatrixBase<ConfigR_t> & upper,
                                  const Eigen::MatrixBase<ConfigOut_t> & qout)
      const
178
    {
179
      R2crossSO2_t ().randomConfiguration(lower, upper, qout);
180
    }
181
182
183
184
185
186

    template <class ConfigL_t, class ConfigR_t>
    static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                         const Eigen::MatrixBase<ConfigR_t> & q1,
                                         const Scalar & prec)
    {
187
      return R2crossSO2_t::isSameConfiguration(q0, q1, prec);
188
189
190
191
192
193
194
195
196
197
    }

    private:
    template<typename V>
    static void forwardKinematics(SE3 & M, const Eigen::MatrixBase<V>& q)
    {
      EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,V);

      const double& c_theta = q(2),
                    s_theta = q(3);
198

199
200
201
      M.rotation().topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta;
      M.translation().head<2>() = q.template head<2>();
    }
202
203
  }; // struct SpecialEuclideanOperation<2>

204
  /// SE(3)
205
206
207
208
209
210
211
212
213
214
215
216
  template<>
  struct SpecialEuclideanOperation<3> : public LieGroupOperationBase <SpecialEuclideanOperation<3> >
  {
    typedef CartesianProductOperation <VectorSpaceOperation<3>, SpecialOrthogonalOperation<3> > R3crossSO3_t;
    typedef SpecialEuclideanOperation LieGroupDerived;
    SE3_LIE_GROUP_TYPEDEF;

    typedef Eigen::Quaternion<Scalar> Quaternion_t;
    typedef Eigen::Map<      Quaternion_t> QuaternionMap_t;
    typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
    typedef SE3 Transformation_t;

217
218
219
220
221
222
223
224
225
226
227
228
229
230
    /// Get dimension of Lie Group vector representation
    ///
    /// For instance, for SO(3), the dimension of the vector representation is
    /// 4 (quaternion) while the dimension of the tangent space is 3.
    Index nq () const
    {
      return NQ;
    }
    /// Get dimension of Lie Group tangent space
    Index nv () const
    {
      return NV;
    }

231
232
233
234
235
    std::string name () const
    {
      return std::string ("SE(3)");
    }

236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
    template <class ConfigL_t, class ConfigR_t, class Tangent_t>
    static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                const Eigen::MatrixBase<ConfigR_t> & q1,
                                const Eigen::MatrixBase<Tangent_t> & d)
    {
      ConstQuaternionMap_t p0 (q0.derived().template tail<4>().data());
      ConstQuaternionMap_t p1 (q1.derived().template tail<4>().data());
      const_cast < Eigen::MatrixBase<Tangent_t>& > (d)
        = log6(  SE3(p0.matrix(), q0.derived().template head<3>()).inverse()
               * SE3(p1.matrix(), q1.derived().template head<3>())).toVector();
    }

    template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
    static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
                               const Eigen::MatrixBase<Velocity_t> & v,
                               const Eigen::MatrixBase<ConfigOut_t> & qout)
    {
      ConfigOut_t& out = (const_cast< Eigen::MatrixBase<ConfigOut_t>& >(qout)).derived();
      ConstQuaternionMap_t quat(q.derived().template tail<4>().data());
      QuaternionMap_t res_quat (out.template tail<4>().data());

      SE3 M0 (quat.matrix(), q.derived().template head<3>());
      SE3 M1 (M0 * exp6(Motion(v)));

      out.template head<3>() = M1.translation();
      res_quat = M1.rotation();
      // Norm of qs might be epsilon-different to 1, so M1.rotation might be epsilon-different to a rotation matrix.
      // It is then safer to re-normalized after converting M1.rotation to quaternion.
      firstOrderNormalize(res_quat);
    }

    // interpolate_impl use default implementation.
    // template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
    // static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                 // const Eigen::MatrixBase<ConfigR_t> & q1,
                                 // const Scalar& u,
                                 // const Eigen::MatrixBase<ConfigOut_t>& qout)
    // {
    // }

    template <class ConfigL_t, class ConfigR_t>
    static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                       const Eigen::MatrixBase<ConfigR_t> & q1)
    {
      TangentVector_t t;
      difference_impl(q0, q1, t);
      return t.squaredNorm();
    }

    template <class Config_t>
286
    void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
287
    {
jcarpent's avatar
jcarpent committed
288
      R3crossSO3_t().random(qout);
289
290
291
    }

    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
292
293
294
295
    void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
                                  const Eigen::MatrixBase<ConfigR_t> & upper,
                                  const Eigen::MatrixBase<ConfigOut_t> & qout)
      const
296
    {
297
      R3crossSO3_t ().randomConfiguration(lower, upper, qout);
298
299
300
301
302
303
304
305
306
307
    }

    template <class ConfigL_t, class ConfigR_t>
    static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                         const Eigen::MatrixBase<ConfigR_t> & q1,
                                         const Scalar & prec)
    {
      return R3crossSO3_t::isSameConfiguration(q0, q1, prec);
    }
  }; // struct SpecialEuclideanOperation<3>
308
309
310
311

} // namespace se3

#endif // ifndef __se3_special_euclidean_operation_hpp__