diff --git a/src/algorithm/geometry.hxx b/src/algorithm/geometry.hxx
index 65e2bfb119f808d679fc7718a9f218e12d4e8126..b747f4df533a8c7f95a741a0bf1cf3be8da87435 100644
--- a/src/algorithm/geometry.hxx
+++ b/src/algorithm/geometry.hxx
@@ -145,7 +145,7 @@ namespace se3
BOOST_FOREACH(const GeometryObject & geom,geomModel.geometryObjects)
{
const boost::shared_ptr<const fcl::CollisionGeometry> & fcl
- = geom.collision_object.collisionGeometry();
+ = geom.collision_geometry;
const SE3 & jMb = geom.placement; // placement in joint.
double radius = geomData.radius[geom.parent];
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index 95a48f1773c55ee328137194f28d4baef9ecadcb..a48a41d41a99347eac1bc555bcc8f778136e5698 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -187,7 +187,7 @@ struct GeometryObject
JointIndex parent;
/// \brief The actual cloud of points representing the collision mesh of the object
- fcl::CollisionObject collision_object;
+ boost::shared_ptr<fcl::CollisionGeometry> collision_geometry;
/// \brief Position of geometry object in parent joint's frame
SE3 placement;
@@ -196,11 +196,11 @@ struct GeometryObject
std::string mesh_path;
- GeometryObject(const std::string & name, const JointIndex parent, const fcl::CollisionObject & collision,
+ GeometryObject(const std::string & name, const JointIndex parent, const boost::shared_ptr<fcl::CollisionGeometry> & collision,
const SE3 & placement, const std::string & mesh_path)
: name(name)
, parent(parent)
- , collision_object(collision)
+ , collision_geometry(collision)
, placement(placement)
, mesh_path(mesh_path)
{}
@@ -209,7 +209,7 @@ struct GeometryObject
{
name = other.name;
parent = other.parent;
- collision_object = other.collision_object;
+ collision_geometry = other.collision_geometry;
placement = other.placement;
mesh_path = other.mesh_path;
return *this;
@@ -221,7 +221,7 @@ struct GeometryObject
{
return ( lhs.name == rhs.name
&& lhs.parent == rhs.parent
- && lhs.collision_object == rhs.collision_object
+ && lhs.collision_geometry == rhs.collision_geometry
&& lhs.placement == rhs.placement
&& lhs.mesh_path == rhs.mesh_path
);
@@ -231,7 +231,7 @@ struct GeometryObject
{
os << "Name: \t \n" << geom_object.name << "\n"
<< "Parent ID: \t \n" << geom_object.parent << "\n"
- // << "collision object: \t \n" << geom_object.collision_object << "\n"
+ // << "collision object: \t \n" << geom_object.collision_geometry << "\n"
<< "Position in parent frame: \t \n" << geom_object.placement << "\n"
<< "Absolute path to mesh file: \t \n" << geom_object.mesh_path << "\n"
<< std::endl;
@@ -271,17 +271,17 @@ struct GeometryObject
~GeometryModel() {};
/**
- * @brief Add a geometry object of a given type to a GeometryModel
+ * @brief Add a geometry object to a GeometryModel
*
* @param[in] parent Index of the parent joint
- * @param[in] co The actual fcl CollisionObject
+ * @param[in] co The actual fcl CollisionGeometry
* @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 geometryObjects
*/
- inline GeomIndex addGeometryObject(const JointIndex parent, const fcl::CollisionObject & co,
+ inline GeomIndex addGeometryObject(const JointIndex parent, const boost::shared_ptr<fcl::CollisionGeometry> & co,
const SE3 & placement, const std::string & geom_name = "",
const std::string & mesh_path = "") throw(std::invalid_argument);
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
index 26f97e26033e1a2482ac8bf1318bcd261aa95ec4..b8f08b879f511348de83c056614a56402f62ea98 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -42,7 +42,7 @@ namespace se3
{
inline GeometryModel::GeomIndex GeometryModel::addGeometryObject(const JointIndex parent,
- const fcl::CollisionObject & co,
+ const boost::shared_ptr<fcl::CollisionGeometry> & co,
const SE3 & placement,
const std::string & geom_name,
const std::string & mesh_path) throw(std::invalid_argument)
@@ -246,8 +246,8 @@ namespace se3
fcl::CollisionRequest collisionRequest (1, false, false, 1, false, true, fcl::GST_INDEP);
fcl::CollisionResult collisionResult;
- fcl::collide (model_geom.geometryObjects[co1].collision_object.collisionGeometry().get(), oMg_fcl[co1],
- model_geom.geometryObjects[co2].collision_object.collisionGeometry().get(), oMg_fcl[co2],
+ fcl::collide (model_geom.geometryObjects[co1].collision_geometry.get(), oMg_fcl[co1],
+ model_geom.geometryObjects[co2].collision_geometry.get(), oMg_fcl[co2],
collisionRequest, collisionResult);
return CollisionResult (collisionResult, co1, co2);
@@ -284,8 +284,8 @@ namespace se3
fcl::DistanceRequest distanceRequest (true, 0, 0, fcl::GST_INDEP);
fcl::DistanceResult result;
- fcl::distance ( model_geom.geometryObjects[co1].collision_object.collisionGeometry().get(), oMg_fcl[co1],
- model_geom.geometryObjects[co2].collision_object.collisionGeometry().get(), oMg_fcl[co2],
+ fcl::distance ( model_geom.geometryObjects[co1].collision_geometry.get(), oMg_fcl[co1],
+ model_geom.geometryObjects[co2].collision_geometry.get(), oMg_fcl[co2],
distanceRequest, result);
return DistanceResult (result, co1, co2);
diff --git a/src/parsers/urdf/geometry.hxx b/src/parsers/urdf/geometry.hxx
index d5d97710fe8002a9682ca3b8cc950fadb281dd29..c3c3a6feb3f10b5aee48ac1e115b5b6060d74a88 100644
--- a/src/parsers/urdf/geometry.hxx
+++ b/src/parsers/urdf/geometry.hxx
@@ -47,15 +47,15 @@ namespace se3
* @brief Get a fcl::CollisionObject from an urdf geometry, searching
* for it in specified package directories
*
- * @param[in] urdf_geometry The input urdf geometry
+ * @param[in] urdf_geometry A shared pointer on 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 geometry converted as a fcl::CollisionObject
+ * @return A shared pointer on the he geometry converted as a fcl::CollisionGeometry
*/
- inline fcl::CollisionObject retrieveCollisionGeometry(const boost::shared_ptr < ::urdf::Geometry> urdf_geometry,
- const std::vector < std::string > & package_dirs,
- std::string & mesh_path)
+ inline boost::shared_ptr<fcl::CollisionGeometry> retrieveCollisionGeometry(const boost::shared_ptr < ::urdf::Geometry> urdf_geometry,
+ const std::vector < std::string > & package_dirs,
+ std::string & mesh_path)
{
boost::shared_ptr < fcl::CollisionGeometry > geometry;
@@ -120,9 +120,8 @@ namespace se3
{
throw std::runtime_error(std::string("The polyhedron retrived is empty"));
}
- fcl::CollisionObject collisionObject (geometry, fcl::Transform3f());
- return collisionObject;
+ return geometry;
}
/**
@@ -202,12 +201,12 @@ namespace se3
std::size_t objectId = 0;
for (typename std::vector< boost::shared_ptr< T > >::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i)
{
- fcl::CollisionObject geometry_object = retrieveCollisionGeometry((*i)->geometry, package_dirs, mesh_path);
+ const boost::shared_ptr<fcl::CollisionGeometry> geometry = retrieveCollisionGeometry((*i)->geometry, package_dirs, mesh_path);
SE3 geomPlacement = convertFromUrdf((*i)->origin);
std::ostringstream geometry_object_suffix;
geometry_object_suffix << "_" << objectId;
const std::string & geometry_object_name = std::string(link_name + geometry_object_suffix.str());
- geom_model.addGeometryObject(model.getFrameParent(link_name), geometry_object, geomPlacement, geometry_object_name, mesh_path);
+ geom_model.addGeometryObject(model.getFrameParent(link_name), geometry, geomPlacement, geometry_object_name, mesh_path);
++objectId;
}
}
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index 09c8a545ccbe20d1bce706873c9fffd73472df53..19595a3f565c881a2f82c2f42ea050f4afd32968 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -149,13 +149,11 @@ BOOST_AUTO_TEST_CASE ( simple_boxes )
idx = model.addJoint(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),"planar2_joint");
model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"planar2_body");
- boost::shared_ptr<fcl::Box> Sample(new fcl::Box(1));
- fcl::CollisionObject box1(Sample, fcl::Transform3f());
- model_geom.addGeometryObject(model.getJointId("planar1_joint"),box1, SE3::Identity(), "ff1_collision_object", "");
+ boost::shared_ptr<fcl::Box> sample(new fcl::Box(1));
+ model_geom.addGeometryObject(model.getJointId("planar1_joint"),sample, SE3::Identity(), "ff1_collision_object", "");
- boost::shared_ptr<fcl::Box> Sample2(new fcl::Box(1));
- fcl::CollisionObject box2(Sample, fcl::Transform3f());
- model_geom.addGeometryObject(model.getJointId("planar2_joint"),box2, SE3::Identity(), "ff2_collision_object", "");
+ boost::shared_ptr<fcl::Box> sample2(new fcl::Box(1));
+ model_geom.addGeometryObject(model.getJointId("planar2_joint"),sample2, SE3::Identity(), "ff2_collision_object", "");
se3::Data data(model);
se3::GeometryData data_geom(model_geom);