Commit 91ffa6a1 authored by Guilhem Saurel's avatar Guilhem Saurel Committed by Justin Carpentier
Browse files

[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)
parent 4d1d1c6b
......@@ -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)
......
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)
......@@ -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 -----------------------------------------
# ----------------------------------------------------
......
......@@ -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);
///
......
......@@ -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
......
#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
#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__
......@@ -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)
......
#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()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment