From f2505698be331d7463c60e6584726543fd7d1a4b Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Mon, 18 Apr 2016 17:41:08 +0200
Subject: [PATCH] [C++][Python][Major] Replaced the way collisions and visuals
are stored in GeometryModel. Now it is two lists of GeometryObject (one for
collisions, one for visuals) aggregating information instead of multiple
vectors. Now handle multiple collisions/visuals for one link when parsing
urdf file and keep track of the mesh absolute path. Changed binding
accordingly.
---
CMakeLists.txt | 1 +
src/algorithm/collisions.hpp | 17 +-
src/multibody/geometry.hpp | 232 ++++++++++++++++++--
src/multibody/geometry.hxx | 145 ++++++++----
src/multibody/parser/urdf-with-geometry.hpp | 13 +-
src/multibody/parser/urdf-with-geometry.hxx | 67 ++++--
src/python/geometry-data.hpp | 11 +-
src/python/geometry-model.hpp | 49 +++--
src/python/geometry-object.hpp | 95 ++++++++
src/python/python.cpp | 4 +-
unittest/geom.cpp | 16 +-
11 files changed, 521 insertions(+), 129 deletions(-)
create mode 100644 src/python/geometry-object.hpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 13557e75d..62ad671b4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -180,6 +180,7 @@ SET(${PROJECT_NAME}_PYTHON_HEADERS
IF(HPP_FCL_FOUND)
LIST(APPEND ${PROJECT_NAME}_PYTHON_HEADERS
+ python/geometry-object.hpp
python/geometry-model.hpp
python/geometry-data.hpp
)
diff --git a/src/algorithm/collisions.hpp b/src/algorithm/collisions.hpp
index cb37daad8..31a825b52 100644
--- a/src/algorithm/collisions.hpp
+++ b/src/algorithm/collisions.hpp
@@ -29,7 +29,7 @@ namespace se3
///
- /// \brief Apply a forward kinematics and update the placement of the collision objects.
+ /// \brief Apply a forward kinematics and update the placement of the geometry objects (both collision's and visual's one).
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
@@ -45,7 +45,7 @@ namespace se3
);
///
- /// \brief Update the placement of the collision objects according to the current joint placements contained in data.
+ /// \brief Update the placement of the geometry objects according to the current joint placements contained in data. (both collision's and visual's one)
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
@@ -102,11 +102,16 @@ namespace se3
GeometryData & data_geom
)
{
- for (GeometryData::GeomIndex i=0; i < (GeometryData::GeomIndex) data_geom.model_geom.ngeom; ++i)
+ for (GeometryData::GeomIndex i=0; i < (GeometryData::GeomIndex) data_geom.model_geom.ncollisions; ++i)
{
- const Model::JointIndex & parent = model_geom.geom_parents[i];
- data_geom.oMg[i] = (data.oMi[parent] * model_geom.geometryPlacement[i]);
- data_geom.oMg_fcl[i] = toFclTransform3f(data_geom.oMg[i]);
+ const Model::JointIndex & parent = model_geom.collision_objects[i].parent;
+ data_geom.oMg_collisions[i] = (data.oMi[parent] * model_geom.collision_objects[i].placement);
+ data_geom.oMg_fcl_collisions[i] = toFclTransform3f(data_geom.oMg_collisions[i]);
+ }
+ for (GeometryData::GeomIndex i=0; i < (GeometryData::GeomIndex) data_geom.model_geom.nvisuals; ++i)
+ {
+ const Model::JointIndex & parent = model_geom.visual_objects[i].parent;
+ data_geom.oMg_visuals[i] = (data.oMi[parent] * model_geom.visual_objects[i].placement);
}
}
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index ebe944454..ef5349e9f 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -156,7 +156,83 @@ namespace se3
GeomIndex object2;
}; // struct CollisionResult
+
+enum GeometryType
+{
+ VISUAL,
+ COLLISION,
+ NONE
+};
+
+struct GeometryObject
+{
+ typedef Model::Index Index;
+ typedef Model::JointIndex JointIndex;
+ typedef Model::GeomIndex GeomIndex;
+
+ /// \brief Type of the GeometryObject. Cann be VISUAL, COLLISION, or NONE (cf GeometryType enumeration)
+ GeometryType type;
+
+ /// \brief Name of the geometry object
+ std::string name;
+
+ /// \brief Index of the parent joint
+ JointIndex parent;
+
+ /// \brief The actual cloud of points representing the collision mesh of the object
+ fcl::CollisionObject collision_object;
+
+ /// \brief Position of geometry object in parent joint's frame
+ SE3 placement;
+
+ /// \brief Absolute path to the mesh file
+ std::string mesh_path;
+
+
+ GeometryObject(const GeometryType type, const std::string & name, const JointIndex parent, const fcl::CollisionObject & collision,
+ const SE3 & placement, const std::string & mesh_path)
+ : type(type)
+ , name(name)
+ , parent(parent)
+ , collision_object(collision)
+ , placement(placement)
+ , mesh_path(mesh_path)
+ {}
+
+ GeometryObject & operator=(const GeometryObject & other)
+ {
+ type = other.type;
+ name = other.name;
+ parent = other.parent;
+ collision_object = other.collision_object;
+ placement = other.placement;
+ mesh_path = other.mesh_path;
+ return *this;
+ }
+
+};
+ inline bool operator==(const GeometryObject & lhs, const GeometryObject & rhs)
+ {
+ return (lhs.type == rhs.type && lhs.name == rhs.name
+ && lhs.parent == rhs.parent
+ // && lhs.collision_object == rhs.collision_object
+ && lhs.placement == rhs.placement
+ && lhs.mesh_path == rhs.mesh_path
+ );
+ }
+
+ inline std::ostream & operator<< (std::ostream & os, const GeometryObject & geom_object)
+ {
+ os << "Type: \t" << geom_object.type
+ << "Name: \t" << geom_object.name
+ << "Parent ID: \t" << geom_object.parent
+ // << "collision object: \t" << geom_object.collision_object
+ << "Position in parent frame: \t" << geom_object.placement
+ << "Absolute path to mesh file: \t" << geom_object.mesh_path
+ << std::endl;
+ return os;
+ }
struct GeometryModel
{
@@ -166,35 +242,140 @@ namespace se3
typedef std::list<GeomIndex> GeomIndexList;
+ /// \brief A const reference to the reference model.
const se3::Model & model;
- Index ngeom;
- std::vector<fcl::CollisionObject> collision_objects;
- std::vector<std::string> geom_names;
- std::vector<JointIndex> geom_parents; // Joint parent of body <i>, denoted <li> (li==parents[i])
- std::vector<SE3> geometryPlacement; // Position of geometry object in parent joint's frame
+
+ /// \brief The number of GeometryObjects that are of type COLLISION
+ Index ncollisions;
+
+ /// \brief The number of GeometryObjects that are of type visuals
+ Index nvisuals;
+
+ /// \brief Vector of GeometryObjects used for collision computations
+ std::vector<GeometryObject> collision_objects;
+
+ /// \brief Vector of GeometryObjects used for visualisation
+ std::vector<GeometryObject> visual_objects;
- std::map < JointIndex, GeomIndexList > innerObjects; // Associate a list of CollisionObjects to a given joint Id
- std::map < JointIndex, GeomIndexList > outerObjects; // Associate a list of CollisionObjects to a given joint Id
+ /// \brief A list of associated collision GeometryObjects to a given joint Id.
+ /// Inner objects can be seen as geometry objects that directly move when the associated joint moves
+ std::map < JointIndex, GeomIndexList > innerObjects;
+
+ /// \brief A list of associated collision GeometryObjects to a given joint Id
+ /// Outer objects can be seen as geometry objects that may often be obstacles to the Inner objects of given joint
+ std::map < JointIndex, GeomIndexList > outerObjects;
GeometryModel(const se3::Model & model)
: model(model)
- , ngeom(0)
+ , ncollisions(0)
+ , nvisuals(0)
, collision_objects()
- , geom_names(0)
- , geom_parents(0)
- , geometryPlacement(0)
+ , visual_objects()
, innerObjects()
, outerObjects()
{}
~GeometryModel() {};
- GeomIndex addGeomObject(const JointIndex parent, const fcl::CollisionObject & co, const SE3 & placement, const std::string & geoName = "");
- GeomIndex getGeomId(const std::string & name) const;
- bool existGeomName(const std::string & name) const;
- const std::string & getGeomName(const GeomIndex index) const;
+ /**
+ * @brief Add a geometry object whose type is COLLISION to a GeometryModel
+ *
+ * @param[in] parent Index of the parent joint
+ * @param[in] co The actual fcl CollisionObject
+ * @param[in] placement The relative placement regarding to the parent frame
+ * @param[in] geom_name The name of the Geometry Object
+ * @param[in] mesh_path The absolute path to the mesh
+ *
+ * @return The index of the new added GeometryObject in collision_objects
+ */
+ GeomIndex addCollisionObject(const JointIndex parent, const fcl::CollisionObject & co,
+ const SE3 & placement, const std::string & geom_name = "",
+ const std::string & mesh_path = "");
+
+ /**
+ * @brief Add a geometry object whose type is VISUAL to a GeometryModel
+ *
+ * @param[in] parent Index of the parent joint
+ * @param[in] co The actual fcl CollisionObject
+ * @param[in] placement The relative placement regarding to the parent frame
+ * @param[in] geom_name The name of the Geometry Object
+ * @param[in] mesh_path The absolute path to the mesh
+ *
+ * @return The index of the new added GeometryObject in visual_objects
+ */
+ GeomIndex addVisualObject(const JointIndex parent, const fcl::CollisionObject & co,
+ const SE3 & placement, const std::string & geom_name = "",
+ const std::string & mesh_path = "");
+
+ /**
+ * @brief Return the index of a GeometryObject of type COLLISION given by its name.
+ *
+ * @param[in] name Name of the GeometryObject
+ *
+ * @return Index of the corresponding GeometryObject
+ */
+ GeomIndex getCollisionId(const std::string & name) const;
+
+ /**
+ * @brief Return the index of a GeometryObject of type VISUAL given by its name.
+ *
+ * @param[in] name Name of the GeometryObject
+ *
+ * @return Index of the corresponding GeometryObject
+ */
+ GeomIndex getVisualId(const std::string & name) const;
+
+ /**
+ * @brief Check if a GeometryObject of type COLLISION given by its name exists.
+ *
+ * @param[in] name Name of the GeometryObject
+ *
+ * @return True if the GeometryObject exists in the collision_objects.
+ */
+ bool existCollisionName(const std::string & name) const;
+
+ /**
+ * @brief Check if a GeometryObject of type VISUAL given by its name exists.
+ *
+ * @param[in] name Name of the GeometryObject
+ *
+ * @return True if the GeometryObject exists in the visual_objects.
+ */
+ bool existVisualName(const std::string & name) const;
+
+ /**
+ * @brief Get the name of a GeometryObject of type COLLISION given by its index.
+ *
+ * @param[in] index Index of the GeometryObject
+ *
+ * @return Name of the GeometryObject
+ */
+ const std::string & getCollisionName(const GeomIndex index) const;
+
+ /**
+ * @brief Get the name of a GeometryObject of type VISUAL given by its index.
+ *
+ * @param[in] index Index of the GeometryObject
+ *
+ * @return Name of the GeometryObject
+ */
+ const std::string & getVisualName(const GeomIndex index) const;
+
+ /**
+ * @brief Associate a GeometryObject of type COLLISION to a joint's inner objects list
+ *
+ * @param[in] joint Index of the joint
+ * @param[in] inner_object Index of the GeometryObject that will be an inner object
+ */
void addInnerObject(const JointIndex joint, const GeomIndex inner_object);
+
+ /**
+ * @brief Associate a GeometryObject of type COLLISION to a joint's outer objects list
+ *
+ * @param[in] joint Index of the joint
+ * @param[in] inner_object Index of the GeometryObject that will be an outer object
+ */
void addOutterObject(const JointIndex joint, const GeomIndex outer_object);
friend std::ostream& operator<<(std::ostream & os, const GeometryModel & model_geom);
@@ -220,17 +401,21 @@ namespace se3
const GeometryModel & model_geom;
///
- /// \brief Vector gathering the SE3 placements of the geometries relative to the world.
+ /// \brief Vector gathering the SE3 placements of the collision objects relative to the world.
/// See updateGeometryPlacements to update the placements.
///
- std::vector<se3::SE3> oMg;
-
+ std::vector<se3::SE3> oMg_collisions;
+
+ ///
+ /// \brief Vector gathering the SE3 placements of the visual objects relative to the world.
+ /// See updateGeometryPlacements to update the placements.
+ ///
+ std::vector<se3::SE3> oMg_visuals;
///
/// \brief Same as oMg but using fcl::Transform3f to store placement.
/// This pre-allocation avoids dynamic allocation during collision checking or distance computations.
///
- std::vector<fcl::Transform3f> oMg_fcl;
-
+ std::vector<fcl::Transform3f> oMg_fcl_collisions;
///
/// \brief Vector of collision pairs.
/// See addCollisionPair, removeCollisionPair to fill or remove elements in the vector.
@@ -255,14 +440,15 @@ namespace se3
GeometryData(const Data & data, const GeometryModel & model_geom)
: data_ref(data)
, model_geom(model_geom)
- , oMg(model_geom.ngeom)
- , oMg_fcl(model_geom.ngeom)
+ , oMg_collisions(model_geom.ncollisions)
+ , oMg_visuals(model_geom.nvisuals)
+ , oMg_fcl_collisions(model_geom.ncollisions)
, collision_pairs()
, nCollisionPairs(0)
, distance_results()
, collision_results()
{
- const std::size_t num_max_collision_pairs = (model_geom.ngeom * (model_geom.ngeom-1))/2;
+ const std::size_t num_max_collision_pairs = (model_geom.ncollisions * (model_geom.ncollisions-1))/2;
collision_pairs.reserve(num_max_collision_pairs);
distance_results.resize(num_max_collision_pairs, DistanceResult( fcl::DistanceResult(), 0, 0) );
collision_results.resize(num_max_collision_pairs, CollisionResult( fcl::CollisionResult(), 0, 0));
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
index b325d6c65..5c4cd8c6e 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -42,48 +42,87 @@
namespace se3
{
- inline GeometryModel::GeomIndex GeometryModel::addGeomObject(const JointIndex parent,
- const fcl::CollisionObject & co,
- const SE3 & placement,
- const std::string & geoName)
+ GeometryModel::GeomIndex GeometryModel::addCollisionObject(const JointIndex parent,
+ const fcl::CollisionObject & co,
+ const SE3 & placement,
+ const std::string & geom_name,
+ const std::string & mesh_path)
{
+ Index idx = (Index) (ncollisions ++);
- Index idx = (Index) (ngeom ++);
+ collision_objects.push_back(GeometryObject(COLLISION, geom_name, parent, co,
+ placement, mesh_path));
+ addInnerObject(parent, idx);
+ return idx;
+ }
+ GeometryModel::GeomIndex GeometryModel::addVisualObject(const JointIndex parent,
+ const fcl::CollisionObject & co,
+ const SE3 & placement,
+ const std::string & geom_name,
+ const std::string & mesh_path)
+ {
- collision_objects .push_back(co);
- geom_parents .push_back(parent);
- geometryPlacement .push_back(placement);
- geom_names .push_back( (geoName!="")?geoName:random(8));
-
- addInnerObject(parent, idx);
+ Index idx = (Index) (nvisuals ++);
+ visual_objects.push_back(GeometryObject(VISUAL, geom_name, parent, co,
+ placement, mesh_path));
+
return idx;
}
- inline GeometryModel::GeomIndex GeometryModel::getGeomId (const std::string & name) const
+ inline GeometryModel::GeomIndex GeometryModel::getCollisionId(const std::string & name) const
{
- std::vector<std::string>::iterator::difference_type
- res = std::find(geom_names.begin(),geom_names.end(),name) - geom_names.begin();
- assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
- assert( (res>=0)&&(res<(long)collision_objects.size())&&"The joint name you asked does not exist" );
- return GeomIndex(res);
+
+ std::vector<GeometryObject>::const_iterator it = std::find_if(collision_objects.begin(),
+ collision_objects.end(),
+ boost::bind(&GeometryObject::name, _1) == name
+ );
+ return GeometryModel::GeomIndex(it - collision_objects.begin());
+ }
+
+ inline GeometryModel::GeomIndex GeometryModel::getVisualId(const std::string & name) const
+ {
+
+ std::vector<GeometryObject>::const_iterator it = std::find_if(visual_objects.begin(),
+ visual_objects.end(),
+ boost::bind(&GeometryObject::name, _1) == name
+ );
+ return GeometryModel::GeomIndex(it - visual_objects.begin());
}
- inline bool GeometryModel::existGeomName (const std::string & name) const
+
+ inline bool GeometryModel::existCollisionName(const std::string & name) const
{
- return (geom_names.end() != std::find(geom_names.begin(),geom_names.end(),name));
+ return std::find_if(collision_objects.begin(),
+ collision_objects.end(),
+ boost::bind(&GeometryObject::name, _1) == name) != collision_objects.end();
}
- inline const std::string& GeometryModel::getGeomName (const GeomIndex index) const
+ inline bool GeometryModel::existVisualName(const std::string & name) const
+ {
+ return std::find_if(visual_objects.begin(),
+ visual_objects.end(),
+ boost::bind(&GeometryObject::name, _1) == name) != visual_objects.end();
+ }
+
+ inline const std::string& GeometryModel::getCollisionName(const GeomIndex index) const
{
assert( index < (GeomIndex)collision_objects.size() );
- return geom_names[index];
+ return collision_objects[index].name;
+ }
+
+ inline const std::string& GeometryModel::getVisualName(const GeomIndex index) const
+ {
+ assert( index < (GeomIndex)visual_objects.size() );
+ return visual_objects[index].name;
}
- inline void GeometryModel::addInnerObject (const JointIndex joint_id, const GeomIndex inner_object)
+ inline void GeometryModel::addInnerObject(const JointIndex joint_id, const GeomIndex inner_object)
{
- if (std::find(innerObjects[joint_id].begin(), innerObjects[joint_id].end(),inner_object)==innerObjects[joint_id].end())
+ if (std::find(innerObjects[joint_id].begin(),
+ innerObjects[joint_id].end(),
+ inner_object) == innerObjects[joint_id].end())
innerObjects[joint_id].push_back(inner_object);
else
std::cout << "inner object already added" << std::endl;
@@ -91,7 +130,9 @@ namespace se3
inline void GeometryModel::addOutterObject (const JointIndex joint, const GeomIndex outer_object)
{
- if (std::find(outerObjects[joint].begin(), outerObjects[joint].end(),outer_object)==outerObjects[joint].end())
+ if (std::find(outerObjects[joint].begin(),
+ outerObjects[joint].end(),
+ outer_object) == outerObjects[joint].end())
outerObjects[joint].push_back(outer_object);
else
std::cout << "outer object already added" << std::endl;
@@ -99,12 +140,18 @@ namespace se3
inline std::ostream & operator<< (std::ostream & os, const GeometryModel & model_geom)
{
- os << "Nb collision objects = " << model_geom.ngeom << std::endl;
+ os << "Nb collision objects = " << model_geom.ncollisions << std::endl;
- for(GeometryModel::Index i=0;i<(GeometryModel::Index)(model_geom.ngeom);++i)
+ for(GeometryModel::Index i=0;i<(GeometryModel::Index)(model_geom.ncollisions);++i)
{
- os << "Collision object " << i << " : " << model_geom.geom_names[i] << ": attached to joint = " << model_geom.geom_parents[i]
- << "\nwith offset \n" << model_geom.geometryPlacement[i] <<std::endl;
+ os << model_geom.collision_objects[i] <<std::endl;
+ }
+
+ os << "Nb visual objects = " << model_geom.nvisuals << std::endl;
+
+ for(GeometryModel::Index i=0;i<(GeometryModel::Index)(model_geom.nvisuals);++i)
+ {
+ os << model_geom.visual_objects[i] <<std::endl;
}
return os;
@@ -112,10 +159,11 @@ namespace se3
inline std::ostream & operator<< (std::ostream & os, const GeometryData & data_geom)
{
-
- for(GeometryData::Index i=0;i<(GeometryData::Index)(data_geom.model_geom.ngeom);++i)
+ os << "Nb collision pairs = " << data_geom.nCollisionPairs << std::endl;
+
+ for(GeometryData::Index i=0;i<(GeometryData::Index)(data_geom.nCollisionPairs);++i)
{
- os << "collision object in position " << data_geom.oMg[i] << std::endl;
+ os << "collision object in position " << data_geom.collision_pairs[i] << std::endl;
}
return os;
@@ -124,7 +172,7 @@ namespace se3
inline void GeometryData::addCollisionPair (const GeomIndex co1, const GeomIndex co2)
{
assert ( co1 != co2);
- assert ( co2 < model_geom.ngeom);
+ assert ( co2 < model_geom.ncollisions);
CollisionPair_t pair(co1, co2);
addCollisionPair(pair);
@@ -132,7 +180,7 @@ namespace se3
inline void GeometryData::addCollisionPair (const CollisionPair_t & pair)
{
- assert(pair.second < model_geom.ngeom);
+ assert(pair.second < model_geom.ncollisions);
if (!existCollisionPair(pair))
{
@@ -144,16 +192,16 @@ namespace se3
inline void GeometryData::addAllCollisionPairs()
{
removeAllCollisionPairs();
- collision_pairs.reserve((model_geom.ngeom * (model_geom.ngeom-1))/2);
- for (Index i = 0; i < model_geom.ngeom; ++i)
- for (Index j = i+1; j < model_geom.ngeom; ++j)
+ collision_pairs.reserve((model_geom.ncollisions * (model_geom.ncollisions-1))/2);
+ for (Index i = 0; i < model_geom.ncollisions; ++i)
+ for (Index j = i+1; j < model_geom.ncollisions; ++j)
addCollisionPair(i,j);
}
inline void GeometryData::removeCollisionPair (const GeomIndex co1, const GeomIndex co2)
{
assert(co1 < co2);
- assert(co2 < model_geom.ngeom);
+ assert(co2 < model_geom.ncollisions);
assert(existCollisionPair(co1,co2));
removeCollisionPair (CollisionPair_t(co1,co2));
@@ -161,9 +209,11 @@ namespace se3
inline void GeometryData::removeCollisionPair (const CollisionPair_t & pair)
{
- assert(pair.second < model_geom.ngeom);
+ assert(pair.second < model_geom.ncollisions);
- CollisionPairsVector_t::iterator it = std::find(collision_pairs.begin(), collision_pairs.end(), pair);
+ CollisionPairsVector_t::iterator it = std::find(collision_pairs.begin(),
+ collision_pairs.end(),
+ pair);
if (it != collision_pairs.end())
{
collision_pairs.erase(it);
@@ -184,8 +234,9 @@ namespace se3
inline bool GeometryData::existCollisionPair (const CollisionPair_t & pair) const
{
- return (std::find(collision_pairs.begin(), collision_pairs.end(), pair)
- != collision_pairs.end());
+ return (std::find(collision_pairs.begin(),
+ collision_pairs.end(),
+ pair) != collision_pairs.end());
}
inline GeometryData::Index GeometryData::findCollisionPair (const GeomIndex co1, const GeomIndex co2) const
@@ -195,7 +246,9 @@ namespace se3
inline GeometryData::Index GeometryData::findCollisionPair (const CollisionPair_t & pair) const
{
- CollisionPairsVector_t::const_iterator it = std::find(collision_pairs.begin(), collision_pairs.end(), pair);
+ CollisionPairsVector_t::const_iterator it = std::find(collision_pairs.begin(),
+ collision_pairs.end(),
+ pair);
return (Index) distance(collision_pairs.begin(), it);
}
@@ -237,8 +290,8 @@ namespace se3
fcl::CollisionRequest collisionRequest (1, false, false, 1, false, true, fcl::GST_INDEP);
fcl::CollisionResult collisionResult;
- fcl::collide (model_geom.collision_objects[co1].collisionGeometry().get(), oMg_fcl[co1],
- model_geom.collision_objects[co2].collisionGeometry().get(), oMg_fcl[co2],
+ fcl::collide (model_geom.collision_objects[co1].collision_object.collisionGeometry().get(), oMg_fcl_collisions[co1],
+ model_geom.collision_objects[co2].collision_object.collisionGeometry().get(), oMg_fcl_collisions[co2],
collisionRequest, collisionResult);
return CollisionResult (collisionResult, co1, co2);
@@ -275,8 +328,8 @@ namespace se3
fcl::DistanceRequest distanceRequest (true, 0, 0, fcl::GST_INDEP);
fcl::DistanceResult result;
- fcl::distance ( model_geom.collision_objects[co1].collisionGeometry().get(), oMg_fcl[co1],
- model_geom.collision_objects[co2].collisionGeometry().get(), oMg_fcl[co2],
+ fcl::distance ( model_geom.collision_objects[co1].collision_object.collisionGeometry().get(), oMg_fcl_collisions[co1],
+ model_geom.collision_objects[co2].collision_object.collisionGeometry().get(), oMg_fcl_collisions[co2],
distanceRequest, result);
return DistanceResult (result, co1, co2);
diff --git a/src/multibody/parser/urdf-with-geometry.hpp b/src/multibody/parser/urdf-with-geometry.hpp
index 784495011..d0aecf248 100644
--- a/src/multibody/parser/urdf-with-geometry.hpp
+++ b/src/multibody/parser/urdf-with-geometry.hpp
@@ -35,15 +35,18 @@ namespace se3
/**
- * @brief Get a CollisionObject from an urdf link, searching
+ * @brief Get a fcl::CollisionObject from an urdf geometry, searching
* for it in specified package directories
*
- * @param[in] link The input urdf link
- * @param[in] package_dirs A vector containing the different directories where to search for packages
+ * @param[in] urdf_geometry The input urdf geometry
+ * @param[in] package_dirs A vector containing the different directories where to search for packages
+ * @param[out] mesh_path The Absolute path of the mesh currently read
*
- * @return The mesh converted as a fcl::CollisionObject
+ * @return The geometry converted as a fcl::CollisionObject
*/
- inline fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & link, const std::vector < std::string > & package_dirs);
+ inline fcl::CollisionObject retrieveCollisionGeometry(const boost::shared_ptr < ::urdf::Geometry > urdf_geometry,
+ const std::vector < std::string > & package_dirs,
+ std::string & mesh_path);
/**
diff --git a/src/multibody/parser/urdf-with-geometry.hxx b/src/multibody/parser/urdf-with-geometry.hxx
index ca12a540d..900b94e8d 100644
--- a/src/multibody/parser/urdf-with-geometry.hxx
+++ b/src/multibody/parser/urdf-with-geometry.hxx
@@ -31,33 +31,35 @@ namespace se3
namespace urdf
{
- inline fcl::CollisionObject retrieveCollisionGeometry (const ::urdf::LinkConstPtr & link, const std::vector < std::string > & package_dirs)
+ inline fcl::CollisionObject retrieveCollisionGeometry(const boost::shared_ptr < ::urdf::Geometry> urdf_geometry,
+ const std::vector < std::string > & package_dirs,
+ std::string & mesh_path)
{
- boost::shared_ptr < ::urdf::Collision> collision = link->collision;
boost::shared_ptr < fcl::CollisionGeometry > geometry;
// Handle the case where collision geometry is a mesh
- if (collision->geometry->type == ::urdf::Geometry::MESH)
+ if (urdf_geometry->type == ::urdf::Geometry::MESH)
{
- boost::shared_ptr < ::urdf::Mesh> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Mesh> (collision->geometry);
+ boost::shared_ptr < ::urdf::Mesh> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry);
std::string collisionFilename = collisionGeometry->filename;
- std::string full_path = convertURDFMeshPathToAbsolutePath(collisionFilename, package_dirs);
+ mesh_path = convertURDFMeshPathToAbsolutePath(collisionFilename, package_dirs);
::urdf::Vector3 scale = collisionGeometry->scale;
// Create FCL mesh by parsing Collada file.
Polyhedron_ptr polyhedron (new PolyhedronType);
- loadPolyhedronFromResource (full_path, scale, polyhedron);
+ loadPolyhedronFromResource (mesh_path, scale, polyhedron);
geometry = polyhedron;
}
// Handle the case where collision geometry is a cylinder
// Use FCL capsules for cylinders
- else if (collision->geometry->type == ::urdf::Geometry::CYLINDER)
+ else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
{
- boost::shared_ptr < ::urdf::Cylinder> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Cylinder> (collision->geometry);
+ mesh_path = "CYLINDER";
+ boost::shared_ptr < ::urdf::Cylinder> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);
double radius = collisionGeometry->radius;
double length = collisionGeometry->length;
@@ -66,9 +68,10 @@ namespace se3
geometry = boost::shared_ptr < fcl::CollisionGeometry >(new fcl::Capsule (radius, length));
}
// Handle the case where collision geometry is a box.
- else if (collision->geometry->type == ::urdf::Geometry::BOX)
+ else if (urdf_geometry->type == ::urdf::Geometry::BOX)
{
- boost::shared_ptr < ::urdf::Box> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Box> (collision->geometry);
+ mesh_path = "BOX";
+ boost::shared_ptr < ::urdf::Box> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry);
double x = collisionGeometry->dim.x;
double y = collisionGeometry->dim.y;
@@ -77,9 +80,10 @@ namespace se3
geometry = boost::shared_ptr < fcl::CollisionGeometry > (new fcl::Box (x, y, z));
}
// Handle the case where collision geometry is a sphere.
- else if (collision->geometry->type == ::urdf::Geometry::SPHERE)
+ else if (urdf_geometry->type == ::urdf::Geometry::SPHERE)
{
- boost::shared_ptr < ::urdf::Sphere> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Sphere> (collision->geometry);
+ mesh_path = "SPHERE";
+ boost::shared_ptr < ::urdf::Sphere> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry);
double radius = collisionGeometry->radius;
@@ -103,25 +107,48 @@ namespace se3
const std::vector<std::string> & package_dirs) throw (std::invalid_argument)
{
+ std::string mesh_path = "";
+
+ std::string link_name = link->name;
// start with first link that is not empty
if(link->collision)
{
-
-
assert(link->getParent()!=NULL);
+ if (link->getParent() == NULL)
+ {
+ const std::string exception_message (link->name + " - joint information missing.");
+ throw std::invalid_argument(exception_message);
+ }
+
+ for (std::vector< boost::shared_ptr< ::urdf::Collision> >::const_iterator i = link->collision_array.begin();i != link->collision_array.end(); ++i)
+ {
+ fcl::CollisionObject collision_object = retrieveCollisionGeometry((*i)->geometry, package_dirs, mesh_path);
+ SE3 geomPlacement = convertFromUrdf((*i)->origin);
+ std::string collision_object_name = (*i)->name ;
+ geom_model.addCollisionObject(model.parents[model.getBodyId(link_name)], collision_object, geomPlacement, collision_object_name, mesh_path);
+
+ }
+ } // if(link->collision)
- fcl::CollisionObject collision_object = retrieveCollisionGeometry(link, package_dirs);
- SE3 geomPlacement = convertFromUrdf(link->collision->origin);
- std::string collision_object_name = link->name ;
- geom_model.addGeomObject(model.parents[model.getBodyId(collision_object_name)], collision_object, geomPlacement, collision_object_name);
-
+ if(link->visual)
+ {
+ assert(link->getParent()!=NULL);
if (link->getParent() == NULL)
{
const std::string exception_message (link->name + " - joint information missing.");
throw std::invalid_argument(exception_message);
}
+
+ for (std::vector< boost::shared_ptr< ::urdf::Visual> >::const_iterator i = link->visual_array.begin(); i != link->visual_array.end(); ++i)
+ {
+ fcl::CollisionObject visual_object = retrieveCollisionGeometry((*i)->geometry, package_dirs, mesh_path);
+ SE3 geomPlacement = convertFromUrdf((*i)->origin);
+ std::string visual_object_name = (*i)->name ;
+ geom_model.addVisualObject(model.parents[model.getBodyId(link_name)], visual_object, geomPlacement, visual_object_name, mesh_path);
+
+ }
+ } // if(link->visual)
- } // if(link->collision)
BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
{
diff --git a/src/python/geometry-data.hpp b/src/python/geometry-data.hpp
index 1e1478711..876e87487 100644
--- a/src/python/geometry-data.hpp
+++ b/src/python/geometry-data.hpp
@@ -94,10 +94,14 @@ namespace se3
.add_property("nCollisionPairs", &GeometryDataPythonVisitor::nCollisionPairs)
- .add_property("oMg",
- bp::make_function(&GeometryDataPythonVisitor::oMg,
+ .add_property("oMg_collisions",
+ bp::make_function(&GeometryDataPythonVisitor::oMg_collisions,
bp::return_internal_reference<>()),
"Vector of collision objects placement relative to the world.")
+ .add_property("oMg_visuals",
+ bp::make_function(&GeometryDataPythonVisitor::oMg_visuals,
+ bp::return_internal_reference<>()),
+ "Vector of visual objects placement relative to the world.")
.add_property("collision_pairs",
bp::make_function(&GeometryDataPythonVisitor::collision_pairs,
bp::return_internal_reference<>()),
@@ -165,7 +169,8 @@ namespace se3
static Index nCollisionPairs(const GeometryDataHandler & m ) { return m->nCollisionPairs; }
- static std::vector<SE3> & oMg(GeometryDataHandler & m) { return m->oMg; }
+ static std::vector<SE3> & oMg_collisions(GeometryDataHandler & m) { return m->oMg_collisions; }
+ static std::vector<SE3> & oMg_visuals(GeometryDataHandler & m) { return m->oMg_visuals; }
static std::vector<CollisionPair_t> & collision_pairs( GeometryDataHandler & m ) { return m->collision_pairs; }
static std::vector<DistanceResult> & distance_results( GeometryDataHandler & m ) { return m->distance_results; }
static std::vector<CollisionResult> & collision_results( GeometryDataHandler & m ) { return m->collision_results; }
diff --git a/src/python/geometry-model.hpp b/src/python/geometry-model.hpp
index 47a162060..fb4c7d44b 100644
--- a/src/python/geometry-model.hpp
+++ b/src/python/geometry-model.hpp
@@ -63,20 +63,23 @@ namespace se3
void visit(PyClass& cl) const
{
cl
- .add_property("ngeom", &GeometryModelPythonVisitor::ngeom)
-
- .def("getGeomId",&GeometryModelPythonVisitor::getGeomId)
-
- .add_property("geometryPlacement",
- bp::make_function(&GeometryModelPythonVisitor::geometryPlacement,
+ .add_property("ncollisions", &GeometryModelPythonVisitor::ncollisions)
+ .add_property("nvisuals", &GeometryModelPythonVisitor::nvisuals)
+
+ .def("getCollisionId",&GeometryModelPythonVisitor::getCollisionId)
+ .def("getVisualId",&GeometryModelPythonVisitor::getVisualId)
+ .def("existCollisionName",&GeometryModelPythonVisitor::existCollisionName)
+ .def("existVisualName",&GeometryModelPythonVisitor::existVisualName)
+ .def("getCollisionName",&GeometryModelPythonVisitor::getCollisionName)
+ .def("getVisualName",&GeometryModelPythonVisitor::getVisualName)
+ .add_property("collision_objects",
+ bp::make_function(&GeometryModelPythonVisitor::collision_objects,
bp::return_internal_reference<>()) )
- .add_property("geom_parents",
- bp::make_function(&GeometryModelPythonVisitor::geom_parents,
- bp::return_internal_reference<>()) )
- .add_property("geom_names",
- bp::make_function(&GeometryModelPythonVisitor::geom_names,
+ .add_property("visual_objects",
+ bp::make_function(&GeometryModelPythonVisitor::visual_objects,
bp::return_internal_reference<>()) )
+
.def("__str__",&GeometryModelPythonVisitor::toString)
.def("BuildGeometryModel",&GeometryModelPythonVisitor::maker_default)
@@ -84,14 +87,26 @@ namespace se3
;
}
- static GeometryModel::Index ngeom( GeometryModelHandler & m ) { return m->ngeom; }
+ static GeometryModel::Index ncollisions( GeometryModelHandler & m ) { return m->ncollisions; }
+ static GeometryModel::Index nvisuals( GeometryModelHandler & m ) { return m->nvisuals; }
- static Model::GeomIndex getGeomId( const GeometryModelHandler & gmodelPtr, const std::string & name )
- { return gmodelPtr->getGeomId(name); }
+ static std::vector<GeometryObject> & collision_objects( GeometryModelHandler & m ) { return m->collision_objects; }
+ static std::vector<GeometryObject> & visual_objects( GeometryModelHandler & m ) { return m->visual_objects; }
- static std::vector<SE3> & geometryPlacement( GeometryModelHandler & m ) { return m->geometryPlacement; }
- static std::vector<Model::JointIndex> & geom_parents( GeometryModelHandler & m ) { return m->geom_parents; }
- static std::vector<std::string> & geom_names ( GeometryModelHandler & m ) { return m->geom_names; }
+ static Model::GeomIndex getCollisionId( const GeometryModelHandler & gmodelPtr, const std::string & name )
+ { return gmodelPtr->getCollisionId(name); }
+ static Model::GeomIndex getVisualId( const GeometryModelHandler & gmodelPtr, const std::string & name )
+ { return gmodelPtr->getVisualId(name); }
+ static bool existCollisionName(const GeometryModelHandler & gmodelPtr, const std::string & name)
+ { return gmodelPtr->existCollisionName(name);}
+ static bool existVisualName(const GeometryModelHandler & gmodelPtr, const std::string & name)
+ { return gmodelPtr->existVisualName(name);}
+ static std::string getCollisionName(const GeometryModelHandler & gmodelPtr, const GeomIndex index)
+ { return gmodelPtr->getCollisionName(index);}
+ static std::string getVisualName(const GeometryModelHandler & gmodelPtr, const GeomIndex index)
+ { return gmodelPtr->getVisualName(index);}
+ // static std::vector<Model::JointIndex> & geom_parents( GeometryModelHandler & m ) { return m->geom_parents; }
+ // static std::vector<std::string> & geom_names ( GeometryModelHandler & m ) { return m->geom_names; }
diff --git a/src/python/geometry-object.hpp b/src/python/geometry-object.hpp
new file mode 100644
index 000000000..1cf84df10
--- /dev/null
+++ b/src/python/geometry-object.hpp
@@ -0,0 +1,95 @@
+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_python_geometry_object_hpp__
+#define __se3_python_geometry_object_hpp__
+
+#include <eigenpy/exception.hpp>
+#include <eigenpy/eigenpy.hpp>
+
+#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
+
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/geometry.hpp"
+
+
+namespace se3
+{
+ namespace python
+ {
+ namespace bp = boost::python;
+
+ struct GeometryObjectPythonVisitor
+ : public boost::python::def_visitor< GeometryObjectPythonVisitor >
+ {
+ typedef eigenpy::UnalignedEquivalent<SE3>::type SE3_fx;
+ typedef Model::Index Index;
+ typedef Model::JointIndex JointIndex;
+ typedef Model::FrameIndex FrameIndex;
+
+ public:
+
+ static PyObject* convert(Frame const& f)
+ {
+ return boost::python::incref(boost::python::object(f).ptr());
+ }
+
+ template<class PyClass>
+ void visit(PyClass& cl) const
+ {
+ cl
+ .add_property("name", &GeometryObjectPythonVisitor::getName, &GeometryObjectPythonVisitor::setName)
+ .add_property("parent_id", &GeometryObjectPythonVisitor::getParentId, &GeometryObjectPythonVisitor::setParentId)
+ .add_property("placement", &GeometryObjectPythonVisitor::getPlacementWrtParentJoint, &GeometryObjectPythonVisitor::setMeshPath)
+ .add_property("mesh_path", &GeometryObjectPythonVisitor::getMeshPath, &GeometryObjectPythonVisitor::setMeshPath)
+ ;
+ }
+
+
+ static std::string getName( const GeometryObject & self) { return self.name; }
+ static void setName(GeometryObject & self, const std::string & name) { self.name = name; }
+ static JointIndex getParentId( const GeometryObject & self) { return self.parent; }
+ static void setParentId(GeometryObject & self, const JointIndex parent) { self.parent = parent; }
+ static SE3_fx getPlacementWrtParentJoint( const GeometryObject & self) { return self.placement; }
+ static void setPlacementWrtParentJoint(GeometryObject & self, const SE3_fx & placement) { self.placement = placement; }
+ static std::string getMeshPath( const GeometryObject & self) { return self.mesh_path; }
+ static void setMeshPath(GeometryObject & self, const std::string & mesh_path) { self.mesh_path = mesh_path; }
+
+ static void expose()
+ {
+ bp::class_<GeometryObject>("GeometryObject",
+ "A wrapper on a collision geometry including its parent joint, placement in parent frame.\n\n",
+ bp::no_init
+ )
+ .def(GeometryObjectPythonVisitor())
+ ;
+
+ // bp::to_python_converter< GeometryObject,GeometryObjectPythonVisitor >();
+
+ bp::class_< std::vector<GeometryObject> >("StdVec_GeometryObject")
+ .def(bp::vector_indexing_suite< std::vector<GeometryObject> >());
+ }
+
+
+ };
+
+
+ } // namespace python
+} // namespace se3
+
+#endif // ifndef __se3_python_geometry_object_hpp__
+
diff --git a/src/python/python.cpp b/src/python/python.cpp
index ee1bee3ed..efff5180b 100644
--- a/src/python/python.cpp
+++ b/src/python/python.cpp
@@ -32,6 +32,7 @@
#include "pinocchio/python/explog.hpp"
#ifdef WITH_HPP_FCL
+#include "pinocchio/python/geometry-object.hpp"
#include "pinocchio/python/geometry-model.hpp"
#include "pinocchio/python/geometry-data.hpp"
#endif
@@ -69,7 +70,8 @@ namespace se3
FramePythonVisitor::expose();
ModelPythonVisitor::expose();
DataPythonVisitor::expose();
-#ifdef WITH_HPP_FCL
+#ifdef WITH_HPP_FCL
+ GeometryObjectPythonVisitor::expose();
CollisionPairPythonVisitor::expose();
GeometryModelPythonVisitor::expose();
GeometryDataPythonVisitor::expose();
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index 133f53ee4..7ad9de83f 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -129,12 +129,12 @@ struct Distance_t
#endif
std::ostream& operator<<(std::ostream& os, const std::pair < se3::Model, se3::GeometryModel >& robot)
{
- os << "Nb collision objects = " << robot.second.ngeom << std::endl;
+ os << "Nb collision objects = " << robot.second.ncollisions << std::endl;
- for(se3::GeometryModel::Index i=0;i<(se3::GeometryModel::Index)(robot.second.ngeom);++i)
+ for(se3::GeometryModel::Index i=0;i<(se3::GeometryModel::Index)(robot.second.ncollisions);++i)
{
- os << "Object n " << i << " : " << robot.second.geom_names[i] << ": attached to joint = " << robot.second.geom_parents[i]
- << "=" << robot.first.getJointName(robot.second.geom_parents[i]) << std::endl;
+ os << "Object n " << i << " : " << robot.second.collision_objects[i].name << ": attached to joint = " << robot.second.collision_objects[i].parent
+ << "=" << robot.first.getJointName(robot.second.collision_objects[i].parent) << std::endl;
}
return os;
}
@@ -156,11 +156,11 @@ BOOST_AUTO_TEST_CASE ( simple_boxes )
boost::shared_ptr<fcl::Box> Sample(new fcl::Box(1));
fcl::CollisionObject box1(Sample, fcl::Transform3f());
- model_geom.addGeomObject(model.getJointId("planar1_joint"),box1, SE3::Identity(), "ff1_collision_object");
+ model_geom.addCollisionObject(model.getJointId("planar1_joint"),box1, SE3::Identity(), "ff1_collision_object", "");
boost::shared_ptr<fcl::Box> Sample2(new fcl::Box(1));
fcl::CollisionObject box2(Sample, fcl::Transform3f());
- model_geom.addGeomObject(model.getJointId("planar2_joint"),box2, SE3::Identity(), "ff2_collision_object");
+ model_geom.addCollisionObject(model.getJointId("planar2_joint"),box2, SE3::Identity(), "ff2_collision_object", "");
se3::Data data(model);
se3::GeometryData data_geom(data, model_geom);
@@ -448,9 +448,9 @@ JointPositionsMap_t fillPinocchioJointPositions(const se3::Data & data)
GeometryPositionsMap_t fillPinocchioGeometryPositions(const se3::GeometryData & data_geom)
{
GeometryPositionsMap_t result;
- for (std::size_t i = 0; i < data_geom.model_geom.ngeom ; ++i)
+ for (std::size_t i = 0; i < data_geom.model_geom.ncollisions ; ++i)
{
- result[data_geom.model_geom.getGeomName(i)] = data_geom.oMg[i];
+ result[data_geom.model_geom.getCollisionName(i)] = data_geom.oMg_collisions[i];
}
return result;
}
--
GitLab