1 |
|
|
// |
2 |
|
|
// Copyright (c) 2019-2020 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include <sstream> |
6 |
|
|
|
7 |
|
|
#include "pinocchio/bindings/python/fwd.hpp" |
8 |
|
|
#include "pinocchio/bindings/python/spatial/se3.hpp" |
9 |
|
|
|
10 |
|
|
namespace pinocchio |
11 |
|
|
{ |
12 |
|
|
namespace python |
13 |
|
|
{ |
14 |
|
|
namespace bp = boost::python; |
15 |
|
|
typedef SE3::Scalar Scalar; |
16 |
|
|
typedef Eigen::Matrix<Scalar, Eigen::Dynamic,1> VectorXd; |
17 |
|
|
typedef Eigen::Matrix<Scalar, 7,1> Vector7d; |
18 |
|
|
typedef Eigen::Map< SE3::Quaternion> QuatMap; |
19 |
|
|
typedef Eigen::Map<const SE3::Quaternion> QuatConstMap; |
20 |
|
|
|
21 |
|
|
VectorXd SE3ToXYZQUAT(const SE3& M) |
22 |
|
|
{ |
23 |
|
|
Vector7d res; |
24 |
|
|
res.head<3>() = M.translation(); |
25 |
|
|
QuatMap (res.tail<4>().data()) = M.rotation(); |
26 |
|
|
return res; |
27 |
|
|
} |
28 |
|
|
|
29 |
|
|
bp::tuple SE3ToXYZQUATtuple(const SE3& M) |
30 |
|
|
{ |
31 |
|
|
SE3::Quaternion q (M.rotation()); |
32 |
|
|
return bp::make_tuple ( |
33 |
|
|
M.translation()(0), M.translation()(1), M.translation()(2), |
34 |
|
|
q.x(), q.y(), q.z(), q.w()); |
35 |
|
|
} |
36 |
|
|
|
37 |
|
|
template <typename TupleOrList> |
38 |
|
|
SE3 XYZQUATToSE3_bp(const TupleOrList& v) |
39 |
|
|
{ |
40 |
|
|
bp::ssize_t size = bp::len(v); |
41 |
|
|
if(size != 7) |
42 |
|
|
{ |
43 |
|
|
throw std::invalid_argument( |
44 |
|
|
"Wrong size: v(" + std::to_string(size) + ") should have 7 elements"); |
45 |
|
|
} |
46 |
|
|
SE3::Quaternion q ( |
47 |
|
|
static_cast<Scalar>(bp::extract<Scalar>(v[6])), |
48 |
|
|
static_cast<Scalar>(bp::extract<Scalar>(v[3])), |
49 |
|
|
static_cast<Scalar>(bp::extract<Scalar>(v[4])), |
50 |
|
|
static_cast<Scalar>(bp::extract<Scalar>(v[5]))); |
51 |
|
|
SE3::Vector3 t ( |
52 |
|
|
static_cast<Scalar>(bp::extract<Scalar>(v[0])), |
53 |
|
|
static_cast<Scalar>(bp::extract<Scalar>(v[1])), |
54 |
|
|
static_cast<Scalar>(bp::extract<Scalar>(v[2]))); |
55 |
|
|
return SE3 (q.matrix(), t); |
56 |
|
|
} |
57 |
|
|
|
58 |
|
|
template <typename Vector7Like> |
59 |
|
|
SE3 XYZQUATToSE3_ei(const Vector7Like& v) |
60 |
|
|
{ |
61 |
|
|
if(v.rows() != 7 || v.cols() != 1) |
62 |
|
|
{ |
63 |
|
|
std::ostringstream shape; |
64 |
|
|
shape << "(" << v.rows() << ", " << v.cols() << ")"; |
65 |
|
|
throw std::invalid_argument("Wrong size: v" + shape.str() + " but should have the following shape (7, 1)"); |
66 |
|
|
} |
67 |
|
|
QuatConstMap q (v.template tail<4>().data()); |
68 |
|
|
return SE3 (q.matrix(), v.template head<3>()); |
69 |
|
|
} |
70 |
|
|
|
71 |
|
19 |
void exposeConversions() |
72 |
|
|
{ |
73 |
|
19 |
const char* doc1 = "Convert the input SE3 object to a numpy array."; |
74 |
✓✗ |
19 |
bp::def("SE3ToXYZQUAT" , SE3ToXYZQUAT , "M", doc1); |
75 |
|
19 |
const char* doc1_tuple = "Convert the input SE3 object to a 7D tuple of floats [X,Y,Z,x,y,z,w]."; |
76 |
✓✗ |
19 |
bp::def("SE3ToXYZQUATtuple", SE3ToXYZQUATtuple, "M", doc1_tuple); |
77 |
|
|
|
78 |
|
19 |
const char* doc2 = "Reverse function of SE3ToXYZQUAT: convert [X,Y,Z,x,y,z,w] to an SE3 element."; |
79 |
✓✗ |
19 |
bp::def("XYZQUATToSE3", |
80 |
|
|
static_cast<SE3 (*) (const bp::tuple&)> (XYZQUATToSE3_bp<bp::tuple>), |
81 |
✓✗ |
38 |
bp::arg("tuple"),doc2); |
82 |
✓✗ |
19 |
bp::def("XYZQUATToSE3", |
83 |
|
|
static_cast<SE3 (*) (const bp::list &)> (XYZQUATToSE3_bp<bp::list >), |
84 |
✓✗ |
38 |
bp::arg("list"),doc2); |
85 |
✓✗ |
19 |
bp::def("XYZQUATToSE3", static_cast<SE3 (*) (const VectorXd &)> (XYZQUATToSE3_ei<VectorXd >), |
86 |
✓✗ |
38 |
bp::arg("array"),doc2); |
87 |
|
19 |
} |
88 |
|
|
|
89 |
|
|
} // namespace python |
90 |
|
|
} // namespace pinocchio |