From 711943043df8be572c695991a7159008bda4a273 Mon Sep 17 00:00:00 2001 From: Puttichai Date: Fri, 15 Dec 2023 00:49:49 +0900 Subject: [PATCH] Fixes bug that grabbed body is not checked for collision if grabbing link is disabled --- src/libopenrave/robotmanipulator.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/libopenrave/robotmanipulator.cpp b/src/libopenrave/robotmanipulator.cpp index 929cebb3c8..293de39766 100644 --- 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) {