conversions.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2020 CNRS INRIA
3 //
4 
7 
8 namespace pinocchio
9 {
10  namespace python
11  {
12  namespace bp = boost::python;
14  typedef Eigen::Matrix<Scalar, Eigen::Dynamic,1> VectorXd;
15  typedef Eigen::Matrix<Scalar, 7,1> Vector7d;
16  typedef Eigen::Map< SE3::Quaternion> QuatMap;
17  typedef Eigen::Map<const SE3::Quaternion> QuatConstMap;
18 
19  VectorXd SE3ToXYZQUAT(const SE3& M)
20  {
21  Vector7d res;
22  res.head<3>() = M.translation();
23  QuatMap (res.tail<4>().data()) = M.rotation();
24  return res;
25  }
26 
27  bp::tuple SE3ToXYZQUATtuple(const SE3& M)
28  {
30  return bp::make_tuple (
31  M.translation()(0), M.translation()(1), M.translation()(2),
32  q.x(), q.y(), q.z(), q.w());
33  }
34 
35  template <typename TupleOrList>
36  SE3 XYZQUATToSE3_bp(const TupleOrList& v)
37  {
38  //bp::extract<SE3::Scalar> to_double;
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]));
44  SE3::Vector3 t (
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);
49  }
50 
51  template <typename Vector7Like>
52  SE3 XYZQUATToSE3_ei(const Vector7Like& v)
53  {
54  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector7Like, v, 7, 1);
55  QuatConstMap q (v.template tail<4>().data());
56  return SE3 (q.matrix(), v.template head<3>());
57  }
58 
60  {
61  const char* doc1 = "Convert the input SE3 object to a numpy array.";
62  bp::def("SE3ToXYZQUAT" , SE3ToXYZQUAT , "M", doc1);
63  const char* doc1_tuple = "Convert the input SE3 object to a 7D tuple of floats [X,Y,Z,x,y,z,w].";
64  bp::def("SE3ToXYZQUATtuple", SE3ToXYZQUATtuple, "M", doc1_tuple);
65 
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);
75  }
76 
77  } // namespace python
78 } // namespace pinocchio
#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) ...
Definition: src/macros.hpp:54
bp::tuple SE3ToXYZQUATtuple(const SE3 &M)
Definition: conversions.cpp:27
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
SE3 XYZQUATToSE3_ei(const Vector7Like &v)
Definition: conversions.cpp:52
SE3::Scalar Scalar
Definition: conversions.cpp:13
VectorXd SE3ToXYZQUAT(const SE3 &M)
Definition: conversions.cpp:19
Eigen::Quaternion< Scalar, Options > Quaternion
Eigen::Map< SE3::Quaternion > QuatMap
Definition: conversions.cpp:16
ConstLinearRef translation() const
Definition: se3-base.hpp:38
traits< SE3Tpl >::Vector3 Vector3
Main pinocchio namespace.
Definition: timings.cpp:30
Eigen::Matrix< Scalar, 7, 1 > Vector7d
Definition: conversions.cpp:15
Eigen::Map< const SE3::Quaternion > QuatConstMap
Definition: conversions.cpp:17
SE3Tpl< double, 0 > SE3
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
SE3 XYZQUATToSE3_bp(const TupleOrList &v)
Definition: conversions.cpp:36
ConstAngularRef rotation() const
Definition: se3-base.hpp:37
void exposeConversions()
Definition: conversions.cpp:59


pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:02