[Python] Add C++ conversion between SE3 and XYZQUAT

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();
   
 }
  

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.')

bindings/python/utils/conversions.cpp

+//
+// 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

bindings/python/utils/conversions.hpp

+//
+// Copyright (c) 2019 CNRS
+//
+
+namespace pinocchio
+{
+  namespace python
+  {
+
+    void exposeConversions();
+
+  } // namespace python
+} // namespace pinocchio