Unverified Commit 66f0b390 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1588 from florent-lamiraux/devel

[multibody/geometry] Add method to remove an object.
parents 16ec0204 71818ae5
Pipeline #17287 passed with stage
in 249 minutes and 27 seconds
......@@ -65,6 +65,15 @@ namespace pinocchio
*/
GeomIndex addGeometryObject(const GeometryObject & object);
/**
* @brief Remove a GeometryObject
*
* @param[in] name Name of the GeometryObject
*
* @node Remove also the collision pairs that contain the object.
*/
void removeGeometryObject(const std::string& name);
/**
* @brief Return the index of a GeometryObject given by its name.
*
......
......@@ -104,6 +104,30 @@ namespace pinocchio
return idx;
}
inline void GeometryModel::removeGeometryObject(const std::string& name)
{
GeomIndex i=0;
GeometryObjectVector::iterator it;
for (it=geometryObjects.begin(); it!=geometryObjects.end(); ++it, ++i){
if (it->name == name){
break;
}
}
PINOCCHIO_THROW(it != geometryObjects.end(),std::invalid_argument, (std::string("Object ") + name + std::string(" does not belong to model")).c_str());
// Remove all collision pairs that contain i as first or second index,
for (CollisionPairVector::iterator itCol = collisionPairs.begin(); itCol != collisionPairs.end(); ++itCol){
if ((itCol->first == i) || (itCol->second == i)) {
itCol = collisionPairs.erase(itCol); itCol--;
} else {
// Indices of objects after the one that is removed should be decreased by one.
if (itCol->first > i) itCol->first--;
if (itCol->second > i) itCol->second--;
}
}
geometryObjects.erase(it);
ngeoms--;
}
inline GeomIndex GeometryModel::getGeometryId(const std::string & name) const
{
#if BOOST_VERSION / 100 % 1000 >= 60
......
......@@ -116,6 +116,20 @@ BOOST_AUTO_TEST_CASE ( simple_boxes )
pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
BOOST_CHECK(computeCollision(geomModel,geomData,0) == false);
geomModel.removeGeometryObject("ff2_collision_object");
geomData = pinocchio::GeometryData(geomModel);
BOOST_CHECK(geomModel.ngeoms == 2);
BOOST_CHECK(geomModel.geometryObjects.size() == 2);
BOOST_CHECK(geomModel.collisionPairs.size() == 1);
BOOST_CHECK((geomModel.collisionPairs[0].first == 0 && geomModel.collisionPairs[0].second == 1) ||
(geomModel.collisionPairs[0].first == 1 && geomModel.collisionPairs[0].second == 0));
BOOST_CHECK(geomData.activeCollisionPairs.size() == 1);
BOOST_CHECK(geomData.distanceRequests.size() == 1);
BOOST_CHECK(geomData.distanceResults.size() == 1);
BOOST_CHECK(geomData.distanceResults.size() == 1);
BOOST_CHECK(geomData.collisionResults.size() == 1);
}
BOOST_AUTO_TEST_CASE ( loading_model )
......
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