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

[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 : https://github.com/rdiankov/openrave/issues/1437

[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)