Commit 65b4bd93 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

Merge pull request #25 from aelkhour/master

Implement exp and log map in C++ and expose them in Python. Still need some adjustments to be really efficient.
parents 3e87918c 31b754d4
......@@ -78,6 +78,7 @@ SET(${PROJECT_NAME}_SPATIAL_HEADERS
spatial/fwd.hpp
spatial/skew.hpp
spatial/act-on-set.hpp
spatial/explog.hpp
)
SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
......@@ -133,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__
......@@ -16,73 +16,25 @@
import libpinocchio_pywrap as se3
import numpy as np
import numpy.linalg as npl
from math import sin,cos
import math
from utils import skew,cross,eye
def exp3(v):
'''
Exp: so3 -> SO3. Return the integral of the input angular velocity during time 1.
'''
nv = npl.norm(v)
return se3.AngleAxis(nv,v/nv).matrix()
def log3(R):
'''
Log: SO3 -> so3. Pseudo-inverse of log from SO3 -> { v \in so3, ||v|| < 2pi }.
'''
return se3.AngleAxis(R).vector()
def exp6( nu ):
'''
Exp: se3 -> SE3. Return the integral of the input spatial velocity during time 1.
'''
if isinstance(nu,se3.Motion): w = nu.angular; v = nu.linear
else: w = nu[3:]; v = nu[:3]
if npl.norm(w)>1e-15:
R = exp3(w)
t = npl.norm(w)
S = skew(w)
V = eye(3) + (1-cos(t))/t**2 * S + (t-sin(t))/t**3 * S*S
p = V*v
return se3.SE3(R,p)
else:
return se3.SE3(eye(3),v)
def log6( m ):
'''
Log: SE3 -> se3. Pseudo-inverse of exp from SE3 -> { v,w \in se3, ||w|| < 2pi }.
'''
if isinstance(m,se3.SE3): R = m.rotation; p = m.translation
else: R = m[:3,:3]; p = m[:3,3]
w = log3(R)
if npl.norm(w)>1e-15:
t = npl.norm(w)
S = skew(w)
V = eye(3) + (1-cos(t))/t**2 * S + (t-sin(t))/t**3 * S*S
v = npl.inv(V)*p
else:
v = p
return se3.Motion(v,w)
def exp(x):
if isinstance(x,se3.Motion): return exp6(x)
if isinstance(x,se3.Motion): return se3.exp6FromMotion(x)
elif np.isscalar(x): return math.exp(x)
elif isinstance(x,np.matrix):
if len(x)==6: return exp6(x)
elif len(x)==3: return exp3(x)
if len(x)==6: return se3.exp6FromVector(x)
elif len(x)==3: return se3.exp3(x)
else: print 'Error only 3 and 6 vectors are allowed.'
else: print 'Error exp is only defined for real, vector3, vector6 and se3.Motion objects.'
def log(x):
if isinstance(x,se3.SE3): return log6(x)
if isinstance(x,se3.SE3): return se3.log6FromSE3(x)
elif np.isscalar(x): return math.log(x)
elif isinstance(x,np.matrix):
if len(x)==6: return log6(x)
elif len(x)==3: return log3(x)
else: print 'Error only 3 and 6 vectors are allowed.'
else: print 'Error log is only defined for real, vector3, vector6 and se3.SE3 objects.'
if len(x)==4: return se3.log6FromMatrix(x)
elif len(x)==3: return se3.log3(x)
else: print 'Error only 3 and 4 matrices are allowed.'
else: print 'Error log is only defined for real, matrix3, matrix4 and se3.SE3 objects.'
__all__ = [ 'exp3', 'log3', 'exp6', 'log6','exp','log' ]
__all__ = [ 'exp','log' ]
......@@ -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();
......
//
// 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 __spatial_explog_hpp__
# define __spatial_explog_hpp__
# include <Eigen/Geometry>
# include "pinocchio/math/sincos.hpp"
# include "pinocchio/spatial/motion.hpp"
# include "pinocchio/spatial/skew.hpp"
# include "pinocchio/spatial/se3.hpp"
namespace se3
{
/// \brief Exp: so3 -> SO3.
///
/// Return the integral of the input angular velocity during time 1.
template <typename D> Eigen::Matrix<typename D::Scalar,3,3,D::Options>
exp3(const Eigen::MatrixBase<D> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
typename D::Scalar nv = v.norm();
return Eigen::AngleAxis<typename D::Scalar>(nv, v / nv).matrix();
}
/// \brief Log: SO3 -> so3.
///
/// Pseudo-inverse of log from SO3 -> { v \in so3, ||v|| < 2pi }.
template <typename D> Eigen::Matrix<typename D::Scalar,3,1,D::Options>
log3(const Eigen::MatrixBase<D> & R)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D, 3, 3);
Eigen::AngleAxis<typename D::Scalar> angleAxis(R);
return angleAxis.axis() * angleAxis.angle();
}
/// \brief Exp: se3 -> SE3.
///
/// Return the integral of the input spatial velocity during time 1.
template <typename _Scalar, int _Options> SE3Tpl<_Scalar, _Options>
exp6(const MotionTpl<_Scalar,_Options> & nu)
{
typedef _Scalar Scalar;
typedef typename MotionTpl<Scalar,_Options>::Vector3 Vector3;
typedef typename MotionTpl<Scalar,_Options>::Matrix3 Matrix3;
const Vector3 & w = nu.angular();
const Vector3 & v = nu.linear();
Scalar t = w.norm();
if (t > 1e-15)
{
Matrix3 R(exp3(w));
Matrix3 S(skew(w));
Matrix3 V(
Matrix3::Identity() +
(1 - cos(t)) / (t * t) * S + (t - sin(t)) / (t * t * t) * S * S);
Vector3 p(V * v);
return SE3Tpl<_Scalar, _Options>(R, p);
}
else
{
return SE3Tpl<_Scalar, _Options>(Matrix3::Identity(), v);
}
}
/// \brief Exp: se3 -> SE3.
///
/// Return the integral of the input spatial velocity during time 1.
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);
return exp6(nu);
}
/// \brief Log: SE3 -> se3.
///
/// Pseudo-inverse of exp from SE3 -> { v,w \in se3, ||w|| < 2pi }.
template <typename _Scalar, int _Options> MotionTpl<_Scalar,_Options>
log6(const SE3Tpl<_Scalar, _Options> & m)
{
typedef _Scalar Scalar;
typedef typename SE3Tpl<Scalar,_Options>::Vector3 Vector3;
typedef typename SE3Tpl<Scalar,_Options>::Matrix3 Matrix3;
const Matrix3 & R = m.rotation();
const Vector3 & p = m.translation();
Vector3 w(log3(R));
Vector3 v;
Scalar t = w.norm();
if (t > 1e-15)
{
Matrix3 S(skew(w));
Matrix3 V(
Matrix3::Identity() +
(1 - cos(t)) / (t * t) * S + (t - sin(t)) / (t * t * t) * S * S);
v = V.inverse() * p;
}
else
{
v = p;
}
return MotionTpl<_Scalar,_Options>(v, w);
}
/// \brief Log: SE3 -> se3.
///
/// Pseudo-inverse of exp from SE3 -> { v,w \in se3, ||w|| < 2pi }.
template <typename D> MotionTpl<typename D::Scalar,D::Options>
log6(const Eigen::MatrixBase<D> & M)
{
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;
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);
return log6(m);
}
} // namespace se3
#endif //#ifndef __math_explog_hpp__
......@@ -42,9 +42,11 @@ ENDMACRO(ADD_TEST_CFLAGS)
ADD_CUSTOM_TARGET(check COMMAND ${CMAKE_CTEST_COMMAND})
ADD_UNIT_TEST(tspatial eigen3)
ADD_UNIT_TEST(symmetric eigen3)
IF(METAPOD_FOUND)
ADD_UNIT_TEST(symmetric metapod eigen3)
ADD_TEST_CFLAGS(symmetric "-DWITH_METAPOD")
ELSE(METAPOD_FOUND)
ADD_UNIT_TEST(symmetric eigen3)
ENDIF(METAPOD_FOUND)
ADD_UNIT_TEST(rnea eigen3)
ADD_UNIT_TEST(non-linear-effects eigen3)
......
......@@ -22,6 +22,7 @@
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/spatial/act-on-set.hpp"
#include "pinocchio/spatial/explog.hpp"
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE tspatialTest
......@@ -281,4 +282,42 @@ BOOST_AUTO_TEST_CASE ( test_ActOnSet )
}
BOOST_AUTO_TEST_CASE ( test_Explog )
{
typedef se3::SE3::Vector3 Vector3;
typedef se3::SE3::Matrix3 Matrix3;
typedef Eigen::Matrix4d Matrix4;
typedef se3::Motion::Vector6 Vector6;
const double EPSILON = 1e-12;
// exp3 and log3.
Vector3 v3(Vector3::Random());
Matrix3 R(se3::exp3(v3));
is_matrix_absolutely_closed(R.transpose(), R.inverse(), EPSILON);
BOOST_CHECK_SMALL(R.determinant() - 1.0, EPSILON);
Vector3 v3FromLog(se3::log3(R));
is_matrix_absolutely_closed(v3, v3FromLog, EPSILON);
// exp6 and log6.
se3::Motion nu = se3::Motion::Random();
se3::SE3 m = se3::exp6(nu);
is_matrix_absolutely_closed(m.rotation().transpose(), m.rotation().inverse(),
EPSILON);
BOOST_CHECK_SMALL(m.rotation().determinant() - 1.0, EPSILON);
se3::Motion nuFromLog(se3::log6(m));
is_matrix_absolutely_closed(nu.linear(), nuFromLog.linear(), EPSILON);
is_matrix_absolutely_closed(nu.angular(), nuFromLog.angular(), EPSILON);
Vector6 v6(Vector6::Random());
se3::SE3 m2(se3::exp6(v6));
is_matrix_absolutely_closed(m2.rotation().transpose(), m2.rotation().inverse(),
EPSILON);
BOOST_CHECK_SMALL(m2.rotation().determinant() - 1.0, EPSILON);
Matrix4 M = m2.toHomogeneousMatrix();
se3::Motion nu2FromLog(se3::log6(M));
Vector6 v6FromLog(nu2FromLog.toVector());
is_matrix_absolutely_closed(v6, v6FromLog, EPSILON);
}
BOOST_AUTO_TEST_SUITE_END ()
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