1 |
|
|
// |
2 |
|
|
// Copyright (c) 2019-2020 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/bindings/python/fwd.hpp" |
6 |
|
|
#include "pinocchio/bindings/python/spatial/se3.hpp" |
7 |
|
|
|
8 |
|
|
namespace pinocchio |
9 |
|
|
{ |
10 |
|
|
namespace python |
11 |
|
|
{ |
12 |
|
|
namespace bp = boost::python; |
13 |
|
|
typedef SE3::Scalar Scalar; |
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 |
|
|
{ |
29 |
|
|
SE3::Quaternion q (M.rotation()); |
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; |
39 |
|
|
SE3::Quaternion q ( |
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 |
|
|
|
59 |
|
19 |
void exposeConversions() |
60 |
|
|
{ |
61 |
|
19 |
const char* doc1 = "Convert the input SE3 object to a numpy array."; |
62 |
✓✗ |
19 |
bp::def("SE3ToXYZQUAT" , SE3ToXYZQUAT , "M", doc1); |
63 |
|
19 |
const char* doc1_tuple = "Convert the input SE3 object to a 7D tuple of floats [X,Y,Z,x,y,z,w]."; |
64 |
✓✗ |
19 |
bp::def("SE3ToXYZQUATtuple", SE3ToXYZQUATtuple, "M", doc1_tuple); |
65 |
|
|
|
66 |
|
19 |
const char* doc2 = "Reverse function of SE3ToXYZQUAT: convert [X,Y,Z,x,y,z,w] to an SE3 element."; |
67 |
✓✗ |
19 |
bp::def("XYZQUATToSE3", |
68 |
|
|
static_cast<SE3 (*) (const bp::tuple&)> (XYZQUATToSE3_bp<bp::tuple>), |
69 |
✓✗ |
38 |
bp::arg("tuple"),doc2); |
70 |
✓✗ |
19 |
bp::def("XYZQUATToSE3", |
71 |
|
|
static_cast<SE3 (*) (const bp::list &)> (XYZQUATToSE3_bp<bp::list >), |
72 |
✓✗ |
38 |
bp::arg("list"),doc2); |
73 |
✓✗ |
19 |
bp::def("XYZQUATToSE3", static_cast<SE3 (*) (const VectorXd &)> (XYZQUATToSE3_ei<VectorXd >), |
74 |
✓✗ |
38 |
bp::arg("array"),doc2); |
75 |
|
19 |
} |
76 |
|
|
|
77 |
|
|
} // namespace python |
78 |
|
|
} // namespace pinocchio |