diff --git a/CMakeLists.txt b/CMakeLists.txt
index 13557e75d3051924c04beaadd4dc231d81b36207..62ad671b468ebbc1750c2f7a1f4d6a811c527cf3 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 cb37daad83ff5e5b5552ba98d90946f3121896ae..31a825b52ca6f70bf262d8887325c0381534d400 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 ebe9444549553667a60ce0dbefa709ddadf1a0e9..ef5349e9fe6bc24d594c1cb977410adf92675d30 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 b325d6c65e858c7b70b1d767c036f098a08708e2..5c4cd8c6ea41097d0d01a97e7898a7cbe8277ad0 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 78449501104535bba7191db94728b4dd8fbfddc8..d0aecf24853972277feb0c36b089d7f23bb0856e 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 ca12a540dfc4cf0a6f9cbbae78098110429095ac..900b94e8db688d6c55dac12e1cafd84c089b1bbb 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 1e1478711e2ff1dc92bf698657c1c35ca5d03b0f..876e87487af1d70171795928b65c30a850d888f1 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 47a16206046004c478523c319ff9e5c82825ff53..fb4c7d44bf2a62fbd7cf3b278589e3f9bf6c64ca 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 0000000000000000000000000000000000000000..1cf84df10d8c9b49049e799e45a613178c841870
--- /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 ee1bee3ed76ec3705e6d71d9c1c0b00e602d40b7..efff5180b11cc8c6d64733a555fd1e77e6caac02 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 133f53ee4faeee4aa0d36102772e4fd99b8a6b76..7ad9de83f34904ef56b93d0ff1c80eea1830e23a 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;
}