Merge pull request #702 from jmirabel/conversions

[Python] Improve conversions and usage of gepetto-gui API.

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/robot_wrapper.py

@@ -379,18 +379,17 @@ class RobotWrapper(object):
             self.forwardKinematics(q)
 
             if self.display_collisions:
-                self.updateGeometryPlacements(visual=False)
-                for collision in self.collision_model.geometryObjects:
-                    M = self.collision_data.oMg[self.collision_model.getGeometryId(collision.name)]
-                    conf = utils.se3ToXYZQUAT(M)
-                    gui.applyConfiguration(self.getViewerNodeName(collision,pin.GeometryType.COLLISION), conf)
+                gui.applyConfigurations (
+                        [ self.getViewerNodeName(collision,pin.GeometryType.COLLISION) for collision in self.collision_model.geometryObjects ],
+                        [ pin.se3ToXYZQUATtuple(self.collision_data.oMg[self.collision_model.getGeometryId(collision.name)]) for collision in self.collision_model.geometryObjects ]
+                        )
 
             if self.display_visuals:
                 self.updateGeometryPlacements(visual=True)
-                for visual in self.visual_model.geometryObjects:
-                    M = self.visual_data.oMg[self.visual_model.getGeometryId(visual.name)]
-                    conf = utils.se3ToXYZQUAT(M)
-                    gui.applyConfiguration(self.getViewerNodeName(visual,pin.GeometryType.VISUAL), conf)
+                gui.applyConfigurations (
+                        [ self.getViewerNodeName(visual,pin.GeometryType.VISUAL) for visual in self.visual_model.geometryObjects ],
+                        [ pin.se3ToXYZQUATtuple(self.visual_data.oMg[self.visual_model.getGeometryId(visual.name)]) for visual in self.visual_model.geometryObjects ]
+                        )
 
             gui.refresh()
         elif self.used_viewer == 'meshcat':

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

unittest/python/bindings_SE3.py

@@ -102,5 +102,33 @@ class TestSE3Bindings(unittest.TestCase):
         bma = amb.inverse()
         self.assertTrue(np.allclose(bma.act(p), (bMa * p_homogeneous)[0:3]))
 
+    def test_conversions(self):
+        def compute (m):
+            tq_vec = pin.se3ToXYZQUAT      (m)
+            tq_tup = pin.se3ToXYZQUATtuple (m)
+            mm_vec = pin.XYZQUATToSe3 (tq_vec)
+            mm_tup = pin.XYZQUATToSe3 (tq_tup)
+            mm_lis = pin.XYZQUATToSe3 (list(tq_tup))
+            return tq_vec, tq_tup, mm_vec, mm_tup, mm_lis
+
+        m = pin.SE3.Identity()
+        tq_vec, tq_tup, mm_vec, mm_tup, mm_lis = compute (m)
+        self.assertTrue(np.allclose(m.homogeneous, mm_tup.homogeneous))
+        self.assertTrue(np.allclose(m.homogeneous, mm_vec.homogeneous))
+        self.assertTrue(np.allclose(m.homogeneous, mm_lis.homogeneous))
+        for i in range(6):
+            self.assertTrue(tq_vec[i] == 0 and tq_tup[i] == 0)
+        self.assertTrue(tq_vec[6] == 1 and tq_tup[6] == 1)
+
+        m = pin.SE3.Random()
+        tq_vec, tq_tup, mm_vec, mm_tup, mm_lis = compute (m)
+        self.assertTrue (len(tq_vec) == 7)
+        self.assertTrue (len(tq_tup) == 7)
+        for a,b in zip(tq_vec,tq_tup):
+            self.assertTrue (a==b)
+        self.assertTrue(np.allclose(m.homogeneous, mm_tup.homogeneous))
+        self.assertTrue (mm_vec == mm_tup)
+        self.assertTrue (mm_vec == mm_lis)
+
 if __name__ == '__main__':
     unittest.main()

unittest/python/utils.py

@@ -19,7 +19,7 @@ class TestUtils(TestCase):
         m = pin.SE3.Identity()
         m.translation = np.matrix('1. 2. 3.').T
         m.rotation = np.matrix('1. 0. 0.;0. 0. -1.;0. 1. 0.')  # rotate('x', pi / 2)
-        self.assertApprox(se3ToXYZQUAT(m), [1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2])
+        self.assertApprox(se3ToXYZQUAT(m).T, [1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2])
         self.assertApprox(XYZQUATToSe3([1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2]), m)
 
     def test_isapprox(self):