From 1caabe21e2a10773a782e49a9152d10887123f80 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Tue, 23 Sep 2014 15:54:33 +0200
Subject: [PATCH] First complete wrapping of the model.
---
CMakeLists.txt | 2 +
python/test_model.py | 17 +++-
src/python/eigen_container.hpp | 56 ++++++++++++
src/python/handler.hpp | 76 ++++++++++++++++
src/python/model.hpp | 161 +++++++--------------------------
src/python/python.cpp | 4 +
6 files changed, 183 insertions(+), 133 deletions(-)
create mode 100644 src/python/eigen_container.hpp
create mode 100644 src/python/handler.hpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index edd3d06a1..2dce72f8b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -67,6 +67,8 @@ SET(HEADERS
algorithm/cholesky.hpp
algorithm/kinematics.hpp
algorithm/center-of-mass.hpp
+ python/eigen_container.hpp
+ python/handler.hpp
python/python.hpp
python/se3.hpp
python/force.hpp
diff --git a/python/test_model.py b/python/test_model.py
index 36cf3e675..1f750db01 100644
--- a/python/test_model.py
+++ b/python/test_model.py
@@ -2,11 +2,20 @@ import pinocchio as se3
from pinocchio.utils import *
model = se3.Model.BuildEmptyModel()
-print model
+assert(model.nbody==1 and model.nq==0 and model.nv==0)
+
model = se3.Model.BuildHumanoidSimple()
-print model
-print "Bye bye"
+nb=28 # We should have 28 bodies, thus 27 joints, one of them a free-flyer.
+assert(model.nbody==nb and model.nq==nb-1+6 and model.nv==nb-1+5)
+model.inertias[1] = model.inertias[2]
+assert( isapprox(model.inertias[1].np,model.inertias[2].np) )
+model.jointPlacements[1] = model.jointPlacements[2]
+assert( isapprox(model.jointPlacements[1].np,model.jointPlacements[2].np) )
+assert(model.parents[0]==0 and model.parents[1] == 0)
+model.parents[2] = model.parents[1]
+assert( model.parents[2] == model.parents[1] )
+assert(model.names[0] == "universe" )
+assert( isapprox(model.gravity.np,np.matrix('0; 0; -9.81; 0; 0; 0')) )
data = model.createData()
-print model.inertias()
diff --git a/src/python/eigen_container.hpp b/src/python/eigen_container.hpp
new file mode 100644
index 000000000..296d2c3bb
--- /dev/null
+++ b/src/python/eigen_container.hpp
@@ -0,0 +1,56 @@
+#ifndef __se3_python_eigen_container_hpp__
+#define __se3_python_eigen_container_hpp__
+
+#include <eigenpy/exception.hpp>
+#include <eigenpy/eigenpy.hpp>
+
+#include <boost/shared_ptr.hpp>
+#include <boost/python/return_internal_reference.hpp>
+
+namespace se3
+{
+ namespace python
+ {
+ namespace bp = boost::python;
+
+ /* Expose a std::vector containing aligned (Eigen) objects, whose unaligned
+ * correspondance is defined in eigenpy::UnalignedEquivalent.
+ * Simply call the "expose" method from inside the python module.
+ */
+ template< typename EigenObject >
+ struct PyWraperForAlignedStdVector
+ : public boost::python::def_visitor< PyWraperForAlignedStdVector<EigenObject> >
+ {
+ typedef typename eigenpy::UnalignedEquivalent<EigenObject>::type EigenObject_fx;
+ typedef std::vector<EigenObject> stdVectorAligned;
+
+ template<class PyClass>
+ void visit(PyClass& cl) const
+ {
+ cl
+ .def("__getitem__", &PyWraperForAlignedStdVector::getItem)
+ .def("__setitem__", &PyWraperForAlignedStdVector::setItem)
+ .def("__len__", &PyWraperForAlignedStdVector::length)
+ ;
+ }
+
+ static EigenObject getItem( const stdVectorAligned & Ys,int i)
+ { return Ys[i]; }
+
+ static void setItem( stdVectorAligned & Ys,
+ int i,const EigenObject_fx & Y)
+ { Ys[i] = Y; }
+ static int length( const stdVectorAligned & Ys )
+ { return Ys.size(); }
+
+ static void expose(const std::string & className)
+ {
+ bp::class_<stdVectorAligned>(className.c_str())
+ .def(PyWraperForAlignedStdVector());
+ }
+ };
+
+ }} // namespace se3::python
+
+#endif // ifndef __se3_python_eigen_container_hpp__
+
diff --git a/src/python/handler.hpp b/src/python/handler.hpp
new file mode 100644
index 000000000..1800d02a6
--- /dev/null
+++ b/src/python/handler.hpp
@@ -0,0 +1,76 @@
+#ifndef __se3_python_handler_hpp__
+#define __se3_python_handler_hpp__
+
+#include <boost/shared_ptr.hpp>
+
+namespace se3
+{
+ namespace python
+ {
+
+ /* This handler is designed first for holding the C++ Model object, then
+ * generalized for the C++ Data object. It might be used as well for
+ * handling other object whose ownership is shared between python and C++.
+ *
+ * There might be several way of building and owning a model object:
+ * - build from C++ (as static or dynamic, wathever), and owned by
+ * C++. In that case, use a ModelHandler with no ownership ModelHandler(
+ * Model*,false).
+ * - build from C++ but owned by python. Python would be in charge of
+ * destroying it when it is done. In that case, use
+ * ModelHandler(Model*,true). Take care not to destroy the object by
+ * yourself, otherwise there might have access to deallocated memory and
+ * double destruction.
+ * - build and managed by shared_ptr. It is the best way, in the sense
+ * that the object is manage both by C++ and python. It will be released
+ * only when both are done with it. In that case, simply give Python the
+ * shared_ptr ModelHandler( shared_ptr<Model> ).
+ *
+ * It is not possible to construct a Model from python, because of Eigen
+ * memory alignement. The python visitor does not define any
+ * constructor. Instead, some static makers (function allocating memory and
+ * returning a pointer Model *) are implemented. When Python is making
+ * such a Model, it is stored in a shared_ptr and can be access directly
+ * from C++. By default, when passing a Model to python, it is kept as a
+ * raw pointer. Access is then done from python to the C++ memory without
+ * any guarantee. If you want to enforce memory access, prefer transmitting
+ * a shared_ptr to python.
+ */
+ template<typename CppObject>
+ struct Handler
+ {
+ typedef boost::shared_ptr<CppObject> SmartPtr_t;
+ typedef CppObject * Ptr_t;
+
+ SmartPtr_t smptr;
+ Ptr_t rawptr;
+ bool smart;
+
+ Handler(CppObject * cppobj,bool transmitOwnership=false)
+ : smptr( transmitOwnership ? cppobj : NULL )
+ , rawptr( cppobj )
+ , smart( transmitOwnership ) {}
+ Handler( SmartPtr_t cppobj )
+ : smptr(cppobj), rawptr(NULL), smart(true) {}
+ ~Handler()
+ {
+ std::cout << "Destroy cppobj handler " << std::endl;
+ if( (!smart) && (rawptr!=NULL) ) delete rawptr;
+ }
+
+ CppObject * ptr() { return smart ? smptr.get() : rawptr; }
+ const CppObject * ptr() const { return smart ? smptr.get() : rawptr; }
+ CppObject * operator->() { return ptr(); }
+ const CppObject * operator->() const { return ptr(); }
+
+ CppObject & get() { return smart ? *smptr : *rawptr; }
+ const CppObject & get() const { return smart ? *smptr : *rawptr; }
+ CppObject & operator*() { return get(); }
+ const CppObject& operator*() const { return get(); }
+ };
+
+
+ }} // namespace se3::python
+
+#endif // ifndef __se3_python_handler_hpp__
+
diff --git a/src/python/model.hpp b/src/python/model.hpp
index 6d8a689c9..8ba728078 100644
--- a/src/python/model.hpp
+++ b/src/python/model.hpp
@@ -1,136 +1,30 @@
#ifndef __se3_python_model_hpp__
#define __se3_python_model_hpp__
+#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <eigenpy/exception.hpp>
#include <eigenpy/eigenpy.hpp>
+
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/parser/sample-models.hpp"
+#include "pinocchio/python/eigen_container.hpp"
+#include "pinocchio/python/handler.hpp"
-#include <boost/shared_ptr.hpp>
-#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
-#include <boost/python/return_internal_reference.hpp>
namespace se3
{
namespace python
{
-
- /* There might be several way of building and owning a model object:
-
- * - build from C++ (as static or dynamic, wathever), and owned by
- * C++. In that case, use a ModelHandler with no ownership ModelHandler(
- * Model*,false).
- * - build from C++ but owned by python. Python would be in charge of
- * destroying it when it is done. In that case, use
- * ModelHandler(Model*,true). Take care not to destroy the object by
- * yourself, otherwise there might have access to deallocated memory and
- * double destruction.
- * - build and managed by shared_ptr. It is the best way, in the sense
- * that the object is manage both by C++ and python. It will be released
- * only when both are done with it. In that case, simply give Python the
- * shared_ptr ModelHandler( shared_ptr<Model> ).
- *
- * It is not possible to construct a Model from python, because of Eigen
- * memory alignement. The python visitor does not define any
- * constructor. Instead, some static makers (function allocating memory and
- * returning a pointer Model *) are implemented. When Python is making
- * such a Model, it is stored in a shared_ptr and can be access directly
- * from C++. By default, when passing a Model to python, it is kept as a
- * raw pointer. Access is then done from python to the C++ memory without
- * any guarantee. If you want to enforce memory access, prefer transmitting
- * a shared_ptr to python.
- */
- struct ModelHandler
- {
- typedef boost::shared_ptr<Model> SmartPtr_t;
- typedef Model * Ptr_t;
-
- SmartPtr_t smptr;
- Ptr_t rawptr;
- bool smart;
-
- ModelHandler(Model * model,bool transmitOwnership=false)
- : smptr( transmitOwnership ? model : NULL )
- , rawptr( model )
- , smart( transmitOwnership ) {}
- ModelHandler( SmartPtr_t model )
- : smptr(model), rawptr(NULL), smart(true) {}
- ~ModelHandler()
- {
- std::cout << "Destroy model handler " << std::endl;
- if( (!smart) && (rawptr!=NULL) ) delete rawptr;
- }
-
- Model * ptr() { return smart ? smptr.get() : rawptr; }
- const Model * ptr() const { return smart ? smptr.get() : rawptr; }
- Model * operator->() { return ptr(); }
- const Model * operator->() const { return ptr(); }
-
- Model & get() { return smart ? *smptr : *rawptr; }
- const Model & get() const { return smart ? *smptr : *rawptr; }
- Model & operator*() { return get(); }
- const Model& operator*() const { return get(); }
- };
-
-
namespace bp = boost::python;
- struct InertiasVisitor
- : public boost::python::def_visitor< InertiasVisitor >
- {
- typedef typename eigenpy::UnalignedEquivalent<Inertia>::type Inertia_fx;
- typedef std::vector<Inertia> Inertias;
-
- template<class PyClass>
- void visit(PyClass& cl) const
- {
- cl
- .def("__getitem__", &InertiasVisitor::getItem)
- .def("__setitem__", &InertiasVisitor::setItem)
- .def("__len__",&InertiasVisitor::length)
- ;
- }
-
- static Inertia getItem( const Inertias & Ys,int i) { return Ys[i]; }
- static void setItem( Inertias & Ys,int i,const Inertia_fx & Y)
- {
- std::cout << "Y = " << Y << std::endl;
- Ys[i] = Y;
- }
- static int length( const Inertias & Ys ) { return Ys.size(); }
-
- };
-
-
- struct ParentsVisitor
- : public boost::python::def_visitor< ParentsVisitor >
- {
- typedef Model::Index Index;
- typedef std::vector<Index> Parents;
-
- template<class PyClass>
- void visit(PyClass& cl) const
- {
- cl
- .def("__getitem__", &ParentsVisitor::getItem)
- .def("__setitem__", &ParentsVisitor::setItem)
- .def("__len__",&ParentsVisitor::length)
- ;
- }
-
- static Index getItem( const Parents & Ys,int i) { return Ys[i]; }
- static void setItem( Parents & Ys,int i,const Index & Y)
- {
- std::cout << "p = " << Y << std::endl;
- Ys[i] = Y;
- }
- static int length( const Parents & Ys ) { return Ys.size(); }
- };
-
+ typedef Handler<Model> ModelHandler;
struct ModelPythonVisitor
: public boost::python::def_visitor< ModelPythonVisitor >
{
+ public:
+ typedef Model::Index Index;
+ typedef typename eigenpy::UnalignedEquivalent<Motion>::type Motion_fx;
public:
@@ -146,7 +40,7 @@ namespace se3
}
/* --- Exposing C++ API to python through the handler ----------------- */
- template<class PyClass>
+ template<class PyClass>
void visit(PyClass& cl) const
{
cl
@@ -155,13 +49,22 @@ namespace se3
.def("__str__",&ModelPythonVisitor::toString)
+ .add_property("nq", &ModelPythonVisitor::nq)
+ .add_property("nv", &ModelPythonVisitor::nv)
+ .add_property("nbody", &ModelPythonVisitor::nbody)
.add_property("inertias",
bp::make_function(&ModelPythonVisitor::inertias,
bp::return_internal_reference<>()) )
-
+ .add_property("jointPlacements",
+ bp::make_function(&ModelPythonVisitor::jointPlacements,
+ bp::return_internal_reference<>()) )
.add_property("parents",
bp::make_function(&ModelPythonVisitor::parents,
bp::return_internal_reference<>()) )
+ .add_property("names",
+ bp::make_function(&ModelPythonVisitor::names,
+ bp::return_internal_reference<>()) )
+ .add_property("gravity",&ModelPythonVisitor::gravity,&ModelPythonVisitor::setGravity)
.def("BuildEmptyModel",&ModelPythonVisitor::maker_empty)
.staticmethod("BuildEmptyModel")
@@ -175,13 +78,15 @@ namespace se3
static boost::shared_ptr<Data> createData(const ModelHandler& m )
{ return boost::shared_ptr<Data>( new Data(*m) ); }
- typedef std::vector<Inertia> Inertias_t;
- static Inertias_t & inertias( ModelHandler & m ) { return m->inertias; }
- static void set_inertias( ModelHandler & m,const Inertias_t & Ys )
- { m->inertias = Ys; }
- static ParentsVisitor::Parents & parents( ModelHandler & m ) { return m->parents; }
-
-
+ static int nq( ModelHandler & m ) { return m->nq; }
+ static int nv( ModelHandler & m ) { return m->nv; }
+ static int nbody( ModelHandler & m ) { return m->nbody; }
+ static std::vector<Inertia> & inertias( ModelHandler & m ) { return m->inertias; }
+ static std::vector<SE3> & jointPlacements( ModelHandler & m ) { return m->jointPlacements; }
+ static std::vector<Model::Index> & parents( ModelHandler & m ) { return m->parents; }
+ static std::vector<std::string> & names ( ModelHandler & m ) { return m->names; }
+ static Motion gravity( ModelHandler & m ) { return m->gravity; }
+ static void setGravity( ModelHandler & m,const Motion_fx & g ) { m->gravity = g; }
static ModelHandler maker_empty()
{
@@ -200,12 +105,10 @@ namespace se3
/* --- Expose --------------------------------------------------------- */
static void expose()
{
- bp::class_<InertiasVisitor::Inertias>("ListInertia")
- .def(InertiasVisitor());
-
- bp::class_<ParentsVisitor::Parents>("ListInertia")
- .def(ParentsVisitor());
- //.def(bp::vector_indexing_suite<Inertias_t>());
+ bp::class_< std::vector<Index> >("StdVec_Index")
+ .def(bp::vector_indexing_suite< std::vector<Index> >());
+ bp::class_< std::vector<std::string> >("StdVec_StdString")
+ .def(bp::vector_indexing_suite< std::vector<std::string> >());
bp::class_<ModelHandler>("Model",
"Articulated rigid body model (const)",
diff --git a/src/python/python.cpp b/src/python/python.cpp
index 58337655b..8545b6171 100644
--- a/src/python/python.cpp
+++ b/src/python/python.cpp
@@ -14,18 +14,22 @@ namespace se3
void exposeSE3()
{
SE3PythonVisitor<SE3>::expose();
+ PyWraperForAlignedStdVector<SE3>::expose("StdVect_SE3");
}
void exposeForce()
{
ForcePythonVisitor<Force>::expose();
+ PyWraperForAlignedStdVector<Force>::expose("StdVec_Force");
}
void exposeMotion()
{
MotionPythonVisitor<Motion>::expose();
+ PyWraperForAlignedStdVector<Motion>::expose("StdVec_Motion");
}
void exposeInertia()
{
InertiaPythonVisitor<Inertia>::expose();
+ PyWraperForAlignedStdVector<Inertia>::expose("StdVec_Inertia");
}
void exposeModel()
{
--
GitLab