| Directory: | ./ |
|---|---|
| File: | bindings/python/utils/conversions.cpp |
| Date: | 2024-08-27 18:20:05 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 16 | 46 | 34.8% |
| Branches: | 8 | 176 | 4.5% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2019-2021 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 | |||
| 16 | struct XYZQUATConverter | ||
| 17 | { | ||
| 18 | typedef context::Scalar Scalar; | ||
| 19 | typedef context::SE3 SE3; | ||
| 20 | typedef context::VectorXs VectorXs; | ||
| 21 | enum | ||
| 22 | { | ||
| 23 | Options = context::Options | ||
| 24 | }; | ||
| 25 | typedef Eigen::Matrix<Scalar, 7, 1, Options> Vector7s; | ||
| 26 | typedef Eigen::Map<SE3::Quaternion> QuatMap; | ||
| 27 | typedef Eigen::Map<const SE3::Quaternion> QuatConstMap; | ||
| 28 | |||
| 29 | ✗ | static VectorXs fromSE3(const SE3 & M) | |
| 30 | { | ||
| 31 | ✗ | Vector7s res; | |
| 32 | ✗ | res.head<3>() = M.translation(); | |
| 33 | ✗ | QuatMap(res.tail<4>().data()) = M.rotation(); | |
| 34 | ✗ | return res; | |
| 35 | } | ||
| 36 | |||
| 37 | ✗ | static bp::tuple fromSE3tuple(const SE3 & M) | |
| 38 | { | ||
| 39 | ✗ | const SE3::Quaternion q(M.rotation()); | |
| 40 | return bp::make_tuple( | ||
| 41 | ✗ | M.translation()(0), M.translation()(1), M.translation()(2), q.x(), q.y(), q.z(), q.w()); | |
| 42 | } | ||
| 43 | |||
| 44 | template<typename TupleOrList> | ||
| 45 | ✗ | static SE3 toSE3fromTupleOrList(const TupleOrList & v) | |
| 46 | { | ||
| 47 | |||
| 48 | ✗ | bp::ssize_t size = bp::len(v); | |
| 49 | ✗ | if (size != 7) | |
| 50 | { | ||
| 51 | ✗ | throw std::invalid_argument( | |
| 52 | "Wrong size: v(" + std::to_string(size) + ") should have 7 elements"); | ||
| 53 | } | ||
| 54 | |||
| 55 | // bp::extract<SE3::Scalar> to_double; | ||
| 56 | ✗ | const Scalar & v0 = bp::extract<Scalar>(v[0]); | |
| 57 | ✗ | const Scalar & v1 = bp::extract<Scalar>(v[1]); | |
| 58 | ✗ | const Scalar & v2 = bp::extract<Scalar>(v[2]); | |
| 59 | ✗ | const Scalar & v3 = bp::extract<Scalar>(v[3]); | |
| 60 | ✗ | const Scalar & v4 = bp::extract<Scalar>(v[4]); | |
| 61 | ✗ | const Scalar & v5 = bp::extract<Scalar>(v[5]); | |
| 62 | ✗ | const Scalar & v6 = bp::extract<Scalar>(v[6]); | |
| 63 | |||
| 64 | ✗ | SE3::Quaternion q(v6, v3, v4, v5); | |
| 65 | ✗ | SE3::Vector3 t(v0, v1, v2); | |
| 66 | ✗ | return SE3(q.matrix(), t); | |
| 67 | } | ||
| 68 | |||
| 69 | template<typename Vector7Like> | ||
| 70 | SE3 XYZQUATToSE3_ei(const Vector7Like & v) | ||
| 71 | { | ||
| 72 | if (v.rows() != 7 || v.cols() != 1) | ||
| 73 | { | ||
| 74 | std::ostringstream shape; | ||
| 75 | shape << "(" << v.rows() << ", " << v.cols() << ")"; | ||
| 76 | throw std::invalid_argument( | ||
| 77 | "Wrong size: v" + shape.str() + " but should have the following shape (7, 1)"); | ||
| 78 | } | ||
| 79 | QuatConstMap q(v.template tail<4>().data()); | ||
| 80 | return SE3(q.matrix(), v.template head<3>()); | ||
| 81 | } | ||
| 82 | |||
| 83 | template<typename Vector7Like> | ||
| 84 | ✗ | static SE3 toSE3(const Vector7Like & v) | |
| 85 | { | ||
| 86 | ✗ | if (v.rows() != 7 || v.cols() != 1) | |
| 87 | { | ||
| 88 | ✗ | std::ostringstream shape; | |
| 89 | ✗ | shape << "(" << v.rows() << ", " << v.cols() << ")"; | |
| 90 | ✗ | throw std::invalid_argument( | |
| 91 | "Wrong size: v" + shape.str() + " but should have the following shape (7, 1)"); | ||
| 92 | } | ||
| 93 | |||
| 94 | ✗ | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector7Like, v, 7, 1); | |
| 95 | ✗ | QuatConstMap q(v.template tail<4>().data()); | |
| 96 | ✗ | return SE3(q.matrix(), v.template head<3>()); | |
| 97 | } | ||
| 98 | |||
| 99 | 20 | static void expose() | |
| 100 | { | ||
| 101 | |||
| 102 | 20 | const char * doc1 = "Convert the input SE3 object to a numpy array."; | |
| 103 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def("SE3ToXYZQUAT", fromSE3, "M", doc1); |
| 104 | 20 | const char * doc1_tuple = | |
| 105 | "Convert the input SE3 object to a 7D tuple of floats [X,Y,Z,x,y,z,w]."; | ||
| 106 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def("SE3ToXYZQUATtuple", fromSE3tuple, "M", doc1_tuple); |
| 107 | |||
| 108 | 20 | const char * doc2 = | |
| 109 | "Reverse function of SE3ToXYZQUAT: convert [X,Y,Z,x,y,z,w] to an SE3 element."; | ||
| 110 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
| 111 | "XYZQUATToSE3", static_cast<SE3 (*)(const bp::tuple &)>(toSE3fromTupleOrList<bp::tuple>), | ||
| 112 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
40 | bp::arg("tuple"), doc2); |
| 113 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
| 114 | "XYZQUATToSE3", static_cast<SE3 (*)(const bp::list &)>(toSE3fromTupleOrList<bp::list>), | ||
| 115 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
40 | bp::arg("list"), doc2); |
| 116 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
| 117 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
40 | "XYZQUATToSE3", static_cast<SE3 (*)(const VectorXs &)>(toSE3<VectorXs>), bp::arg("array"), |
| 118 | doc2); | ||
| 119 | 20 | } | |
| 120 | }; | ||
| 121 | |||
| 122 | 20 | void exposeConversions() | |
| 123 | { | ||
| 124 | 20 | XYZQUATConverter::expose(); | |
| 125 | 20 | } | |
| 126 | |||
| 127 | } // namespace python | ||
| 128 | } // namespace pinocchio | ||
| 129 |