Commit 24fdaffc authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Merge pull request #1 from nassimeblinlaas/master

 	Added sphere geometry type in Parser::addSolidComponentToJoint
parents a0e5670c 1fe7c345
......@@ -711,33 +711,44 @@ namespace hpp
loadPolyhedronFromResource (collisionFilename, scale, polyhedron);
geometry = polyhedron;
}
// Handle the case where collision geometry is a cylinder
// Use FCL capsules for cylinders
if (collision->geometry->type == ::urdf::Geometry::CYLINDER) {
boost::shared_ptr < ::urdf::Cylinder> collisionGeometry
= boost::dynamic_pointer_cast< ::urdf::Cylinder>
(collision->geometry);
double radius = collisionGeometry->radius;
double length = collisionGeometry->length;
// Create fcl capsule geometry.
geometry = fcl::CollisionGeometryPtr_t
(new fcl::Capsule (radius, length));
else if (collision->geometry->type == ::urdf::Geometry::CYLINDER) {
boost::shared_ptr < ::urdf::Cylinder> collisionGeometry
= boost::dynamic_pointer_cast< ::urdf::Cylinder>
(collision->geometry);
double radius = collisionGeometry->radius;
double length = collisionGeometry->length;
// Create fcl capsule geometry.
geometry = fcl::CollisionGeometryPtr_t
(new fcl::Capsule (radius, length));
}
// Handle the case where collision geometry is a box.
if (collision->geometry->type == ::urdf::Geometry::BOX) {
boost::shared_ptr < ::urdf::Box> collisionGeometry
= boost::dynamic_pointer_cast< ::urdf::Box> (collision->geometry);
else if (collision->geometry->type == ::urdf::Geometry::BOX) {
boost::shared_ptr < ::urdf::Box> collisionGeometry
= boost::dynamic_pointer_cast< ::urdf::Box> (collision->geometry);
double x = collisionGeometry->dim.x;
double y = collisionGeometry->dim.y;
double z = collisionGeometry->dim.z;
geometry = fcl::CollisionGeometryPtr_t (new fcl::Box (x, y, z));
}
// Handle the case where collision geometry is a sphere.
else if (collision->geometry->type == ::urdf::Geometry::SPHERE) {
boost::shared_ptr < ::urdf::Sphere> collisionGeometry
= boost::dynamic_pointer_cast< ::urdf::Sphere> (collision->geometry);
double x = collisionGeometry->dim.x;
double y = collisionGeometry->dim.y;
double z = collisionGeometry->dim.z;
double radius = collisionGeometry->radius;
geometry = fcl::CollisionGeometryPtr_t (new fcl::Sphere (radius));
}
else throw std::runtime_error (std::string ("Unknown geometry type :"));// +
//collision->geometry->type);
geometry = fcl::CollisionGeometryPtr_t (new fcl::Box (x, y, z));
}
// Compute body position in world frame.
MatrixHomogeneousType position =
computeBodyAbsolutePosition (link, collision->origin);
......
Supports Markdown
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