From 1873999de6cacd23765f478ce84077742a9314fc Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Tue, 12 Jul 2016 17:13:54 +0200
Subject: [PATCH] [C++] Remove duplicata of collision/visual in geometry
---
benchmark/timings-geometry.cpp | 2 +-
python/bindings_geometry_object.py | 8 +-
src/algorithm/collisions.hpp | 17 ++---
src/multibody/geometry.hpp | 109 ++++++++-------------------
src/multibody/geometry.hxx | 116 ++++++++++-------------------
src/parsers/urdf.hpp | 6 +-
src/parsers/urdf/geometry.hxx | 98 ++++++++++++++----------
src/parsers/utils.hpp | 9 ++-
src/python/geometry-data.hpp | 11 +--
src/python/geometry-model.hpp | 57 ++++++--------
src/python/parsers.hpp | 15 ++--
src/python/robot_wrapper.py | 26 ++++---
unittest/geom.cpp | 26 +++----
13 files changed, 213 insertions(+), 287 deletions(-)
diff --git a/benchmark/timings-geometry.cpp b/benchmark/timings-geometry.cpp
index 6faa9ab49..98dd427f4 100644
--- a/benchmark/timings-geometry.cpp
+++ b/benchmark/timings-geometry.cpp
@@ -71,7 +71,7 @@ int main()
package_dirs.push_back(romeo_meshDir);
se3::Model model = se3::urdf::buildModel(romeo_filename, se3::JointModelFreeFlyer());
- se3::GeometryModel geom_model = se3::urdf::buildGeom(model, romeo_filename, package_dirs);
+ se3::GeometryModel geom_model = se3::urdf::buildGeom(model, romeo_filename, package_dirs, COLLISION);
Data data(model);
GeometryData geom_data(data, geom_model);
diff --git a/python/bindings_geometry_object.py b/python/bindings_geometry_object.py
index 00a64978c..bbb5a8fe8 100644
--- a/python/bindings_geometry_object.py
+++ b/python/bindings_geometry_object.py
@@ -25,13 +25,13 @@ class TestGeometryObjectBindings(unittest.TestCase):
robot = RobotWrapper(romeo_model_path, hint_list, se3.JointModelFreeFlyer())
def test_name_get_set(self):
- col = self.robot.geometry_model.collision_objects[1]
+ col = self.robot.collision_model.geometry_objects[1]
self.assertTrue(col.name == 'LHipPitchLink')
col.name = 'new_collision_name'
self.assertTrue(col.name == 'new_collision_name')
def test_parent_get_set(self):
- col = self.robot.geometry_model.collision_objects[1]
+ col = self.robot.collision_model.geometry_objects[1]
self.assertTrue(col.parent == 4)
col.parent = 5
self.assertTrue(col.parent == 5)
@@ -39,14 +39,14 @@ class TestGeometryObjectBindings(unittest.TestCase):
def test_placement_get_set(self):
m = se3.SE3(self.m3ones, self.v3zero)
new_m = se3.SE3(rand([3,3]), rand(3))
- col = self.robot.geometry_model.collision_objects[1]
+ col = self.robot.collision_model.geometry_objects[1]
self.assertTrue(np.allclose(col.placement.homogeneous,m.homogeneous))
col.placement = new_m
self.assertTrue(np.allclose(col.placement.homogeneous , new_m.homogeneous))
def test_meshpath_get(self):
expected_mesh_path = os.path.join(self.pinocchio_models_dir,'meshes/romeo/collision/LHipPitch.dae')
- col = self.robot.geometry_model.collision_objects[1]
+ col = self.robot.collision_model.geometry_objects[1]
self.assertTrue(col.mesh_path == expected_mesh_path)
if __name__ == '__main__':
diff --git a/src/algorithm/collisions.hpp b/src/algorithm/collisions.hpp
index 31a825b52..b6c6ad0f3 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 geometry objects (both collision's and visual's one).
+ /// \brief Apply a forward kinematics and update the placement of the geometry objects.
///
/// \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 geometry objects according to the current joint placements contained in data. (both collision's and visual's one)
+ /// \brief Update the placement of the geometry objects according to the current joint placements contained in data.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
@@ -102,16 +102,11 @@ namespace se3
GeometryData & data_geom
)
{
- for (GeometryData::GeomIndex i=0; i < (GeometryData::GeomIndex) data_geom.model_geom.ncollisions; ++i)
+ for (GeometryData::GeomIndex i=0; i < (GeometryData::GeomIndex) data_geom.model_geom.ngeoms; ++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);
+ const Model::JointIndex & parent = model_geom.geometry_objects[i].parent;
+ data_geom.oMg_geometries[i] = (data.oMi[parent] * model_geom.geometry_objects[i].placement);
+ data_geom.oMg_fcl_geometries[i] = toFclTransform3f(data_geom.oMg_geometries[i]);
}
}
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index 76c55b4d6..ee211562a 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -252,17 +252,11 @@ struct GeometryObject
/// \brief A const reference to the reference model.
const se3::Model & model;
- /// \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 The number of GeometryObjects
+ Index ngeoms;
/// \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::vector<GeometryObject> geometry_objects;
/// \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
@@ -274,100 +268,61 @@ struct GeometryObject
GeometryModel(const se3::Model & model)
: model(model)
- , ncollisions(0)
- , nvisuals(0)
- , collision_objects()
- , visual_objects()
+ , ngeoms(0)
+ , geometry_objects()
, innerObjects()
, outerObjects()
{}
~GeometryModel() {};
-
/**
- * @brief Add a geometry object whose type is COLLISION to a GeometryModel
+ * @brief Add a geometry object of a given type 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
+ * @param[in] type The type of the GeometryObject
*
- * @return The index of the new added GeometryObject in collision_objects
+ * @return The index of the new added GeometryObject in geometry_objects
*/
- inline 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
- */
- inline GeomIndex addVisualObject(const JointIndex parent, const fcl::CollisionObject & co,
- const SE3 & placement, const std::string & geom_name = "",
- const std::string & mesh_path = "");
+ inline GeomIndex addGeometryObject(const JointIndex parent, const fcl::CollisionObject & co,
+ const SE3 & placement, const std::string & geom_name = "",
+ const std::string & mesh_path = "", const GeometryType type = NONE) throw(std::invalid_argument);
+
+
/**
- * @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.
+ * @brief Return the index of a GeometryObject 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;
+ GeomIndex getGeometryId(const std::string & name) const;
+
/**
- * @brief Check if a GeometryObject of type VISUAL given by its name exists.
+ * @brief Check if a GeometryObject given by its name exists.
*
* @param[in] name Name of the GeometryObject
*
- * @return True if the GeometryObject exists in the visual_objects.
+ * @return True if the GeometryObject exists in the geometry_objects.
*/
- bool existVisualName(const std::string & name) const;
+ bool existGeometryName(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.
+ * @brief Get the name of a GeometryObject given by its index.
*
* @param[in] index Index of the GeometryObject
*
* @return Name of the GeometryObject
*/
- const std::string & getVisualName(const GeomIndex index) const;
+ const std::string & getGeometryName(const GeomIndex index) const;
+
/**
* @brief Associate a GeometryObject of type COLLISION to a joint's inner objects list
@@ -408,21 +363,16 @@ struct GeometryObject
const GeometryModel & model_geom;
///
- /// \brief Vector gathering the SE3 placements of the collision objects relative to the world.
+ /// \brief Vector gathering the SE3 placements of the geometry objects relative to the world.
/// See updateGeometryPlacements to update the placements.
///
- std::vector<se3::SE3> oMg_collisions;
+ std::vector<se3::SE3> oMg_geometries;
- ///
- /// \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_collisions;
+ std::vector<fcl::Transform3f> oMg_fcl_geometries;
///
/// \brief Vector of collision pairs.
/// See addCollisionPair, removeCollisionPair to fill or remove elements in the vector.
@@ -447,15 +397,14 @@ struct GeometryObject
GeometryData(const Data & data, const GeometryModel & model_geom)
: data_ref(data)
, model_geom(model_geom)
- , oMg_collisions(model_geom.ncollisions)
- , oMg_visuals(model_geom.nvisuals)
- , oMg_fcl_collisions(model_geom.ncollisions)
+ , oMg_geometries(model_geom.ngeoms)
+ , oMg_fcl_geometries(model_geom.ngeoms)
, collision_pairs()
, nCollisionPairs(0)
, distance_results()
, collision_results()
{
- const std::size_t num_max_collision_pairs = (model_geom.ncollisions * (model_geom.ncollisions-1))/2;
+ const std::size_t num_max_collision_pairs = (model_geom.ngeoms * (model_geom.ngeoms-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 7c69015b9..c36fe8e17 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -42,82 +42,55 @@
namespace se3
{
- inline 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 ++);
+ inline GeometryModel::GeomIndex GeometryModel::addGeometryObject(const JointIndex parent,
+ const fcl::CollisionObject & co,
+ const SE3 & placement,
+ const std::string & geom_name,
+ const std::string & mesh_path,
+ const GeometryType type) throw(std::invalid_argument)
+ {
+ if (type == NONE)
+ {
+ const std::string exception_message ("You must specify if the geometryobject you want to add is of type VISUAL or COLLISION");
+ throw std::invalid_argument(exception_message);
+ }
+
+ Index idx = (Index) (ngeoms ++);
- collision_objects.push_back(GeometryObject(COLLISION, geom_name, parent, co,
+ geometry_objects.push_back(GeometryObject(type, geom_name, parent, co,
placement, mesh_path));
addInnerObject(parent, idx);
return idx;
}
- inline GeometryModel::GeomIndex GeometryModel::addVisualObject(const JointIndex parent,
- const fcl::CollisionObject & co,
- const SE3 & placement,
- const std::string & geom_name,
- const std::string & mesh_path)
- {
-
- Index idx = (Index) (nvisuals ++);
- visual_objects.push_back(GeometryObject(VISUAL, geom_name, parent, co,
- placement, mesh_path));
-
- return idx;
- }
-
- inline GeometryModel::GeomIndex GeometryModel::getCollisionId(const std::string & name) const
+ inline GeometryModel::GeomIndex GeometryModel::getGeometryId(const std::string & name) const
{
- std::vector<GeometryObject>::const_iterator it = std::find_if(collision_objects.begin(),
- collision_objects.end(),
+ std::vector<GeometryObject>::const_iterator it = std::find_if(geometry_objects.begin(),
+ geometry_objects.end(),
boost::bind(&GeometryObject::name, _1) == name
);
- return GeometryModel::GeomIndex(it - collision_objects.begin());
+ return GeometryModel::GeomIndex(it - geometry_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::existCollisionName(const std::string & name) const
- {
- return std::find_if(collision_objects.begin(),
- collision_objects.end(),
- boost::bind(&GeometryObject::name, _1) == name) != collision_objects.end();
- }
-
- inline bool GeometryModel::existVisualName(const std::string & name) const
+ inline bool GeometryModel::existGeometryName(const std::string & name) const
{
- return std::find_if(visual_objects.begin(),
- visual_objects.end(),
- boost::bind(&GeometryObject::name, _1) == name) != visual_objects.end();
+ return std::find_if(geometry_objects.begin(),
+ geometry_objects.end(),
+ boost::bind(&GeometryObject::name, _1) == name) != geometry_objects.end();
}
- inline const std::string& GeometryModel::getCollisionName(const GeomIndex index) const
- {
- assert( index < (GeomIndex)collision_objects.size() );
- return collision_objects[index].name;
- }
- inline const std::string& GeometryModel::getVisualName(const GeomIndex index) const
+ inline const std::string& GeometryModel::getGeometryName(const GeomIndex index) const
{
- assert( index < (GeomIndex)visual_objects.size() );
- return visual_objects[index].name;
+ assert( index < (GeomIndex)geometry_objects.size() );
+ return geometry_objects[index].name;
}
+
inline void GeometryModel::addInnerObject(const JointIndex joint_id, const GeomIndex inner_object)
{
if (std::find(innerObjects[joint_id].begin(),
@@ -140,18 +113,11 @@ namespace se3
inline std::ostream & operator<< (std::ostream & os, const GeometryModel & model_geom)
{
- os << "Nb collision objects = " << model_geom.ncollisions << std::endl;
-
- for(GeometryModel::Index i=0;i<(GeometryModel::Index)(model_geom.ncollisions);++i)
- {
- os << model_geom.collision_objects[i] <<std::endl;
- }
-
- os << "Nb visual objects = " << model_geom.nvisuals << std::endl;
+ os << "Nb geometry objects = " << model_geom.ngeoms << std::endl;
- for(GeometryModel::Index i=0;i<(GeometryModel::Index)(model_geom.nvisuals);++i)
+ for(GeometryModel::Index i=0;i<(GeometryModel::Index)(model_geom.ngeoms);++i)
{
- os << model_geom.visual_objects[i] <<std::endl;
+ os << model_geom.geometry_objects[i] <<std::endl;
}
return os;
@@ -172,7 +138,7 @@ namespace se3
inline void GeometryData::addCollisionPair (const GeomIndex co1, const GeomIndex co2)
{
assert ( co1 != co2);
- assert ( co2 < model_geom.ncollisions);
+ assert ( co2 < model_geom.ngeoms);
CollisionPair_t pair(co1, co2);
addCollisionPair(pair);
@@ -180,7 +146,7 @@ namespace se3
inline void GeometryData::addCollisionPair (const CollisionPair_t & pair)
{
- assert(pair.second < model_geom.ncollisions);
+ assert(pair.second < model_geom.ngeoms);
if (!existCollisionPair(pair))
{
@@ -192,16 +158,16 @@ namespace se3
inline void GeometryData::addAllCollisionPairs()
{
removeAllCollisionPairs();
- 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)
+ collision_pairs.reserve((model_geom.ngeoms * (model_geom.ngeoms-1))/2);
+ for (Index i = 0; i < model_geom.ngeoms; ++i)
+ for (Index j = i+1; j < model_geom.ngeoms; ++j)
addCollisionPair(i,j);
}
inline void GeometryData::removeCollisionPair (const GeomIndex co1, const GeomIndex co2)
{
assert(co1 < co2);
- assert(co2 < model_geom.ncollisions);
+ assert(co2 < model_geom.ngeoms);
assert(existCollisionPair(co1,co2));
removeCollisionPair (CollisionPair_t(co1,co2));
@@ -209,7 +175,7 @@ namespace se3
inline void GeometryData::removeCollisionPair (const CollisionPair_t & pair)
{
- assert(pair.second < model_geom.ncollisions);
+ assert(pair.second < model_geom.ngeoms);
CollisionPairsVector_t::iterator it = std::find(collision_pairs.begin(),
collision_pairs.end(),
@@ -290,8 +256,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].collision_object.collisionGeometry().get(), oMg_fcl_collisions[co1],
- model_geom.collision_objects[co2].collision_object.collisionGeometry().get(), oMg_fcl_collisions[co2],
+ fcl::collide (model_geom.geometry_objects[co1].collision_object.collisionGeometry().get(), oMg_fcl_geometries[co1],
+ model_geom.geometry_objects[co2].collision_object.collisionGeometry().get(), oMg_fcl_geometries[co2],
collisionRequest, collisionResult);
return CollisionResult (collisionResult, co1, co2);
@@ -328,8 +294,8 @@ namespace se3
fcl::DistanceRequest distanceRequest (true, 0, 0, fcl::GST_INDEP);
fcl::DistanceResult result;
- 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],
+ fcl::distance ( model_geom.geometry_objects[co1].collision_object.collisionGeometry().get(), oMg_fcl_geometries[co1],
+ model_geom.geometry_objects[co2].collision_object.collisionGeometry().get(), oMg_fcl_geometries[co2],
distanceRequest, result);
return DistanceResult (result, co1, co2);
diff --git a/src/parsers/urdf.hpp b/src/parsers/urdf.hpp
index 8e63f552a..dcb4b28bb 100644
--- a/src/parsers/urdf.hpp
+++ b/src/parsers/urdf.hpp
@@ -35,6 +35,7 @@
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/shape/geometric_shapes.h>
+#include "pinocchio/multibody/geometry.hpp"
#endif
namespace urdf
@@ -90,12 +91,15 @@ namespace se3
* where to search for models and meshes, typically
* obtained from calling se3::rosPaths()
*
+ *@param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION, or NONE)
+ *
* @return The GeometryModel associated to the urdf file and the given Model.
*
*/
inline GeometryModel buildGeom(const Model & model,
const std::string & filename,
- const std::vector<std::string> & package_dirs = std::vector<std::string> ()) throw (std::invalid_argument);
+ const std::vector<std::string> & package_dirs = std::vector<std::string> (),
+ const GeometryType type = NONE) throw (std::invalid_argument);
#endif
diff --git a/src/parsers/urdf/geometry.hxx b/src/parsers/urdf/geometry.hxx
index c158e948a..2b08e98d6 100644
--- a/src/parsers/urdf/geometry.hxx
+++ b/src/parsers/urdf/geometry.hxx
@@ -134,58 +134,69 @@ namespace se3
* @param model The model to which is the GeometryModel associated
* @param model_geom The GeometryModel where the Collision Objects must be added
* @param[in] package_dirs A vector containing the different directories where to search for packages
- * @param[in] rootJointAdded If a root joint was added at the begining of the urdf kinematic chain by user when constructing the Model
+ * @param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION)
*/
inline void parseTreeForGeom(::urdf::LinkConstPtr link,
const Model & model,
GeometryModel & geom_model,
- const std::vector<std::string> & package_dirs) throw (std::invalid_argument)
+ const std::vector<std::string> & package_dirs,
+ const GeometryType type) 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);
- const std::string & collision_object_name = link_name;
- geom_model.addCollisionObject(model.getFrameParent(link_name), collision_object, geomPlacement, collision_object_name, mesh_path);
- }
- } // if(link->collision)
-
- if(link->visual)
+
+ switch(type)
{
- 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);
- const std::string & visual_object_name = link_name;
- geom_model.addVisualObject(model.getFrameParent(link_name), visual_object, geomPlacement, visual_object_name, mesh_path);
- }
- } // if(link->visual)
+ case COLLISION:
+ // 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);
+ const std::string & collision_object_name = link_name;
+ geom_model.addGeometryObject(model.getFrameParent(link_name), collision_object, geomPlacement, collision_object_name, mesh_path, COLLISION);
+ }
+ } // if(link->collision)
+ break;
+ case VISUAL:
+ 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);
+ const std::string & visual_object_name = link_name;
+ geom_model.addGeometryObject(model.getFrameParent(link_name), visual_object, geomPlacement, visual_object_name, mesh_path, VISUAL);
+ }
+ } // if(link->visual)
+ break;
+ default:
+ break;
+ }
+
BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
{
- parseTreeForGeom(child, model, geom_model, package_dirs);
+ parseTreeForGeom(child, model, geom_model, package_dirs,type);
}
}
@@ -197,8 +208,15 @@ namespace se3
inline GeometryModel buildGeom(const Model & model,
const std::string & filename,
- const std::vector<std::string> & package_dirs) throw(std::invalid_argument)
+ const std::vector<std::string> & package_dirs,
+ const GeometryType type) throw(std::invalid_argument)
{
+ if (type == NONE)
+ {
+ const std::string exception_message ("You must specify if you want to load VISUAL or COLLISION meshes");
+ throw std::invalid_argument(exception_message);
+ }
+
GeometryModel model_geom(model);
std::vector<std::string> hint_directories(package_dirs);
@@ -213,7 +231,7 @@ namespace se3
}
::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
- details::parseTreeForGeom(urdfTree->getRoot(), model, model_geom, hint_directories);
+ details::parseTreeForGeom(urdfTree->getRoot(), model, model_geom, hint_directories,type);
return model_geom;
}
diff --git a/src/parsers/utils.hpp b/src/parsers/utils.hpp
index e686012a0..87e33ab4b 100644
--- a/src/parsers/utils.hpp
+++ b/src/parsers/utils.hpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2015 CNRS
+// Copyright (c) 2015 - 2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
@@ -15,6 +15,9 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
+#ifndef __se3_parsers_utils_hpp__
+#define __se3_parsers_utils_hpp__
+
#include <iostream>
#include <limits>
#include <sstream>
@@ -46,7 +49,7 @@ namespace se3
///
/// \return The type of the extension of the model file
///
- ModelFileExtensionType checkModelFileExtension (const std::string & filename)
+ inline ModelFileExtensionType checkModelFileExtension (const std::string & filename)
{
const std::string extension = filename.substr(filename.find_last_of(".") + 1);
@@ -92,3 +95,5 @@ namespace se3
}
} // namespace se3
+
+#endif // __se3_parsers_utils_hpp__
\ No newline at end of file
diff --git a/src/python/geometry-data.hpp b/src/python/geometry-data.hpp
index 876e87487..8f4d53b74 100644
--- a/src/python/geometry-data.hpp
+++ b/src/python/geometry-data.hpp
@@ -94,14 +94,10 @@ namespace se3
.add_property("nCollisionPairs", &GeometryDataPythonVisitor::nCollisionPairs)
- .add_property("oMg_collisions",
- bp::make_function(&GeometryDataPythonVisitor::oMg_collisions,
+ .add_property("oMg_geometries",
+ bp::make_function(&GeometryDataPythonVisitor::oMg_geometries,
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<>()),
@@ -169,8 +165,7 @@ namespace se3
static Index nCollisionPairs(const GeometryDataHandler & m ) { return m->nCollisionPairs; }
- 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<SE3> & oMg_geometries(GeometryDataHandler & m) { return m->oMg_geometries; }
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 fb4c7d44b..f2e13dcff 100644
--- a/src/python/geometry-model.hpp
+++ b/src/python/geometry-model.hpp
@@ -63,23 +63,14 @@ namespace se3
void visit(PyClass& cl) const
{
cl
- .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("visual_objects",
- bp::make_function(&GeometryModelPythonVisitor::visual_objects,
- bp::return_internal_reference<>()) )
-
+ .add_property("ngeoms", &GeometryModelPythonVisitor::ngeoms)
+ .def("getGeometryId",&GeometryModelPythonVisitor::getGeometryId)
+ .def("existGeometryName",&GeometryModelPythonVisitor::existGeometryName)
+ .def("getGeometryName",&GeometryModelPythonVisitor::getGeometryName)
+ .add_property("geometry_objects",
+ bp::make_function(&GeometryModelPythonVisitor::geometry_objects,
+ bp::return_internal_reference<>()) )
.def("__str__",&GeometryModelPythonVisitor::toString)
.def("BuildGeometryModel",&GeometryModelPythonVisitor::maker_default)
@@ -87,27 +78,17 @@ namespace se3
;
}
- static GeometryModel::Index ncollisions( GeometryModelHandler & m ) { return m->ncollisions; }
- static GeometryModel::Index nvisuals( GeometryModelHandler & m ) { return m->nvisuals; }
+ static GeometryModel::Index ngeoms( GeometryModelHandler & m ) { return m->ngeoms; }
- 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 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; }
+ static Model::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 std::string getGeometryName(const GeometryModelHandler & gmodelPtr, const GeomIndex index)
+ { return gmodelPtr->getGeometryName(index);}
+ static std::vector<GeometryObject> & geometry_objects( GeometryModelHandler & m ) { return m->geometry_objects; }
+
static GeometryModelHandler maker_default(const ModelHandler & model)
@@ -123,6 +104,12 @@ namespace se3
static void expose()
{
+ bp::enum_<GeometryType>("GeometryType")
+ .value("VISUAL",VISUAL)
+ .value("COLLISION",COLLISION)
+ .value("NONE",NONE)
+ ;
+
bp::class_<GeometryModelHandler>("GeometryModel",
"Geometry model (const)",
bp::no_init)
diff --git a/src/python/parsers.hpp b/src/python/parsers.hpp
index 284cdc62e..ab0d7e9fe 100644
--- a/src/python/parsers.hpp
+++ b/src/python/parsers.hpp
@@ -89,10 +89,12 @@ namespace se3
static GeometryModelHandler
buildGeomFromUrdf(const ModelHandler & model,
- const std::string & filename
+ const std::string & filename,
+ const GeometryType type
)
{
- GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename));
+ std::vector<std::string> hints;
+ GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename,hints, type));
return GeometryModelHandler(geometry_model, true);
}
@@ -100,10 +102,11 @@ namespace se3
static GeometryModelHandler
buildGeomFromUrdf(const ModelHandler & model,
const std::string & filename,
- std::vector<std::string> & package_dirs
+ std::vector<std::string> & package_dirs,
+ const GeometryType type
)
{
- GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename, package_dirs));
+ GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename, package_dirs, type));
return GeometryModelHandler(geometry_model, true);
}
@@ -159,14 +162,14 @@ namespace se3
bp::to_python_converter<std::pair<ModelHandler, GeometryModelHandler>, PairToTupleConverter<ModelHandler, GeometryModelHandler> >();
bp::def("buildGeomFromUrdf",
- static_cast <GeometryModelHandler (*) (const ModelHandler &, const std::string &, std::vector<std::string> &)> (&ParsersPythonVisitor::buildGeomFromUrdf),
+ static_cast <GeometryModelHandler (*) (const ModelHandler &, const std::string &, 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 ModelHandler &, const std::string &)> (&ParsersPythonVisitor::buildGeomFromUrdf),
+ static_cast <GeometryModelHandler (*) (const ModelHandler &, 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).");
diff --git a/src/python/robot_wrapper.py b/src/python/robot_wrapper.py
index ae8517468..9579d584e 100644
--- a/src/python/robot_wrapper.py
+++ b/src/python/robot_wrapper.py
@@ -33,21 +33,25 @@ class RobotWrapper(object):
self.model_filename = filename
if "buildGeomFromUrdf" not in dir(se3):
- self.geometry_model = None
- self.geometry_data = None
+ self.collision_model = None
+ self.visual_model = None
+ self.collision_data = None
if verbose:
print 'Info: the Geometry Module has not been compiled with Pinocchio. No geometry model and data have been built.'
else:
if package_dirs is None:
- self.geometry_model = se3.buildGeomFromUrdf(self.model, filename)
- self.geometry_data = se3.GeometryData(self.data, self.geometry_model)
+ self.collision_model = se3.buildGeomFromUrdf(self.model, filename)
+ self.visual_model = se3.buildGeomFromUrdf(self.model, filename)
+ self.collision_data = se3.GeometryData(self.data, self.collision_model)
else:
if not all(isinstance(item, basestring) for item in package_dirs):
raise Exception('The list of package directories is wrong. At least one is not a string')
else:
- self.geometry_model = se3.buildGeomFromUrdf(self.model, filename,
- utils.fromListToVectorOfString(package_dirs))
- self.geometry_data = se3.GeometryData(self.data, self.geometry_model)
+ self.collision_model = se3.buildGeomFromUrdf(self.model, filename,
+ utils.fromListToVectorOfString(package_dirs), se3.GeometryType.COLLISION)
+ self.visual_model = se3.buildGeomFromUrdf(self.model, filename,
+ utils.fromListToVectorOfString(package_dirs), se3.GeometryType.VISUAL)
+ self.collision_data = se3.GeometryData(self.data, self.collision_model)
self.v0 = utils.zero(self.nv)
self.q0 = utils.zero(self.nq)
@@ -120,7 +124,7 @@ class RobotWrapper(object):
return se3.computeJacobians(self.model, self.data, q)
def updateGeometryPlacements(self, q):
- se3.updateGeometryPlacements(self.model, self.data, self.geometry_model, self.geometry_data, q)
+ se3.updateGeometryPlacements(self.model, self.data, self.collision_model, self.collision_data, q)
# --- ACCESS TO NAMES ----
@@ -166,7 +170,7 @@ class RobotWrapper(object):
self.viewer.gui.createGroup(nodeName)
# iterate over visuals and create the meshes in the viewer
- for visual in self.geometry_model.visual_objects :
+ for visual in self.visual_model.geometry_objects :
meshName = self.viewerNodeNames(visual)
meshPath = visual.mesh_path
self.viewer.gui.addMesh(meshName, meshPath)
@@ -183,8 +187,8 @@ class RobotWrapper(object):
self.updateGeometryPlacements(q)
- for visual in self.geometry_model.visual_objects :
- M = self.geometry_data.oMg_visuals[self.geometry_model.getVisualId(visual.name)]
+ for visual in self.visual_model.geometry_objects :
+ M = self.collision_data.oMg_geometries[self.visual_model.getGeometryId(visual.name)]
pinocchioConf = utils.se3ToXYZQUAT(M)
viewerConf = utils.XYZQUATToViewerConfiguration(pinocchioConf)
self.viewer.gui.applyConfiguration(self.viewerNodeNames(visual), viewerConf)
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index 1ef097e09..18f24bf55 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -128,12 +128,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.ncollisions << std::endl;
+ os << "Nb collision objects = " << robot.second.ngeoms << std::endl;
- for(se3::GeometryModel::Index i=0;i<(se3::GeometryModel::Index)(robot.second.ncollisions);++i)
+ for(se3::GeometryModel::Index i=0;i<(se3::GeometryModel::Index)(robot.second.ngeoms);++i)
{
- 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;
+ os << "Object n " << i << " : " << robot.second.geometry_objects[i].name << ": attached to joint = " << robot.second.geometry_objects[i].parent
+ << "=" << robot.first.getJointName(robot.second.geometry_objects[i].parent) << std::endl;
}
return os;
}
@@ -155,11 +155,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.addCollisionObject(model.getJointId("planar1_joint"),box1, SE3::Identity(), "ff1_collision_object", "");
+ model_geom.addGeometryObject(model.getJointId("planar1_joint"),box1, SE3::Identity(), "ff1_collision_object", "", COLLISION);
boost::shared_ptr<fcl::Box> Sample2(new fcl::Box(1));
fcl::CollisionObject box2(Sample, fcl::Transform3f());
- model_geom.addCollisionObject(model.getJointId("planar2_joint"),box2, SE3::Identity(), "ff2_collision_object", "");
+ model_geom.addGeometryObject(model.getJointId("planar2_joint"),box2, SE3::Identity(), "ff2_collision_object", "", COLLISION);
se3::Data data(model);
se3::GeometryData data_geom(data, model_geom);
@@ -209,7 +209,7 @@ BOOST_AUTO_TEST_CASE ( loading_model )
package_dirs.push_back(meshDir);
Model model = se3::urdf::buildModel(filename, se3::JointModelFreeFlyer());
- GeometryModel geometry_model = se3::urdf::buildGeom(model, filename, package_dirs);
+ GeometryModel geometry_model = se3::urdf::buildGeom(model, filename, package_dirs, se3::COLLISION);
Data data(model);
GeometryData geometry_data(data, geometry_model);
@@ -249,7 +249,7 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
package_dirs.push_back(meshDir);
se3::Model model = se3::urdf::buildModel(filename, se3::JointModelFreeFlyer());
- se3::GeometryModel geom = se3::urdf::buildGeom(model, filename, package_dirs);
+ se3::GeometryModel geom = se3::urdf::buildGeom(model, filename, package_dirs, COLLISION);
std::cout << model << std::endl;
@@ -357,7 +357,7 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
package_dirs.push_back(meshDir);
se3::Model model = se3::urdf::buildModel(filename, se3::JointModelFreeFlyer());
- se3::GeometryModel geom = se3::urdf::buildGeom(model, filename, package_dirs);
+ se3::GeometryModel geom = se3::urdf::buildGeom(model, filename, package_dirs, COLLISION);
std::cout << model << std::endl;
@@ -421,8 +421,8 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
std::cout << "comparison between " << body1 << " and " << body2 << std::endl;
- se3::DistanceResult dist_pin = data_geom.computeDistance( geom.getCollisionId(body1),
- geom.getCollisionId(body2));
+ se3::DistanceResult dist_pin = data_geom.computeDistance( geom.getGeometryId(body1),
+ geom.getGeometryId(body2));
Distance_t distance_pin(dist_pin.fcl_distance_result);
distance_hpp.checkClose(distance_pin);
@@ -447,9 +447,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.ncollisions ; ++i)
+ for (std::size_t i = 0; i < data_geom.model_geom.ngeoms ; ++i)
{
- result[data_geom.model_geom.getCollisionName(i)] = data_geom.oMg_collisions[i];
+ result[data_geom.model_geom.getGeometryName(i)] = data_geom.oMg_geometries[i];
}
return result;
}
--
GitLab