pinocchio  2.7.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
symmetric3.hpp
1 //
2 // Copyright (c) 2014-2019 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_symmetric3_hpp__
6 #define __pinocchio_symmetric3_hpp__
7 
8 #include "pinocchio/macros.hpp"
9 #include "pinocchio/math/matrix.hpp"
10 
11 namespace pinocchio
12 {
13 
14  template<typename _Scalar, int _Options>
15  class Symmetric3Tpl
16  {
17  public:
18  typedef _Scalar Scalar;
19  enum { Options = _Options };
20  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
21  typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
22  typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
23  typedef Eigen::Matrix<Scalar,2,2,Options> Matrix2;
24  typedef Eigen::Matrix<Scalar,3,2,Options> Matrix32;
25 
26  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
27 
28  public:
29  Symmetric3Tpl(): m_data() {}
30 
31  template<typename Sc,int Opt>
32  explicit Symmetric3Tpl(const Eigen::Matrix<Sc,3,3,Opt> & I)
33  {
34  assert( (I-I.transpose()).isMuchSmallerThan(I) );
35  m_data(0) = I(0,0);
36  m_data(1) = I(1,0); m_data(2) = I(1,1);
37  m_data(3) = I(2,0); m_data(4) = I(2,1); m_data(5) = I(2,2);
38  }
39 
40  explicit Symmetric3Tpl(const Vector6 & I) : m_data(I) {}
41 
42  Symmetric3Tpl(const Scalar & a0, const Scalar & a1, const Scalar & a2,
43  const Scalar & a3, const Scalar & a4, const Scalar & a5)
44  { m_data << a0,a1,a2,a3,a4,a5; }
45 
46  static Symmetric3Tpl Zero() { return Symmetric3Tpl(Vector6::Zero()); }
47  void setZero() { m_data.setZero(); }
48 
49  static Symmetric3Tpl Random() { return RandomPositive(); }
50  void setRandom()
51  {
52  Scalar
53  a = Scalar(std::rand())/RAND_MAX*2.0-1.0,
54  b = Scalar(std::rand())/RAND_MAX*2.0-1.0,
55  c = Scalar(std::rand())/RAND_MAX*2.0-1.0,
56  d = Scalar(std::rand())/RAND_MAX*2.0-1.0,
57  e = Scalar(std::rand())/RAND_MAX*2.0-1.0,
58  f = Scalar(std::rand())/RAND_MAX*2.0-1.0;
59 
60  m_data << a, b, c, d, e, f;
61  }
62 
63  static Symmetric3Tpl Identity() { return Symmetric3Tpl(1, 0, 1, 0, 0, 1); }
64  void setIdentity() { m_data << 1, 0, 1, 0, 0, 1; }
65 
66  /* Required by Inertia::operator== */
67  bool operator==(const Symmetric3Tpl & other) const
68  { return m_data == other.m_data; }
69 
70  bool operator!=(const Symmetric3Tpl & other) const
71  { return !(*this == other); }
72 
73  bool isApprox(const Symmetric3Tpl & other,
74  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
75  { return m_data.isApprox(other.m_data,prec); }
76 
77  bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
78  { return m_data.isZero(prec); }
79 
80  void fill(const Scalar value) { m_data.fill(value); }
81 
82  struct SkewSquare
83  {
84  const Vector3 & v;
85  SkewSquare( const Vector3 & v ) : v(v) {}
86  operator Symmetric3Tpl () const
87  {
88  const Scalar & x = v[0], & y = v[1], & z = v[2];
89  return Symmetric3Tpl( -y*y-z*z,
90  x*y , -x*x-z*z,
91  x*z , y*z , -x*x-y*y );
92  }
93  }; // struct SkewSquare
94 
95  Symmetric3Tpl operator- (const SkewSquare & v) const
96  {
97  const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
98  return Symmetric3Tpl(m_data[0]+y*y+z*z,
99  m_data[1]-x*y,m_data[2]+x*x+z*z,
100  m_data[3]-x*z,m_data[4]-y*z,m_data[5]+x*x+y*y);
101  }
102 
103  Symmetric3Tpl& operator-= (const SkewSquare & v)
104  {
105  const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
106  m_data[0]+=y*y+z*z;
107  m_data[1]-=x*y; m_data[2]+=x*x+z*z;
108  m_data[3]-=x*z; m_data[4]-=y*z; m_data[5]+=x*x+y*y;
109  return *this;
110  }
111 
112  template<typename D>
113  friend Matrix3 operator- (const Symmetric3Tpl & S, const Eigen::MatrixBase <D> & M)
114  {
115  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D,3,3);
116  Matrix3 result (S.matrix());
117  result -= M;
118 
119  return result;
120  }
121 
122  struct AlphaSkewSquare
123  {
124  const Scalar & m;
125  const Vector3 & v;
126 
127  AlphaSkewSquare(const Scalar & m, const SkewSquare & v) : m(m),v(v.v) {}
128  AlphaSkewSquare(const Scalar & m, const Vector3 & v) : m(m),v(v) {}
129 
130  operator Symmetric3Tpl () const
131  {
132  const Scalar & x = v[0], & y = v[1], & z = v[2];
133  return Symmetric3Tpl(-m*(y*y+z*z),
134  m* x*y,-m*(x*x+z*z),
135  m* x*z,m* y*z,-m*(x*x+y*y));
136  }
137  };
138 
139  friend AlphaSkewSquare operator* (const Scalar & m, const SkewSquare & sk)
140  { return AlphaSkewSquare(m,sk); }
141 
142  Symmetric3Tpl operator- (const AlphaSkewSquare & v) const
143  {
144  const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
145  return Symmetric3Tpl(m_data[0]+v.m*(y*y+z*z),
146  m_data[1]-v.m* x*y, m_data[2]+v.m*(x*x+z*z),
147  m_data[3]-v.m* x*z, m_data[4]-v.m* y*z,
148  m_data[5]+v.m*(x*x+y*y));
149  }
150 
151  Symmetric3Tpl& operator-= (const AlphaSkewSquare & v)
152  {
153  const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
154  m_data[0]+=v.m*(y*y+z*z);
155  m_data[1]-=v.m* x*y; m_data[2]+=v.m*(x*x+z*z);
156  m_data[3]-=v.m* x*z; m_data[4]-=v.m* y*z; m_data[5]+=v.m*(x*x+y*y);
157  return *this;
158  }
159 
160  const Vector6 & data () const {return m_data;}
161  Vector6 & data () {return m_data;}
162 
163  // static Symmetric3Tpl SkewSq( const Vector3 & v )
164  // {
165  // const Scalar & x = v[0], & y = v[1], & z = v[2];
166  // return Symmetric3Tpl(-y*y-z*z,
167  // x*y, -x*x-z*z,
168  // x*z, y*z, -x*x-y*y );
169  // }
170 
171  /* Shoot a positive definite matrix. */
172  static Symmetric3Tpl RandomPositive()
173  {
174  Scalar
175  a = Scalar(std::rand())/RAND_MAX*2.0-1.0,
176  b = Scalar(std::rand())/RAND_MAX*2.0-1.0,
177  c = Scalar(std::rand())/RAND_MAX*2.0-1.0,
178  d = Scalar(std::rand())/RAND_MAX*2.0-1.0,
179  e = Scalar(std::rand())/RAND_MAX*2.0-1.0,
180  f = Scalar(std::rand())/RAND_MAX*2.0-1.0;
181  return Symmetric3Tpl(a*a+b*b+d*d,
182  a*b+b*c+d*e, b*b+c*c+e*e,
183  a*d+b*e+d*f, b*d+c*e+e*f, d*d+e*e+f*f );
184  }
185 
186  Matrix3 matrix() const
187  {
188  Matrix3 res;
189  res(0,0) = m_data(0); res(0,1) = m_data(1); res(0,2) = m_data(3);
190  res(1,0) = m_data(1); res(1,1) = m_data(2); res(1,2) = m_data(4);
191  res(2,0) = m_data(3); res(2,1) = m_data(4); res(2,2) = m_data(5);
192  return res;
193  }
194  operator Matrix3 () const { return matrix(); }
195 
196  Scalar vtiv (const Vector3 & v) const
197  {
198  const Scalar & x = v[0];
199  const Scalar & y = v[1];
200  const Scalar & z = v[2];
201 
202  const Scalar xx = x*x;
203  const Scalar xy = x*y;
204  const Scalar xz = x*z;
205  const Scalar yy = y*y;
206  const Scalar yz = y*z;
207  const Scalar zz = z*z;
208 
209  return m_data(0)*xx + m_data(2)*yy + m_data(5)*zz + 2.*(m_data(1)*xy + m_data(3)*xz + m_data(4)*yz);
210  }
211 
222  template<typename Vector3, typename Matrix3>
223  static void vxs(const Eigen::MatrixBase<Vector3> & v,
224  const Symmetric3Tpl & S3,
225  const Eigen::MatrixBase<Matrix3> & M)
226  {
227  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
228  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
229 
230  const Scalar & a = S3.data()[0];
231  const Scalar & b = S3.data()[1];
232  const Scalar & c = S3.data()[2];
233  const Scalar & d = S3.data()[3];
234  const Scalar & e = S3.data()[4];
235  const Scalar & f = S3.data()[5];
236 
237 
238  const typename Vector3::RealScalar & v0 = v[0];
239  const typename Vector3::RealScalar & v1 = v[1];
240  const typename Vector3::RealScalar & v2 = v[2];
241 
242  Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,M);
243  M_(0,0) = d * v1 - b * v2;
244  M_(1,0) = a * v2 - d * v0;
245  M_(2,0) = b * v0 - a * v1;
246 
247  M_(0,1) = e * v1 - c * v2;
248  M_(1,1) = b * v2 - e * v0;
249  M_(2,1) = c * v0 - b * v1;
250 
251  M_(0,2) = f * v1 - e * v2;
252  M_(1,2) = d * v2 - f * v0;
253  M_(2,2) = e * v0 - d * v1;
254  }
255 
266  template<typename Vector3>
267  Matrix3 vxs(const Eigen::MatrixBase<Vector3> & v) const
268  {
269  Matrix3 M;
270  vxs(v,*this,M);
271  return M;
272  }
273 
283  template<typename Vector3, typename Matrix3>
284  static void svx(const Eigen::MatrixBase<Vector3> & v,
285  const Symmetric3Tpl & S3,
286  const Eigen::MatrixBase<Matrix3> & M)
287  {
288  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
289  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
290 
291  const Scalar & a = S3.data()[0];
292  const Scalar & b = S3.data()[1];
293  const Scalar & c = S3.data()[2];
294  const Scalar & d = S3.data()[3];
295  const Scalar & e = S3.data()[4];
296  const Scalar & f = S3.data()[5];
297 
298  const typename Vector3::RealScalar & v0 = v[0];
299  const typename Vector3::RealScalar & v1 = v[1];
300  const typename Vector3::RealScalar & v2 = v[2];
301 
302  Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,M);
303  M_(0,0) = b * v2 - d * v1;
304  M_(1,0) = c * v2 - e * v1;
305  M_(2,0) = e * v2 - f * v1;
306 
307  M_(0,1) = d * v0 - a * v2;
308  M_(1,1) = e * v0 - b * v2;
309  M_(2,1) = f * v0 - d * v2;
310 
311  M_(0,2) = a * v1 - b * v0;
312  M_(1,2) = b * v1 - c * v0;
313  M_(2,2) = d * v1 - e * v0;
314  }
315 
324  template<typename Vector3>
325  Matrix3 svx(const Eigen::MatrixBase<Vector3> & v) const
326  {
327  Matrix3 M;
328  svx(v,*this,M);
329  return M;
330  }
331 
332  Symmetric3Tpl operator+(const Symmetric3Tpl & s2) const
333  {
334  return Symmetric3Tpl((m_data+s2.m_data).eval());
335  }
336 
337  Symmetric3Tpl & operator+=(const Symmetric3Tpl & s2)
338  {
339  m_data += s2.m_data; return *this;
340  }
341 
342  template<typename V3in, typename V3out>
343  static void rhsMult(const Symmetric3Tpl & S3,
344  const Eigen::MatrixBase<V3in> & vin,
345  const Eigen::MatrixBase<V3out> & vout)
346  {
347  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3in,Vector3);
348  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3out,Vector3);
349 
350  V3out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3out,vout);
351 
352  vout_[0] = S3.m_data(0) * vin[0] + S3.m_data(1) * vin[1] + S3.m_data(3) * vin[2];
353  vout_[1] = S3.m_data(1) * vin[0] + S3.m_data(2) * vin[1] + S3.m_data(4) * vin[2];
354  vout_[2] = S3.m_data(3) * vin[0] + S3.m_data(4) * vin[1] + S3.m_data(5) * vin[2];
355  }
356 
357  template<typename V3>
358  Vector3 operator*(const Eigen::MatrixBase<V3> & v) const
359  {
360  Vector3 res;
361  rhsMult(*this,v,res);
362  return res;
363  }
364 
365  // Matrix3 operator*(const Matrix3 &a) const
366  // {
367  // Matrix3 r;
368  // for(unsigned int i=0; i<3; ++i)
369  // {
370  // r(0,i) = m_data(0) * a(0,i) + m_data(1) * a(1,i) + m_data(3) * a(2,i);
371  // r(1,i) = m_data(1) * a(0,i) + m_data(2) * a(1,i) + m_data(4) * a(2,i);
372  // r(2,i) = m_data(3) * a(0,i) + m_data(4) * a(1,i) + m_data(5) * a(2,i);
373  // }
374  // return r;
375  // }
376 
377  const Scalar& operator()(const int &i,const int &j) const
378  {
379  return ((i!=2)&&(j!=2)) ? m_data[i+j] : m_data[i+j+1];
380  }
381 
382  Symmetric3Tpl operator-(const Matrix3 &S) const
383  {
384  assert( (S-S.transpose()).isMuchSmallerThan(S) );
385  return Symmetric3Tpl( m_data(0)-S(0,0),
386  m_data(1)-S(1,0), m_data(2)-S(1,1),
387  m_data(3)-S(2,0), m_data(4)-S(2,1), m_data(5)-S(2,2) );
388  }
389 
390  Symmetric3Tpl operator+(const Matrix3 &S) const
391  {
392  assert( (S-S.transpose()).isMuchSmallerThan(S) );
393  return Symmetric3Tpl( m_data(0)+S(0,0),
394  m_data(1)+S(1,0), m_data(2)+S(1,1),
395  m_data(3)+S(2,0), m_data(4)+S(2,1), m_data(5)+S(2,2) );
396  }
397 
398  /* --- Symmetric R*S*R' and R'*S*R products --- */
399  public: //private:
400 
403  Matrix32 decomposeltI() const
404  {
405  Matrix32 L;
406  L <<
407  m_data(0) - m_data(5), m_data(1),
408  m_data(1), m_data(2) - m_data(5),
409  2*m_data(3), m_data(4) + m_data(4);
410  return L;
411  }
412 
413  /* R*S*R' */
414  template<typename D>
415  Symmetric3Tpl rotate(const Eigen::MatrixBase<D> & R) const
416  {
417  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D,3,3);
418  assert(isUnitary(R.transpose()*R) && "R is not a Unitary matrix");
419 
420  Symmetric3Tpl Sres;
421 
422  // 4 a
423  const Matrix32 L( decomposeltI() );
424 
425  // Y = R' L ===> (12 m + 8 a)
426  const Matrix2 Y( R.template block<2,3>(1,0) * L );
427 
428  // Sres= Y R ===> (16 m + 8a)
429  Sres.m_data(1) = Y(0,0)*R(0,0) + Y(0,1)*R(0,1);
430  Sres.m_data(2) = Y(0,0)*R(1,0) + Y(0,1)*R(1,1);
431  Sres.m_data(3) = Y(1,0)*R(0,0) + Y(1,1)*R(0,1);
432  Sres.m_data(4) = Y(1,0)*R(1,0) + Y(1,1)*R(1,1);
433  Sres.m_data(5) = Y(1,0)*R(2,0) + Y(1,1)*R(2,1);
434 
435  // r=R' v ( 6m + 3a)
436  const Vector3 r(-R(0,0)*m_data(4) + R(0,1)*m_data(3),
437  -R(1,0)*m_data(4) + R(1,1)*m_data(3),
438  -R(2,0)*m_data(4) + R(2,1)*m_data(3));
439 
440  // Sres_11 (3a)
441  Sres.m_data(0) = L(0,0) + L(1,1) - Sres.m_data(2) - Sres.m_data(5);
442 
443  // Sres + D + (Ev)x ( 9a)
444  Sres.m_data(0) += m_data(5);
445  Sres.m_data(1) += r(2); Sres.m_data(2)+= m_data(5);
446  Sres.m_data(3) +=-r(1); Sres.m_data(4)+= r(0); Sres.m_data(5) += m_data(5);
447 
448  return Sres;
449  }
450 
452  template<typename NewScalar>
453  Symmetric3Tpl<NewScalar,Options> cast() const
454  {
455  return Symmetric3Tpl<NewScalar,Options>(m_data.template cast<NewScalar>());
456  }
457 
458  // TODO: adjust code
459 // bool isValid() const
460 // {
461 // return
462 // m_data(0) >= Scalar(0)
463 // && m_data(2) >= Scalar(0)
464 // && m_data(5) >= Scalar(0);
465 // }
466 
467  protected:
468  Vector6 m_data;
469 
470  };
471 
472 } // namespace pinocchio
473 
474 #endif // ifndef __pinocchio_symmetric3_hpp__
475 
pinocchio::Symmetric3Tpl::vxs
static void vxs(const Eigen::MatrixBase< Vector3 > &v, const Symmetric3Tpl &S3, const Eigen::MatrixBase< Matrix3 > &M)
Performs the operation . This operation is equivalent to applying the cross product of v on each colu...
Definition: symmetric3.hpp:223
pinocchio::isUnitary
bool isUnitary(const Eigen::MatrixBase< MatrixLike > &mat, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Check whether the input matrix is Unitary within the given precision.
Definition: matrix.hpp:140
continuous.x
x
— Training
Definition: continuous.py:157
pinocchio::Symmetric3Tpl::AlphaSkewSquare
Definition: symmetric3.hpp:122
pinocchio::Symmetric3Tpl::svx
static void svx(const Eigen::MatrixBase< Vector3 > &v, const Symmetric3Tpl &S3, const Eigen::MatrixBase< Matrix3 > &M)
Performs the operation .
Definition: symmetric3.hpp:284
pinocchio::Symmetric3Tpl::vxs
Matrix3 vxs(const Eigen::MatrixBase< Vector3 > &v) const
Performs the operation . This operation is equivalent to applying the cross product of v on each colu...
Definition: symmetric3.hpp:267
pinocchio::Symmetric3Tpl::decomposeltI
Matrix32 decomposeltI() const
Computes L for a symmetric matrix A.
Definition: symmetric3.hpp:403
pinocchio::Symmetric3Tpl::SkewSquare
Definition: symmetric3.hpp:82
pinocchio::Symmetric3Tpl
Definition: fwd.hpp:53
pinocchio::Symmetric3Tpl::cast
Symmetric3Tpl< NewScalar, Options > cast() const
Definition: symmetric3.hpp:453
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::Symmetric3Tpl::svx
Matrix3 svx(const Eigen::MatrixBase< Vector3 > &v) const
Performs the operation .
Definition: symmetric3.hpp:325