Skip to content
Snippets Groups Projects
Commit 6839d122 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Valenza Florian
Browse files

First complete bindings of spatial algerba.

parent b48660ed
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 692 additions and 20 deletions
CMakeLists.txt
+ 60
− 20
View file @ 6839d122
...@@ -21,6 +21,13 @@ SET(CMAKE_VERBOSE_MAKEFILE True) ...@@ -21,6 +21,13 @@ SET(CMAKE_VERBOSE_MAKEFILE True)
project(${PROJECT_NAME}) project(${PROJECT_NAME})
SETUP_PROJECT() SETUP_PROJECT()
IF(WIN32)
SET(LINK copy_if_different)
ELSE(WIN32)
SET(LINK create_symlink)
ENDIF(WIN32)
# ---------------------------------------------------- # ----------------------------------------------------
# --- DEPENDANCIES ----------------------------------- # --- DEPENDANCIES -----------------------------------
# ---------------------------------------------------- # ----------------------------------------------------
...@@ -30,7 +37,7 @@ ADD_REQUIRED_DEPENDENCY("urdfdom >= v0.3.0") ...@@ -30,7 +37,7 @@ ADD_REQUIRED_DEPENDENCY("urdfdom >= v0.3.0")
# ---------------------------------------------------- # ----------------------------------------------------
# --- INCLUDE ---------------------------------------- # --- INCLUDE ----------------------------------------
# ---------------------------------------------------- # ----------------------------------------------------
SET(${PROJECT_NAME}_HEADERS SET(HEADERS
exception.hpp exception.hpp
assert.hpp assert.hpp
spatial/symmetric3.hpp spatial/symmetric3.hpp
...@@ -76,31 +83,64 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/tools") ...@@ -76,31 +83,64 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/tools")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/algorithm") MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/algorithm")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/python") MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/python")
FOREACH(header ${${PROJECT_NAME}_HEADERS}) FOREACH(header ${HEADERS})
GET_FILENAME_COMPONENT(headerName ${header} NAME) GET_FILENAME_COMPONENT(headerName ${header} NAME)
IF(WIN32) GET_FILENAME_COMPONENT(headerPath ${header} PATH)
execute_process(COMMAND ${CMAKE_COMMAND} -E copy_if_different EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E ${LINK}
${${PROJECT_NAME}_SOURCE_DIR}/src/${header} ${${PROJECT_NAME}_SOURCE_DIR}/src/${header}
${${PROJECT_NAME}_BINARY_DIR}/include/${PROJECT_NAME}/${header}) ${${PROJECT_NAME}_BINARY_DIR}/include/${PROJECT_NAME}/${header})
ELSE(WIN32) INSTALL(FILES ${${PROJECT_NAME}_SOURCE_DIR}/src/${header}
execute_process(COMMAND ${CMAKE_COMMAND} -E create_symlink DESTINATION ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}/${headerPath}
${${PROJECT_NAME}_SOURCE_DIR}/src/${header}
${${PROJECT_NAME}_BINARY_DIR}/include/${PROJECT_NAME}/${header})
ENDIF(WIN32)
INSTALL(FILES ${${PROJECT_NAME}_SOURCE_DIR}/${header}
DESTINATION ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}
PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE) PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
ENDFOREACH(header) ENDFOREACH(header)
# ---------------------------------------------------- # ----------------------------------------------------
# --- TARGETS ---------------------------------------- # --- PYTHON -----------------------------------------
# ---------------------------------------------------- # ----------------------------------------------------
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/lib/python") FINDPYTHON()
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/lib/python/${PROJECT_NAME}")
ADD_LIBRARY(pynocchio SHARED src/python/module.cpp src/python/python.cpp)
PKG_CONFIG_USE_DEPENDENCY(pynocchio eigenpy) # --- COMPILE WRAPPER
SET_TARGET_PROPERTIES(pynocchio PROPERTIES PREFIX "") SET(PYWRAP ${PROJECT_NAME}_pywrap)
TARGET_LINK_LIBRARIES(pynocchio ${Boost_LIBRARIES} eigenpy) ADD_LIBRARY(${PYWRAP} SHARED src/python/module.cpp src/python/python.cpp)
PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} eigenpy)
TARGET_LINK_LIBRARIES(${PYWRAP} ${Boost_LIBRARIES} eigenpy)
SET_TARGET_PROPERTIES(${PYWRAP} PROPERTIES
LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/lib/python/${PROJECT_NAME}")
# --- INSTALL SCRIPTS
SET(PYTHON_FILES
python/__init__.py
python/utils.py
)
FOREACH(python ${PYTHON_FILES})
GET_FILENAME_COMPONENT(pythonFile ${python} NAME)
EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E ${LINK}
${${PROJECT_NAME}_SOURCE_DIR}/src/${python}
${${PROJECT_NAME}_BINARY_DIR}/lib/python/${PROJECT_NAME}/${pythonFile})
# Tag pyc file as generated.
#SET_SOURCE_FILES_PROPERTIES(
# "${CMAKE_CURRENT_BINARY_DIR}/lib/python/${pythonFile}c"
# PROPERTIES GENERATED TRUE)
# Clean generated files.
#SET_PROPERTY(
# DIRECTORY APPEND PROPERTY
# ADDITIONAL_MAKE_CLEAN_FILES
# "${CMAKE_CURRENT_BINARY_DIR}/lib/python/${pythonFile}c")
#INSTALL(CODE
# "EXECUTE_PROCESS(COMMAND
# \"${PYTHON_EXECUTABLE}\"
# \"-m py_compile\"
# \"${CMAKE_CURRENT_BINARY_DIR}/lib/python/${pythonFile}\") ")
INSTALL(FILES
"${CMAKE_CURRENT_BINARY_DIR}/lib/python/${PROJECT_NAME}/${pythonFile}"
# "${CMAKE_CURRENT_BINARY_DIR}/lib/python/${pythonFile}c"
DESTINATION ${PYTHON_SITELIB}/${PROJECT_NAME})
ENDFOREACH(python)
# ---------------------------------------------------- # ----------------------------------------------------
# --- UNITTESTS -------------------------------------- # --- UNITTESTS --------------------------------------
......
python/bindings.py 0 → 100644
+ 53
− 0
View file @ 6839d122
import pinocchio as se3
from pinocchio.utils import *
# --- SE3
R = rand([3,3]); [R,tmp,tmp] = npl.svd(R)
p = rand(3)
m = se3.SE3(R,p)
X = np.vstack( [ np.hstack([ R, skew(p)*R ]), np.hstack([ zero([3,3]), R ]) ])
assert( isapprox(m.action(),X))
M = np.vstack( [ np.hstack([R,p]), np.matrix('0 0 0 1',np.double) ] )
assert( isapprox(m.homogeneous(),M) )
p = rand(3)
assert(isapprox(m*p,m.rotation*p+m.translation))
assert(isapprox(m.actInv(p),m.rotation.T*p-m.rotation.T*m.translation))
p = np.vstack([p,1])
assert(isapprox(m*p,m.homogeneous()*p))
assert(isapprox(m.actInv(p),npl.inv(m.homogeneous())*p))
p = rand(6)
assert(isapprox(m*p,m.action()*p))
assert(isapprox(m.actInv(p),npl.inv(m.action())*p))
# --- Motion
assert(isapprox(se3.Motion.Zero().vector(),zero(6)))
v = se3.Motion.Random()
assert(isapprox((m*v).vector(),m.action()*v.vector()))
assert(isapprox((m.actInv(v)).vector(),npl.inv(m.action())*v.vector()))
vv = v.linear; vw = v.angular
assert(isapprox( v.vector(),np.vstack([vv,vw]) ))
assert(isapprox((v**v).vector(),zero(6)))
# --- Force ---
assert(isapprox(se3.Force.Zero().vector(),zero(6)))
f = se3.Force.Random()
ff = f.linear; ft = f.angular
assert(isapprox( f.vector(),np.vstack([ff,ft]) ))
assert(isapprox((m*f).vector(),npl.inv(m.action().T)*f.vector()))
assert(isapprox((m.actInv(f)).vector(),m.action().T*f.vector()))
f = se3.Force( np.vstack([ v.vector()[3:], v.vector()[:3] ]) )
assert(isapprox( (v**f).vector(),zero(6) ))
# --- Inertia ---
Y1 = se3.Inertia.Random()
Y2 = se3.Inertia.Random()
Y=Y1+Y2
assert(isapprox(Y1.matrix()+Y2.matrix(),Y.matrix()))
assert(isapprox((Y*v).vector(),Y.matrix()*v.vector()))
assert(isapprox( (m*Y).matrix(),m.inverse().action().T*Y.matrix()*m.inverse().action()))
assert(isapprox( (m.actInv(Y)).matrix(),m.action().T*Y.matrix()*m.action()))
src/python/__init__.py 0 → 100644
+ 52
− 0
View file @ 6839d122
import numpy as np
import libpinocchio_pywrap as se3
import utils
# --- SE3 action ---
def SE3act(m,x):
assert(isinstance(m,se3.SE3))
if isinstance(x,np.ndarray):
if x.shape[0]==3:
return m.rotation*x+m.translation
elif x.shape[0] == 4:
return m.homogeneous()*x
elif x.shape[0] == 6:
return m.action()*x
else: raise Exception('Error: m can only act on linear object of size 3, 4 and 6.')
elif 'se3Action' in x.__class__.__dict__:
return x.se3Action(m)
else:
print 'Error: SE3 cannot act on the given object'
setattr(se3.SE3,'__mul__',SE3act)
setattr(se3.SE3,'act',SE3act)
def SE3actinv(m,x):
assert(isinstance(m,se3.SE3))
if isinstance(x,np.ndarray):
if x.shape[0]==3:
return m.rotation.T*x-m.rotation.T*m.translation
elif x.shape[0] == 4:
return m.inverse().homogeneous()*x
elif x.shape[0] == 6:
return m.inverse().action()*x
else: raise Exception('Error: m can only act on linear object of size 3, 4 and 6.')
elif 'se3Action' in x.__class__.__dict__:
return x.se3ActionInverse(m)
else:
print 'Error: SE3 cannot act on the given object'
setattr(se3.SE3,'actInv',SE3actinv)
# --- M6/F6 cross product --
def SE3cross(self,y):
assert(isinstance(self,se3.Motion))
if isinstance(y,se3.Motion):
return self.cross_motion(y)
elif isinstance(y,se3.Force):
return self.cross_force(y)
else: raise Exception('Error: SE3 cross product only apply on M6xM6 or M6xF6.')
setattr(se3.Motion,'__pow__',SE3cross)
setattr(se3.Motion,'cross',SE3cross)
from libpinocchio_pywrap import *
src/python/force.hpp 0 → 100644
+ 98
− 0
View file @ 6839d122
#ifndef __se3_python_force_hpp__
#define __se3_python_force_hpp__
#include <eigenpy/exception.hpp>
#include <eigenpy/eigenpy.hpp>
#include "pinocchio/spatial/force.hpp"
namespace eigenpy
{
template<>
struct UnalignedEquivalent<se3::Force>
{
typedef se3::ForceTpl<double,Eigen::DontAlign> type;
};
} // namespace eigenpy
namespace se3
{
namespace python
{
namespace bp = boost::python;
template<typename Force>
struct ForcePythonVisitor
: public boost::python::def_visitor< ForcePythonVisitor<Force> >
{
typedef typename eigenpy::UnalignedEquivalent<Force>::type Force_fx;
typedef typename Force::Matrix3 Matrix3;
typedef typename Force::Matrix6 Matrix6;
typedef typename Force::Vector6 Vector6;
typedef typename Force::Vector3 Vector3;
typedef typename Force_fx::Matrix3 Matrix3_fx;
typedef typename Force_fx::Matrix6 Matrix6_fx;
typedef typename Force_fx::Vector6 Vector6_fx;
typedef typename Force_fx::Vector3 Vector3_fx;
public:
static PyObject* convert(Force const& m)
{
Force_fx m_fx = m;
return boost::python::incref(boost::python::object(m_fx).ptr());
}
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def(bp::init<Vector3_fx,Vector3_fx>
((bp::arg("linear"),bp::arg("angular")),
"Initialize from linear and angular components (dont mix the order)."))
.def(bp::init<Vector6_fx>((bp::arg("Vector 6d")),"Init from vector 6 [f,n]"))
.add_property("linear",&ForcePythonVisitor::getLinear,&ForcePythonVisitor::setLinear)
.add_property("angular",&ForcePythonVisitor::getAngular,&ForcePythonVisitor::setAngular)
.def("vector",&Force_fx::toVector)
.def("se3Action",&Force_fx::se3Action)
.def("se3ActionInverse",&Force_fx::se3ActionInverse)
.def("__str__",&ForcePythonVisitor::toString)
.def("Random",&Force_fx::Random)
.staticmethod("Random")
.def("Zero",&Force_fx::Zero)
.staticmethod("Zero")
;
}
static Vector3_fx getLinear( const Force_fx & self ) { return self.linear(); }
static void setLinear( Force_fx & self, const Vector3_fx & R ) { self.linear(R); }
static Vector3_fx getAngular( const Force_fx & self ) { return self.angular(); }
static void setAngular( Force_fx & self, const Vector3_fx & R ) { self.angular(R); }
static std::string toString(const Force_fx& m)
{ std::ostringstream s; s << m; return s.str(); }
static void expose()
{
bp::class_<Force_fx>("Force",
"Force vectors, in se3* == F^6.\n\n"
"Supported operations ...",
bp::init<>())
.def(ForcePythonVisitor<Force>())
;
bp::to_python_converter< Force,ForcePythonVisitor<Force> >();
}
};
}} // namespace se3::python
#endif // ifndef __se3_python_se3_hpp__
src/python/inertia.hpp 0 → 100644
+ 116
− 0
View file @ 6839d122
#ifndef __se3_python_inertia_hpp__
#define __se3_python_inertia_hpp__
#include <eigenpy/exception.hpp>
#include <eigenpy/eigenpy.hpp>
#include "pinocchio/spatial/inertia.hpp"
namespace eigenpy
{
template<>
struct UnalignedEquivalent<se3::Inertia>
{
typedef se3::InertiaTpl<double,Eigen::DontAlign> type;
};
} // namespace eigenpy
namespace se3
{
namespace python
{
namespace bp = boost::python;
template<typename Inertia>
struct InertiaPythonVisitor
: public boost::python::def_visitor< InertiaPythonVisitor<Inertia> >
{
typedef typename eigenpy::UnalignedEquivalent<Inertia>::type Inertia_fx;
typedef typename Inertia::Matrix3 Matrix3;
typedef typename Inertia::Matrix6 Matrix6;
typedef typename Inertia::Vector6 Vector6;
typedef typename Inertia::Vector3 Vector3;
typedef typename Inertia_fx::Matrix3 Matrix3_fx;
typedef typename Inertia_fx::Matrix6 Matrix6_fx;
typedef typename Inertia_fx::Vector6 Vector6_fx;
typedef typename Inertia_fx::Vector3 Vector3_fx;
typedef typename Inertia_fx::Motion Motion_fx ;
public:
static PyObject* convert(Inertia const& m)
{
Inertia_fx m_fx = m;
return bp::incref(bp::object(m_fx).ptr());
}
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def("__init__",
bp::make_constructor(&InertiaPythonVisitor::makeFromMCI,
bp::default_call_policies(),
(bp::arg("mass"),bp::arg("lever"),bp::arg("inertia"))),
"Initialize from mass, lever and 3d inertia.")
.add_property("mass",&Inertia_fx::mass)
.add_property("lever",&InertiaPythonVisitor::lever)
.add_property("inertia",&InertiaPythonVisitor::inertia)
.def("matrix",&Inertia_fx::matrix)
.def("se3Action",&Inertia_fx::se3Action)
.def("se3ActionInverse",&Inertia_fx::se3ActionInverse)
.def("__str__",&InertiaPythonVisitor::toString)
.def( bp::self + bp::self)
.def( bp::self * bp::other<Motion_fx>() )
.def("Identity",&Inertia_fx::Identity)
.staticmethod("Identity")
.def("Zero",&Inertia_fx::Zero)
.staticmethod("Zero")
.def("Random",&Inertia_fx::Random)
.staticmethod("Random")
;
}
static Inertia_fx* makeFromMCI(const double & mass,
const Vector3_fx & lever,
const Matrix3_fx & inertia)
{
if(! inertia.isApprox(inertia.transpose()) )
throw eigenpy::Exception("The 3d inertia should be symmetric.");
if( (Eigen::Vector3d::UnitX().transpose()*inertia*Eigen::Vector3d::UnitX()<0)
|| (Eigen::Vector3d::UnitY().transpose()*inertia*Eigen::Vector3d::UnitY()<0)
|| (Eigen::Vector3d::UnitZ().transpose()*inertia*Eigen::Vector3d::UnitZ()<0) )
throw eigenpy::Exception("The 3d inertia should be positive.");
return new Inertia_fx(mass,lever,inertia);
}
static Matrix3_fx inertia(const Inertia_fx& Y) { return Y.inertia().matrix(); }
static Vector3_fx lever(const Inertia_fx& Y) { return Y.lever(); }
static std::string toString(const Inertia_fx& m)
{ std::ostringstream s; s << m; return s.str(); }
static void expose()
{
bp::class_<Inertia_fx>("Inertia",
"Inertia matrix, in L(se3,se3*) == R^6x6.\n\n"
"Supported operations ...",
bp::init<>())
.def(InertiaPythonVisitor<Inertia>())
;
bp::to_python_converter< Inertia,InertiaPythonVisitor<Inertia> >();
}
};
}} // namespace se3::python
#endif // ifndef __se3_python_se3_hpp__
src/python/module.cpp 0 → 100644
+ 26
− 0
View file @ 6839d122
#include <eigenpy/eigenpy.hpp>
#include <eigenpy/geometry.hpp>
#include "pinocchio/python/python.hpp"
#include <iostream>
namespace bp = boost::python;
BOOST_PYTHON_MODULE(libpinocchio_pywrap)
{
eigenpy::enableEigenPy();
eigenpy::exposeAngleAxis();
eigenpy::exposeQuaternion();
typedef Eigen::Matrix<double,6,6> Matrix6d;
typedef Eigen::Matrix<double,6,1> Vector6d;
eigenpy::enableEigenPySpecific<Matrix6d,Matrix6d>();
eigenpy::enableEigenPySpecific<Vector6d,Vector6d>();
se3::python::exposeSE3();
se3::python::exposeForce();
se3::python::exposeMotion();
se3::python::exposeInertia();
}
src/python/motion.hpp 0 → 100644
+ 108
− 0
View file @ 6839d122
#ifndef __se3_python_motion_hpp__
#define __se3_python_motion_hpp__
#include <eigenpy/exception.hpp>
#include <eigenpy/eigenpy.hpp>
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/force.hpp"
namespace eigenpy
{
template<>
struct UnalignedEquivalent<se3::Motion>
{
typedef se3::MotionTpl<double,Eigen::DontAlign> type;
};
} // namespace eigenpy
namespace se3
{
namespace python
{
namespace bp = boost::python;
template<typename Motion>
struct MotionPythonVisitor
: public boost::python::def_visitor< MotionPythonVisitor<Motion> >
{
typedef typename Motion::Force Force;
typedef typename Motion::Matrix3 Matrix3;
typedef typename Motion::Matrix6 Matrix6;
typedef typename Motion::Vector6 Vector6;
typedef typename Motion::Vector3 Vector3;
typedef typename eigenpy::UnalignedEquivalent<Motion>::type Motion_fx;
typedef typename Motion_fx::Force Force_fx;
typedef typename Motion_fx::Matrix3 Matrix3_fx;
typedef typename Motion_fx::Matrix6 Matrix6_fx;
typedef typename Motion_fx::Vector6 Vector6_fx;
typedef typename Motion_fx::Vector3 Vector3_fx;
public:
static PyObject* convert(Motion const& m)
{
Motion_fx m_fx = m;
return boost::python::incref(boost::python::object(m_fx).ptr());
}
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def(bp::init<Vector3_fx,Vector3_fx>
((bp::arg("linear"),bp::arg("angular")),
"Initialize from linear and angular components (dont mix the order)."))
.def(bp::init<Vector6_fx>((bp::arg("Vector 6d")),"Init from vector 6 [f,n]"))
.add_property("linear",&MotionPythonVisitor::getLinear,&MotionPythonVisitor::setLinear)
.add_property("angular",&MotionPythonVisitor::getAngular,&MotionPythonVisitor::setAngular)
.def("vector",&Motion_fx::toVector)
.def("se3Action",&Motion_fx::se3Action)
.def("se3ActionInverse",&Motion_fx::se3ActionInverse)
.def("cross_motion",&MotionPythonVisitor::cross_motion)
.def("cross_force",&MotionPythonVisitor::cross_force)
.def("__str__",&MotionPythonVisitor::toString)
.def("Random",&Motion_fx::Random)
.staticmethod("Random")
.def("Zero",&Motion_fx::Zero)
.staticmethod("Zero")
;
}
static Vector3_fx getLinear( const Motion_fx & self ) { return self.linear(); }
static void setLinear( Motion_fx & self, const Vector3_fx & R ) { self.linear(R); }
static Vector3_fx getAngular( const Motion_fx & self ) { return self.angular(); }
static void setAngular( Motion_fx & self, const Vector3_fx & R ) { self.angular(R); }
static Motion_fx cross_motion( const Motion_fx& m1,const Motion_fx& m2 ) { return m1.cross(m2); }
static Force_fx cross_force( const Motion_fx& m,const Force_fx& f ) { return m.cross(f); }
static std::string toString(const Motion_fx& m)
{ std::ostringstream s; s << m; return s.str(); }
static void expose()
{
bp::class_<Motion_fx>("Motion",
"Motion vectors, in se3* == F^6.\n\n"
"Supported operations ...",
bp::init<>())
.def(MotionPythonVisitor<Motion>())
;
bp::to_python_converter< Motion,MotionPythonVisitor<Motion> >();
}
};
}} // namespace se3::python
#endif // ifndef __se3_python_se3_hpp__
src/python/python.cpp 0 → 100644
+ 35
− 0
View file @ 6839d122
#include "pinocchio/python/python.hpp"
#include "pinocchio/python/se3.hpp"
#include "pinocchio/python/force.hpp"
#include "pinocchio/python/motion.hpp"
#include "pinocchio/python/inertia.hpp"
namespace se3
{
namespace python
{
void exposeSE3()
{
SE3PythonVisitor<SE3>::expose();
}
void exposeForce()
{
ForcePythonVisitor<Force>::expose();
}
void exposeMotion()
{
MotionPythonVisitor<Motion>::expose();
}
void exposeInertia()
{
InertiaPythonVisitor<Inertia>::expose();
}
void exposeStatics()
{
}
}} // namespace se3::python
src/python/python.hpp 0 → 100644
+ 16
− 0
View file @ 6839d122
#ifndef __se3_python_python_hpp__
#define __se3_python_python_hpp__
namespace se3
{
namespace python
{
void exposeSE3();
void exposeForce();
void exposeMotion();
void exposeInertia();
}} // namespace se3::python
#endif // ifndef __se3_python_python_hpp__
src/python/se3.hpp 0 → 100644
+ 112
− 0
View file @ 6839d122
#ifndef __se3_python_se3_hpp__
#define __se3_python_se3_hpp__
#include <eigenpy/exception.hpp>
#include <eigenpy/eigenpy.hpp>
#include "pinocchio/spatial/se3.hpp"
namespace eigenpy
{
template<>
struct UnalignedEquivalent<se3::SE3>
{
typedef se3::SE3Tpl<double,Eigen::DontAlign> type;
};
} // namespace eigenpy
namespace se3
{
namespace python
{
namespace bp = boost::python;
template<typename SE3>
struct SE3PythonVisitor
: public boost::python::def_visitor< SE3PythonVisitor<SE3> >
{
typedef typename eigenpy::UnalignedEquivalent<SE3>::type SE3_fx;
typedef typename SE3::Matrix3 Matrix3;
typedef typename SE3::Matrix6 Matrix6;
typedef typename SE3::Matrix4 Matrix4;
typedef typename SE3::Vector6 Vector6;
typedef typename SE3::Vector3 Vector3;
typedef typename SE3_fx::Matrix3 Matrix3_fx;
typedef typename SE3_fx::Matrix6 Matrix6_fx;
typedef typename SE3_fx::Matrix4 Matrix4_fx;
typedef typename SE3_fx::Vector6 Vector6_fx;
typedef typename SE3_fx::Vector3 Vector3_fx;
public:
static PyObject* convert(SE3 const& m)
{
SE3_fx m_fx = m;
return boost::python::incref(boost::python::object(m_fx).ptr());
}
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def(bp::init<Matrix3_fx,Vector3_fx>
((bp::arg("Rotation"),bp::arg("translation")),
"Initialize from rotation and translation."))
.def(bp::init<int>
((bp::arg("trivial arg (should be 1)")),
"Init to identity."))
.add_property("rotation",&SE3PythonVisitor::getRotation,&SE3PythonVisitor::setRotation)
.add_property("translation",&SE3PythonVisitor::getTranslation,&SE3PythonVisitor::setTranslation)
.def("homogeneous",&SE3_fx::toHomogeneousMatrix)
.def("action",&SE3_fx::toActionMatrix)
.def("inverse", &SE3_fx::inverse)
.def("act_point", &SE3PythonVisitor::act_point)
.def("actInv_point", &SE3PythonVisitor::actInv_point)
.def("act_se3", &SE3PythonVisitor::act_se3)
.def("actInv_se3", &SE3PythonVisitor::actInv_se3)
.def("__str__",&SE3PythonVisitor::toString)
.def("Identity",&SE3_fx::Identity)
.staticmethod("Identity")
.def("Random",&SE3_fx::Random)
.staticmethod("Random")
;
}
static Matrix3_fx getRotation( const SE3_fx & self ) { return self.rotation(); }
static void setRotation( SE3_fx & self, const Matrix3_fx & R ) { self.rotation(R); }
static Vector3_fx getTranslation( const SE3_fx & self ) { return self.translation(); }
static void setTranslation( SE3_fx & self, const Vector3_fx & p ) { self.translation(p); }
static Vector3_fx act_point( const SE3_fx & self, const Vector3_fx & pt ) { return self.act(pt); }
static Vector3_fx actInv_point( const SE3_fx & self, const Vector3_fx & pt ) { return self.actInv(pt); }
static SE3_fx act_se3( const SE3_fx & self, const SE3_fx & x ) { return self.act(x); }
static SE3_fx actInv_se3( const SE3_fx & self, const SE3_fx & x ) { return self.actInv(x); }
static std::string toString(const SE3_fx& m)
{ std::ostringstream s; s << m; return s.str(); }
static void expose()
{
bp::class_<SE3_fx>("SE3",
"SE3 transformations, composing rotation and translation.\n\n"
"Supported operations ...",
bp::init<>())
.def(SE3PythonVisitor<SE3>())
;
bp::to_python_converter< SE3,SE3PythonVisitor<SE3> >();
}
};
}} // namespace se3::python
#endif // ifndef __se3_python_se3_hpp__
src/python/utils.py 0 → 100644
+ 16
− 0
View file @ 6839d122
import numpy as np
import numpy.linalg as npl
eye = lambda n: np.matrix(np.eye(n),np.double)
zero = lambda n: np.matrix(np.zeros([n,1] if isinstance(n,int) else n),np.double)
rand = lambda n: np.matrix(np.random.rand(n,1) if isinstance(n,int) else np.random.rand(n[0],n[1]),np.double)
def skew(p):
x=p[0];y=p[1];z=p[2]
return np.matrix([ [ 0,-z,y ], [ z,0,-x ], [ -y,x,0 ] ],np.double)
def isapprox(a,b,epsilon=1e-6):
if issubclass(a.__class__,np.ndarray) and issubclass(b.__class__,np.ndarray):
return np.allclose(a,b,epsilon)
else:
return abs(a-b)<epsilon
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