First complete wrapping of the model.

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

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()

src/python/eigen_container.hpp

+#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__
+

src/python/handler.hpp

+#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__
+

src/python/model.hpp

 #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)",

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()
     {