Commit 493b1aa4 authored by Antonio El Khoury's avatar Antonio El Khoury Committed by Antonio El Khoury
Browse files

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

parent 0dff9d21
......@@ -134,6 +134,7 @@ SET(${PROJECT_NAME}_PYTHON_HEADERS
python/data.hpp
python/algorithms.hpp
python/parsers.hpp
python/explog.hpp
)
SET(HEADERS
......
//
// 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__
......@@ -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();
......
......@@ -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
......@@ -26,6 +26,7 @@ namespace se3
void exposeForce();
void exposeMotion();
void exposeInertia();
void exposeExplog();
void exposeModel();
void exposeAlgorithms();
......
......@@ -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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment