inertia.hpp 12.4 KB
Newer Older
1
//
2
// Copyright (c) 2015-2017 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); }
64
    Matrix6 variation(const Motion & v) const { return derived().variation_impl(v); }
Valenza Florian's avatar
Valenza Florian committed
65

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

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

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

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

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


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

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

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

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

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

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

Valenza Florian's avatar
Valenza Florian committed
164
165
166
167
168
169
    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
170

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

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

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

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

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

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

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

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

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

      return M;
    }

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

    // Requiered by std::vector boost::python bindings. 
254
    bool isEqual( const InertiaTpl& Y2 ) const
Valenza Florian's avatar
Valenza Florian committed
255
256
257
    { 
      return (m==Y2.m) && (c==Y2.c) && (I==Y2.I);
    }
258
259
260
261
262
263
264
265
    
    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
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
293

    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
294
295
296
297
      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
298
    }
jcarpent's avatar
jcarpent committed
299
    
300
    Scalar vtiv_impl(const Motion & v) const
jcarpent's avatar
jcarpent committed
301
302
    {
      const Vector3 cxw (c.cross(v.angular()));
303
      Scalar res = m * (v.linear().squaredNorm() - 2.*v.linear().dot(cxw));
jcarpent's avatar
jcarpent committed
304
305
306
307
308
309
      const Vector3 mcxcxw (-m*c.cross(cxw));
      res += v.angular().dot(mcxcxw);
      res += I.vtiv(v.angular());
      
      return res;
    }
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
    
    Matrix6 variation(const Motion & v) const
    {
      Matrix6 res;
      const Motion mv(v*m);
      
      res.template block<3,3>(LINEAR,ANGULAR) = -skew(mv.linear()) - skewSquare(mv.angular(),c) + skewSquare(c,mv.angular());
      res.template block<3,3>(ANGULAR,LINEAR) = res.template block<3,3>(LINEAR,ANGULAR).transpose();
      
      res.template block<3,3>(LINEAR,LINEAR) = mv.linear()*c.transpose(); // use as temporary variable
      res.template block<3,3>(ANGULAR,ANGULAR) = res.template block<3,3>(LINEAR,LINEAR) - res.template block<3,3>(LINEAR,LINEAR).transpose();
      res.template block<3,3>(ANGULAR,ANGULAR) = -skewSquare(mv.linear(),c) - skewSquare(c,mv.linear());
      
      res.template block<3,3>(LINEAR,LINEAR) = (typename Symmetric3::Matrix3)(I - AlphaSkewSquare(m,c));
      
      res.template block<3,3>(ANGULAR,ANGULAR) -= res.template block<3,3>(LINEAR,LINEAR) * skew(v.angular());
      res.template block<3,3>(ANGULAR,ANGULAR) += cross(v.angular(),res.template block<3,3>(LINEAR,LINEAR));
      
      res.template block<3,3>(LINEAR,LINEAR).setZero();
      return res;
    }
Valenza Florian's avatar
Valenza Florian committed
331
332

    // Getters
333
    Scalar           mass()    const { return m; }
Valenza Florian's avatar
Valenza Florian committed
334
335
    const Vector3 &    lever()   const { return c; }
    const Symmetric3 & inertia() const { return I; }
jcarpent's avatar
jcarpent committed
336
    
337
    Scalar &   mass()    { return m; }
jcarpent's avatar
jcarpent committed
338
339
    Vector3 &    lever()   { return c; }
    Symmetric3 & inertia() { return I; }
Valenza Florian's avatar
Valenza Florian committed
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367

    /// 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) );
    }

368
    void disp_impl(std::ostream & os) const
Valenza Florian's avatar
Valenza Florian committed
369
    {
370
371
      os  << "  m = " << m << "\n"
      << "  c = " << c.transpose() << "\n"
372
      << "  I = \n" << I.matrix() << "";
Valenza Florian's avatar
Valenza Florian committed
373
374
    }

jcarpent's avatar
jcarpent committed
375
  protected:
376
    Scalar m;
Valenza Florian's avatar
Valenza Florian committed
377
378
379
380
    Vector3 c;
    Symmetric3 I;
    
  }; // class InertiaTpl
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
381
382
383
384
    
} // namespace se3

#endif // ifndef __se3_inertia_hpp__