Merge remote-tracking branch 'origin/fixCloneAndRestoreForInvalidNonC…

…olCache' into fixGrabbedSelfCollisionCheckInOtherAPIs

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
 ===============

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;
@@ -3868,7 +3880,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, 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() {
     }
 

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

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

src/libopenrave/kinbodygrab.cpp

@@ -51,24 +51,17 @@ static bool _IsLinkPairIncluded(const KinBody::Link* pLink1ToSearch,
 static void _PushLinkPairsIfNonCollidingWithOtherGrabbedBody(std::list<std::pair<KinBody::LinkConstPtr, KinBody::LinkConstPtr> >& 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);
-        for (const KinBody::LinkPtr& pGrabbedBodylink : grabbedBody.GetLinks()) {
+    for (const KinBody::LinkPtr& pGrabbedBodyLink : grabbedBody.GetLinks()) {
+        for (const KinBody::LinkPtr& pOtherGrabbedLink : otherGrabbedBody.GetLinks()) {
             // if already in the list, no need to check collision.
-            if( _IsLinkPairIncluded(pOtherGrabbedLinkToCheckConst.get(), pGrabbedBodylink.get(), listNonCollidingLinkPairs) ) {
+            if( _IsLinkPairIncluded(pGrabbedBodyLink.get(), pOtherGrabbedLink.get(), listNonCollidingLinkPairs) ) {
                 continue;
             }
             // if not colliding, push.
-            if( !pchecker->CheckCollision(pOtherGrabbedLinkToCheckConst, KinBody::LinkConstPtr(pGrabbedBodylink)) ) {
-                if( bInvertFirstSecond ) {
-                    listNonCollidingLinkPairs.emplace_back(pOtherGrabbedLink, pGrabbedBodylink);
-                }
-                else {
-                    listNonCollidingLinkPairs.emplace_back(pGrabbedBodylink, pOtherGrabbedLink);
-                }
+            if( !pchecker->CheckCollision(KinBody::LinkConstPtr(pGrabbedBodyLink), KinBody::LinkConstPtr(pOtherGrabbedLink)) ) {
+                listNonCollidingLinkPairs.emplace_back(pGrabbedBodyLink, pOtherGrabbedLink);
             }
         }
     }
@@ -114,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<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, const uint64_t uniqueId)
     : _uniqueId(uniqueId)
 {
@@ -134,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<KinBody>(_pGrabbingLink->GetParent());
+    _CreateSaverForGrabbedAndGrabber(_pGrabberSaver,
+                                     pGrabber,
+                                     referenceGrabbed._pGrabberSaver);
+} // end Grabbed
+
 void Grabbed::AddMoreIgnoreLinks(const std::set<int>& setAdditionalGrabberLinksToIgnore)
 {
     KinBodyPtr pGrabber = RaveInterfaceCast<KinBody>(_pGrabbingLink->GetParent());
@@ -302,25 +327,23 @@ void Grabbed::_PushNonCollidingLinkPairsForGrabbedBodies(KinBodyPtr& pGrabber,
     const uint64_t indexForTest0 = KinBody::_ComputeEnvironmentBodyIndicesPair(grabbedBody, otherGrabbedBody);
     std::unordered_map<uint64_t, KinBody::ListNonCollidingLinkPairs>::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;
     }
 
     // Find item by (otherGrabbedBody, grabbedBody) pair.
     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.
-    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);
+    if( listNonCollidingLinkPairs.size() > 0 ) {
+        itInfo = pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(indexForTest0, std::move(listNonCollidingLinkPairs)).first;
     }
 }
 

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 ) {
@@ -187,6 +211,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();
@@ -259,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 ) {

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