From 6839d1224810a47b982eefc271368c4416824662 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Mon, 22 Sep 2014 21:33:56 +0200
Subject: [PATCH] First complete bindings of spatial algerba.
---
CMakeLists.txt | 80 +++++++++++++++++++++-------
python/bindings.py | 53 +++++++++++++++++++
src/python/__init__.py | 52 ++++++++++++++++++
src/python/force.hpp | 98 ++++++++++++++++++++++++++++++++++
src/python/inertia.hpp | 116 +++++++++++++++++++++++++++++++++++++++++
src/python/module.cpp | 26 +++++++++
src/python/motion.hpp | 108 ++++++++++++++++++++++++++++++++++++++
src/python/python.cpp | 35 +++++++++++++
src/python/python.hpp | 16 ++++++
src/python/se3.hpp | 112 +++++++++++++++++++++++++++++++++++++++
src/python/utils.py | 16 ++++++
11 files changed, 692 insertions(+), 20 deletions(-)
create mode 100644 python/bindings.py
create mode 100644 src/python/__init__.py
create mode 100644 src/python/force.hpp
create mode 100644 src/python/inertia.hpp
create mode 100644 src/python/module.cpp
create mode 100644 src/python/motion.hpp
create mode 100644 src/python/python.cpp
create mode 100644 src/python/python.hpp
create mode 100644 src/python/se3.hpp
create mode 100644 src/python/utils.py
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5c7593842..ef510c34c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -21,6 +21,13 @@ SET(CMAKE_VERBOSE_MAKEFILE True)
project(${PROJECT_NAME})
SETUP_PROJECT()
+IF(WIN32)
+ SET(LINK copy_if_different)
+ELSE(WIN32)
+ SET(LINK create_symlink)
+ENDIF(WIN32)
+
+
# ----------------------------------------------------
# --- DEPENDANCIES -----------------------------------
# ----------------------------------------------------
@@ -30,7 +37,7 @@ ADD_REQUIRED_DEPENDENCY("urdfdom >= v0.3.0")
# ----------------------------------------------------
# --- INCLUDE ----------------------------------------
# ----------------------------------------------------
-SET(${PROJECT_NAME}_HEADERS
+SET(HEADERS
exception.hpp
assert.hpp
spatial/symmetric3.hpp
@@ -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/python")
-FOREACH(header ${${PROJECT_NAME}_HEADERS})
+FOREACH(header ${HEADERS})
GET_FILENAME_COMPONENT(headerName ${header} NAME)
- IF(WIN32)
- execute_process(COMMAND ${CMAKE_COMMAND} -E copy_if_different
- ${${PROJECT_NAME}_SOURCE_DIR}/src/${header}
- ${${PROJECT_NAME}_BINARY_DIR}/include/${PROJECT_NAME}/${header})
- ELSE(WIN32)
- execute_process(COMMAND ${CMAKE_COMMAND} -E create_symlink
- ${${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}
+ GET_FILENAME_COMPONENT(headerPath ${header} PATH)
+ EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E ${LINK}
+ ${${PROJECT_NAME}_SOURCE_DIR}/src/${header}
+ ${${PROJECT_NAME}_BINARY_DIR}/include/${PROJECT_NAME}/${header})
+ INSTALL(FILES ${${PROJECT_NAME}_SOURCE_DIR}/src/${header}
+ DESTINATION ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}/${headerPath}
PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
ENDFOREACH(header)
# ----------------------------------------------------
-# --- TARGETS ----------------------------------------
+# --- PYTHON -----------------------------------------
# ----------------------------------------------------
-MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/lib/python")
-
-ADD_LIBRARY(pynocchio SHARED src/python/module.cpp src/python/python.cpp)
-PKG_CONFIG_USE_DEPENDENCY(pynocchio eigenpy)
-SET_TARGET_PROPERTIES(pynocchio PROPERTIES PREFIX "")
-TARGET_LINK_LIBRARIES(pynocchio ${Boost_LIBRARIES} eigenpy)
+FINDPYTHON()
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/lib/python/${PROJECT_NAME}")
+
+# --- COMPILE WRAPPER
+SET(PYWRAP ${PROJECT_NAME}_pywrap)
+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 --------------------------------------
diff --git a/python/bindings.py b/python/bindings.py
new file mode 100644
index 000000000..8f37b9e4a
--- /dev/null
+++ b/python/bindings.py
@@ -0,0 +1,53 @@
+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()))
diff --git a/src/python/__init__.py b/src/python/__init__.py
new file mode 100644
index 000000000..03bbca1e6
--- /dev/null
+++ b/src/python/__init__.py
@@ -0,0 +1,52 @@
+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 *
+
+
diff --git a/src/python/force.hpp b/src/python/force.hpp
new file mode 100644
index 000000000..13eb0b61f
--- /dev/null
+++ b/src/python/force.hpp
@@ -0,0 +1,98 @@
+#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__
+
diff --git a/src/python/inertia.hpp b/src/python/inertia.hpp
new file mode 100644
index 000000000..f0d96275d
--- /dev/null
+++ b/src/python/inertia.hpp
@@ -0,0 +1,116 @@
+#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__
+
diff --git a/src/python/module.cpp b/src/python/module.cpp
new file mode 100644
index 000000000..2e089477a
--- /dev/null
+++ b/src/python/module.cpp
@@ -0,0 +1,26 @@
+#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();
+}
+
diff --git a/src/python/motion.hpp b/src/python/motion.hpp
new file mode 100644
index 000000000..425e42fbe
--- /dev/null
+++ b/src/python/motion.hpp
@@ -0,0 +1,108 @@
+#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__
+
diff --git a/src/python/python.cpp b/src/python/python.cpp
new file mode 100644
index 000000000..0b33429e5
--- /dev/null
+++ b/src/python/python.cpp
@@ -0,0 +1,35 @@
+#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
diff --git a/src/python/python.hpp b/src/python/python.hpp
new file mode 100644
index 000000000..e82dec694
--- /dev/null
+++ b/src/python/python.hpp
@@ -0,0 +1,16 @@
+#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__
+
diff --git a/src/python/se3.hpp b/src/python/se3.hpp
new file mode 100644
index 000000000..08660c838
--- /dev/null
+++ b/src/python/se3.hpp
@@ -0,0 +1,112 @@
+#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__
+
diff --git a/src/python/utils.py b/src/python/utils.py
new file mode 100644
index 000000000..0b2152f0e
--- /dev/null
+++ b/src/python/utils.py
@@ -0,0 +1,16 @@
+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
--
GitLab