Commit b19949e5 authored by jcarpent's avatar jcarpent
Browse files

[Python] Clean GeometryModel bindings

parent 5b8822ce
......@@ -28,61 +28,61 @@ namespace se3
static void updateGeometryPlacements_proxy(const Model & model,
Data & data,
const GeometryModelHandler & geom_model,
GeometryDataHandler & geom_data,
const GeometryModel & model_geom,
GeometryDataHandler & data_geom,
const Eigen::VectorXd & q
)
{
return updateGeometryPlacements(model, data, *geom_model, *geom_data, q);
return updateGeometryPlacements(model, data, model_geom, *data_geom, q);
}
#ifdef WITH_HPP_FCL
static bool computeCollision_proxy(const GeometryModelHandler & model_geom,
static bool computeCollision_proxy(const GeometryModel & model_geom,
GeometryDataHandler & data_geom,
const PairIndex & pairId)
{
return computeCollision(*model_geom, *data_geom, pairId);
return computeCollision(model_geom, *data_geom, pairId);
}
static bool computeCollisions_proxy(const GeometryModelHandler & model_geom,
static bool computeCollisions_proxy(const GeometryModel & model_geom,
GeometryDataHandler & data_geom,
const bool stopAtFirstCollision)
{
return computeCollisions(*model_geom, *data_geom, stopAtFirstCollision);
return computeCollisions(model_geom, *data_geom, stopAtFirstCollision);
}
static bool computeGeometryAndCollisions_proxy(const Model & model,
Data & data,
const GeometryModelHandler & model_geom,
const GeometryModel & model_geom,
GeometryDataHandler & data_geom,
const Eigen::VectorXd & q,
const bool stopAtFirstCollision)
{
return computeCollisions(model,data,*model_geom, *data_geom, q, stopAtFirstCollision);
return computeCollisions(model,data,model_geom, *data_geom, q, stopAtFirstCollision);
}
static fcl::DistanceResult computeDistance_proxy(const GeometryModelHandler & model_geom,
static fcl::DistanceResult computeDistance_proxy(const GeometryModel & model_geom,
GeometryDataHandler & data_geom,
const PairIndex & pairId)
{
return computeDistance(*model_geom, *data_geom, pairId);
return computeDistance(model_geom, *data_geom, pairId);
}
static std::size_t computeDistances_proxy(const GeometryModelHandler & model_geom,
static std::size_t computeDistances_proxy(const GeometryModel & model_geom,
GeometryDataHandler & data_geom)
{
return computeDistances(*model_geom, *data_geom);
return computeDistances(model_geom, *data_geom);
}
static std::size_t computeGeometryAndDistances_proxy(const Model & model,
Data & data,
const GeometryModelHandler & model_geom,
const GeometryModel & model_geom,
GeometryDataHandler & data_geom,
const Eigen::VectorXd & q
)
{
return computeDistances<true>(model, data, *model_geom, *data_geom, q);
return computeDistances<true>(model, data, model_geom, *data_geom, q);
}
#endif // WITH_HPP_FCL
......
......@@ -145,9 +145,9 @@ namespace se3
;
}
static GeometryDataHandler* makeDefault(const GeometryModelHandler & geometry_model)
static GeometryDataHandler* makeDefault(const GeometryModel & geometry_model)
{
return new GeometryDataHandler(new GeometryData(*geometry_model), true);
return new GeometryDataHandler(new GeometryData(geometry_model), true);
}
static container::aligned_vector<SE3> & oMg(GeometryDataHandler & m) { return m->oMg; }
......@@ -159,8 +159,8 @@ namespace se3
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 fillInnerOuterObjectMaps(GeometryDataHandler & m, const GeometryModel & model)
{m->fillInnerOuterObjectMaps(model);}
static void activateCollisionPair(GeometryDataHandler & m,
Index pairID) { m->activateCollisionPair(pairID); }
......
......@@ -19,118 +19,67 @@
#define __se3_python_geometry_model_hpp__
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <eigenpy/exception.hpp>
#include <eigenpy/eigenpy.hpp>
#include <eigenpy/memory.hpp>
#include "pinocchio/bindings/python/utils/eigen_container.hpp"
#include "pinocchio/bindings/python/utils/handler.hpp"
#include "pinocchio/bindings/python/utils/printable.hpp"
#include "pinocchio/multibody/geometry.hpp"
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(se3::GeometryModel)
namespace se3
{
namespace python
{
namespace bp = boost::python;
typedef Handler<GeometryModel> GeometryModelHandler;
struct GeometryModelPythonVisitor
: public boost::python::def_visitor< GeometryModelPythonVisitor >
{
public:
/* --- Convert From C++ to Python ------------------------------------- */
static PyObject* convert(GeometryModelHandler::SmartPtr_t const& ptr)
{
return boost::python::incref(boost::python::object(GeometryModelHandler(ptr)).ptr());
}
/* --- Exposing C++ API to python through the handler ----------------- */
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.add_property("ngeoms", &GeometryModelPythonVisitor::ngeoms)
.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)
cl
.def(bp::init<>("Default constructor"))
.add_property("ngeoms", &GeometryModel::ngeoms, "Number of geometries contained in the Geometry Model.")
.add_property("geometryObjects",
&GeometryModel::geometryObjects,"Vector of geometries objects.")
.def("addGeometryObject", &GeometryModel::addGeometryObject,
bp::args("GeometryObject", "Model", "bool"),
"Add a GeometryObject to a GeometryModel")
.def("getGeometryId",&GeometryModel::getGeometryId)
.def("existGeometryName",&GeometryModel::existGeometryName)
#ifdef WITH_HPP_FCL
.add_property("collisionPairs",
bp::make_function(&GeometryModelPythonVisitor::collisionPairs,
bp::return_internal_reference<>()),
"Vector of collision pairs.")
.def("addCollisionPair",&GeometryModelPythonVisitor::addCollisionPair,
bp::args("co1 (index)","co2 (index)"),
"Add a collision pair given by the index of the two collision objects."
" Remark: co1 < co2")
.def("addAllCollisionPairs",&GeometryModelPythonVisitor::addAllCollisionPairs,
"Add all collision pairs.")
.def("removeCollisionPair",&GeometryModelPythonVisitor::removeCollisionPair,
bp::args("co1 (index)","co2 (index)"),
"Remove a collision pair given by the index of the two collision objects."
" Remark: co1 < co2")
.def("removeAllCollisionPairs",&GeometryModelPythonVisitor::removeAllCollisionPairs,
"Remove all collision pairs.")
.def("existCollisionPair",&GeometryModelPythonVisitor::existCollisionPair,
bp::args("co1 (index)","co2 (index)"),
"Check if a collision pair given by the index of the two collision objects exists or not."
" Remark: co1 < co2")
.def("findCollisionPair", &GeometryModelPythonVisitor::findCollisionPair,
bp::args("co1 (index)","co2 (index)"),
"Return the index of a collision pair given by the index of the two collision objects exists or not."
" Remark: co1 < co2")
.add_property("collisionPairs",
&GeometryModel::collisionPairs,
"Vector of collision pairs.")
.def("addCollisionPair",&GeometryModel::addCollisionPair,
bp::args("co1 (index)","co2 (index)"),
"Add a collision pair given by the index of the two collision objects."
" Remark: co1 < co2")
.def("addAllCollisionPairs",&GeometryModel::addAllCollisionPairs,
"Add all collision pairs.")
.def("removeCollisionPair",&GeometryModel::removeCollisionPair,
bp::args("co1 (index)","co2 (index)"),
"Remove a collision pair given by the index of the two collision objects."
" Remark: co1 < co2")
.def("removeAllCollisionPairs",&GeometryModel::removeAllCollisionPairs,
"Remove all collision pairs.")
.def("existCollisionPair",&GeometryModel::existCollisionPair,
bp::args("co1 (index)","co2 (index)"),
"Check if a collision pair given by the index of the two collision objects exists or not."
" Remark: co1 < co2")
.def("findCollisionPair", &GeometryModel::findCollisionPair,
bp::args("co1 (index)","co2 (index)"),
"Return the index of a collision pair given by the index of the two collision objects exists or not."
" Remark: co1 < co2")
#endif // WITH_HPP_FCL
.def("BuildGeometryModel",&GeometryModelPythonVisitor::maker_default)
.staticmethod("BuildGeometryModel")
;
;
}
static Index ngeoms( GeometryModelHandler & m ) { return m->ngeoms; }
static GeomIndex getGeometryId( const GeometryModelHandler & gmodelPtr, const std::string & name )
{ return gmodelPtr->getGeometryId(name); }
static bool existGeometryName(const GeometryModelHandler & gmodelPtr, const std::string & name)
{ return gmodelPtr->existGeometryName(name);}
static container::aligned_vector<GeometryObject> & geometryObjects( GeometryModelHandler & m ) { return m->geometryObjects; }
static GeomIndex addGeometryObject( GeometryModelHandler & m, GeometryObject gobject, const Model & model, const bool autofillJointParent)
{ return m-> addGeometryObject(gobject, model, autofillJointParent); }
#ifdef WITH_HPP_FCL
static std::vector<CollisionPair> & collisionPairs( GeometryModelHandler & m )
{ return m->collisionPairs; }
static void addCollisionPair (GeometryModelHandler & m, const GeomIndex co1, const GeomIndex co2)
{ m->addCollisionPair(CollisionPair(co1, co2)); }
static void addAllCollisionPairs (GeometryModelHandler & m)
{ m->addAllCollisionPairs(); }
static void removeCollisionPair (GeometryModelHandler & m, const GeomIndex co1, const GeomIndex co2)
{ m->removeCollisionPair( CollisionPair(co1,co2) ); }
static void removeAllCollisionPairs (GeometryModelHandler & m)
{ m->removeAllCollisionPairs(); }
static bool existCollisionPair (const GeometryModelHandler & m, const GeomIndex co1, const GeomIndex co2)
{ return m->existCollisionPair(CollisionPair(co1,co2)); }
static Index findCollisionPair (const GeometryModelHandler & m, const GeomIndex co1,
const GeomIndex co2)
{ return m->findCollisionPair( CollisionPair(co1,co2) ); }
#endif // WITH_HPP_FCL
static GeometryModelHandler maker_default()
{ return GeometryModelHandler(new GeometryModel(), true); }
static std::string toString(const GeometryModelHandler& m)
{ std::ostringstream s; s << *m; return s.str(); }
/* --- Expose --------------------------------------------------------- */
static void expose()
......@@ -141,12 +90,12 @@ namespace se3
.value("COLLISION",COLLISION)
;
bp::class_<GeometryModelHandler>("GeometryModel",
"Geometry model (const)",
bp::no_init)
.def(GeometryModelPythonVisitor());
bp::to_python_converter< GeometryModelHandler::SmartPtr_t,GeometryModelPythonVisitor >();
bp::class_<GeometryModel>("GeometryModel",
"Geometry model (const)",
bp::no_init)
.def(GeometryModelPythonVisitor())
.def(PrintableVisitor<GeometryModel>())
;
}
};
......
......@@ -82,11 +82,11 @@ namespace se3
.add_property("njoints", &Model::njoints)
.add_property("nbodies", &Model::nbodies)
.add_property("nframes", &Model::nframes)
.def_readwrite("inertias",&Model::inertias)
.def_readwrite("jointPlacements",&Model::jointPlacements)
.def_readwrite("joints",&Model::joints)
.def_readwrite("parents",&Model::parents)
.def_readwrite("names",&Model::names)
.add_property("inertias",&Model::inertias)
.add_property("jointPlacements",&Model::jointPlacements)
.add_property("joints",&Model::joints)
.add_property("parents",&Model::parents)
.add_property("names",&Model::names)
.add_property("neutralConfiguration",
make_getter(&Model::neutralConfiguration, bp::return_value_policy<bp::return_by_value>()),
......
......@@ -71,40 +71,40 @@ namespace se3
}
static GeometryModelHandler
static GeometryModel
buildGeomFromUrdf(const Model & model,
const std::string & filename,
const GeometryType type
)
{
std::vector<std::string> hints;
GeometryModel * geometry_model = new GeometryModel();
se3::urdf::buildGeom(model, filename, type,*geometry_model,hints);
GeometryModel geometry_model;
se3::urdf::buildGeom(model,filename,type,geometry_model,hints);
return GeometryModelHandler(geometry_model, true);
return geometry_model;
}
static GeometryModelHandler
static GeometryModel
buildGeomFromUrdf(const Model & model,
const std::string & filename,
std::vector<std::string> & package_dirs,
const std::vector<std::string> & package_dirs,
const GeometryType type
)
{
GeometryModel * geometry_model = new GeometryModel();
se3::urdf::buildGeom(model, filename, type,*geometry_model,package_dirs);
GeometryModel geometry_model;
se3::urdf::buildGeom(model,filename,type,geometry_model,package_dirs);
return GeometryModelHandler(geometry_model, true);
return geometry_model;
}
#ifdef WITH_HPP_FCL
static void removeCollisionPairsFromSrdf(Model & model,
GeometryModelHandler& geometry_model,
GeometryModel & geometry_model,
const std::string & filename,
bool verbose
)
{
se3::srdf::removeCollisionPairsFromSrdf(model, *geometry_model, filename, verbose);
se3::srdf::removeCollisionPairsFromSrdf(model,geometry_model,filename,verbose);
}
#endif // #ifdef WITH_HPP_FCL
......@@ -155,14 +155,14 @@ namespace se3
bp::def("buildGeomFromUrdf",
static_cast <GeometryModelHandler (*) (const Model &, const std::string &, std::vector<std::string> &, const GeometryType)> (&ParsersPythonVisitor::buildGeomFromUrdf),
static_cast <GeometryModel (*) (const Model &, const std::string &, const std::vector<std::string> &, const GeometryType)> (&ParsersPythonVisitor::buildGeomFromUrdf),
bp::args("Model to assosiate the Geometry","filename (string)", "package_dirs (vector of strings)"
),
"Parse the urdf file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model "
"(remember to create the corresponding data structures).");
bp::def("buildGeomFromUrdf",
static_cast <GeometryModelHandler (*) (const Model &, const std::string &, const GeometryType)> (&ParsersPythonVisitor::buildGeomFromUrdf),
static_cast <GeometryModel (*) (const Model &, const std::string &, const GeometryType)> (&ParsersPythonVisitor::buildGeomFromUrdf),
bp::args("Model to assosiate the Geometry","filename (string)"),
"Parse the urdf file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model "
"(remember to create the corresponding data structures).");
......
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