pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
skew.hpp
1 //
2 // Copyright (c) 2015-2018 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_skew_hpp__
6 #define __pinocchio_skew_hpp__
7 
8 #include "pinocchio/macros.hpp"
9 
10 namespace pinocchio
11 {
12 
20  template <typename Vector3, typename Matrix3>
21  inline void skew(const Eigen::MatrixBase<Vector3> & v,
22  const Eigen::MatrixBase<Matrix3> & M)
23  {
24  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
25  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
26 
27  Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,M);
28  typedef typename Matrix3::RealScalar Scalar;
29 
30  M_(0,0) = Scalar(0); M_(0,1) = -v[2]; M_(0,2) = v[1];
31  M_(1,0) = v[2]; M_(1,1) = Scalar(0); M_(1,2) = -v[0];
32  M_(2,0) = -v[1]; M_(2,1) = v[0]; M_(2,2) = Scalar(0);
33  }
34 
43  template <typename D>
44  inline Eigen::Matrix<typename D::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(D)::Options>
45  skew(const Eigen::MatrixBase<D> & v)
46  {
47  Eigen::Matrix<typename D::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(D)::Options> M;
48  skew(v,M);
49  return M;
50  }
51 
59  template <typename Vector3Like, typename Matrix3Like>
60  inline void addSkew(const Eigen::MatrixBase<Vector3Like> & v,
61  const Eigen::MatrixBase<Matrix3Like> & M)
62  {
63  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
64  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like,3,3);
65 
66  Matrix3Like & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,M);
67 
68  M_(0,1) -= v[2]; M_(0,2) += v[1];
69  M_(1,0) += v[2]; M_(1,2) -= v[0];
70  M_(2,0) -= v[1]; M_(2,1) += v[0]; ;
71  }
72 
81  template <typename Matrix3, typename Vector3>
82  inline void unSkew(const Eigen::MatrixBase<Matrix3> & M,
83  const Eigen::MatrixBase<Vector3> & v)
84  {
85  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
86  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
87  assert((M + M.transpose()).isMuchSmallerThan(M));
88 
89  Vector3 & v_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3,v);
90  typedef typename Vector3::RealScalar Scalar;
91 
92  v_[0] = Scalar(0.5) * (M(2,1) - M(1,2));
93  v_[1] = Scalar(0.5) * (M(0,2) - M(2,0));
94  v_[2] = Scalar(0.5) * (M(1,0) - M(0,1));
95  }
96 
106  template <typename Matrix3>
107  inline Eigen::Matrix<typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Options>
108  unSkew(const Eigen::MatrixBase<Matrix3> & M)
109  {
110  Eigen::Matrix<typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Options> v;
111  unSkew(M,v);
112  return v;
113  }
114 
123  template <typename Scalar, typename Vector3, typename Matrix3>
124  void alphaSkew(const Scalar alpha,
125  const Eigen::MatrixBase<Vector3> & v,
126  const Eigen::MatrixBase<Matrix3> & M)
127  {
128  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
129  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
130 
131  Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,M);
132  typedef typename Matrix3::RealScalar RealScalar;
133 
134  M_(0,0) = RealScalar(0); M_(0,1) = -v[2] * alpha; M_(0,2) = v[1] * alpha;
135  M_(1,0) = -M_(0,1); M_(1,1) = RealScalar(0); M_(1,2) = -v[0] * alpha;
136  M_(2,0) = -M_(0,2); M_(2,1) = -M_(1,2); M_(2,2) = RealScalar(0);
137  }
138 
148  template <typename Scalar, typename Vector3>
149  inline Eigen::Matrix<typename Vector3::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3)::Options>
150  alphaSkew(const Scalar alpha,
151  const Eigen::MatrixBase<Vector3> & v)
152  {
153  Eigen::Matrix<typename Vector3::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3)::Options> M;
154  alphaSkew(alpha,v,M);
155  return M;
156  }
157 
165  template <typename V1, typename V2, typename Matrix3>
166  inline void skewSquare(const Eigen::MatrixBase<V1> & u,
167  const Eigen::MatrixBase<V2> & v,
168  const Eigen::MatrixBase<Matrix3> & C)
169  {
170  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V1,3);
171  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V2,3);
172  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
173 
174  Matrix3 & C_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,C);
175  typedef typename Matrix3::RealScalar Scalar;
176 
177  C_.noalias() = v*u.transpose();
178  const Scalar udotv(u.dot(v));
179  C_.diagonal().array() -= udotv;
180  }
181 
190  template <typename V1, typename V2>
191  inline Eigen::Matrix<typename V1::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(V1)::Options>
192  skewSquare(const Eigen::MatrixBase<V1> & u,
193  const Eigen::MatrixBase<V2> & v)
194  {
195 
196  Eigen::Matrix<typename V1::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(V1)::Options> M;
197  skewSquare(u,v,M);
198  return M;
199  }
200 
210  template <typename Vector3, typename Matrix3xIn, typename Matrix3xOut>
211  inline void cross(const Eigen::MatrixBase<Vector3> & v,
212  const Eigen::MatrixBase<Matrix3xIn> & Min,
213  const Eigen::MatrixBase<Matrix3xOut> & Mout)
214  {
215  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
216  EIGEN_STATIC_ASSERT(Matrix3xIn::RowsAtCompileTime==3,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
217  EIGEN_STATIC_ASSERT(Matrix3xOut::RowsAtCompileTime==3,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
218 
219  Matrix3xOut & Mout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut,Mout);
220 
221  Mout_.row(0) = v[1]*Min.row(2) - v[2]*Min.row(1);
222  Mout_.row(1) = v[2]*Min.row(0) - v[0]*Min.row(2);
223  Mout_.row(2) = v[0]*Min.row(1) - v[1]*Min.row(0);
224  }
225 
234  template <typename Vector3, typename Matrix3x>
235  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3x)
236  cross(const Eigen::MatrixBase<Vector3> & v,
237  const Eigen::MatrixBase<Matrix3x> & M)
238  {
239  typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3x) res(3,M.cols());
240  cross(v,M,res);
241  return res;
242  }
243 
244 } // namespace pinocchio
245 
246 #endif // ifndef __pinocchio_skew_hpp__
void unSkew(const Eigen::MatrixBase< Matrix3 > &M, const Eigen::MatrixBase< Vector3 > &v)
Inverse of skew operator. From a given skew-symmetric matrix M of dimension 3x3, it extracts the supp...
Definition: skew.hpp:82
Definition: casadi.hpp:47
void cross(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3xIn > &Min, const Eigen::MatrixBase< Matrix3xOut > &Mout)
Applies the cross product onto the columns of M.
Definition: skew.hpp:211
void addSkew(const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix3Like > &M)
Add skew matrix represented by a 3d vector to a given matrix, i.e. add the antisymmetric matrix repre...
Definition: skew.hpp:60
void skewSquare(const Eigen::MatrixBase< V1 > &u, const Eigen::MatrixBase< V2 > &v, const Eigen::MatrixBase< Matrix3 > &C)
Computes the square cross product linear operator C(u,v) such that for any vector w...
Definition: skew.hpp:166
Main pinocchio namespace.
Definition: treeview.dox:24
void alphaSkew(const Scalar alpha, const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector multiplied by a given scalar. i.e. the antisymmetric matrix representation of the cross product operator ( )
Definition: skew.hpp:124
void skew(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector, i.e. the antisymmetric matrix representation o...
Definition: skew.hpp:21