Merge pull request #302 from stack-of-tasks/hpp-devel

Hpp devel

CMakeLists.txt

@@ -118,6 +118,7 @@ SET(${PROJECT_NAME}_MATH_HEADERS
   math/fwd.hpp
   math/sincos.hpp
   math/quaternion.hpp
+  math/matrix.hpp
   )
 
 SET(${PROJECT_NAME}_TOOLS_HEADERS
@@ -156,6 +157,7 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
   multibody/joint/joint.hpp
   multibody/joint/joint-basic-visitors.hpp
   multibody/joint/joint-basic-visitors.hxx
+  multibody/joint/joint-composite.hpp
   ) 
 
 SET(${PROJECT_NAME}_MULTIBODY_HEADERS

benchmark/timings-geometry.cpp

@@ -70,8 +70,9 @@ int main()
   std::string romeo_meshDir  = PINOCCHIO_SOURCE_DIR"/models/";
   package_dirs.push_back(romeo_meshDir);
 
-  se3::Model model = se3::urdf::buildModel(romeo_filename, se3::JointModelFreeFlyer());
-  se3::GeometryModel geom_model = se3::urdf::buildGeom(model, romeo_filename, package_dirs, COLLISION);
+  se3::Model model;
+  se3::urdf::buildModel(romeo_filename, se3::JointModelFreeFlyer(),model);
+  se3::GeometryModel geom_model; se3::urdf::buildGeom(model, romeo_filename, COLLISION, geom_model, package_dirs);
 #ifdef WITH_HPP_FCL  
   geom_model.addAllCollisionPairs();
 #endif // WITH_HPP_FCL
@@ -113,7 +114,7 @@ int main()
     updateGeometryPlacements(model,data,geom_model,geom_data,qs_romeo[_smooth]);
     for (std::vector<se3::CollisionPair>::iterator it = geom_model.collisionPairs.begin(); it != geom_model.collisionPairs.end(); ++it)
     {
-      geom_data.computeCollision(*it);
+      computeCollision(geom_model,geom_data,std::size_t(it-geom_model.collisionPairs.begin()));
     }
   }
   double collideTime = timer.toc(StackTicToc::US)/NBT - (update_col_time + geom_time);
@@ -124,7 +125,7 @@ int main()
   timer.tic();
   SMOOTH(NBT)
   {
-    computeCollisions(model,data,geom_model,geom_data,qs_romeo[_smooth], true);
+    computeCollisions(geom_model,geom_data, true);
   }
   double is_colliding_time = timer.toc(StackTicToc::US)/NBT - (update_col_time + geom_time);
   std::cout << "Collision Test : robot in collision? = \t" << is_colliding_time
@@ -203,7 +204,7 @@ hpp::model::HumanoidRobotPtr_t humanoidRobot =
   timer.tic();
   SMOOTH(NBT)
   {
-    computeCollisions(geom_data, true);
+    computeCollisions(geom_model, geom_data, true);
   }
   double is_romeo_colliding_time_pino = timer.toc(StackTicToc::US)/NBT;
   std::cout << "Pinocchio - Collision Test : robot in collision? (G) = \t" << is_romeo_colliding_time_pino
@@ -244,7 +245,7 @@ hpp::model::HumanoidRobotPtr_t humanoidRobot =
   timer.tic();
   SMOOTH(NBD)
   {
-    computeDistances(geom_data);
+    computeDistances(geom_model, geom_data);
   }
   computeDistancesTime = timer.toc(StackTicToc::US)/NBD ;
   std::cout << "Pinocchio - Compute distances (D) " << geom_model.collisionPairs.size() << " col pairs\t" << computeDistancesTime 

benchmark/timings.cpp

@@ -59,7 +59,7 @@ int main(int argc, const char ** argv)
   else if( filename == "H2" )
     se3::buildModels::humanoid2d(model);
   else
-    model = se3::urdf::buildModel(filename,JointModelFreeFlyer());
+    se3::urdf::buildModel(filename,JointModelFreeFlyer(),model);
   std::cout << "nq = " << model.nq << std::endl;
 
   se3::Data data(model);

bindings/python/algorithm/expose-geometry.cpp

@@ -36,11 +36,20 @@ namespace se3
       return updateGeometryPlacements(*model, *data, *geom_model, *geom_data, q);
     }
 
-#ifdef WITH_HPP_FCL    
-    static bool computeCollisions_proxy(GeometryDataHandler & data_geom,
+#ifdef WITH_HPP_FCL   
+
+    static bool computeCollision_proxy(const GeometryModelHandler & model_geom,
+                                       GeometryDataHandler & data_geom,
+                                       const PairIndex & pairId)
+    {
+      return computeCollision(*model_geom, *data_geom, pairId);
+    }
+
+    static bool computeCollisions_proxy(const GeometryModelHandler & model_geom,
+                                        GeometryDataHandler & data_geom,
                                         const bool stopAtFirstCollision)
     {
-      return computeCollisions(*data_geom, stopAtFirstCollision);
+      return computeCollisions(*model_geom, *data_geom, stopAtFirstCollision);
     }
     
     static bool computeGeometryAndCollisions_proxy(const ModelHandler & model,
@@ -53,20 +62,29 @@ namespace se3
       return computeCollisions(*model,*data,*model_geom, *data_geom, q, stopAtFirstCollision);
     }
     
-    static void computeDistances_proxy(GeometryDataHandler & data_geom)
+    static fcl::DistanceResult computeDistance_proxy(const GeometryModelHandler & model_geom,
+                                                     GeometryDataHandler & data_geom,
+                                                     const PairIndex & pairId)
+    {
+      return computeDistance(*model_geom, *data_geom, pairId);
+    }
+
+    static std::size_t computeDistances_proxy(const GeometryModelHandler & model_geom,
+                                              GeometryDataHandler & data_geom)
     {
-      computeDistances(*data_geom);
+      return computeDistances(*model_geom, *data_geom);
     }
     
-    static void computeGeometryAndDistances_proxy(const ModelHandler & model,
-                                                  DataHandler & data,
-                                                  const GeometryModelHandler & model_geom,
-                                                  GeometryDataHandler & data_geom,
-                                                  const Eigen::VectorXd & q
-                                                  )
+    static std::size_t computeGeometryAndDistances_proxy(const ModelHandler & model,
+                                                         DataHandler & data,
+                                                         const GeometryModelHandler & model_geom,
+                                                         GeometryDataHandler & data_geom,
+                                                         const Eigen::VectorXd & q
+                                                         )
     {
-      computeDistances(*model, *data, *model_geom, *data_geom, q);
+      return computeDistances<true>(*model, *data, *model_geom, *data_geom, q);
     }
+
 #endif // WITH_HPP_FCL
 
     void exposeGeometryAlgo()
@@ -78,6 +96,12 @@ namespace se3
               );
       
 #ifdef WITH_HPP_FCL       
+      bp::def("computeCollision", computeCollision_proxy,
+              bp::args("GoometryModel", "GeometryData", "pairIndex"),
+              "Check if the collision objects of a collision pair for a given Geometry Model and Data are in collision."
+             "The collision pair is given by the two index of the collision objects."
+              );
+
       bp::def("computeCollisions",computeCollisions_proxy,
               bp::args("GeometryData","bool"),
               "Determine if collision pairs are effectively in collision."
@@ -89,9 +113,14 @@ namespace se3
               "determine if all collision pairs are effectively in collision or not."
               );
       
+      bp::def("computeDistance",computeDistance_proxy,
+              bp::args("GeometryModel","GeometryData", "pairIndex"),
+              "Compute the distance between the two geometry objects of a given collision pair for a GeometryModel and associated GeometryData."
+              );
+
       bp::def("computeDistances",computeDistances_proxy,
-              bp::args("GeometryData"),
-              "Compute the distance between each collision pair."
+              bp::args("GeometryModel","GeometryData"),
+              "Compute the distance between each collision pair for a given GeometryModel and associated GeometryData."
               );
       
       bp::def("computeGeometryAndDistances",computeGeometryAndDistances_proxy,

bindings/python/algorithm/expose-joints.cpp

@@ -57,6 +57,21 @@ namespace se3
     {
       return randomConfiguration(*model, lowerPosLimit, upperPosLimit);
     }
+
+    static void normalize_proxy(const ModelHandler & model,
+                                VectorXd_fx & config)
+    {
+      Eigen::VectorXd q(config);
+      normalize(*model, q);
+      config = q;
+    }
+
+    static bool isSameConfiguration_proxy(const ModelHandler & model,
+                                          const VectorXd_fx & q1,
+                                          const VectorXd_fx & q2)
+    {
+      return isSameConfiguration(*model, q1, q2);
+    }
     
     void exposeJointsAlgo()
     {
@@ -88,6 +103,15 @@ namespace se3
                        "Joint lower limits (size Model::nq)",
                        "Joint upper limits (size Model::nq)"),
               "Generate a random configuration ensuring provied joint limits are respected ");
+      bp::def("normalize",normalize_proxy,
+              bp::args("Model",
+                       "Configuration q (size Model::nq)"),
+              "return the configuration normalized ");
+      bp::def("isSameConfiguration",isSameConfiguration_proxy,
+              bp::args("Model",
+                       "Configuration q1 (size Model::nq)",
+                       "Configuration q2 (size Model::nq)"),
+              "Return true if two configurations are equivalent");
     }
     
   } // namespace python

bindings/python/force.hpp

@@ -85,6 +85,7 @@ namespace se3
         
         .def("__add__",&ForcePythonVisitor::add)
         .def("__sub__",&ForcePythonVisitor::subst)
+        .def("__neg__",&ForcePythonVisitor::neg)
         .def(bp::self_ns::str(bp::self_ns::self))
         .def(bp::self_ns::repr(bp::self_ns::self))
         
@@ -108,6 +109,7 @@ namespace se3
       
       static Force_fx add(const Force_fx & f1, const Force_fx & f2) { return f1+f2; }
       static Force_fx subst(const Force_fx & f1, const Force_fx & f2) { return f1-f2; }
+      static Force_fx neg(const Force_fx & f1) { return -f1; }
 
       static void expose()
       {

bindings/python/frame.hpp

@@ -52,16 +52,20 @@ namespace se3
       void visit(PyClass& cl) const 
       {
         cl
-          .def(bp::init< const std::string&,const JointIndex, const SE3_fx&,FrameType> ((bp::arg("name (string)"),bp::arg("parent (index)"), bp::arg("SE3 placement"), bp::arg("type (FrameType)")),
-                "Initialize from name, parent id and placement wrt parent joint."))
+          .def(bp::init< const std::string&,const JointIndex, const FrameIndex, const SE3_fx&,FrameType> ((bp::arg("name (string)"),bp::arg("index of parent joint"), bp::args("index of parent frame"), bp::arg("SE3 placement"), bp::arg("type (FrameType)")),
+                "Initialize from name, parent joint id, parent frame id and placement wrt parent joint."))
 
           .def_readwrite("name", &Frame::name, "name  of the frame")
           .def_readwrite("parent", &Frame::parent, "id of the parent joint")
+          .def_readwrite("previousFrame", &Frame::previousFrame, "id of the previous frame") 
           .add_property("placement", 
                         &FramePythonVisitor::getPlacementWrtParentJoint, 
                         &FramePythonVisitor::setPlacementWrtParentJoint, 
                         "placement in the parent joint local frame")
           .def_readwrite("type", &Frame::type, "type of the frame")
+
+          .def(bp::self_ns::str(bp::self_ns::self))
+          .def(bp::self_ns::repr(bp::self_ns::self))
           ;
       }
 

bindings/python/geometry-data.hpp

@@ -28,6 +28,20 @@
 #include "pinocchio/bindings/python/data.hpp"
 #include "pinocchio/bindings/python/geometry-model.hpp"
 
+namespace fcl
+{
+  // This operator is defined here temporary, as it is needed by vector_indexing_suite
+  // It has also been defined in hpp-fcl in a pending pull request.
+  // Once it has been integrated in releases of hpp-fcl, please remove this operator
+  inline bool operator ==(const DistanceResult & dr1, const DistanceResult& dr2)
+  {
+    return dr1.min_distance == dr2.min_distance
+        && dr1.o1 == dr2.o1
+        && dr1.o2 == dr2.o2
+        && dr1.nearest_points[0] == dr2.nearest_points[0]
+        && dr1.nearest_points[1] == dr2.nearest_points[1];
+  }
+}
 namespace se3
 {
   namespace python
@@ -96,18 +110,33 @@ namespace se3
                       bp::make_function(&GeometryDataPythonVisitor::oMg,
                                         bp::return_internal_reference<>()),
                       "Vector of collision objects placement relative to the world.")
-#ifdef WITH_HPP_FCL        
-        .add_property("distance_results",
-                      bp::make_function(&GeometryDataPythonVisitor::distance_results,
+#ifdef WITH_HPP_FCL
+        .add_property("activeCollisionPairs",
+                      bp::make_function(&GeometryDataPythonVisitor::activeCollisionPairs,
+                                        bp::return_internal_reference<>()))
+        .add_property("distanceRequest",
+                      bp::make_function(&GeometryDataPythonVisitor::distanceRequest,
+                                        bp::return_internal_reference<>()),
+                      "Defines what information should be computed by fcl for distances")
+        .add_property("distanceResults",
+                      bp::make_function(&GeometryDataPythonVisitor::distanceResults,
                                         bp::return_internal_reference<>()),
                       "Vector of distance results computed in ")
+        .add_property("CollisionRequest",
+                      bp::make_function(&GeometryDataPythonVisitor::CollisionRequest,
+                                        bp::return_internal_reference<>()),
+                      "Defines what information should be computed by fcl for collision tests")
         .add_property("collision_results",
                       bp::make_function(&GeometryDataPythonVisitor::collision_results,
                                         bp::return_internal_reference<>())  )
-        .add_property("activeCollisionPairs",
-                      bp::make_function(&GeometryDataPythonVisitor::activeCollisionPairs,
-                                        bp::return_internal_reference<>()))
+        .add_property("radius",
+                      bp::make_function(&GeometryDataPythonVisitor::radius,
+                                        bp::return_internal_reference<>()),
+                      "Vector of radius of bodies, ie distance of the further point of the geometry object from the joint center ")
         
+        .def("fillInnerOuterObjectMaps", &GeometryDataPythonVisitor::fillInnerOuterObjectMaps,
+             bp::args("GeometryModel"),
+             "Fill inner and outer objects maps")
         .def("activateCollisionPair",&GeometryDataPythonVisitor::activateCollisionPair,
              bp::args("pairIndex (int)"),
              "Activate pair ID <pairIndex> in geomModel.collisionPairs."
@@ -116,23 +145,6 @@ namespace se3
              bp::args("pairIndex (int)"),
              "Deactivate pair ID <pairIndex> in geomModel.collisionPairs.")
 
-        .def("computeCollision",&GeometryDataPythonVisitor::computeCollision,
-             bp::args("co1 (index)","co2 (index)"),
-             "Check if the two collision objects of a collision pair are in collision."
-             "The collision pair is given by the two index of the collision objects.")
-        .def("computeAllCollisions",&GeometryDataPythonVisitor::computeAllCollisions,
-             "Same as computeCollision. It applies computeCollision to all collision pairs contained collision_pairs."
-             "The results are stored in collision_results.")
-        .def("isColliding",&GeometryDataPythonVisitor::isColliding,
-             "Check if at least one of the collision pairs is in collision.")
-        
-        .def("computeDistance",&GeometryDataPythonVisitor::computeDistance,
-             bp::args("co1 (index)","co2 (index)"),
-             "Compute the distance result between two collision objects of a collision pair."
-             "The collision pair is given by the two index of the collision objects.")
-        .def("computeAllDistances",&GeometryDataPythonVisitor::computeAllDistances,
-             "Same as computeDistance. It applies computeDistance to all collision pairs contained collision_pairs."
-             "The results are stored in collision_distances.")
 #endif // WITH_HPP_FCL        
         .def("__str__",&GeometryDataPythonVisitor::toString)
         ;
@@ -145,25 +157,21 @@ namespace se3
 
       static std::vector<SE3> & oMg(GeometryDataHandler & m) { return m->oMg; }
 #ifdef WITH_HPP_FCL      
-      static std::vector<DistanceResult> & distance_results( GeometryDataHandler & m ) { return m->distance_results; }
-      static std::vector<CollisionResult> & collision_results( GeometryDataHandler & m ) { return m->collision_results; }
       static std::vector<bool> & activeCollisionPairs(GeometryDataHandler & m) { return m->activeCollisionPairs; }
-      static CollisionResult computeCollision(const GeometryDataHandler & m, const GeomIndex co1, const GeomIndex co2)
-      {
-        return m->computeCollision(CollisionPair(co1, co2));
-      }
-      static bool isColliding(const GeometryDataHandler & m) { return m->isColliding(); }
-      static void computeAllCollisions(GeometryDataHandler & m) { m->computeAllCollisions(); }
+      static fcl::DistanceRequest & distanceRequest( GeometryDataHandler & m ) { return m->distanceRequest; }
+      static std::vector<fcl::DistanceResult> & distanceResults( GeometryDataHandler & m ) { return m->distanceResults; }
+      static fcl::CollisionRequest & CollisionRequest( GeometryDataHandler & m ) { return m->collisionRequest; } 
+      static std::vector<fcl::CollisionResult> & collision_results( GeometryDataHandler & m ) { return m->collisionResults; }
+      static std::vector<double> & radius( GeometryDataHandler & m ) { return m->radius; }
+
+      static void fillInnerOuterObjectMaps(GeometryDataHandler & m, const GeometryModelHandler & model)
+      {m->fillInnerOuterObjectMaps(*model);}
+
       static void activateCollisionPair(GeometryDataHandler & m,
                                         Index pairID) { m->activateCollisionPair(pairID); } 
       static void deactivateCollisionPair(GeometryDataHandler & m,
                                           Index pairID) { m->deactivateCollisionPair(pairID); } 
       
-      static DistanceResult computeDistance(const GeometryDataHandler & m, const GeomIndex co1, const GeomIndex co2)
-      {
-        return m->computeDistance(CollisionPair(co1, co2));
-      }
-      static void computeAllDistances(GeometryDataHandler & m) { m->computeAllDistances(); }
 #endif // WITH_HPP_FCL      
       
       static std::string toString(const GeometryDataHandler& m)
@@ -173,11 +181,11 @@ namespace se3
       static void expose()
       {
 #ifdef WITH_HPP_FCL        
-        bp::class_< std::vector<DistanceResult> >("StdVec_DistanceResult")
-        .def(bp::vector_indexing_suite< std::vector<DistanceResult> >());
+        bp::class_< std::vector<fcl::DistanceResult> >("StdVec_DistanceResult")
+        .def(bp::vector_indexing_suite< std::vector<fcl::DistanceResult> >());
   
-        bp::class_< std::vector<CollisionResult> >("StdVec_CollisionResult")
-        .def(bp::vector_indexing_suite< std::vector<CollisionResult> >());
+        bp::class_< std::vector<fcl::CollisionResult> >("StdVec_CollisionResult")
+        .def(bp::vector_indexing_suite< std::vector<fcl::CollisionResult> >());
 #endif // WITH_HPP_FCL
         bp::class_<GeometryDataHandler>("GeometryData",
                                         "Geometry data linked to a geometry model and data struct.",

bindings/python/geometry-model.hpp

@@ -28,6 +28,8 @@
 
 #include "pinocchio/multibody/geometry.hpp"
 
+#include "pinocchio/bindings/python/model.hpp"
+
 namespace se3
 {
   namespace python
@@ -62,17 +64,19 @@ namespace se3
       {
 	cl
           .add_property("ngeoms", &GeometryModelPythonVisitor::ngeoms)
-
-          .def("getGeometryId",&GeometryModelPythonVisitor::getGeometryId)
-          .def("existGeometryName",&GeometryModelPythonVisitor::existGeometryName)
-          .def("getGeometryName",&GeometryModelPythonVisitor::getGeometryName)
           .add_property("geometryObjects",
                         bp::make_function(&GeometryModelPythonVisitor::geometryObjects,
                                           bp::return_internal_reference<>())  )
+
+          .def("addGeometryObject", &GeometryModelPythonVisitor::addGeometryObject,
+               bp::args("GeometryObject", "Model", "bool"),
+               "Add a GeometryObject to a GeometryModel")
+          .def("getGeometryId",&GeometryModelPythonVisitor::getGeometryId)
+          .def("existGeometryName",&GeometryModelPythonVisitor::existGeometryName)
           .def("__str__",&GeometryModelPythonVisitor::toString)
 #ifdef WITH_HPP_FCL
-          .add_property("collision_pairs",
-                        bp::make_function(&GeometryModelPythonVisitor::collision_pairs,
+          .add_property("collisionPairs",
+                        bp::make_function(&GeometryModelPythonVisitor::collisionPairs,
                                           bp::return_internal_reference<>()),
                         "Vector of collision pairs.")
           .def("addCollisionPair",&GeometryModelPythonVisitor::addCollisionPair,
@@ -106,12 +110,14 @@ namespace se3
       { return  gmodelPtr->getGeometryId(name); }
       static bool existGeometryName(const GeometryModelHandler & gmodelPtr, const std::string & name)
       { return gmodelPtr->existGeometryName(name);}
-      static std::string getGeometryName(const GeometryModelHandler & gmodelPtr, const GeomIndex index)
-      { return gmodelPtr->getGeometryName(index);}
+
       
       static std::vector<GeometryObject> & geometryObjects( GeometryModelHandler & m ) { return m->geometryObjects; }
+      static GeomIndex addGeometryObject( GeometryModelHandler & m, GeometryObject gobject, const ModelHandler & model, const bool autofillJointParent)
+      { return m-> addGeometryObject(gobject, *model, autofillJointParent); }
+
 #ifdef WITH_HPP_FCL      
-      static std::vector<CollisionPair> & collision_pairs( GeometryModelHandler & m ) 
+      static std::vector<CollisionPair> & collisionPairs( GeometryModelHandler & m ) 
       { return m->collisionPairs; }
       
       static void addCollisionPair (GeometryModelHandler & m, const GeomIndex co1, const GeomIndex co2)
@@ -146,7 +152,6 @@ namespace se3
         bp::enum_<GeometryType>("GeometryType")
         .value("VISUAL",VISUAL)
         .value("COLLISION",COLLISION)
-        .value("NONE",NONE)
         ;
         
         bp::class_<GeometryModelHandler>("GeometryModel",

bindings/python/geometry-object.hpp

@@ -53,11 +53,12 @@ namespace se3
       {
         cl
           .def_readwrite("name", &GeometryObject::name, "Name of the GeometryObject")
-          .def_readwrite("parent", &GeometryObject::parent, "Index of the parent joint")
+          .def_readwrite("parentJoint", &GeometryObject::parentJoint, "Index of the parent joint")
+          .def_readwrite("parentFrame", &GeometryObject::parentFrame, "Index of the parent frame")
           .add_property("placement", &GeometryObjectPythonVisitor::getPlacementWrtParentJoint,
                                       &GeometryObjectPythonVisitor::setPlacementWrtParentJoint,
                                       "Position of geometry object in parent joint's frame")
-          .def_readonly("mesh_path", &GeometryObject::mesh_path, "Absolute path to the mesh file")
+          .def_readonly("meshPath", &GeometryObject::meshPath, "Absolute path to the mesh file")
           ;
       }
 
@@ -67,7 +68,7 @@ namespace se3
       static void expose()
       {
         bp::class_<GeometryObject>("GeometryObject",
-                           "A wrapper on a collision geometry including its parent joint, placement in parent frame.\n\n",
+                           "A wrapper on a collision geometry including its parent joint, parent frame, placement in parent joint's frame.\n\n",
 	                         bp::no_init
                          )
 	                       .def(GeometryObjectPythonVisitor())

bindings/python/joint.hpp

@@ -20,7 +20,7 @@
 
 #include <eigenpy/exception.hpp>
 #include <eigenpy/eigenpy.hpp>
-#include "pinocchio/multibody/joint/joint.hpp"
+#include "pinocchio/multibody/joint/joint-composite.hpp"
 
 namespace se3
 {

bindings/python/model.hpp

@@ -94,16 +94,15 @@ namespace se3
       void visit(PyClass& cl) const 
       {
         cl
-          .def("getBodyId",&ModelPythonVisitor::getBodyId)
-          .def("getJointId",&ModelPythonVisitor::getJointId)
-          .def("createData",&ModelPythonVisitor::createData)
+          // Class Members
 
           .def("__str__",&ModelPythonVisitor::toString)
 
           .add_property("nq", &ModelPythonVisitor::nq)
           .add_property("nv", &ModelPythonVisitor::nv)
-          .add_property("njoint", &ModelPythonVisitor::njoint)
-          .add_property("nbody", &ModelPythonVisitor::nbody)
+          .add_property("njoints", &ModelPythonVisitor::njoints)
+          .add_property("nbodies", &ModelPythonVisitor::nbodies)
+          .add_property("nframes", &ModelPythonVisitor::nframes)
           .add_property("inertias",
             bp::make_function(&ModelPythonVisitor::inertias,
                   bp::return_internal_reference<>())  )
@@ -116,32 +115,41 @@ namespace se3
           .add_property("parents", 
             bp::make_function(&ModelPythonVisitor::parents,
                   bp::return_internal_reference<>())  )
-        .add_property("subtrees",
-                      bp::make_function(&ModelPythonVisitor::subtrees,
-                                        bp::return_internal_reference<>()), "Vector of subtrees. subtree[j] corresponds to the subtree supported by the joint j.")
           .add_property("names",
             bp::make_function(&ModelPythonVisitor::names,
                   bp::return_internal_reference<>())  )
-        
-        .def("addJoint",&ModelPythonVisitor::addJoint,bp::args("parent_id","joint_model","joint_placement","joint_name"),"Adds a joint to the kinematic tree. The joint is defined by its placement relative to its parent joint and its name.")
-        .def("appendBodyToJoint",&ModelPythonVisitor::appendBodyToJoint,bp::args("joint_id","body_inertia","body_placement","body_name"),"Appends a body to the joint given by its index. The body is defined by its inertia, its relative placement regarding to the joint and its name.")
-
-
-          .add_property("effortLimit", bp::make_function(&ModelPythonVisitor::effortLimit), "Joint max effort")
           .add_property("neutralConfiguration", bp::make_function(&ModelPythonVisitor::neutralConfiguration), "Joint's neutral configurations")
+          .add_property("effortLimit", bp::make_function(&ModelPythonVisitor::effortLimit), "Joint max effort")
           .add_property("velocityLimit", bp::make_function(&ModelPythonVisitor::velocityLimit), "Joint max velocity")
           .add_property("lowerPositionLimit", bp::make_function(&ModelPythonVisitor::lowerPositionLimit), "Limit for joint lower position")
           .add_property("upperPositionLimit", bp::make_function(&ModelPythonVisitor::upperPositionLimit), "Limit for joint upper position")
+        
+          .add_property("frames", bp::make_function(&ModelPythonVisitor::frames, bp::return_internal_reference<>()),"Vector of frames contained in the model.")
 
-          .def("getFrameParent", &ModelPythonVisitor::getFrameParent)
-          .def("getFramePlacement", &ModelPythonVisitor::getFramePlacement)
-        .def("addFrame",(bool (*)(ModelHandler&,const std::string &,const JointIndex, const SE3_fx &,const FrameType &)) &ModelPythonVisitor::addFrame,bp::args("name","parent_id","placement","type"),"Add a frame to the vector of frames. See also Frame for more details. Returns False if the frame already exists.")
-        .def("addFrame",(bool (*)(ModelHandler&,const Frame &)) &ModelPythonVisitor::addFrame,bp::args("frame"),"Add a frame to the vector of frames.")
-        .add_property("frames", bp::make_function(&ModelPythonVisitor::frames, bp::return_internal_reference<>()),"Vector of frames contained in the model.")
-        .def("existFrame",&ModelPythonVisitor::existFrame,bp::args("name"),"Returns true if the frame given by its name exists inside the Model.")
-        .def("getFrameId",&ModelPythonVisitor::getFrameId,bp::args("name"),"Returns the index of the frame given by its name. If the frame is not in the frames vector, it returns the current size of the frames vector.")
-
+          .add_property("subtrees",
+                      bp::make_function(&ModelPythonVisitor::subtrees,
+                                        bp::return_internal_reference<>()), "Vector of subtrees. subtree[j] corresponds to the subtree supported by the joint j.")
+        
           .add_property("gravity",&ModelPythonVisitor::gravity,&ModelPythonVisitor::setGravity)
+
+          // Class Methods
+          .def("addJoint",&ModelPythonVisitor::addJoint,bp::args("parent_id","joint_model","joint_placement","joint_name"),"Adds a joint to the kinematic tree. The joint is defined by its placement relative to its parent joint and its name.")
+          // ADD addJoint with limits ? See boost::python overloading/default parameters
+          .def("addJointFrame", &ModelPythonVisitor::addJointFrame, bp::args("jointIndex", "frameIndex"), "add the joint at index jointIndex as a frame to the frame tree")
+          .def("appendBodyToJoint",&ModelPythonVisitor::appendBodyToJoint,bp::args("joint_id","body_inertia","body_placement"),"Appends a body to the joint given by its index. The body is defined by its inertia, its relative placement regarding to the joint and its name.")
+
+          .def("addBodyFrame", &ModelPythonVisitor::addBodyFrame, bp::args("body_name", "parentJoint", "body_plaement", "previous_frame(parent frame)"), "add a body to the frame tree")
+          .def("getBodyId",&ModelPythonVisitor::getBodyId, bp::args("name"), "Return the index of a frame of type BODY given by its name")
+          .def("existBodyName", &ModelPythonVisitor::existBodyName, bp::args("name"), "Check if a frame of type BODY exists, given its name")
+          .def("getJointId",&ModelPythonVisitor::getJointId, bp::args("name"), "Return the index of a joint given by its name")
+          .def("existJointName", &ModelPythonVisitor::existJointName, bp::args("name"), "Check if a joint given by its name exists")
+          .def("getFrameId",&ModelPythonVisitor::getFrameId,bp::args("name"),"Returns the index of the frame given by its name. If the frame is not in the frames vector, it returns the current size of the frames vector.")
+          .def("existFrame",&ModelPythonVisitor::existFrame,bp::args("name"),"Returns true if the frame given by its name exists inside the Model.")
+
+          .def("addFrame",(bool (*)(ModelHandler&,const std::string &,const JointIndex, const FrameIndex, const SE3_fx &,const FrameType &)) &ModelPythonVisitor::addFrame,bp::args("name","parent_id","placement","type"),"Add a frame to the vector of frames. See also Frame for more details. Returns False if the frame already exists.")
+          .def("addFrame",(bool (*)(ModelHandler&,const Frame &)) &ModelPythonVisitor::addFrame,bp::args("frame"),"Add a frame to the vector of frames.")
+
+          .def("createData",&ModelPythonVisitor::createData)
           .def("BuildEmptyModel",&ModelPythonVisitor::maker_empty)
           .staticmethod("BuildEmptyModel")
           .def("BuildHumanoidSimple",&ModelPythonVisitor::maker_humanoidSimple)
@@ -149,24 +157,28 @@ namespace se3
           ;
       }
 
-      static Model::Index getBodyId( const ModelHandler & modelPtr, const std::string & name )
-      { return  modelPtr->getBodyId(name); }
-      static Model::Index getJointId( const ModelHandler & modelPtr, const std::string & name )
-      { return  modelPtr->getJointId(name); }
-      static boost::shared_ptr<Data> createData(const ModelHandler& m )
-      { return boost::shared_ptr<Data>( new Data(*m) );      } 
       
       static int nq( ModelHandler & m ) { return m->nq; }
       static int nv( ModelHandler & m ) { return m->nv; }
-      static int njoint( ModelHandler & m ) { return m->njoint; }
-      static int nbody( ModelHandler & m ) { return m->nbody; }
+      static int njoints( ModelHandler & m ) { return m->njoints; }
+      static int nbodies( ModelHandler & m ) { return m->nbodies; }
+      static int nframes( ModelHandler & m ) { return m->nframes; }
       static std::vector<Inertia> & inertias( ModelHandler & m ) { return m->inertias; }
       static std::vector<SE3> & jointPlacements( ModelHandler & m ) { return m->jointPlacements; }
       static JointModelVector & joints( ModelHandler & m ) { return m->joints; }
       static std::vector<Model::JointIndex> & parents( ModelHandler & m ) { return m->parents; }
       static std::vector<std::string> & names ( ModelHandler & m ) { return m->names; }
+      static Eigen::VectorXd neutralConfiguration(ModelHandler & m) {return m->neutralConfiguration;}
+      static Eigen::VectorXd effortLimit(ModelHandler & m) {return m->effortLimit;}
+      static Eigen::VectorXd velocityLimit(ModelHandler & m) {return m->velocityLimit;}