allow EnvironmentBodyRemover not to restore a grabbed body when the grabbing link does not exist

CMakeLists.txt

@@ -4,8 +4,8 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE )
 
 # Define here the needed parameters
 set (OPENRAVE_VERSION_MAJOR 0)
-set (OPENRAVE_VERSION_MINOR 139)
-set (OPENRAVE_VERSION_PATCH 1)
+set (OPENRAVE_VERSION_MINOR 140)
+set (OPENRAVE_VERSION_PATCH 0)
 set (OPENRAVE_VERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR}.${OPENRAVE_VERSION_PATCH})
 set (OPENRAVE_SOVERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR})
 message(STATUS "Compiling OpenRAVE Version ${OPENRAVE_VERSION}, soversion=${OPENRAVE_SOVERSION}")

docs/source/changelog.rst

@@ -6,6 +6,11 @@ ChangeLog
 Unreleased
 ==========
 
+Version 0.140.0
+===============
+
+* Allow EnvironmentBodyRemover not to restore a grabbed body when the grabbing link does not exist as we do for active manipulator
+
 Version 0.139.1
 ===============
 

include/openrave/robot.h

@@ -1435,6 +1435,20 @@ class OPENRAVE_API RobotBase : public KinBody
     friend class Grabbed;
 };
 
+///\brief removes the robot from the environment temporarily while in scope
+class OPENRAVE_API EnvironmentBodyRemover
+{
+public:
+    EnvironmentBodyRemover(KinBodyPtr pBody);
+    ~EnvironmentBodyRemover();
+
+private:
+    KinBodyPtr _pBody;
+    std::vector<KinBody::GrabbedInfoPtr> _pGrabbedInfos; ///< the list of the current grabbed info of pBody at the time of removal.
+    RobotBasePtr _pBodyRobot;
+    std::string _activeManipName; ///< the name of the current active manipulator of pBody at the time of removal.
+};
+
 } // end namespace OpenRAVE
 
 #endif   // ROBOT_H

src/libopenrave/robot.cpp

@@ -447,42 +447,69 @@ void RobotBase::RobotStateSaver::Release()
     KinBodyStateSaver::Release();
 }
 
-///\brief removes the robot from the environment temporarily while in scope
-class EnvironmentBodyRemover
-{
-public:
-    EnvironmentBodyRemover(OpenRAVE::KinBodyPtr pBody) : _pBody(pBody), _grabbedStateSaver(pBody, OpenRAVE::KinBody::Save_GrabbedBodies) {
-        if( _pBody->IsRobot() ) {
-            // If the manip comes from a connected body, the information of which manip is active is lost once the robot
-            // is removed from env. Need to save the active manip name so that we can set it back later when the robot
-            // is re-added to the env.
-            _pBodyRobot = OpenRAVE::RaveInterfaceCast<OpenRAVE::RobotBase>(_pBody);
-            if( !!_pBodyRobot->GetActiveManipulator() ) {
-                _activeManipName = _pBodyRobot->GetActiveManipulator()->GetName();
-            }
+EnvironmentBodyRemover::EnvironmentBodyRemover(KinBodyPtr pBody) : _pBody(pBody) {
+    if( _pBody->IsRobot() ) {
+        // If the manip comes from a connected body, the information of which manip is active is lost once the robot
+        // is removed from env. Need to save the active manip name so that we can set it back later when the robot
+        // is re-added to the env.
+        _pBodyRobot = RaveInterfaceCast<RobotBase>(_pBody);
+        if( !!_pBodyRobot->GetActiveManipulator() ) {
+            _activeManipName = _pBodyRobot->GetActiveManipulator()->GetName();
         }
-        _pBody->GetEnv()->Remove(_pBody);
     }
+    _pBody->GetGrabbedInfo(_pGrabbedInfos);
+    _pBody->GetEnv()->Remove(_pBody);
+}
 
-    ~EnvironmentBodyRemover() {
-        _pBody->GetEnv()->Add(_pBody, IAM_StrictNameChecking);
-        _grabbedStateSaver.Restore();
-        _grabbedStateSaver.Release();
-        if( !!_pBodyRobot && !_activeManipName.empty() ) {
-            OpenRAVE::RobotBase::ManipulatorPtr pmanip = _pBodyRobot->GetManipulator(_activeManipName);
-            // it might be ok with manipulator doesn't exist if ConnectedBody acitve state changes.
-            if( !!pmanip ) {
-                _pBodyRobot->SetActiveManipulator(pmanip);
+EnvironmentBodyRemover::~EnvironmentBodyRemover() {
+    _pBody->GetEnv()->Add(_pBody, IAM_StrictNameChecking);
+    if( !!_pBodyRobot && !_activeManipName.empty() ) {
+        RobotBase::ManipulatorPtr pmanip = _pBodyRobot->GetManipulator(_activeManipName);
+        // it might be ok with manipulator doesn't exist if ConnectedBody acitve state changes.
+        if( !!pmanip ) {
+            _pBodyRobot->SetActiveManipulator(pmanip);
+        }
+        else {
+            pmanip = _pBodyRobot->GetActiveManipulator();
+            RAVELOG_WARN_FORMAT("env=%s, robot=%s, cannot restore previous active manip=%s because it does not exist anymore. current active manip=%s", _pBodyRobot->GetEnv()->GetNameId()%_pBodyRobot->GetName()%_activeManipName%(!!pmanip ? pmanip->GetName() : ""));
+        }
+    }
+    if( !_pGrabbedInfos.empty() ) {
+        // it might be ok with grabbing link doesn't exist if ConnectedBody acitve state changes.
+        std::vector<KinBody::GrabbedInfoPtr>::iterator itRemoveFirst = _pGrabbedInfos.begin();
+        for( std::vector<KinBody::GrabbedInfoPtr>::const_iterator itGrabbedInfo = _pGrabbedInfos.begin(); itGrabbedInfo != _pGrabbedInfos.end(); ++itGrabbedInfo ) {
+            const KinBody::GrabbedInfoPtr pGrabbedInfo = *itGrabbedInfo;
+            bool needToDelete = false;
+            if( !_pBody->GetLink(pGrabbedInfo->_robotlinkname) ) {
+                RAVELOG_WARN_FORMAT("env=%s, body=%s, cannot re-grab '%s' because grabbing link '%s' does not exist anymore.", _pBody->GetEnv()->GetNameId()%_pBody->GetName()%pGrabbedInfo->_grabbedname%pGrabbedInfo->_robotlinkname);
+                needToDelete = true;
+            }
+            else if( !_pBody->GetEnv()->GetKinBody(pGrabbedInfo->_grabbedname) ) {
+                RAVELOG_WARN_FORMAT("env=%s, body=%s, cannot re-grab '%s' because it does not exist in the environment.", _pBody->GetEnv()->GetNameId()%_pBody->GetName()%pGrabbedInfo->_grabbedname);
+                needToDelete = true;
             }
+            else {
+                for( std::set<std::string>::const_iterator itLinkName = pGrabbedInfo->_setIgnoreRobotLinkNames.begin(); itLinkName != pGrabbedInfo->_setIgnoreRobotLinkNames.end(); ++itLinkName ) {
+                    if( !_pBody->GetLink(*itLinkName) ) {
+                        RAVELOG_WARN_FORMAT("env=%s, body=%s, cannot re-grab '%s' because '%s' in _setIgnoreRobotLinkNames does not exist anymore.", _pBody->GetEnv()->GetNameId()%_pBody->GetName()%pGrabbedInfo->_grabbedname%(*itLinkName));
+                        needToDelete = true;
+                        break;
+                    }
+                }
+            }
+            if( needToDelete ) {
+                continue;
+            }
+            // will regrasp this info
+            if( itRemoveFirst != itGrabbedInfo ) {
+                *itRemoveFirst = std::move(*itGrabbedInfo);
+            }
+            ++itRemoveFirst;
         }
+        const std::vector<KinBody::GrabbedInfoConstPtr> pConstGrabbedInfos(_pGrabbedInfos.begin(), itRemoveFirst);
+        _pBody->ResetGrabbed(pConstGrabbedInfos);
     }
-
-private:
-    OpenRAVE::KinBodyPtr _pBody;
-    OpenRAVE::KinBody::KinBodyStateSaver _grabbedStateSaver;
-    OpenRAVE::RobotBasePtr _pBodyRobot;
-    std::string _activeManipName; ///< the name of the current active manipulator of pBody at the time of removal.
-};
+}
 
 void RobotBase::RobotStateSaver::_RestoreRobot(boost::shared_ptr<RobotBase> probot)
 {