Enable to copy the saver's state when constructing Grabbed from cloni…

…ng context. - Issue - #1455 - When _listNonCollidingIsValid = false and cloning of grabbed is conducted, the new saver state is different from the original saver state. - This leads the inconsistent results between the new Grabbed and the original Grabbed. - Resolution - Directly copy the saver state from the original one.
rdiankov · Dec 27, 2024 · bd2097a · bd2097a
1 parent 522410a
commit bd2097a
Show file tree

Hide file tree

Showing 6 changed files with 146 additions and 2 deletions.
diff --git 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<Transform> _vLinkTransforms;
@@ -3867,7 +3879,13 @@ class OPENRAVE_API KinBody : public InterfaceBase
 class OPENRAVE_API Grabbed : public UserData, public boost::enable_shared_from_this<Grabbed>
 {
 public:
+    /// \brief constructor. This is standard one.
     Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink);
+
+    /// \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 Grabbed& referenceGrabbed);
+
     virtual ~Grabbed() {
     }
 

diff --git a/include/openrave/robot.h b/include/openrave/robot.h
@@ -919,7 +919,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
@@ -5901,7 +5901,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())));
+                GrabbedPtr pgrabbed(new Grabbed(pgrabbedbody,_veclinks.at(KinBody::LinkPtr(pgrabbedref->_pGrabbingLink)->GetIndex()), *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
@@ -113,6 +113,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<RobotBase>(pBody);
+        const RobotBase::RobotStateSaverPtr pRobotReferenceSaver = OPENRAVE_DYNAMIC_POINTER_CAST<RobotBase::RobotStateSaver>(pReferenceSaver);
+        pSaver.reset(new RobotBase::RobotStateSaver(pRobot, *pRobotReferenceSaver));
+    }
+    else {
+        pSaver.reset(new KinBody::KinBodyStateSaver(pBody, *pReferenceSaver));
+    }
+}
+
 Grabbed::Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink)
 {
     _pGrabbedBody = pGrabbedBody;
@@ -132,6 +147,22 @@ Grabbed::Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink)
                                      bDisableRestoreOnDestructor);
 } // end Grabbed
 
+Grabbed::Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink, const Grabbed& referenceGrabbed)
+{
+    _pGrabbedBody = pGrabbedBody;
+    _pGrabbingLink = pGrabbingLink;
+    _pGrabbingLink->GetRigidlyAttachedLinks(_vAttachedToGrabbingLink);
+    _listNonCollidingIsValid = false;
+    _CreateSaverForGrabbedAndGrabber(_pGrabbedSaver,
+                                     pGrabbedBody,
+                                     referenceGrabbed._pGrabbedSaver);
+
+    KinBodyPtr pGrabber = RaveInterfaceCast<KinBody>(_pGrabbingLink->GetParent());
+    _CreateSaverForGrabbedAndGrabber(_pGrabberSaver,
+                                     pGrabber,
+                                     referenceGrabbed._pGrabberSaver);
+} // end Grabbed
+
 void Grabbed::AddMoreIgnoreLinks(const std::set<int>& setAdditionalGrabberLinksToIgnore)
 {
     KinBodyPtr pGrabber = RaveInterfaceCast<KinBody>(_pGrabbingLink->GetParent());

diff --git 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.
@@ -115,7 +139,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));
+                    GrabbedPtr pNewGrabbed(new Grabbed(pNewGrabbedBody, pNewGrabbingLink, *pGrabbed));
                     pNewGrabbed->_tRelative = pGrabbed->_tRelative;
                     pNewGrabbed->_setGrabberLinkIndicesToIgnore = savedGrabbedData.setGrabberLinkIndicesToIgnore;
                     if( savedGrabbedData.listNonCollidingIsValid ) {
@@ -273,6 +297,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
@@ -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 ) {