5 #ifndef __pinocchio_skew_hpp__ 6 #define __pinocchio_skew_hpp__ 8 #include "pinocchio/macros.hpp" 20 template <
typename Vector3,
typename Matrix3>
21 inline void skew(
const Eigen::MatrixBase<Vector3> &
v,
22 const Eigen::MatrixBase<Matrix3> & M)
24 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
25 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
28 typedef typename Matrix3::RealScalar
Scalar;
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);
44 inline Eigen::Matrix<typename D::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(D)::Options>
45 skew(
const Eigen::MatrixBase<D> &
v)
47 Eigen::Matrix<typename D::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(D)::Options>
M;
59 template <
typename Vector3Like,
typename Matrix3Like>
60 inline void addSkew(
const Eigen::MatrixBase<Vector3Like> &
v,
61 const Eigen::MatrixBase<Matrix3Like> & M)
63 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
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]; ;
81 template <
typename Matrix3,
typename Vector3>
82 inline void unSkew(
const Eigen::MatrixBase<Matrix3> & M,
83 const Eigen::MatrixBase<Vector3> &
v)
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));
90 typedef typename Vector3::RealScalar
Scalar;
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));
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)
110 Eigen::Matrix<typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Options>
v;
123 template <
typename Scalar,
typename Vector3,
typename Matrix3>
125 const Eigen::MatrixBase<Vector3> &
v,
126 const Eigen::MatrixBase<Matrix3> & M)
128 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
129 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
132 typedef typename Matrix3::RealScalar RealScalar;
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);
148 template <
typename Scalar,
typename Vector3>
149 inline Eigen::Matrix<typename Vector3::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3)::Options>
151 const Eigen::MatrixBase<Vector3> &
v)
153 Eigen::Matrix<typename Vector3::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3)::Options>
M;
165 template <
typename V1,
typename V2,
typename Matrix3>
167 const Eigen::MatrixBase<V2> &
v,
168 const Eigen::MatrixBase<Matrix3> &
C)
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);
175 typedef typename Matrix3::RealScalar
Scalar;
177 C_.noalias() = v*u.transpose();
178 const Scalar udotv(u.dot(v));
179 C_.diagonal().array() -= udotv;
190 template <
typename V1,
typename V2>
191 inline Eigen::Matrix<typename V1::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(V1)::Options>
193 const Eigen::MatrixBase<V2> &
v)
196 Eigen::Matrix<typename V1::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(V1)::Options>
M;
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)
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);
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);
234 template <
typename Vector3,
typename Matrix3x>
236 cross(
const Eigen::MatrixBase<Vector3> &
v,
237 const Eigen::MatrixBase<Matrix3x> & M)
246 #endif // ifndef __pinocchio_skew_hpp__ JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
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...
#define PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(type, M, nrows, ncols)
Ensure that a matrix (or vector) is of correct size (compile-time and run-time assertion) ...
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > const Scalar & u
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.
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...
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...
Main pinocchio namespace.
PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(const ModelTpl< Scalar
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
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 ( )
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...