[Bindings] Expose C++ explog functions in Python.

CMakeLists.txt

@@ -134,6 +134,7 @@ SET(${PROJECT_NAME}_PYTHON_HEADERS
   python/data.hpp
   python/algorithms.hpp
   python/parsers.hpp
+  python/explog.hpp
   )
 
 SET(HEADERS

src/python/explog.hpp

+//
+// Copyright (c) 2015 CNRS
+// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_python_explog_hpp__
+# define __se3_python_explog_hpp__
+
+# include <eigenpy/eigenpy.hpp>
+
+# include "pinocchio/spatial/explog.hpp"
+# include "pinocchio/python/se3.hpp"
+# include "pinocchio/python/motion.hpp"
+
+namespace se3
+{
+  namespace python
+  {
+    struct ExplogPythonVisitor
+    {
+      typedef Eigen::Matrix3d Matrix3d;
+      typedef Eigen::Vector3d Vector3d;
+      typedef Eigen::Matrix4d Matrix4d;
+      typedef Eigen::Matrix<double,6,1> Vector6d;
+      typedef eigenpy::Matrix3d_fx Matrix3d_fx;
+      typedef eigenpy::Vector3d_fx Vector3d_fx;
+      typedef eigenpy::Matrix4d_fx Matrix4d_fx;
+      typedef eigenpy::UnalignedEquivalent<Vector6d>::type Vector6d_fx;
+      typedef SE3PythonVisitor<SE3>::SE3_fx SE3_fx;
+      typedef MotionPythonVisitor<Motion>::Motion_fx Motion_fx;
+
+      static Matrix3d exp3_proxy(const Vector3d_fx & v)
+      {
+        return exp3(v);
+      }
+
+      static Vector3d log3_proxy(const Matrix3d_fx & R)
+      {
+        return log3(R);
+      }
+
+      static SE3_fx exp6FromMotion_proxy(const Motion_fx & nu)
+      {
+        return exp6(nu);
+      }
+
+      static SE3_fx exp6FromVector_proxy(const Vector6d_fx & v)
+      {
+        return exp6(v);
+      }
+
+      static Motion_fx log6FromSE3_proxy(const SE3_fx & m)
+      {
+        return log6(m);
+      }
+
+      static Motion_fx log6FromMatrix_proxy(const Matrix4d_fx & M)
+      {
+        return log6(M);
+      }
+
+      static void expose()
+      {
+        bp::def("exp3",exp3_proxy,
+                bp::args("Angular velocity (double size 3)"),
+                "Exp: so3 -> SO3. Return the integral of the input"
+                " angular velocity during time 1.");
+        bp::def("log3",log3_proxy,
+                bp::args("Rotation matrix (double size 3x3)"),
+                "Log: SO3 -> so3. Pseudo-inverse of log from SO3"
+                " -> { v \in so3, ||v|| < 2pi }.Exp: so3 -> SO3.");
+        bp::def("exp6FromMotion",exp6FromMotion_proxy,
+                bp::args("Spatial velocity (se3.Motion)"),
+                "Exp: se3 -> SE3. Return the integral of the input"
+                " spatial velocity during time 1.");
+        bp::def("exp6FromVector",exp6FromVector_proxy,
+                bp::args("Spatial velocity (double size 6)"),
+                "Exp: se3 -> SE3. Return the integral of the input"
+                " spatial velocity during time 1.");
+        bp::def("log6FromSE3",log6FromSE3_proxy,
+                bp::args("Spatial transform (se3.SE3)"),
+                "Log: SE3 -> se3. Pseudo-inverse of exp from SE3"
+                " -> { v,w \in se3, ||w|| < 2pi }.");
+        bp::def("log6FromMatrix",log6FromMatrix_proxy,
+                bp::args("Spatial transform (double size 4x4)"),
+                "Log: SE3 -> se3. Pseudo-inverse of exp from SE3"
+                " -> { v,w \in se3, ||w|| < 2pi }.");
+      }
+    };
+  } // namespace python
+} //namespace se3
+
+#endif // ifndef __se3_python_explog_hpp__

src/python/module.cpp

@@ -39,6 +39,7 @@ BOOST_PYTHON_MODULE(libpinocchio_pywrap)
   se3::python::exposeForce();
   se3::python::exposeMotion();
   se3::python::exposeInertia();
+  se3::python::exposeExplog();
 
   se3::python::exposeModel();
   se3::python::exposeAlgorithms();

src/python/python.cpp

@@ -25,6 +25,7 @@
 #include "pinocchio/python/data.hpp"
 #include "pinocchio/python/algorithms.hpp"
 #include "pinocchio/python/parsers.hpp"
+#include "pinocchio/python/explog.hpp"
 
 namespace se3
 {
@@ -63,4 +64,8 @@ namespace se3
     {
       ParsersPythonVisitor::expose();
     }
+    void exposeExplog()
+    {
+      ExplogPythonVisitor::expose();
+    }
   }} // namespace se3::python

src/python/python.hpp

@@ -26,6 +26,7 @@ namespace se3
     void exposeForce();
     void exposeMotion();
     void exposeInertia();
+    void exposeExplog();
 
     void exposeModel();
     void exposeAlgorithms();

src/spatial/explog.hpp

@@ -35,7 +35,8 @@ namespace se3
   exp3(const Eigen::MatrixBase<D> & v)
   {
     EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
-    return Eigen::AngleAxis<typename D::Scalar>(v.norm(), v).matrix();
+    typename D::Scalar nv = v.norm();
+    return Eigen::AngleAxis<typename D::Scalar>(nv, v / nv).matrix();
   }
 
   /// \brief Log: SO3 -> so3.
@@ -81,13 +82,12 @@ namespace se3
   /// \brief Exp: se3 -> SE3.
   ///
   /// Return the integral of the input spatial velocity during time 1.
-  template <typename D> Eigen::Matrix<typename D::Scalar,6,6,D::Options>
+  template <typename D> SE3Tpl<typename D::Scalar, D::Options>
   exp6(const Eigen::MatrixBase<D> & v)
   {
     EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,6);
     MotionTpl<typename D::Scalar,D::Options> nu(v);
-    SE3Tpl<typename D::Scalar,D::Options> m(exp6(nu));
-    return m.toActionMatrix();
+    return exp6(nu);
   }
 
   /// \brief Log: SE3 -> se3.
@@ -123,23 +123,17 @@ namespace se3
   /// \brief Log: SE3 -> se3.
   ///
   /// Pseudo-inverse of exp from SE3 -> { v,w \in se3, ||w|| < 2pi }.
-  template <typename D> Eigen::Matrix<typename D::Scalar,6,1,D::Options>
+  template <typename D> MotionTpl<typename D::Scalar,D::Options>
   log6(const Eigen::MatrixBase<D> & M)
   {
-    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D, 6, 6);
+    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D, 4, 4);
     typedef typename SE3Tpl<typename D::Scalar,D::Options>::Vector3 Vector3;
     typedef typename SE3Tpl<typename D::Scalar,D::Options>::Matrix3 Matrix3;
-    enum {
-      LINEAR = SE3Tpl<typename D::Scalar,D::Options>::LINEAR,
-      ANGULAR = SE3Tpl<typename D::Scalar,D::Options>::ANGULAR
-    };
 
-    Matrix3 rot(M.template block<3,3>(ANGULAR,ANGULAR));
-    Matrix3 skew(M.template block<3,3>(LINEAR,ANGULAR) * rot.transpose());
-    Vector3 trans(skew(2,1), skew(0,2), skew(1,0));
+    Matrix3 rot(M.template block<3,3>(0,0));
+    Vector3 trans(M.template block<3,1>(0,3));
     SE3Tpl<typename D::Scalar,D::Options> m(rot, trans);
-    MotionTpl<typename D::Scalar,D::Options> nu(log6(m));
-    return nu.toVector();
+    return log6(m);
   }
 } // namespace se3