[python] Expose skew and unSkew

bindings/python/fwd.hpp

@@ -18,6 +18,9 @@ namespace pinocchio
     void exposeMotion();
     void exposeInertia();
     void exposeExplog();
+    void exposeSkew();
+
+    // Expose math module
     void exposeRpy();
     
     // Expose multibody classes

bindings/python/module.cpp

@@ -53,7 +53,8 @@ BOOST_PYTHON_MODULE(libpinocchio_pywrap)
   exposeJoints();
   exposeExplog();
   exposeRpy();
-  
+  exposeSkew();
+
   bp::enum_< ::pinocchio::ReferenceFrame >("ReferenceFrame")
   .value("WORLD",::pinocchio::WORLD)
   .value("LOCAL",::pinocchio::LOCAL)

bindings/python/spatial/expose-skew.cpp

+//
+// Copyright (c) 2015-2019 CNRS INRIA
+// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
+//
+
+#include "pinocchio/bindings/python/fwd.hpp"
+#include <boost/python.hpp>
+#include "pinocchio/spatial/skew.hpp"
+
+namespace pinocchio
+{
+  namespace python
+  {
+    namespace bp = boost::python;
+
+    Eigen::Matrix3d skew_proxy(const Eigen::Vector3d & v)
+    {
+      return pinocchio::skew(v);
+    }
+
+    Eigen::Vector3d unSkew_proxy(const Eigen::Matrix3d & M)
+    {
+      return pinocchio::unSkew(M);
+    }
+
+    void exposeSkew()
+    {
+
+      bp::def("skew",&skew_proxy,
+              bp::arg("Angular velocity (vector of size 3)"),
+              "Computes the skew representation of a given 3D vector, "
+              "i.e. the antisymmetric matrix representation of the cross product operator.");
+
+      bp::def("unSkew",&unSkew_proxy,
+              bp::arg("M: a 3x3 matrix."),
+              "Inverse of skew operator. From a given skew-symmetric matrix M "
+              "of dimension 3x3, it extracts the supporting vector, i.e. the entries of M."
+              "Mathematically speacking, it computes v such that M x = cross(x, v).");
+    }
+
+  } // namespace python
+} // namespace pinocchio

unittest/python/CMakeLists.txt

@@ -13,6 +13,7 @@ SET(${PROJECT_NAME}_PYTHON_TESTS
   bindings_force
   bindings_frame
   bindings_inertia
+  bindings_spatial
   bindings_joint_composite
   bindings_motion
   bindings_SE3

unittest/python/bindings_spatial.py

+import unittest
+
+import numpy as np
+from numpy.random import rand
+
+import pinocchio as pin
+pin.switchToNumpyMatrix()
+from pinocchio import skew, unSkew
+
+from test_case import PinocchioTestCase
+
+class TestSpatial(PinocchioTestCase):
+    def test_skew(self):
+        v3 = rand(3)
+        self.assertApprox(v3, unSkew(skew(v3)))
+        self.assertLess(np.linalg.norm(skew(v3).dot(v3)), 1e-10)
+
+        x, y, z = tuple(rand(3).tolist())
+        M = np.array([[ 0.,  x, y],
+                      [-x,  0., z],
+                      [-y, -z, 0.]])
+        self.assertApprox(M, skew(unSkew(M)))
+
+        rhs = rand(3)
+        self.assertApprox(np.cross(v3,rhs,axis=0), skew(v3).dot(rhs))
+        self.assertApprox(M.dot(rhs), np.cross(unSkew(M),rhs,axis=0))
+
+        x, y, z = tuple(v3.tolist())
+        self.assertApprox(skew(v3), np.array([[0, -z, y], [z, 0, -x], [-y, x, 0]]))
+
+if __name__ == '__main__':
+    unittest.main()