inertia.hpp 11.2 KB
Newer Older
1
//
jcarpent's avatar
jcarpent committed
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
#ifndef __se3_inertia_hpp__
#define __se3_inertia_hpp__

22
#include <Eigen/Core>
23
#include <iostream>
24

25
#include "pinocchio/spatial/symmetric3.hpp"
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
26
27
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/spatial/motion.hpp"
28
#include "pinocchio/spatial/skew.hpp"
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
29
30
31

namespace se3
{
Valenza Florian's avatar
Valenza Florian committed
32
33
34
35
36

  template< class Derived>
  class InertiaBase
  {
  protected:
Valenza Florian's avatar
Valenza Florian committed
37

Valenza Florian's avatar
Valenza Florian committed
38
    typedef Derived  Derived_t;
39
    SPATIAL_TYPEDEF_TEMPLATE(Derived_t);
Valenza Florian's avatar
Valenza Florian committed
40
41
42

  public:
    Derived_t & derived() { return *static_cast<Derived_t*>(this); }
43
    const Derived_t & derived() const { return *static_cast<const Derived_t*>(this); }
Valenza Florian's avatar
Valenza Florian committed
44

45
46
    Scalar           mass()    const { return static_cast<const Derived_t*>(this)->mass(); }
    Scalar &         mass() { return static_cast<const Derived_t*>(this)->mass(); }
Valenza Florian's avatar
Valenza Florian committed
47
    const Vector3 &    lever()   const { return static_cast<const Derived_t*>(this)->lever(); }
48
    Vector3 &          lever() { return static_cast<const Derived_t*>(this)->lever(); }
Valenza Florian's avatar
Valenza Florian committed
49
    const Symmetric3 & inertia() const { return static_cast<const Derived_t*>(this)->inertia(); }
50
    Symmetric3 &       inertia() { return static_cast<const Derived_t*>(this)->inertia(); }
Valenza Florian's avatar
Valenza Florian committed
51

52
    Matrix6 matrix() const { return derived().matrix_impl(); }
Valenza Florian's avatar
Valenza Florian committed
53
    operator Matrix6 () const { return matrix(); }
Valenza Florian's avatar
Valenza Florian committed
54

Valenza Florian's avatar
Valenza Florian committed
55
    Derived_t& operator= (const Derived_t& clone){return derived().__equl__(clone);}
jcarpent's avatar
jcarpent committed
56
57
58
    bool operator==(const Derived_t & other) const {return derived().isEqual(other);}
    bool operator!=(const Derived_t & other) const { return !(*this == other); }
    
Valenza Florian's avatar
Valenza Florian committed
59
60
61
    Derived_t& operator+= (const Derived_t & Yb) { return derived().__pequ__(Yb); }
    Derived_t operator+(const Derived_t & Yb) const { return derived().__plus__(Yb); }
    Force operator*(const Motion & v) const    { return derived().__mult__(v); }
62

63
    Scalar vtiv(const Motion & v) const { return derived().vtiv_impl(v); }
Valenza Florian's avatar
Valenza Florian committed
64

65
66
67
    void setZero() { derived().setZero(); }
    void setIdentity() { derived().setIdentity(); }
    void setRandom() { derived().setRandom(); }
68
69
70
    
    bool isApprox (const Derived & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    { return derived().isApprox_impl(other, prec); }
71

Valenza Florian's avatar
Valenza Florian committed
72
    /// aI = aXb.act(bI)
73
    Derived_t se3Action(const SE3 & M) const { return derived().se3Action_impl(M); }
Valenza Florian's avatar
Valenza Florian committed
74

Valenza Florian's avatar
Valenza Florian committed
75
    /// bI = aXb.actInv(aI)
76
    Derived_t se3ActionInverse(const SE3 & M) const { return derived().se3ActionInverse_impl(M); }
Valenza Florian's avatar
Valenza Florian committed
77

78
    void disp(std::ostream & os) const { static_cast<const Derived_t*>(this)->disp_impl(os); }
Valenza Florian's avatar
Valenza Florian committed
79
80
81
82
83
    friend std::ostream & operator << (std::ostream & os,const InertiaBase<Derived_t> & X)
    { 
      X.disp(os);
      return os;
    }
Valenza Florian's avatar
Valenza Florian committed
84

Valenza Florian's avatar
Valenza Florian committed
85
  }; // class InertiaBase
Valenza Florian's avatar
Valenza Florian committed
86
87
88
89


  template<typename T, int U>
  struct traits< InertiaTpl<T, U> >
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
90
  {
91
    typedef T Scalar;
Valenza Florian's avatar
Valenza Florian committed
92
93
94
95
96
97
98
99
100
    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 Matrix6 ActionMatrix_t;
    typedef Vector3 Angular_t;
    typedef Vector3 Linear_t;
101
102
    typedef const Vector3 ConstAngular_t;
    typedef const Vector3 ConstLinear_t;
Valenza Florian's avatar
Valenza Florian committed
103
104
105
106
107
108
109
110
111
    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
    };
Valenza Florian's avatar
Valenza Florian committed
112
  }; // traits InertiaTpl
Valenza Florian's avatar
Valenza Florian committed
113
114

  template<typename _Scalar, int _Options>
Valenza Florian's avatar
Valenza Florian committed
115
116
  class InertiaTpl : public InertiaBase< InertiaTpl< _Scalar, _Options > >
  {
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
117
  public:
Valenza Florian's avatar
Valenza Florian committed
118
    friend class InertiaBase< InertiaTpl< _Scalar, _Options > >;
119
    SPATIAL_TYPEDEF_TEMPLATE(InertiaTpl);
120
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
121
    
122
123
    typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare;
    
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
124
125
  public:
    // Constructors
Valenza Florian's avatar
Valenza Florian committed
126
127
    InertiaTpl() : m(), c(), I() {}

128
    InertiaTpl(const Scalar m_, const Vector3 &c_, const Matrix3 &I_)
129
130
131
132
    : m(m_), c(c_), I(I_)
    {}
    
    InertiaTpl(const Matrix6 & I6)
Valenza Florian's avatar
Valenza Florian committed
133
    {
134
      assert((I6 - I6.transpose()).isMuchSmallerThan(I6));
135
136
      m = I6(LINEAR, LINEAR);
      const Matrix3 & mc_cross = I6.template block <3,3> (ANGULAR,LINEAR);
137
      c = unSkew(mc_cross);
138
139
140
141
142
143
      c /= m;
      
      Matrix3 I3 (mc_cross * mc_cross);
      I3 /= m;
      I3 += I6.template block<3,3>(ANGULAR,ANGULAR);
      I = Symmetric3(I3);
Valenza Florian's avatar
Valenza Florian committed
144
    }
Valenza Florian's avatar
Valenza Florian committed
145

146
    InertiaTpl(Scalar _m, 
Valenza Florian's avatar
Valenza Florian committed
147
148
149
150
151
152
     const Vector3 &_c, 
     const Symmetric3 &_I)
    : m(_m),
    c(_c),
    I(_I)
    {
Valenza Florian's avatar
Valenza Florian committed
153

Valenza Florian's avatar
Valenza Florian committed
154
    }
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
155
    InertiaTpl(const InertiaTpl & clone)  // Clone constructor for std::vector 
Valenza Florian's avatar
Valenza Florian committed
156
157
158
159
    : m(clone.m),
    c(clone.c),
    I(clone.I)    
    {
Valenza Florian's avatar
Valenza Florian committed
160

Valenza Florian's avatar
Valenza Florian committed
161
    }
Valenza Florian's avatar
Valenza Florian committed
162

Valenza Florian's avatar
Valenza Florian committed
163
164
165
166
167
168
    template<typename S2,int O2>
    InertiaTpl( const InertiaTpl<S2,O2> & clone )
    : m(clone.mass()),
    c(clone.lever()),
    I(clone.inertia().matrix())
    {
Valenza Florian's avatar
Valenza Florian committed
169

Valenza Florian's avatar
Valenza Florian committed
170
    }
Valenza Florian's avatar
Valenza Florian committed
171

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
172
    // Initializers
Valenza Florian's avatar
Valenza Florian committed
173
174
175
176
177
178
    static InertiaTpl Zero() 
    {
      return InertiaTpl(0., 
                        Vector3::Zero(), 
                        Symmetric3::Zero());
    }
179
180
    
    void setZero() { m = 0.; c.setZero(); I.setZero(); }
Valenza Florian's avatar
Valenza Florian committed
181

Valenza Florian's avatar
Valenza Florian committed
182
183
184
185
186
187
    static InertiaTpl Identity() 
    {
      return InertiaTpl(1., 
                        Vector3::Zero(), 
                        Symmetric3::Identity());
    }
188
189
    
    void setIdentity () { m = 1.; c.setZero(); I.setIdentity(); }
Valenza Florian's avatar
Valenza Florian committed
190

Valenza Florian's avatar
Valenza Florian committed
191
192
193
    static InertiaTpl Random()
    {
        // We have to shoot "I" definite positive and not only symmetric.
194
      return InertiaTpl(Eigen::internal::random<Scalar>()+1,
Valenza Florian's avatar
Valenza Florian committed
195
196
197
                        Vector3::Random(),
                        Symmetric3::RandomPositive());
    }
198
199

    static InertiaTpl FromEllipsoid(
200
        const Scalar m, const Scalar x, const Scalar y, const Scalar z)
201
    {
202
203
204
      Scalar a = m * (y*y + z*z) / 5;
      Scalar b = m * (x*x + z*z) / 5;
      Scalar c = m * (y*y + x*x) / 5;
205
206
207
208
      return InertiaTpl(m, Vector3::Zero(), Symmetric3(a, 0, b, 0, 0, c));
    }

    static InertiaTpl FromCylinder(
209
        const Scalar m, const Scalar r, const Scalar l)
210
    {
211
212
      Scalar a = m * (r*r / 4 + l*l / 12);
      Scalar c = m * (r*r / 2);
213
214
215
216
      return InertiaTpl(m, Vector3::Zero(), Symmetric3(a, 0, a, 0, 0, c));
    }

    static InertiaTpl FromBox(
217
        const Scalar m, const Scalar x, const Scalar y, const Scalar z)
218
    {
219
220
221
      Scalar a = m * (y*y + z*z) / 12;
      Scalar b = m * (x*x + z*z) / 12;
      Scalar c = m * (y*y + x*x) / 12;
222
223
224
      return InertiaTpl(m, Vector3::Zero(), Symmetric3(a, 0, b, 0, 0, c));
    }

225
226
227
    
    void setRandom()
    {
228
      m = static_cast<Scalar> (std::rand()) / static_cast<Scalar> (RAND_MAX);
229
230
      c.setRandom(); I.setRandom();
    }
Valenza Florian's avatar
Valenza Florian committed
231

Valenza Florian's avatar
Valenza Florian committed
232
233
234
    Matrix6 matrix_impl() const
    {
      Matrix6 M;
235
      
236
237
      M.template block<3,3>(LINEAR, LINEAR ).setZero ();
      M.template block<3,3>(LINEAR, LINEAR ).diagonal ().fill (m);
238
      M.template block<3,3>(ANGULAR,LINEAR ) = alphaSkew(m,c);
Valenza Florian's avatar
Valenza Florian committed
239
      M.template block<3,3>(LINEAR, ANGULAR) = -M.template block<3,3> (ANGULAR, LINEAR);
240
      M.template block<3,3>(ANGULAR,ANGULAR) = (typename Symmetric3::Matrix3)(I - AlphaSkewSquare(m,c));
Valenza Florian's avatar
Valenza Florian committed
241
242
243
244

      return M;
    }

Valenza Florian's avatar
Valenza Florian committed
245
    // Arithmetic operators
Valenza Florian's avatar
Valenza Florian committed
246
    InertiaTpl& __equl__ (const InertiaTpl& clone)
Valenza Florian's avatar
Valenza Florian committed
247
248
249
250
251
252
    {
      m=clone.m; c=clone.c; I=clone.I;
      return *this;
    }

    // Requiered by std::vector boost::python bindings. 
253
    bool isEqual( const InertiaTpl& Y2 ) const
Valenza Florian's avatar
Valenza Florian committed
254
255
256
    { 
      return (m==Y2.m) && (c==Y2.c) && (I==Y2.I);
    }
257
258
259
260
261
262
263
264
    
    bool isApprox_impl(const InertiaTpl & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    {
      using std::fabs;
      return fabs(m - other.m) <= prec
      && c.isApprox(other.c,prec)
      && I.isApprox(other.I,prec);
    }
Valenza Florian's avatar
Valenza Florian committed
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292

    InertiaTpl __plus__(const InertiaTpl &Yb) const
    {
      /* Y_{a+b} = ( m_a+m_b,
       *             (m_a*c_a + m_b*c_b ) / (m_a + m_b),
       *             I_a + I_b - (m_a*m_b)/(m_a+m_b) * AB_x * AB_x )
       */

      const double & mab = m+Yb.m;
      const Vector3 & AB = (c-Yb.c).eval();
      return InertiaTpl( mab,
                         (m*c+Yb.m*Yb.c)/mab,
                         I+Yb.I - (m*Yb.m/mab)* typename Symmetric3::SkewSquare(AB));
    }

    InertiaTpl& __pequ__(const InertiaTpl &Yb)
    {
      const InertiaTpl& Ya = *this;
      const double & mab = Ya.m+Yb.m;
      const Vector3 & AB = (Ya.c-Yb.c).eval();
      c *= m; c += Yb.m*Yb.c; c /= mab;
      I += Yb.I; I -= (Ya.m*Yb.m/mab)* typename Symmetric3::SkewSquare(AB);
      m  = mab;
      return *this;
    }

    Force __mult__(const Motion &v) const 
    {
jcarpent's avatar
jcarpent committed
293
294
295
296
      Force f;
      f.linear() = m*(v.linear() - c.cross(v.angular()));
      f.angular() = c.cross(f.linear()) + I*v.angular();
      return f;
Valenza Florian's avatar
Valenza Florian committed
297
    }
jcarpent's avatar
jcarpent committed
298
    
299
    Scalar vtiv_impl(const Motion & v) const
jcarpent's avatar
jcarpent committed
300
301
    {
      const Vector3 cxw (c.cross(v.angular()));
302
      Scalar res = m * (v.linear().squaredNorm() - 2.*v.linear().dot(cxw));
jcarpent's avatar
jcarpent committed
303
304
305
306
307
308
      const Vector3 mcxcxw (-m*c.cross(cxw));
      res += v.angular().dot(mcxcxw);
      res += I.vtiv(v.angular());
      
      return res;
    }
Valenza Florian's avatar
Valenza Florian committed
309
310

    // Getters
311
    Scalar           mass()    const { return m; }
Valenza Florian's avatar
Valenza Florian committed
312
313
    const Vector3 &    lever()   const { return c; }
    const Symmetric3 & inertia() const { return I; }
jcarpent's avatar
jcarpent committed
314
    
315
    Scalar &   mass()    { return m; }
jcarpent's avatar
jcarpent committed
316
317
    Vector3 &    lever()   { return c; }
    Symmetric3 & inertia() { return I; }
Valenza Florian's avatar
Valenza Florian committed
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345

    /// aI = aXb.act(bI)
    InertiaTpl se3Action_impl(const SE3 & M) const
    {
      /* The multiplication RIR' has a particular form that could be used, however it
       * does not seems to be more efficient, see http://stackoverflow.com/questions/
       * 13215467/eigen-best-way-to-evaluate-asa-transpose-and-store-the-result-in-a-symmetric .*/
       return InertiaTpl( m,
                          M.translation()+M.rotation()*c,
                          I.rotate(M.rotation()) );
     }

    ///bI = aXb.actInv(aI)
    InertiaTpl se3ActionInverse_impl(const SE3 & M) const
    {
      return InertiaTpl(m,
                        M.rotation().transpose()*(c-M.translation()),
                        I.rotate(M.rotation().transpose()) );
    }

    Force vxiv( const Motion& v ) const 
    {
      const Vector3 & mcxw = m*c.cross(v.angular());
      const Vector3 & mv_mcxw = m*v.linear()-mcxw;
      return Force( v.angular().cross(mv_mcxw),
                    v.angular().cross(c.cross(mv_mcxw)+I*v.angular())-v.linear().cross(mcxw) );
    }

346
    void disp_impl(std::ostream & os) const
Valenza Florian's avatar
Valenza Florian committed
347
    {
348
349
      os  << "  m = " << m << "\n"
      << "  c = " << c.transpose() << "\n"
350
      << "  I = \n" << I.matrix() << "";
Valenza Florian's avatar
Valenza Florian committed
351
352
    }

jcarpent's avatar
jcarpent committed
353
  protected:
354
    Scalar m;
Valenza Florian's avatar
Valenza Florian committed
355
356
357
358
    Vector3 c;
    Symmetric3 I;
    
  }; // class InertiaTpl
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
359
360
361
362
    
} // namespace se3

#endif // ifndef __se3_inertia_hpp__