Fixes bug that grabbed body is not checked for collision if grabbing …

…link is disabled
rdiankov · Dec 14, 2023 · 7119430 · 7119430
1 parent 85fe60b
commit 7119430
Showing 1 changed file with 10 additions and 6 deletions.
diff --git a/src/libopenrave/robotmanipulator.cpp b/src/libopenrave/robotmanipulator.cpp
@@ -912,9 +912,11 @@ bool RobotBase::Manipulator::CheckEndEffectorCollision(CollisionReportPtr report
 
     FOREACHC(itlink, probot->GetLinks()) {
         int ilink = (*itlink)->GetIndex();
-        if( !(*itlink)->IsEnabled() ) {
-            continue;
-        }
+        // Do not skip the iteration even if the link is not enabled since the link can be grabbing stuff that is
+        // enabled. CheckLinkCollision will take care if the fact that this link is not enabled.
+        // if( !(*itlink)->IsEnabled() ) {
+        //     continue;
+        // }
         // gripper needs to be affected by all joints
         bool bGripperLink = true;
         FOREACHC(itarmdof,__varmdofindices) {
@@ -972,9 +974,11 @@ bool RobotBase::Manipulator::_CheckEndEffectorCollision(const Transform& tEE, Ki
 
     FOREACHC(itlink, probot->GetLinks()) {
         int ilink = (*itlink)->GetIndex();
-        if( !(*itlink)->IsEnabled() ) {
-            continue;
-        }
+        // Do not skip the iteration even if the link is not enabled since the link can be grabbing stuff that is
+        // enabled. CheckLinkCollision will take care if the fact that this link is not enabled.
+        // if( !(*itlink)->IsEnabled() ) {
+        //     continue;
+        // }
         // gripper needs to be affected by all joints
         bool bGripperLink = true;
         FOREACHC(itarmdof,__varmdofindices) {