Commit 6e5fa47e authored by Valenza Florian's avatar Valenza Florian
Browse files

[C++][Parser Geom] Look for link that have collision instead of inertia

parent 9ad8a3d8
......@@ -105,33 +105,26 @@ namespace se3
{
// start with first link that is not empty
if(link->inertial)
if(link->collision)
{
::urdf::JointConstPtr joint = link->parent_joint;
if (joint == NULL && rootJointAdded )
{
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.getJointId("root_joint"), collision_object, geomPlacement, collision_object_name);
}
}
if(joint!=NULL)
else if(joint!=NULL)
{
assert(link->getParent()!=NULL);
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.getJointId(joint->name), collision_object, geomPlacement, collision_object_name);
}
geom_model.addGeomObject(model.getJointId(joint->name), collision_object, geomPlacement, collision_object_name);
}
else if (link->getParent() != NULL)
{
......@@ -139,7 +132,7 @@ namespace se3
throw std::invalid_argument(exception_message);
}
}
} // if(link->inertial)
BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
{
......
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