Merge pull request #1588 from florent-lamiraux/devel

[multibody/geometry] Add method to remove an object.

src/multibody/geometry.hpp

@@ -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.
      *

src/multibody/geometry.hxx

@@ -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

unittest/geom.cpp

@@ -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 )