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

[eisoku9618]

Currently we allow EnvironmentBodyRemover not to restore active manipulator on restore if the active manipulator does not exist anymore in the body in case of CBAS change.

I think that we should do the same thing for grabbed bodies because a grabbing link might not exist in case of CBAS change.

• a grabbing link might not exist
• a link in _setIgnoreRobotLinkNames might not exist
• grabbed body might not exist

[yoshikikanemoto]

@eisoku9618 I think you should not change the behavior of EnvironmentBodyRemover. There is a complication like this: If there is a grabbed body that has _setIgnoreRobotLinkNames and some of links are removed by the CBAS change, it is not obvious how this class should be doing.

[eisoku9618]

@yoshikikanemoto Eventually we need to take care of grabbed bodies restoring failures either by
EnvironmentBodyRemover itself or by callers of EnvironmentBodyRemover.

My only concern is an inconsistency of current EnvironmentBodyRemover between active manipulator and grabbed bodies. EnvironmentBodyRemover tries to restore both active manipulator and grabbed bodies, and the inconsistency is that it allows not restoring active manipulator while it terminates the program (c++11 and later) if unable to restore grabbed bodies.

it is not obvious how this class should be doing.

How about adding the input arguments to EnvironmentBodyRemover to clarify how this class should behave?

EnvironmentBodyRemover(KinBodyPtr pBody, bool abortOnActiveManipulatorLost=false, bool abortOnGrabbedBodiesLost=true);

If this is still not good and we can leave the inconsistent behavior of EnvironmentBodyRemover for now, I can make another utility function SetConnectedBodyActiveStatesKeepingGrabbedBodiesAsMuchAsPossible and move the implementation to this.

void SetConnectedBodyActiveStatesKeepingGrabbedBodiesAsMuchAsPossible(pRobot, requestedConnectedBodyActiveStates) {
    std::vector<KinBody::GrabbedInfoPtr> pGrabbedInfos;
    pRobot->GetGrabbedInfo(pGrabbedInfos);

    // manually restore grabbed state instead of using EnvironmentBodyRemover because a grabbing link might get removed on connected body active state change.
    pRobot->ReleaseAllGrabbed();
    {
        EnvironmentBodyRemover robotremover(pRobot);
        pRobot->SetConnectedBodyActiveStates(requestedConnectedBodyActiveStates);
    }

    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( !pRobot->GetLink(pGrabbedInfo->_robotlinkname) ) {
            RAVELOG_WARN_FORMAT("env=%s, body=%s, cannot re-grab '%s' because grabbing link '%s' does not exist anymore.", pRobot->GetEnv()->GetNameId()%pRobot->GetName()%pGrabbedInfo->_grabbedname%pGrabbedInfo->_robotlinkname);
            needToDelete = true;
        }
        else if( !pRobot->GetEnv()->GetKinBody(pGrabbedInfo->_grabbedname) ) {
            RAVELOG_WARN_FORMAT("env=%s, body=%s, cannot re-grab '%s' because it does not exist in the environment.", pRobot->GetEnv()->GetNameId()%pRobot->GetName()%pGrabbedInfo->_grabbedname);
            needToDelete = true;
        }
        else {
            for( std::set<std::string>::const_iterator itLinkName = pGrabbedInfo->_setIgnoreRobotLinkNames.begin(); itLinkName != pGrabbedInfo->_setIgnoreRobotLinkNames.end(); ++itLinkName ) {
                if( !pRobot->GetLink(*itLinkName) ) {
                    RAVELOG_WARN_FORMAT("env=%s, body=%s, cannot re-grab '%s' because '%s' in _setIgnoreRobotLinkNames does not exist anymore.", pRobot->GetEnv()->GetNameId()%pRobot->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);
    pRobot->ResetGrabbed(pConstGrabbedInfos);
}

[eisoku9618]

discussed with @yoshikikanemoto
At least, it is better to use a bitmask instead of two booleans for the argument.

[eisoku9618]

@yoshikikanemoto As we discussed, I changed this PR to use a bit mask for the options. Could you check this PR and the changes in the related internal repositories? pipelineid=456553

[rdiankov]

@snozawa can you take a look at this MR? Thanks

[yoshikikanemoto]

@snozawa can you take a look at this MR? Thanks

@snozawa Eisoku revived this PR just now. Can you check?

[snozawa]

@eisoku9618

As for the restoring of Grabbed,

• The production branch uses KinBodyStateSaver.
• This PR uses ResetGrabbed.

The former one just restores the original ptr of Grabbed instance, while the latter one creates the new Grabbed instance.

In this case, the former one preserves _pGrabberSaver and _pGrabbedSaver.
But, the latter one does not, so the resultant behavior of Grabbed becomes different.

So, I personally would like to revert the grabbed-restoring code, and would like to use the KinBodyStateSaver as in the original production branch.

Then, the next discussion would be how to handle the exception from the KinBodyStateSaver.
I'm not sure what's the best way to do it, but one of the way is adding the Save_GrabbedBodiesNoExceptionOnInfoLost or something.

Btw, throwing from KinBodyStateSaver invokes another issue : #1437

[snozawa]

@eisoku9618
Is this equivalent to the followig?
the following is slightly readable since it does not invert the flag:

        else if( (_restoreOptions & EBRRO_AbortOnActiveManipulatorLost) ) {
            stringstream ss;
            ss << "available manipulators are [";
            for( const RobotBase::ManipulatorPtr& pManip : _pBodyRobot->GetManipulators() ) {
                ss << pManip->GetName() << ", ";
            }
            ss << "]";
            throw OPENRAVE_EXCEPTION_FORMAT(_("env=%s, robot=%s, cannot restore the original active manip=%s because it does not exist anymore. %s"), _pBodyRobot->GetEnv()->GetNameId()%_pBodyRobot->GetName()%_activeManipName%ss.str(), ORE_Failed);
        }
        else {
            pmanip = _pBodyRobot->GetActiveManipulator();
            RAVELOG_WARN_FORMAT("env=%s, robot=%s, cannot restore the original active manip=%s because it does not exist anymore. current active manip=%s", _pBodyRobot->GetEnv()->GetNameId()%_pBodyRobot->GetName()%_activeManipName%(!!pmanip ? pmanip->GetName() : ""));
        }

[snozawa]

what about like this?

Suggested change

	EBRRO_NoAbortOnInfoLost = 0, ///< will not abort even if any info cannot be restored.
	EBRRO_NoAbortOnInfoLost = 0, ///< will not abort even if any info handled in this enum cannot be restored. Note that the remover would abort if there is an exception which is not handled in this enum.

[snozawa]

@eisoku9618

Any problem if we use RobotStateSaver's Save_ActiveManipulator here?
maybe you need to restore by the name, not by the pointer?

[snozawa]

talked in person with @eisoku9618

• If no-abort option is supplied, if the grabbed is not restored or manipulator is not restored, warning is shown.
• If it's from the old connected body, that's reasonable behavior. But, if it's from the unexpected bug, it might be dangerous.
• So, before calling EnvironmentBodyRemover, or at the very beginning of EnvironmentBodyRemover, check the CBAS change. and then, check if the restored active manipulator is available or not. same for grabbed bodies. if not available, do not resotre them.

what do you think? @kanbouchou @yoshikikanemoto

[eisoku9618]

So, before calling EnvironmentBodyRemover, or at the very beginning of EnvironmentBodyRemover, check the CBAS change. and then, check if the restored active manipulator is available or not. same for grabbed bodies. if not available, do not resotre them.

@kanbouchou @yoshikikanemoto May I know your opinions about this. I also think this is a good direction. Some callers of EnvironmentBodyRemover for CBAS change need to reject the CBAS change request when restoring active manipulator and grabbed bodies is impossible, others accept CBAS change without trying restoring active manipulator and grabbed bodies. (i.e. depending on the context of the callers)