Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Open
wants to merge 11 commits into
base: production
Choose a base branch
from
Open

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
916ce09
allow EnvironmentBodyRemover not to restore a grabbed body when the g…
eisoku9618 Feb 27, 2024
75bd3d9
bump version
eisoku9618 Feb 27, 2024
a9b772c
need to check _setIgnoreRobotLinkNames existance as well
eisoku9618 Feb 28, 2024
3ba1b8a
Merge branch 'production' of https://github.com/rdiankov/openrave int…
eisoku9618 Feb 29, 2024
dd08d7f
clarify EnvironmentBodyRemover behavior by taking input arguments
eisoku9618 Feb 29, 2024
f22dd99
Merge branch 'production' of https://github.com/rdiankov/openrave int…
eisoku9618 Mar 19, 2024
d551f19
add enum type for EnvironmentBodyRemover options
eisoku9618 Mar 19, 2024
cbdb9bc
fix the default argument
eisoku9618 Mar 19, 2024
8397d76
add the option not to abort
eisoku9618 Mar 19, 2024
a001c6a
add the option to abort if any info is lost
eisoku9618 Mar 19, 2024
ad73c54
fix typo
eisoku9618 Mar 19, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 141)
set (OPENRAVE_VERSION_PATCH 1)
set (OPENRAVE_VERSION_MINOR 142)
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}")
Expand Down
5 changes: 5 additions & 0 deletions docs/source/changelog.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,11 @@
ChangeLog
#########

Version 0.142.0
===============

* Allow EnvironmentBodyRemover not to restore a grabbed body when the grabbing link does not exist as we do for active manipulator

Version 0.141.1
===============

Expand Down
23 changes: 23 additions & 0 deletions include/openrave/robot.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1437,6 +1437,29 @@ class OPENRAVE_API RobotBase : public KinBody
friend class Grabbed;
};

enum EnvironmentBodyRemoverRestoreOptions : uint8_t
{
EBRRO_NoAbortOnInfoLost = 0, ///< will not abort even if any info cannot be restored.
EBRRO_AbortOnActiveManipulatorLost = 0b01, ///< will abort if active manipulator cannot be restored.
EBRRO_AbortOnGrabbedBodiesLost = 0b10, ///< will abort if grabbed bodies cannot be restored.
EBRRO_AbortOnInfoLost = EBRRO_AbortOnActiveManipulatorLost | EBRRO_AbortOnGrabbedBodiesLost, ///< will abort if any info cannot be restored.
};

///\brief removes the robot from the environment temporarily while in scope
class OPENRAVE_API EnvironmentBodyRemover
{
public:
EnvironmentBodyRemover(KinBodyPtr pBody, int restoreOptions=EBRRO_AbortOnGrabbedBodiesLost); // abort on grabbed bodies lost for backward compatibility
~EnvironmentBodyRemover() noexcept(true);

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.
int _restoreOptions;
};

} // end namespace OpenRAVE

#endif // ROBOT_H
99 changes: 69 additions & 30 deletions src/libopenrave/robot.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -447,42 +447,81 @@ 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, int restoreOptions) : _pBody(pBody), _restoreOptions(restoreOptions) {
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() noexcept(true) {
_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 if( !(_restoreOptions & EBRRO_AbortOnActiveManipulatorLost) ) {
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() : ""));
}
else {
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);
}
}
if( !_pGrabbedInfos.empty() ) {
// it might be ok with grabbing link doesn't exist if ConnectedBody acitve state changes.
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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);
}

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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

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 ) {
if( _restoreOptions & EBRRO_AbortOnGrabbedBodiesLost ) {
throw OPENRAVE_EXCEPTION_FORMAT(_("env=%s, robot=%s, cannot restore the original grabbed bodies."), _pBodyRobot->GetEnv()->GetNameId()%_pBodyRobot->GetName(), ORE_Failed);
}
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)
{
Expand Down
Loading