14 typedef Eigen::Matrix<Scalar, Eigen::Dynamic,1>
VectorXd;
16 typedef Eigen::Map< SE3::Quaternion>
QuatMap;
30 return bp::make_tuple (
32 q.x(),
q.y(),
q.z(),
q.w());
35 template <
typename TupleOrList>
40 (Scalar)bp::extract<Scalar>(v[6]),
41 (Scalar)bp::extract<Scalar>(v[3]),
42 (Scalar)bp::extract<Scalar>(v[4]),
43 (Scalar)bp::extract<Scalar>(v[5]));
45 (Scalar)bp::extract<Scalar>(v[0]),
46 (Scalar)bp::extract<Scalar>(v[1]),
47 (Scalar)bp::extract<Scalar>(v[2]));
48 return SE3 (q.matrix(),
t);
51 template <
typename Vector7Like>
55 QuatConstMap
q (v.template tail<4>().data());
56 return SE3 (
q.matrix(), v.template head<3>());
61 const char* doc1 =
"Convert the input SE3 object to a numpy array.";
63 const char* doc1_tuple =
"Convert the input SE3 object to a 7D tuple of floats [X,Y,Z,x,y,z,w].";
66 const char* doc2 =
"Reverse function of SE3ToXYZQUAT: convert [X,Y,Z,x,y,z,w] to an SE3 element.";
67 bp::def(
"XYZQUATToSE3",
68 static_cast<SE3 (*) (
const bp::tuple&)
> (XYZQUATToSE3_bp<bp::tuple>),
69 bp::arg(
"tuple"),doc2);
70 bp::def(
"XYZQUATToSE3",
71 static_cast<SE3 (*) (
const bp::list &)
> (XYZQUATToSE3_bp<bp::list >),
72 bp::arg(
"list"),doc2);
73 bp::def(
"XYZQUATToSE3",
static_cast<SE3 (*) (
const VectorXd &)
> (XYZQUATToSE3_ei<VectorXd >),
74 bp::arg(
"array"),doc2);
#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) ...
bp::tuple SE3ToXYZQUATtuple(const SE3 &M)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
SE3 XYZQUATToSE3_ei(const Vector7Like &v)
VectorXd SE3ToXYZQUAT(const SE3 &M)
Eigen::Quaternion< Scalar, Options > Quaternion
Eigen::Map< SE3::Quaternion > QuatMap
ConstLinearRef translation() const
traits< SE3Tpl >::Vector3 Vector3
Main pinocchio namespace.
Eigen::Matrix< Scalar, 7, 1 > Vector7d
Eigen::Map< const SE3::Quaternion > QuatConstMap
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
SE3 XYZQUATToSE3_bp(const TupleOrList &v)
ConstAngularRef rotation() const