diff --git a/CMakeLists.txt b/CMakeLists.txt
index a72281a5f5007cf7192233b83239305de098e5a6..bf81344f5fe8a9c95c5bb6bff6fbdf31181f508c 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/src/python/explog.hpp b/src/python/explog.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0580572e837a6308717dcc2d03746f38ebdcdcbe
--- /dev/null
+++ b/src/python/explog.hpp
@@ -0,0 +1,106 @@
+//
+// 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__
diff --git a/src/python/module.cpp b/src/python/module.cpp
index aee25f25c0bf9365d7ec432d2a9e978bbaca077a..866fc07a43e98670ae27cf80c405875c7cb484e5 100644
--- a/src/python/module.cpp
+++ b/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();
diff --git a/src/python/python.cpp b/src/python/python.cpp
index 2f5e7299ac7c1cf07fb2871a65c72f26baaa34e4..951a2f99269fbb26626c1d24a2d2edf46a65f445 100644
--- a/src/python/python.cpp
+++ b/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
diff --git a/src/python/python.hpp b/src/python/python.hpp
index afb12a5b1c1072985a151315959c5c12e92aefa5..f939c0d2e4e6ffaf07487d86f0ea49250614b2b0 100644
--- a/src/python/python.hpp
+++ b/src/python/python.hpp
@@ -26,6 +26,7 @@ namespace se3
void exposeForce();
void exposeMotion();
void exposeInertia();
+ void exposeExplog();
void exposeModel();
void exposeAlgorithms();
diff --git a/src/spatial/explog.hpp b/src/spatial/explog.hpp
index 566d3b1e1f053a47dac496a63879d226b0815aa5..de71ab8efebe4f662a26a2e8d67714c099d4444d 100644
--- a/src/spatial/explog.hpp
+++ b/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