Optimize MOVE_SHAPE operations for FCL (#3601)

Don't recreate the FCL object when moving shapes, but just update its transforms.
moveit · Jul 31, 2024 · 1d93035 · 1d93035
1 parent 47681f3
commit 1d93035
Show file tree

Hide file tree

Showing 3 changed files with 57 additions and 6 deletions.
diff --git a/...core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/...core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h
@@ -155,11 +155,24 @@ TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_1)
   ASSERT_TRUE(res.collision);
   res.clear();
 
+  pos1.translation().z() = 0.25;
+  this->cenv_->getWorld()->moveObject("box", pos1);
+  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
+  ASSERT_FALSE(res.collision);
+  res.clear();
+
+  pos1.translation().z() = 0.05;
   this->cenv_->getWorld()->moveObject("box", pos1);
   this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
   ASSERT_TRUE(res.collision);
   res.clear();
 
+  pos1.translation().z() = 0.25;
+  this->cenv_->getWorld()->moveObject("box", pos1);
+  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
+  ASSERT_FALSE(res.collision);
+  res.clear();
+
   this->cenv_->getWorld()->moveObject("box", pos1);
   ASSERT_FALSE(res.collision);
 }

diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp
@@ -450,6 +450,38 @@ void CollisionEnvFCL::notifyObjectChange(const ObjectConstPtr& obj, World::Actio
     }
     cleanCollisionGeometryCache();
   }
+  else if (action == World::MOVE_SHAPE)
+  {
+    auto it = fcl_objs_.find(obj->id_);
+    if (it == fcl_objs_.end())
+    {
+      RCLCPP_ERROR(getLogger(), "Cannot move shapes of unknown FCL object: '%s'", obj->id_.c_str());
+      return;
+    }
+
+    if (obj->global_shape_poses_.size() != it->second.collision_objects_.size())
+    {
+      RCLCPP_ERROR(getLogger(),
+                   "Cannot move shapes, shape size mismatch between FCL object and world object: '%s'. Respectively "
+                   "%zu and %zu.",
+                   obj->id_.c_str(), it->second.collision_objects_.size(), it->second.collision_objects_.size());
+      return;
+    }
+
+    for (std::size_t i = 0; i < it->second.collision_objects_.size(); ++i)
+    {
+      it->second.collision_objects_[i]->setTransform(transform2fcl(obj->global_shape_poses_[i]));
+
+      // compute AABB, order matters
+      it->second.collision_geometry_[i]->collision_geometry_->computeLocalAABB();
+      it->second.collision_objects_[i]->computeAABB();
+    }
+
+    // update AABB in the FCL broadphase manager tree
+    // see https://github.com/moveit/moveit/pull/3601 for benchmarks
+    it->second.unregisterFrom(manager_.get());
+    it->second.registerTo(manager_.get());
+  }
   else
   {
     updateFCLObject(obj->id_);

diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -655,9 +655,13 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& sce
       if (it.first == OCTOMAP_NS)
       {
         if (it.second == collision_detection::World::DESTROY)
+        {
           scene_msg.world.octomap.octomap.id = "cleared";  // indicate cleared octomap
+        }
         else
+        {
           do_omap = true;
+        }
       }
       else if (it.second == collision_detection::World::DESTROY)
       {
@@ -1880,8 +1884,9 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision
       std::size_t shape_size = object.primitive_poses.size() + object.mesh_poses.size() + object.plane_poses.size();
       if (shape_size != world_object->shape_poses_.size())
       {
-        ROS_ERROR_NAMED(LOGNAME, "Move operation for object '%s' must have same number of geometry poses. Cannot move.",
-                        object.id.c_str());
+        RCLCPP_ERROR(getLogger(),
+                     "Move operation for object '%s' must have same number of geometry poses. Cannot move.",
+                     object.id.c_str());
         return false;
       }
 
@@ -1890,22 +1895,23 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision
       for (const auto& shape_pose : object.primitive_poses)
       {
         shape_poses.emplace_back();
-        PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back());
+        utilities::poseMsgToEigen(shape_pose, shape_poses.back());
       }
       for (const auto& shape_pose : object.mesh_poses)
       {
         shape_poses.emplace_back();
-        PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back());
+        utilities::poseMsgToEigen(shape_pose, shape_poses.back());
       }
       for (const auto& shape_pose : object.plane_poses)
       {
         shape_poses.emplace_back();
-        PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back());
+        utilities::poseMsgToEigen(shape_pose, shape_poses.back());
       }
 
       if (!world_->moveShapesInObject(object.id, shape_poses))
       {
-        ROS_ERROR_NAMED(LOGNAME, "Move operation for object '%s' internal world error. Cannot move.", object.id.c_str());
+        RCLCPP_ERROR(getLogger(), "Move operation for object '%s' internal world error. Cannot move.",
+                     object.id.c_str());
         return false;
       }
     }