WIP: Fix clone and restore for invalid non colliding cache

CMakeLists.txt

@@ -4,7 +4,7 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE )
 
 # Define here the needed parameters
 set (OPENRAVE_VERSION_MAJOR 0)
-set (OPENRAVE_VERSION_MINOR 158)
+set (OPENRAVE_VERSION_MINOR 160)
 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})

docs/source/changelog.rst

@@ -3,6 +3,16 @@
 ChangeLog
 #########
 
+Version 0.160.0
+===============
+
+- Copy the internal states of `_pGrabbedSaver` and `_pGrabberSaver` of `Grabbed` instances when cloning or restoring, to avoid incorrect computation of the non colliding list cache.
+
+Version 0.159.0
+===============
+
+- Revive the order of grabbed bodies to avoid less deterministic grabbed self collision checking.
+
 Version 0.158.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;
@@ -2489,6 +2501,7 @@ class OPENRAVE_API KinBody : public InterfaceBase
         std::vector<dReal> _vMaxVelocities, _vMaxAccelerations, _vMaxJerks, _vDOFWeights, _vDOFLimits[2], _vDOFResolutions;
         std::unordered_map<int, SavedGrabbedData> _grabbedDataByEnvironmentIndex;
         std::unordered_map<uint64_t, ListNonCollidingLinkPairs> _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed;
+        uint64_t _nextGrabbedBodyUniqueId = 0;
         bool _bRestoreOnDestructor;
 private:
         virtual void _RestoreKinBody(boost::shared_ptr<KinBody> body);
@@ -2536,6 +2549,7 @@ class OPENRAVE_API KinBody : public InterfaceBase
         std::vector<dReal> _vMaxVelocities, _vMaxAccelerations, _vMaxJerks, _vDOFWeights, _vDOFLimits[2], _vDOFResolutions;
         std::unordered_map<int, SavedGrabbedData> _grabbedDataByEnvironmentIndex;
         std::unordered_map<uint64_t, ListNonCollidingLinkPairs> _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed;
+        uint64_t _nextGrabbedBodyUniqueId = 0;
         bool _bRestoreOnDestructor;
         bool _bReleased; ///< if true, then body should not be restored
 private:
@@ -3722,11 +3736,13 @@ class OPENRAVE_API KinBody : public InterfaceBase
     /// \param[in] options : SaveParameters inside of saver.
     /// \param[in] savedGrabbedBodiesByEnvironmentIndex : _grabbedBodiesByEnvironmentIndex held in saver.
     /// \param[in] savedMapListNonCollidingInterGrabbedLinkPairsWhenGrabbed : _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed held in saver.
+    /// \param[in] savedNextGrabbedBodyUniqueId : _nextGrabbedBodyUniqueId in saver.
     /// \param[in] bCalledFromClone : true this is called from clone, e.g. called from _RestoreGrabbedBodiesForClone. false if  Assumes that this is called from _RestoreKinBody of saver classes.
     void _RestoreGrabbedBodiesFromSavedData(const KinBody& savedBody,
                                             const int options,
                                             const std::unordered_map<int, SavedGrabbedData>& savedGrabbedDataByEnvironmentIndex,
                                             const std::unordered_map<uint64_t, ListNonCollidingLinkPairs>& savedMapListNonCollidingInterGrabbedLinkPairsWhenGrabbed,
+                                            const uint64_t savedNextGrabbedBodyUniqueId,
                                             const bool bCalledFromClone = false);
 
     /// \brief Save this kinbody's information.
@@ -3751,6 +3767,12 @@ class OPENRAVE_API KinBody : public InterfaceBase
     /// \brief Extract the first body's environmentBodyIndex from environment body indices pair.
     static int _GetSecondEnvironmentBodyIndexFromPair(const uint64_t pair);
 
+    /// \brief print the computed _vNonAdjacentLinks contents.
+    /// \param[in] vNonAdjacentLinks : from KinBody::_vNonAdjacentLinks
+    /// \param[in] nonAdjacentMask : index of element to print, which is mask. _vNonAdjacentLinks[nonAdjacentMask] is printed.
+    /// \param[in] envNameId, bodyName : for print message.
+    static void _PrintNonAdjacentLinks(const boost::array<std::vector<int>, 4>& vNonAdjacentLinks, const size_t nonAdjacentMask, const std::string& envNameId, const std::string& bodyName);
+
     std::string _name; ///< name of body
 
     std::vector<JointPtr> _vecjoints; ///< \see GetJoints
@@ -3832,6 +3854,7 @@ class OPENRAVE_API KinBody : public InterfaceBase
     /// - Each link pair (grabbed1Link, grabbed2Link) in ListNonCollidingLinkPair must be such that the first element
     ///   corresponds to the grabbed body with lower environment body index.
     std::unordered_map<uint64_t, ListNonCollidingLinkPairs> _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed;
+    uint64_t _nextGrabbedBodyUniqueId = 0; ///< This indicates the unique id of the next grabbed body. Monotonically increasing, except for the resetting when all grabbed bodies are released, ...etc. Lower unique id means grabbed earlier, and we can identify the order of grabbed bodies through this Id.
 
 private:
     mutable std::vector<dReal> _vTempJoints;
@@ -3867,7 +3890,13 @@ class OPENRAVE_API KinBody : public InterfaceBase
 class OPENRAVE_API Grabbed : public UserData, public boost::enable_shared_from_this<Grabbed>
 {
 public:
-    Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink);
+    /// \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() {
     }
 
@@ -3911,6 +3940,7 @@ class OPENRAVE_API Grabbed : public UserData, public boost::enable_shared_from_t
     Transform _tRelative; ///< the relative transform between the grabbed body and the grabbing link. tGrabbingLink*tRelative = tGrabbedBody.
     std::set<int> _setGrabberLinkIndicesToIgnore; ///< indices to the links of the grabber whose collisions with the grabbed bodies should be ignored.
     rapidjson::Document _rGrabbedUserData; ///< user-defined data to be updated when kinbody grabs and releases objects
+    const uint64_t _uniqueId = 0; ///< The unique id of this Grabbed instance. Lower unique id means grabbed earlier, and we can identify the order of grabbed bodies through this Id.
 private:
 
     /// \brief update grabber's _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed. if there is the existing list, push the inter-grabbed link pairs to it. otherwise, create the new list in the map and push the inter-grabbed link pairs to it.

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

@@ -65,6 +65,21 @@ inline void _ResizeVectorFor2DTable(std::vector<int8_t>& vec, size_t vectorSize)
     }
 }
 
+static void _PrintDOFValuesForInitialLinkTransformations(const KinBody& body, const std::vector<dReal>& vdoflastsetvalues, const char* context)
+{
+    if( body.GetDOF() == 0 ) {
+        return;
+    }
+    std::stringstream ssJoints;
+    for(size_t iDOF = 0; iDOF < vdoflastsetvalues.size(); ++iDOF) {
+        if( iDOF > 0 ) {
+            ssJoints << ",";
+        }
+        ssJoints << vdoflastsetvalues[iDOF];
+    }
+    RAVELOG_INFO_FORMAT("env='%s', body '%s' _vInitialLinkTransformations is updated in %s by dofValues=[%s]", body.GetEnv()->GetNameId()%body.GetName()%context%ssJoints.str());
+}
+
 class ChangeCallbackData : public UserData
 {
 public:
@@ -5113,6 +5128,7 @@ void KinBody::_ComputeInternalInformation()
                 RAVELOG_VERBOSE(str(boost::format("dof %d has different values after SetDOFValues %d!=%d, this could be due to mimic joint equations kicking into effect.")%i%vprevdoflastsetvalues.at(i)%vnewdoflastsetvalues.at(i)));
             }
         }
+        _PrintDOFValuesForInitialLinkTransformations(*this, vnewdoflastsetvalues, __FUNCTION__);
         _vInitialLinkTransformations = vnewtrans;
     }
 
@@ -5585,6 +5601,7 @@ void KinBody::SetNonCollidingConfiguration()
     _ResetInternalCollisionCache();
     vector<dReal> vdoflastsetvalues;
     GetLinkTransformations(_vInitialLinkTransformations, vdoflastsetvalues);
+    _PrintDOFValuesForInitialLinkTransformations(*this, vdoflastsetvalues, __FUNCTION__);
 }
 
 void KinBody::_ResetInternalCollisionCache()
@@ -5615,6 +5632,20 @@ bool CompareNonAdjacentFarthest(int pair0, int pair1)
     return dist0 > dist1;
 }
 
+void KinBody::_PrintNonAdjacentLinks(const boost::array<std::vector<int>, 4>& vNonAdjacentLinks, const size_t nonAdjacentMask, const std::string& envNameId, const std::string& bodyName)
+{
+    std::stringstream ssLinks;
+    const std::vector<int>& vSelectedNonAdjacentLinks = vNonAdjacentLinks[nonAdjacentMask];
+    for(size_t iLinks = 0; iLinks < vSelectedNonAdjacentLinks.size(); ++iLinks) {
+        const int value = vSelectedNonAdjacentLinks[iLinks];
+        if( iLinks > 0 ) {
+            ssLinks << ",";
+        }
+        ssLinks << "(" << (value & 0xffff) << "," << (value>>16) << ")";
+    }
+    RAVELOG_INFO_FORMAT("env='%s', body '%s' computes the cache for GetNonAdjacentLinks(%d). linkPairs=[%s]", envNameId%bodyName%nonAdjacentMask%ssLinks.str());
+}
+
 const std::vector<int>& KinBody::GetNonAdjacentLinks(int adjacentoptions) const
 {
     class TransformsSaver
@@ -5663,6 +5694,7 @@ const std::vector<int>& KinBody::GetNonAdjacentLinks(int adjacentoptions) const
         std::sort(_vNonAdjacentLinks[0].begin(), _vNonAdjacentLinks[0].end(), CompareNonAdjacentFarthest);
         _nUpdateStampId++; // because transforms were modified
         _nNonAdjacentLinkCache = 0;
+        KinBody::_PrintNonAdjacentLinks(_vNonAdjacentLinks, 0, GetEnv()->GetNameId(), GetName());
     }
     if( (_nNonAdjacentLinkCache&adjacentoptions) != adjacentoptions ) {
         int requestedoptions = (~_nNonAdjacentLinkCache)&adjacentoptions;
@@ -5878,6 +5910,7 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions)
     _listAttachedBodies.clear(); // will be set in the environment
     _grabbedBodiesByEnvironmentIndex.clear();
     _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.clear();
+    _nextGrabbedBodyUniqueId = r->_nextGrabbedBodyUniqueId;
     if ((cloningoptions & Clone_IgnoreGrabbedBodies) != Clone_IgnoreGrabbedBodies) {
         for (const MapGrabbedByEnvironmentIndex::value_type& otherGrabPair : r->_grabbedBodiesByEnvironmentIndex) {
             const GrabbedPtr& pgrabbedref = otherGrabPair.second;
@@ -5901,7 +5934,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->_uniqueId, *pgrabbedref));
                 pgrabbed->_tRelative = pgrabbedref->_tRelative;
                 pgrabbed->_setGrabberLinkIndicesToIgnore = pgrabbedref->_setGrabberLinkIndicesToIgnore; // can do this since link indices are the same
                 CopyRapidJsonDoc(pgrabbedref->_rGrabbedUserData, pgrabbed->_rGrabbedUserData);