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