From 1f1cdb76e6bf812ac80fcd34f8f3ea76743bad06 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 8 Feb 2019 09:33:28 +0100
Subject: [PATCH] [Python] Add C++ conversion between SE3 and XYZQUAT
---
bindings/python/module.cpp | 2 +
bindings/python/pinocchio/utils.py | 18 +------
bindings/python/utils/conversions.cpp | 73 +++++++++++++++++++++++++++
bindings/python/utils/conversions.hpp | 13 +++++
4 files changed, 90 insertions(+), 16 deletions(-)
create mode 100644 bindings/python/utils/conversions.cpp
create mode 100644 bindings/python/utils/conversions.hpp
diff --git a/bindings/python/module.cpp b/bindings/python/module.cpp
index 689bd06ac..d23a30ef9 100644
--- a/bindings/python/module.cpp
+++ b/bindings/python/module.cpp
@@ -9,6 +9,7 @@
#include "pinocchio/bindings/python/fwd.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/bindings/python/utils/version.hpp"
+#include "pinocchio/bindings/python/utils/conversions.hpp"
namespace bp = boost::python;
using namespace pinocchio::python;
@@ -54,6 +55,7 @@ BOOST_PYTHON_MODULE(libpinocchio_pywrap)
#endif // PINOCCHIO_WITH_HPP_FCL
exposeVersion();
+ exposeConversions();
}
diff --git a/bindings/python/pinocchio/utils.py b/bindings/python/pinocchio/utils.py
index c8bbcc102..87fcde548 100644
--- a/bindings/python/pinocchio/utils.py
+++ b/bindings/python/pinocchio/utils.py
@@ -28,23 +28,9 @@ def skew(p):
return np.matrix([[0, -z, y], [z, 0, -x], [-y, x, 0]], np.double)
-def se3ToXYZQUAT(M):
- '''
- Convert the input SE3 object to a 7D tuple of floats [X,Y,Z,Q1,Q2,Q3,Q4] .
- '''
- xyz = M.translation
- quat = pin.Quaternion(M.rotation).coeffs()
- return [float(xyz[0, 0]), float(xyz[1, 0]), float(xyz[2, 0]),
- float(quat[0, 0]), float(quat[1, 0]), float(quat[2, 0]), float(quat[3, 0])]
-
+se3ToXYZQUAT = pin.se3ToXYZQUAT
-def XYZQUATToSe3(xyzq):
- '''
- Reverse function of se3ToXYZQUAT: convert [X,Y,Z,Q1,Q2,Q3,Q4] to a SE3 element
- '''
- if isinstance(xyzq, (tuple, list)):
- xyzq = np.matrix(xyzq, np.float).T
- return pin.SE3(pin.Quaternion(xyzq[6, 0], xyzq[3, 0], xyzq[4, 0], xyzq[5, 0]).matrix(), xyzq[:3])
+XYZQUATToSe3 = pin.XYZQUATToSe3
@deprecated('Now useless.')
diff --git a/bindings/python/utils/conversions.cpp b/bindings/python/utils/conversions.cpp
new file mode 100644
index 000000000..7ac52db54
--- /dev/null
+++ b/bindings/python/utils/conversions.cpp
@@ -0,0 +1,73 @@
+//
+// Copyright (c) 2019 CNRS
+//
+
+#include "pinocchio/bindings/python/fwd.hpp"
+#include "pinocchio/bindings/python/spatial/se3.hpp"
+
+namespace pinocchio
+{
+ namespace python
+ {
+ namespace bp = boost::python;
+ typedef SE3::Scalar Scalar;
+ typedef Eigen::Matrix<Scalar, Eigen::Dynamic,1> VectorXd;
+ typedef Eigen::Matrix<Scalar, 7, 1> Vector7d;
+ typedef Eigen::Map< SE3::Quaternion> QuatMap;
+ typedef Eigen::Map<const SE3::Quaternion> QuatConstMap;
+
+ VectorXd se3ToXYZQUAT (const SE3& M)
+ {
+ Vector7d res;
+ res.head<3>() = M.translation();
+ QuatMap (res.tail<4>().data()) = M.rotation();
+ return res;
+ }
+
+ bp::tuple se3ToXYZQUATtuple (const SE3& M)
+ {
+ SE3::Quaternion q (M.rotation());
+ return bp::make_tuple (
+ M.translation()(0), M.translation()(1), M.translation()(2),
+ q.x(), q.y(), q.z(), q.w());
+ }
+
+ template <typename TupleOfList>
+ SE3 XYZQUATToSe3_bp(const TupleOfList& v)
+ {
+ //bp::extract<SE3::Scalar> to_double;
+ SE3::Quaternion q (
+ (Scalar)bp::extract<Scalar>(v[6]),
+ (Scalar)bp::extract<Scalar>(v[3]),
+ (Scalar)bp::extract<Scalar>(v[4]),
+ (Scalar)bp::extract<Scalar>(v[5]));
+ SE3::Vector3 t (
+ (Scalar)bp::extract<Scalar>(v[0]),
+ (Scalar)bp::extract<Scalar>(v[1]),
+ (Scalar)bp::extract<Scalar>(v[2]));
+ return SE3 (q.matrix(), t);
+ }
+
+ template <typename Vector7Like>
+ SE3 XYZQUATToSe3_ei(const Vector7Like& v)
+ {
+ PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector7Like, v, 7, 1);
+ QuatConstMap q (v.template tail<4>().data());
+ return SE3 (q.matrix(), v.template head<3>());
+ }
+
+ void exposeConversions()
+ {
+ const char* doc1 = "Convert the input SE3 object to a 7D tuple of floats [X,Y,Z,Q1,Q2,Q3,Q4] .";
+ bp::def ("se3ToXYZQUAT" , se3ToXYZQUAT , doc1);
+ bp::def ("se3ToXYZQUATtuple", se3ToXYZQUATtuple, doc1);
+
+ const char* doc2 = "Reverse function of se3ToXYZQUAT: convert [X,Y,Z,Q1,Q2,Q3,Q4] to a SE3 element";
+ bp::def ("XYZQUATToSe3", static_cast<SE3 (*) (const bp::tuple&)> (XYZQUATToSe3_bp<bp::tuple>), doc2);
+ bp::def ("XYZQUATToSe3", static_cast<SE3 (*) (const bp::list &)> (XYZQUATToSe3_bp<bp::list >), doc2);
+ bp::def ("XYZQUATToSe3", static_cast<SE3 (*) (const VectorXd &)> (XYZQUATToSe3_ei<VectorXd >), doc2);
+ bp::def ("XYZQUATToSe3", static_cast<SE3 (*) (const Vector7d &)> (XYZQUATToSe3_ei<Vector7d >), doc2);
+ }
+
+ } // namespace python
+} // namespace pinocchio
diff --git a/bindings/python/utils/conversions.hpp b/bindings/python/utils/conversions.hpp
new file mode 100644
index 000000000..1f6b5f64c
--- /dev/null
+++ b/bindings/python/utils/conversions.hpp
@@ -0,0 +1,13 @@
+//
+// Copyright (c) 2019 CNRS
+//
+
+namespace pinocchio
+{
+ namespace python
+ {
+
+ void exposeConversions();
+
+ } // namespace python
+} // namespace pinocchio
--
GitLab