7 #include "pinocchio/spatial/se3.hpp" 8 #include "pinocchio/spatial/skew.hpp" 17 template<
typename Vector3>
18 Eigen::Matrix<typename Vector3::Scalar,3,3,Vector3::Options>
skew(
const Vector3 &
v)
24 template<
typename Vector3>
25 Eigen::Matrix<typename Vector3::Scalar,3,3,Vector3::Options>
skewSquare(
const Vector3 &
u,
const Vector3 &
v)
31 template<
typename Matrix3>
32 Eigen::Matrix<typename Matrix3::Scalar,3,1,Matrix3::Options>
unSkew(
const Matrix3 & mat)
42 bp::def(
"skew",&skew<Vector3>,
44 "Computes the skew representation of a given 3d vector, " 45 "i.e. the antisymmetric matrix representation of the cross product operator, aka U = [u]x.\n" 47 "\tu: the input vector of dimension 3");
49 bp::def(
"skewSquare",&skewSquare<Vector3>,
51 "Computes the skew square representation of two given 3d vectors, " 52 "i.e. the antisymmetric matrix representation of the chained cross product operator, u x (v x w), where w is another 3d vector.\n" 54 "\tu: the first input vector of dimension 3\n" 55 "\tv: the second input vector of dimension 3");
57 bp::def(
"unSkew",&unSkew<Matrix3>,
59 "Inverse of skew operator. From a given skew symmetric matrix U (i.e U = -U.T)" 60 "of dimension 3x3, it extracts the supporting vector, i.e. the entries of U.\n" 61 "Mathematically speacking, it computes v such that U.dot(x) = cross(u, x).\n" 63 "\tU: the input skew symmetric matrix of dimension 3x3.");
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...
Eigen::Matrix< typename Vector3::Scalar, 3, 3, Vector3::Options > skew(const Vector3 &v)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > const Scalar & u
Eigen::Matrix< typename Vector3::Scalar, 3, 3, Vector3::Options > skewSquare(const Vector3 &u, const Vector3 &v)
traits< SE3Tpl >::Vector3 Vector3
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.
Eigen::Matrix< typename Matrix3::Scalar, 3, 1, Matrix3::Options > unSkew(const Matrix3 &mat)
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...
traits< SE3Tpl >::Matrix3 Matrix3