From a0a5a633fe7ceba1f7bce2e7e363bf8fb7310525 Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Fri, 19 Jul 2024 06:49:45 -0400
Subject: [PATCH] Optimize MOVE_SHAPE operations for FCL (#3601)

Don't recreate the FCL object when moving shapes, but just update its transforms.
---
 .../test_collision_common_panda.h             | 13 ++++++++
 .../src/collision_env_fcl.cpp                 | 32 +++++++++++++++++++
 2 files changed, 45 insertions(+)

diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h
index 92163406aa..061a304085 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h
+++ b/moveit_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
index 54a007d325..86a48b8320 100644
--- 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_);