From 3c9728576daf6a5046f8110654d7762a543f1e75 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Sat, 19 Oct 2024 10:06:11 +0900 Subject: [PATCH 1/6] Update changelog for grabbed api update --- docs/source/changelog.rst | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst index ec117fc5df..43a03ed7eb 100644 --- a/docs/source/changelog.rst +++ b/docs/source/changelog.rst @@ -8,6 +8,8 @@ Version 0.156.0 - Speed up environment loading for big scenes with lots of static links. - Deprecate restoring of grabbed bodies by `KinBodyStateSaver` from one env to another env since it's hard to restore any kind of bodies into different env. Instead, use the dedicated private API in `Environment::_Clone`. +- Remove unnecessary APIs about `CheckGrabbedInfo` and `InvalidateListNonCollidingLinks`. +- Add python enum for `GICR_UserDataNotMatch` Version 0.155.0 =============== From 0ae0e0e5fccba6933c58ea5e3cd5826485a99a29 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Tue, 22 Oct 2024 14:02:18 +0900 Subject: [PATCH 2/6] Add kinematics hash checking for map list noncolliding for inter grabbed --- src/libopenrave/kinbodystatesaver.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/libopenrave/kinbodystatesaver.cpp b/src/libopenrave/kinbodystatesaver.cpp index 6fe64a5575..0a84f78828 100644 --- a/src/libopenrave/kinbodystatesaver.cpp +++ b/src/libopenrave/kinbodystatesaver.cpp @@ -185,6 +185,14 @@ void KinBody::_RestoreGrabbedBodiesFromSavedData(const KinBody& savedBody, RAVELOG_WARN_FORMAT("env=%s, could not find bodies with envBodyIndex '%s' (%d).", GetEnv()->GetNameId()%pSecondSaved->GetName()%pSecondSaved->GetEnvironmentBodyIndex()); continue; } + if( pFirstSaved->GetKinematicsGeometryHash() != pFirst->GetKinematicsGeometryHash() ) { + RAVELOG_WARN_FORMAT("env=%s, new grabbed body '%s' kinematics-geometry hash is different from original grabbed body '%s' from env=%s", GetEnv()->GetNameId()%pFirst->GetName()%pFirstSaved->GetName()%savedBody.GetEnv()->GetNameId()); + continue; + } + if( pSecondSaved->GetKinematicsGeometryHash() != pSecond->GetKinematicsGeometryHash() ) { + RAVELOG_WARN_FORMAT("env=%s, new grabbed body '%s' kinematics-geometry hash is different from original grabbed body '%s' from env=%s", GetEnv()->GetNameId()%pSecond->GetName()%pSecondSaved->GetName()%savedBody.GetEnv()->GetNameId()); + continue; + } KinBody::ListNonCollidingLinkPairs listNonCollidingLinkPairs; FOREACHC(itLinkPairSaved, itInfoSaved->second) { const int linkIndexFirst = (*itLinkPairSaved).first->GetIndex(); From fa2cc0cae2c56a472f6589ce9702cd1dd51f3470 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Tue, 22 Oct 2024 14:03:16 +0900 Subject: [PATCH 3/6] No need to check iterator since the code right before already checked it --- src/libopenrave/kinbodygrab.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/libopenrave/kinbodygrab.cpp b/src/libopenrave/kinbodygrab.cpp index 9bc70ef841..2810c9ca7e 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -306,12 +306,10 @@ void Grabbed::_PushNonCollidingLinkPairsForGrabbedBodies(KinBodyPtr& pGrabber, } // If not found, try checking the non-colliding lists. If non-colliding list is not empty, push the new info. - if( itInfo == pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.end() ) { - KinBody::ListNonCollidingLinkPairs listNonCollidingLinkPairs; - _PushLinkPairsIfNonCollidingWithOtherGrabbedBody(listNonCollidingLinkPairs, pchecker, grabbedBody, otherGrabbedBody, /* bInvertFirstSecond */ false); - if( listNonCollidingLinkPairs.size() > 0 ) { - itInfo = pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(indexForTest0, std::move(listNonCollidingLinkPairs)).first; - } + KinBody::ListNonCollidingLinkPairs listNonCollidingLinkPairs; + _PushLinkPairsIfNonCollidingWithOtherGrabbedBody(listNonCollidingLinkPairs, pchecker, grabbedBody, otherGrabbedBody, /* bInvertFirstSecond */ false); + if( listNonCollidingLinkPairs.size() > 0 ) { + itInfo = pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(indexForTest0, std::move(listNonCollidingLinkPairs)).first; } } From e21a2e7783684fe7e4e08f45d40cb624403cba8c Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Tue, 22 Oct 2024 14:04:04 +0900 Subject: [PATCH 4/6] no need of bInvertFirstSecond --- src/libopenrave/kinbodygrab.cpp | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/src/libopenrave/kinbodygrab.cpp b/src/libopenrave/kinbodygrab.cpp index 2810c9ca7e..6d66e7417e 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -51,8 +51,7 @@ static bool _IsLinkPairIncluded(const KinBody::Link* pLink1ToSearch, static void _PushLinkPairsIfNonCollidingWithOtherGrabbedBody(std::list >& listNonCollidingLinkPairs, CollisionCheckerBasePtr& pchecker, const KinBody& grabbedBody, - const KinBody& otherGrabbedBody, - const bool bInvertFirstSecond = false) + const KinBody& otherGrabbedBody) { for (const KinBody::LinkPtr& pOtherGrabbedLink : otherGrabbedBody.GetLinks()) { KinBody::LinkConstPtr pOtherGrabbedLinkToCheckConst(pOtherGrabbedLink); @@ -63,12 +62,7 @@ static void _PushLinkPairsIfNonCollidingWithOtherGrabbedBody(std::listCheckCollision(pOtherGrabbedLinkToCheckConst, KinBody::LinkConstPtr(pGrabbedBodylink)) ) { - if( bInvertFirstSecond ) { - listNonCollidingLinkPairs.emplace_back(pOtherGrabbedLink, pGrabbedBodylink); - } - else { - listNonCollidingLinkPairs.emplace_back(pGrabbedBodylink, pOtherGrabbedLink); - } + listNonCollidingLinkPairs.emplace_back(pGrabbedBodylink, pOtherGrabbedLink); } } } @@ -293,7 +287,7 @@ void Grabbed::_PushNonCollidingLinkPairsForGrabbedBodies(KinBodyPtr& pGrabber, const uint64_t indexForTest0 = KinBody::_ComputeEnvironmentBodyIndicesPair(grabbedBody, otherGrabbedBody); std::unordered_map::iterator itInfo = pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.find(indexForTest0); if( itInfo != pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.end() ) { - _PushLinkPairsIfNonCollidingWithOtherGrabbedBody((*itInfo).second, pchecker, grabbedBody, otherGrabbedBody, /* bInvertFirstSecond */ false); + _PushLinkPairsIfNonCollidingWithOtherGrabbedBody((*itInfo).second, pchecker, grabbedBody, otherGrabbedBody); return; } @@ -301,13 +295,13 @@ void Grabbed::_PushNonCollidingLinkPairsForGrabbedBodies(KinBodyPtr& pGrabber, const uint64_t indexForTest1 = KinBody::_ComputeEnvironmentBodyIndicesPair(otherGrabbedBody, grabbedBody); itInfo = pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.find(indexForTest1); if( itInfo != pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.end() ) { - _PushLinkPairsIfNonCollidingWithOtherGrabbedBody((*itInfo).second, pchecker, grabbedBody, otherGrabbedBody, /* bInvertFirstSecond */ true); + _PushLinkPairsIfNonCollidingWithOtherGrabbedBody((*itInfo).second, pchecker, otherGrabbedBody, grabbedBody); return; } // If not found, try checking the non-colliding lists. If non-colliding list is not empty, push the new info. KinBody::ListNonCollidingLinkPairs listNonCollidingLinkPairs; - _PushLinkPairsIfNonCollidingWithOtherGrabbedBody(listNonCollidingLinkPairs, pchecker, grabbedBody, otherGrabbedBody, /* bInvertFirstSecond */ false); + _PushLinkPairsIfNonCollidingWithOtherGrabbedBody(listNonCollidingLinkPairs, pchecker, grabbedBody, otherGrabbedBody); if( listNonCollidingLinkPairs.size() > 0 ) { itInfo = pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(indexForTest0, std::move(listNonCollidingLinkPairs)).first; } From 8bf75a0627835d3c9e99207ca0b402191b13036b Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Tue, 22 Oct 2024 14:07:13 +0900 Subject: [PATCH 5/6] Swap the order of code, for better readability --- src/libopenrave/kinbodygrab.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/libopenrave/kinbodygrab.cpp b/src/libopenrave/kinbodygrab.cpp index 6d66e7417e..21180a43ea 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -53,16 +53,15 @@ static void _PushLinkPairsIfNonCollidingWithOtherGrabbedBody(std::listCheckCollision(pOtherGrabbedLinkToCheckConst, KinBody::LinkConstPtr(pGrabbedBodylink)) ) { - listNonCollidingLinkPairs.emplace_back(pGrabbedBodylink, pOtherGrabbedLink); + if( !pchecker->CheckCollision(KinBody::LinkConstPtr(pGrabbedBodyLink), KinBody::LinkConstPtr(pOtherGrabbedLink)) ) { + listNonCollidingLinkPairs.emplace_back(pGrabbedBodyLink, pOtherGrabbedLink); } } } From 54cdeddd0f83ef8786963545d71c875c7ce297d8 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 24 Oct 2024 18:31:04 +0900 Subject: [PATCH 6/6] Add copying from other Grabbed and StateSaver to fix https://github.com/rdiankov/openrave/issues/1442 - Issue - When the new Grabbed instance is created in saver or Clone, the _pGrabbedSaver and _pGrabberSaver of the new Grabbed are different from those of the original Grabbed. - Thus, if isListNonCollidingLinksValid=false, wrong state was used in ComputeListNonCollidingLinks. - Resolution - Copy the consistent states from the original Grabbed to the new Grabbed. - To do so, introduce the constructor of Grabbed to copy the saver states from the original Grabbed. In addition, introduce the constructor of StateSavers to copy the states from the original savers. - If the states are not copy-able, throw exception. Also, this API is very advanced and not expected to be used from the usual users. Thus, make such functionality as the constructor. --- include/openrave/kinbody.h | 18 ++++++++ include/openrave/robot.h | 9 ++++ src/libopenrave/kinbody.cpp | 2 +- src/libopenrave/kinbodygrab.cpp | 32 +++++++++++++ src/libopenrave/kinbodystatesaver.cpp | 66 ++++++++++++++++++++++++++- src/libopenrave/robot.cpp | 22 +++++++++ 6 files changed, 147 insertions(+), 2 deletions(-) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index e68ee802d6..5a0fc0671c 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -2460,7 +2460,16 @@ class OPENRAVE_API KinBody : public InterfaceBase class OPENRAVE_API KinBodyStateSaver { public: + /// \brief construct KinBodyStateSaver to save the states of the given pbody. This constructor is standard one. + /// \param[in] pbody : the states of this body are saved. later on, the saved states will be restored, depending on the usage such as _bRestoreOnDestructor. + /// \param[in] options : options to represent which state to save. KinBodyStateSaver(KinBodyPtr pbody, int options = Save_LinkTransformation|Save_LinkEnable); + + /// \brief construct KinBodyStateSaver by copying from other saver. This constructor is advanced and expected to be used in very limited use cases. + /// \param[in] pbody : the saved states will be restored, depending on the usage such as _bRestoreOnDestructor. Note that the saver's states are not coming from this pbody. + /// \param[in] referenceSaver : the states are copied from this saver. Note that some of the states are not copied, such as pointers. + KinBodyStateSaver(KinBodyPtr pbody, const KinBodyStateSaver& referenceSaver); + virtual ~KinBodyStateSaver(); inline const KinBodyPtr& GetBody() const { return _pbody; @@ -2480,6 +2489,9 @@ class OPENRAVE_API KinBody : public InterfaceBase /// \brief sets whether the state saver will restore the state on destruction. by default this is true. virtual void SetRestoreOnDestructor(bool restore); protected: + /// \brief Throw exception due to the wrong usage of constructor call with wrong options. + virtual void _ThrowOnInvalidCopyFromOtherSaver(const char* envNameId, const char* className, const SaveParameters param, const int options); + KinBodyPtr _pbody; int _options; ///< saved options std::vector _vLinkTransforms; @@ -3854,7 +3866,13 @@ class OPENRAVE_API KinBody : public InterfaceBase class OPENRAVE_API Grabbed : public UserData, public boost::enable_shared_from_this { public: + /// \brief constructor. This is standard one. Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink, const uint64_t uniqueId); + + /// \brief constructor. This constructor is advanced and expected to be used in very limited use cases. + /// \param[in] referenceGrabbed : reference of Grabbed. The internal state savers will copy the states from referenceGrabbed's savers. + Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink, const uint64_t uniqueId, const Grabbed& referenceGrabbed); + virtual ~Grabbed() { } diff --git a/include/openrave/robot.h b/include/openrave/robot.h index 8e1f0e817a..b5f68a014a 100644 --- a/include/openrave/robot.h +++ b/include/openrave/robot.h @@ -915,7 +915,16 @@ class OPENRAVE_API RobotBase : public KinBody class OPENRAVE_API RobotStateSaver : public KinBodyStateSaver { public: + /// \brief construct RobotStateSaver to save the states of the given probot. This constructor is standard one. + /// \param[in] probot : the states of this robot are saved. later on, the saved states will be restored, depending on the usage such as _bRestoreOnDestructor. + /// \param[in] options : options to represent which state to save. RobotStateSaver(RobotBasePtr probot, int options = Save_LinkTransformation|Save_LinkEnable|Save_ActiveDOF|Save_ActiveManipulator); + + /// \brief construct RobotStateSaver by copying from other saver. This constructor is advanced and expected to be used in very limited use cases. + /// \param[in] probot : the saved states will be restored, depending on the usage such as _bRestoreOnDestructor. Note that the saver's states are not coming from this probot. + /// \param[in] referenceSaver : the states are copied from this saver. Note that some of the states are not copied, such as pointers. + RobotStateSaver(RobotBasePtr probot, const RobotStateSaver& referenceSaver); + virtual ~RobotStateSaver(); /// \brief restore the state diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 7300d865d8..c288cff9bc 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -5885,7 +5885,7 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) } //BOOST_ASSERT(pgrabbedbody->GetName() == pbodyref->GetName()); - GrabbedPtr pgrabbed(new Grabbed(pgrabbedbody,_veclinks.at(KinBody::LinkPtr(pgrabbedref->_pGrabbingLink)->GetIndex()), pgrabbedref->_uniqueId)); + GrabbedPtr pgrabbed(new Grabbed(pgrabbedbody,_veclinks.at(KinBody::LinkPtr(pgrabbedref->_pGrabbingLink)->GetIndex()), pgrabbedref->_uniqueId, *pgrabbedref)); pgrabbed->_tRelative = pgrabbedref->_tRelative; pgrabbed->_setGrabberLinkIndicesToIgnore = pgrabbedref->_setGrabberLinkIndicesToIgnore; // can do this since link indices are the same CopyRapidJsonDoc(pgrabbedref->_rGrabbedUserData, pgrabbed->_rGrabbedUserData); diff --git a/src/libopenrave/kinbodygrab.cpp b/src/libopenrave/kinbodygrab.cpp index e1776434bd..59c31e20ba 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -107,6 +107,21 @@ static void _CreateSaverForGrabbedAndGrabber(KinBody::KinBodyStateSaverPtr& pSav } } +/// \brief create saver for grabbed/grabber. +static void _CreateSaverForGrabbedAndGrabber(KinBody::KinBodyStateSaverPtr& pSaver, + const KinBodyPtr& pBody, + const KinBody::KinBodyStateSaverPtr& pReferenceSaver) +{ + if( pBody->IsRobot() ) { + RobotBasePtr pRobot = OPENRAVE_DYNAMIC_POINTER_CAST(pBody); + const RobotBase::RobotStateSaverPtr pRobotReferenceSaver = OPENRAVE_DYNAMIC_POINTER_CAST(pReferenceSaver); + pSaver.reset(new RobotBase::RobotStateSaver(pRobot, *pRobotReferenceSaver)); + } + else { + pSaver.reset(new KinBody::KinBodyStateSaver(pBody, *pReferenceSaver)); + } +} + Grabbed::Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink, const uint64_t uniqueId) : _uniqueId(uniqueId) { @@ -127,6 +142,23 @@ Grabbed::Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink, const bDisableRestoreOnDestructor); } // end Grabbed +Grabbed::Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink, const uint64_t uniqueId, const Grabbed& referenceGrabbed) + : _uniqueId(uniqueId) +{ + _pGrabbedBody = pGrabbedBody; + _pGrabbingLink = pGrabbingLink; + _pGrabbingLink->GetRigidlyAttachedLinks(_vAttachedToGrabbingLink); + _listNonCollidingIsValid = false; + _CreateSaverForGrabbedAndGrabber(_pGrabbedSaver, + pGrabbedBody, + referenceGrabbed._pGrabbedSaver); + + KinBodyPtr pGrabber = RaveInterfaceCast(_pGrabbingLink->GetParent()); + _CreateSaverForGrabbedAndGrabber(_pGrabberSaver, + pGrabber, + referenceGrabbed._pGrabberSaver); +} // end Grabbed + void Grabbed::AddMoreIgnoreLinks(const std::set& setAdditionalGrabberLinksToIgnore) { KinBodyPtr pGrabber = RaveInterfaceCast(_pGrabbingLink->GetParent()); diff --git a/src/libopenrave/kinbodystatesaver.cpp b/src/libopenrave/kinbodystatesaver.cpp index fee64a9d90..32dbabf161 100644 --- a/src/libopenrave/kinbodystatesaver.cpp +++ b/src/libopenrave/kinbodystatesaver.cpp @@ -31,6 +31,30 @@ static bool _IsValidLinkIndexForListNonCollidingLinkPairs(const int linkIndex, return true; } +static const char* _GetSaveParameterString(const KinBody::SaveParameters param) +{ + switch(param) + { + case KinBody::Save_LinkTransformation: return "LinkTransformation"; + case KinBody::Save_LinkEnable: return "LinkEnable"; + case KinBody::Save_LinkVelocities: return "LinkVelocities"; + case KinBody::Save_JointMaxVelocityAndAcceleration: return "JointMaxVelocityAndAcceleration"; + case KinBody::Save_JointWeights: return "JointWeights"; + case KinBody::Save_JointLimits: return "JointLimits"; + case KinBody::Save_JointResolutions: return "JointResolutions"; + case KinBody::Save_ActiveDOF: return "ActiveDOF"; + case KinBody::Save_ActiveManipulator: return "ActiveManipulator"; + case KinBody::Save_GrabbedBodies: return "GrabbedBodies"; + case KinBody::Save_ActiveManipulatorToolTransform: return "ActiveManipulatorToolTransform"; + case KinBody::Save_ManipulatorsToolTransform: return "ManipulatorsToolTransform"; + case KinBody::Save_ConnectedBodies: return "ConnectedBodies"; + default: + break; + } + // should throw an exception? + return ""; +} + void KinBody::_RestoreStateForClone(const KinBodyPtr& pOriginalBody) { // In the old code, this is done by KinBodyStateSaver's KinBody::Save_GrabbedBodies|KinBody::Save_LinkVelocities. @@ -117,7 +141,7 @@ void KinBody::_RestoreGrabbedBodiesFromSavedData(const KinBody& savedBody, // initialized Grabbed objects will save the current state of pbody for later computation of // _listNonCollidingLinksWhenGrabbed (in case it is not yet computed). KinBody::LinkPtr pNewGrabbingLink = GetLinks().at(pGrabbingLink->GetIndex()); - GrabbedPtr pNewGrabbed(new Grabbed(pNewGrabbedBody, pNewGrabbingLink, pGrabbed->_uniqueId)); + GrabbedPtr pNewGrabbed(new Grabbed(pNewGrabbedBody, pNewGrabbingLink, pGrabbed->_uniqueId, *pGrabbed)); pNewGrabbed->_tRelative = pGrabbed->_tRelative; pNewGrabbed->_setGrabberLinkIndicesToIgnore = savedGrabbedData.setGrabberLinkIndicesToIgnore; if( savedGrabbedData.listNonCollidingIsValid ) { @@ -267,6 +291,46 @@ KinBody::KinBodyStateSaver::KinBodyStateSaver(KinBodyPtr pbody, int options) : _ } } +KinBody::KinBodyStateSaver::KinBodyStateSaver(KinBodyPtr pbody, const KinBodyStateSaver& reference) : _pbody(pbody), _options(reference._options), _bRestoreOnDestructor(reference._bRestoreOnDestructor) +{ + if( _options & Save_LinkTransformation ) { + _vLinkTransforms = reference._vLinkTransforms; + _vdoflastsetvalues = reference._vdoflastsetvalues; + } + if( _options & Save_LinkEnable ) { + _vEnabledLinks = reference._vEnabledLinks; + } + if( _options & Save_LinkVelocities ) { + _vLinkVelocities = reference._vLinkVelocities; + } + if( _options & Save_JointMaxVelocityAndAcceleration ) { + _vMaxVelocities = reference._vMaxVelocities; + _vMaxAccelerations = reference._vMaxAccelerations; + _vMaxJerks = reference._vMaxJerks; + } + if( _options & Save_JointWeights ) { + _vDOFWeights = reference._vDOFWeights; + } + if( _options & Save_JointLimits ) { + _vDOFLimits[0] = reference._vDOFLimits[0]; + _vDOFLimits[1] = reference._vDOFLimits[1]; + } + if( _options & Save_JointResolutions ) { + _vDOFResolutions = reference._vDOFResolutions; + } + if( _options & Save_GrabbedBodies ) { + _ThrowOnInvalidCopyFromOtherSaver(pbody->GetEnv()->GetNameId().c_str(), "KinBodyStateSaver", Save_GrabbedBodies, _options); + } +} + +void KinBody::KinBodyStateSaver::_ThrowOnInvalidCopyFromOtherSaver(const char* envNameId, const char* className, const SaveParameters param, const int options) +{ + // The 'param' is not supported since it requires cloning of pointer and it's not always feasible between two difference state savers. + throw OPENRAVE_EXCEPTION_FORMAT(_("env=%s, %s construct from other saver for option=\"%s\" is not supported. (options=%d)"), + envNameId % className % _GetSaveParameterString(param) % options, + ORE_InvalidArguments); +} + KinBody::KinBodyStateSaver::~KinBodyStateSaver() { if( _bRestoreOnDestructor && !!_pbody && _pbody->GetEnvironmentBodyIndex() != 0 ) { diff --git a/src/libopenrave/robot.cpp b/src/libopenrave/robot.cpp index c9c796eec4..fb3ab867bd 100644 --- a/src/libopenrave/robot.cpp +++ b/src/libopenrave/robot.cpp @@ -540,6 +540,28 @@ RobotBase::RobotStateSaver::RobotStateSaver(RobotBasePtr probot, int options) : } } +RobotBase::RobotStateSaver::RobotStateSaver(RobotBasePtr probot, const RobotStateSaver& referenceSaver) : KinBodyStateSaver(probot, referenceSaver), _probot(probot) +{ + if( _options & Save_ActiveDOF ) { + vactivedofs = referenceSaver.vactivedofs; + affinedofs = referenceSaver.affinedofs; + rotationaxis = referenceSaver.rotationaxis; + } + if( _options & Save_ActiveManipulator ) { + _ThrowOnInvalidCopyFromOtherSaver(probot->GetEnv()->GetNameId().c_str(), "RobotStateSaver", Save_ActiveManipulator, _options); + } + if( _options & Save_ActiveManipulatorToolTransform ) { + _ThrowOnInvalidCopyFromOtherSaver(probot->GetEnv()->GetNameId().c_str(), "RobotStateSaver", Save_ActiveManipulatorToolTransform, _options); + } + if( _options & Save_ManipulatorsToolTransform ) { + _ThrowOnInvalidCopyFromOtherSaver(probot->GetEnv()->GetNameId().c_str(), "RobotStateSaver", Save_ManipulatorsToolTransform, _options); + } + + if( (_options & Save_ConnectedBodies) || (_options & Save_GrabbedBodies) ) { + _vConnectedBodyActiveStates = referenceSaver._vConnectedBodyActiveStates; + } +} + RobotBase::RobotStateSaver::~RobotStateSaver() { if( _bRestoreOnDestructor && !!_probot && _probot->GetEnvironmentBodyIndex() != 0 ) {