[C++][Python] python model parser

As it is done in src/multibody/parser/python.cpp, one can create a shared_ptr to a new Model in c++,
send this into a python namespace, update it in python, and continue to work on the Model in c++.

An example to use this as a simple parser is given in unittest/python.cpp and models/simple_model.py

(squashed version)

CMakeLists.txt

@@ -219,6 +219,10 @@ IF(BUILD_PYTHON_INTERFACE)
     python/parsers.hpp
     python/explog.hpp
     )
+
+  LIST(APPEND ${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
+    multibody/parser/python.hpp
+    )
 ENDIF(BUILD_PYTHON_INTERFACE)
 
 IF(HPP_FCL_FOUND)

models/simple_model.py

+from math import pi
+from pinocchio.utils import np, rotate, XYZQUATToViewerConfiguration, se3ToXYZQUAT
+
+DENSITY = 1
+
+
+def placement(x=0, y=0, z=0, rx=0, ry=0, rz=0):
+    m = se3.SE3.Identity()
+    m.translation = np.matrix([[float(i)] for i in [x, y, z]])
+    m.rotation *= rotate('x', rx)
+    m.rotation *= rotate('y', ry)
+    m.rotation *= rotate('z', rz)
+    return m
+
+
+def color(body_number=1):
+    return [int(i) for i in '%03d' % int(bin(body_number % 8)[2:])] + [1]
+
+
+class ModelWrapper(object):
+    def __init__(self, model, name=None, display=False):
+        self.visuals = [('universe', se3.SE3.Identity(), se3.SE3.Identity().translation)]
+        self.name = self.__class__.__name__ if name is None else name
+        self.model = model
+        self.display = display
+        self.add_joints()
+
+    def add_joints(self):
+        for joint in self.joints:
+            self.add_joint(**joint)
+
+    def add_joint(self, joint_name, joint_model=None, joint_placement=None, lever=None, shape="box",
+                  dimensions=1, mass=None, body_color=1, parent=0):
+        if joint_model is None:
+            joint_model = se3.JointModelFreeFlyer()
+        elif isinstance(joint_model, str):
+            joint_model = se3.__dict__['JointModel' + joint_model]()
+        if joint_placement is None:
+            joint_placement = se3.SE3.Identity()
+        elif isinstance(joint_placement, dict):
+            joint_placement = placement(**joint_placement)
+        if lever is None:
+            lever = se3.SE3.Identity()
+        elif isinstance(lever, dict):
+            lever = placement(**lever)
+        joint_name, body_name = ('world/%s_%s_%s' % (self.name, joint_name, i) for i in ('joint', 'body'))
+        body_inertia = se3.Inertia.Random()
+        if shape == "box":
+            w, h, d = (float(i) for i in dimensions) if isinstance(dimensions, tuple) else [float(dimensions)] * 3
+            if mass is None:
+                mass = w * h * d * DENSITY
+            body_inertia = se3.Inertia.FromBox(mass, w, h, d)
+            if self.display:
+                self.display.viewer.gui.addBox(body_name, w, h, d, color(body_color))
+        elif shape == "cylinder":
+            r, h = dimensions
+            if mass is None:
+                mass = pi * r ** 2 * h * DENSITY
+            body_inertia = se3.Inertia.FromCylinder(mass, r, h)
+            if self.display:
+                self.display.viewer.gui.addCylinder(body_name, r, h, color(body_color))
+        elif shape == "sphere":
+            w, h, d = (float(i) for i in dimensions) if isinstance(dimensions, tuple) else [float(dimensions)] * 3
+            if mass is None:
+                mass = 4. / 3. * pi * w * h * d * DENSITY
+            body_inertia = se3.Inertia.FromEllipsoid(mass, w, h, d)
+            if self.display:
+                self.display.viewer.gui.addSphere(body_name, dimensions, color(body_color))
+        body_inertia.lever = lever.translation
+        self.model.addJointAndBody(parent, joint_model, joint_placement, body_inertia, joint_name, body_name)
+        self.visuals.append((body_name, joint_placement, lever))
+        self.data = self.model.createData()
+        if self.display:
+            self.place()
+
+    def place(self):
+        for i, (name, placement, lever) in enumerate(self.visuals):
+            if i == 0:
+                continue
+            self.display.place(name, self.data.oMi[i] * placement * lever)
+        self.display.viewer.gui.refresh()
+
+
+class RobotDisplay(object):
+    def __init__(self, window_name="pinocchio"):
+        from gepetto import corbaserver
+
+        self.viewer = corbaserver.Client()
+        try:
+            window_id = self.viewer.gui.getWindowID(window_name)
+        except:
+            window_id = self.viewer.gui.createWindow(window_name)
+            self.viewer.gui.createSceneWithFloor("world")
+            self.viewer.gui.addSceneToWindow("world", window_id)
+        self.viewer.gui.setLightingMode('world', 'OFF')
+        self.viewer.gui.setVisibility('world/floor', "OFF")
+        self.viewer.gui.refresh()
+
+    def place(self, obj_name, m):
+        self.viewer.gui.applyConfiguration(obj_name, XYZQUATToViewerConfiguration(se3ToXYZQUAT(m)))
+
+
+class SimplestWalker(ModelWrapper):
+    joints = [
+        {'joint_name': "pelvis",
+         'dimensions': (.1, .2, .1),
+         'mass': .5,
+         },
+        {'joint_name': "left_leg",
+         'joint_model': "RY",
+         'joint_placement': {'y': -.15},
+         'lever': {'z': -.45},
+         'shape': "cylinder",
+         'dimensions': (.1, .9),
+         'mass': 20,
+         'body_color': 2,
+         'parent': 1,
+         },
+        {'joint_name': "right_leg",
+         'joint_model': "RY",
+         'joint_placement': {'y': .15},
+         'lever': {'z': -.45},
+         'shape': "cylinder",
+         'dimensions': (.1, .9),
+         'mass': 20,
+         'body_color': 3,
+         'parent': 1,
+         },
+    ]
+
+SimplestWalker(model)

src/CMakeLists.txt

@@ -61,35 +61,6 @@ IF(LUA5_1_FOUND)
 ENDIF(LUA5_1_FOUND)
 
 
-IF (UNIX)
-    # Create target libpinocchio.so
-    ADD_LIBRARY ( ${PROJECT_NAME} SHARED ${HEADERS} ${${PROJECT_NAME}_SOURCES} )
-		SET_TARGET_PROPERTIES( ${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX)
-    PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} eigen3)
-    TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${Boost_LIBRARIES})
-
-    IF(URDFDOM_FOUND)
-      PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} urdfdom)
-      IF(HPP_FCL_FOUND)
-        PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} assimp)
-      ENDIF(HPP_FCL_FOUND)
-    ENDIF(URDFDOM_FOUND)
-
-    IF(HPP_FCL_FOUND)
-      PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} hpp-fcl)
-      ADD_TARGET_CFLAGS (${PROJECT_NAME} "-DWITH_HPP_FCL")
-    ENDIF(HPP_FCL_FOUND)
-
-    IF(LUA5_1_FOUND)
-      PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} lua5.1)
-    ENDIF(LUA5_1_FOUND)
-    
-    ADD_HEADER_GROUP(HEADERS)  
-    ADD_SOURCE_GROUP(${PROJECT_NAME}_SOURCES)  
-
-    INSTALL(TARGETS ${PROJECT_NAME} DESTINATION lib)
-ENDIF ( )
-
 # ----------------------------------------------------
 # --- PYTHON -----------------------------------------
 # ----------------------------------------------------

src/multibody/geometry.hpp

@@ -158,7 +158,7 @@ namespace se3
   }; // struct CollisionResult
 
 /// \brief Return true if the intrinsic geometry of the two CollisionObject is the same
-bool operator == (const fcl::CollisionObject & lhs, const fcl::CollisionObject & rhs)
+inline bool operator == (const fcl::CollisionObject & lhs, const fcl::CollisionObject & rhs)
 {
   return lhs.collisionGeometry() == rhs.collisionGeometry()
           && lhs.getAABB().min_ == rhs.getAABB().min_
@@ -296,7 +296,7 @@ struct GeometryObject
      *
      * @return     The index of the new added GeometryObject in collision_objects
      */
-    GeomIndex addCollisionObject(const JointIndex parent, const fcl::CollisionObject & co,
+    inline GeomIndex addCollisionObject(const JointIndex parent, const fcl::CollisionObject & co,
                                  const SE3 & placement, const std::string & geom_name = "",
                                  const std::string & mesh_path = "");
     
@@ -311,7 +311,7 @@ struct GeometryObject
      *
      * @return     The index of the new added GeometryObject in visual_objects
      */
-    GeomIndex addVisualObject(const JointIndex parent, const fcl::CollisionObject & co,
+    inline GeomIndex addVisualObject(const JointIndex parent, const fcl::CollisionObject & co,
                               const SE3 & placement, const std::string & geom_name = "",
                               const std::string & mesh_path = "");
 
@@ -554,7 +554,7 @@ struct GeometryObject
     /// \param[in] filename The complete path to the SRDF file.
     /// \param[in] verbose Verbosity mode.
     ///
-    void addCollisionPairsFromSrdf(const std::string & filename,
+    inline void addCollisionPairsFromSrdf(const std::string & filename,
                                    const bool verbose = false) throw (std::invalid_argument);
 
     ///

src/multibody/geometry.hxx

@@ -42,7 +42,7 @@
 namespace se3
 {
 
-  GeometryModel::GeomIndex GeometryModel::addCollisionObject(const JointIndex parent,
+  inline GeometryModel::GeomIndex GeometryModel::addCollisionObject(const JointIndex parent,
                                                              const fcl::CollisionObject & co,
                                                              const SE3 & placement,
                                                              const std::string & geom_name,
@@ -56,7 +56,7 @@ namespace se3
     return idx;
   }
 
-  GeometryModel::GeomIndex GeometryModel::addVisualObject(const JointIndex parent,
+  inline GeometryModel::GeomIndex GeometryModel::addVisualObject(const JointIndex parent,
                                                           const fcl::CollisionObject & co,
                                                           const SE3 & placement,
                                                           const std::string & geom_name,
@@ -349,7 +349,7 @@ namespace se3
     std::fill(distance_results.begin(), distance_results.end(), DistanceResult( fcl::DistanceResult(), 0, 0) );
   }
   
-  void GeometryData::addCollisionPairsFromSrdf(const std::string & filename,
+  inline void GeometryData::addCollisionPairsFromSrdf(const std::string & filename,
                                                const bool verbose) throw (std::invalid_argument)
   {
     // Add all collision pairs

src/multibody/parser/python.cpp

+#include <iostream>
+
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/python/motion.hpp"
+#include "pinocchio/python/inertia.hpp"
+#include "pinocchio/python/model.hpp"
+
+#include "pinocchio/multibody/parser/python.hpp"
+
+namespace bp = boost::python;
+
+namespace se3
+{
+  namespace python
+  {
+    Model buildModel (const std::string & filename, bool verbose)
+    {
+      Py_Initialize();
+      // Get a dict for the global namespace to exec further python code with
+      bp::object main_module = bp::import("__main__");
+      bp::dict globals = bp::extract<bp::dict>(main_module.attr("__dict__"));
+
+      // This will run all the needed expose() functions in python/python.cpp
+      // for boost::python to know how to convert a PyObject * into an object
+      bp::exec("import pinocchio as se3", globals);
+
+      // Create a new Model, get a shared_ptr to it, and include this pointer
+      // into the global namespace. See python/handler.hpp for more details.
+      boost::shared_ptr<Model> model(new Model());
+      bp::object obj(bp::handle<>(ModelPythonVisitor::convert( model )));
+      globals["model"] = obj;
+
+      // That's it, you can exec your python script, starting with a model you
+      // can update as you want.
+      try {
+        bp::exec_file((bp::str)filename, globals);
+      }
+      catch (bp::error_already_set& e)
+      {
+        PyErr_PrintEx(0);
+      }
+      if (verbose)
+      {
+        std::cout << "Your model has been built. It has " << model->nv;
+        std::cout << " degrees of freedom." << std::endl;
+      }
+      return *model;
+    }
+  } // namespace python
+} // namespace se3

src/multibody/parser/python.hpp

+#ifndef __se3_pythonparser_hpp__
+#define __se3_pythonparser_hpp__
+
+#include "pinocchio/multibody/model.hpp"
+
+namespace se3
+{
+  namespace python
+  {
+    Model buildModel (const std::string & filename, bool verbose = false);
+  } // namespace python
+} // namespace se3
+
+#endif // ifndef __se3_pythonparser_hpp__

unittest/CMakeLists.txt

@@ -82,6 +82,15 @@ IF(LUA5_1_FOUND)
   ADD_TEST_CFLAGS(lua '-DPINOCCHIO_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
 ENDIF(LUA5_1_FOUND)
 
+IF(${BUILD_PYTHON_INTERFACE} STREQUAL "ON")
+  IF(EIGENPY_FOUND)
+    ADD_UNIT_TEST(python_parser eigen3)
+    TARGET_LINK_LIBRARIES(python_parser ${Boost_LIBRARIES} ${PROJECT_NAME} ${PYTHON_LIBRARIES}
+      ${Boost_PYTHON_LIBRARIES})
+    ADD_TEST_CFLAGS(python_parser '-DPINOCCHIO_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
+  ENDIF(EIGENPY_FOUND)
+ENDIF(${BUILD_PYTHON_INTERFACE} STREQUAL "ON")
+
 # Work in progress
 ADD_UNIT_TEST(constraint eigen3)
 #ADD_UNIT_TEST(variant eigen3)

unittest/python_parser.cpp

+#include <iostream>
+
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/parser/python.hpp"
+
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MODULE PythonTest
+#include <boost/test/unit_test.hpp>
+
+
+BOOST_AUTO_TEST_SUITE ( ParsingPythonFile )
+
+BOOST_AUTO_TEST_CASE ( buildModel )
+{
+  std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_model.py";
+
+  #ifndef NDEBUG
+   std::cout << "Parse filename \"" << filename << "\"" << std::endl;
+  #endif
+  se3::Model model = se3::python::buildModel(filename, true);
+  #ifndef NDEBUG
+   std::cout << "This model has \"" << model.nq << "\" DoF" << std::endl;
+  #endif
+}
+
+BOOST_AUTO_TEST_SUITE_END()