Commit 463a9d65 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Handle the case when a link has several collision geometries.

parent 6ac9790e
......@@ -702,7 +702,19 @@ namespace hpp
void Parser::addSolidComponentToJoint (const UrdfLinkConstPtrType& link,
const JointPtr_t& joint)
{
boost::shared_ptr < ::urdf::Collision> collision = link->collision;
typedef std::vector < boost::shared_ptr < ::urdf::Collision > >
Collisions_t;
Collisions_t collisions (link->collision_array);
std::size_t objectId = 0;
for (Collisions_t::iterator it = collisions.begin ();
it != collisions.end (); ++it) {
std::ostringstream oss;
oss << link->name << "_" << objectId;
std::string objectName (oss.str ());
++objectId;
boost::shared_ptr < ::urdf::Collision> collision = *it;
fcl::CollisionGeometryPtr_t geometry;
// Handle the case where collision geometry is a mesh
......@@ -711,7 +723,6 @@ namespace hpp
= boost::dynamic_pointer_cast< ::urdf::Mesh> (collision->geometry);
std::string collisionFilename = collisionGeometry->filename;
::urdf::Vector3 scale = collisionGeometry->scale;
// Create FCL mesh by parsing Collada file.
PolyhedronPtrType polyhedron (new PolyhedronType);
......@@ -755,14 +766,13 @@ namespace hpp
}
else throw std::runtime_error (std::string ("Unknown geometry type :"));// +
//collision->geometry->type);
// Compute body position in world frame.
MatrixHomogeneousType position =
computeBodyAbsolutePosition (link, collision->origin);
if (geometry) {
CollisionObjectPtr_t collisionObject (CollisionObject::create
(geometry, position, prependPrefix (link->name)));
(geometry, position, prependPrefix (objectName)));
// Add solid component.
Body* body = joint->linkedBody ();
......@@ -771,6 +781,7 @@ namespace hpp
hppDout (info, "Adding object " << collisionObject->name ()
<< " to body " << body->name ());
}
}
}
void Parser::fillGaze ()
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment