From 2198a21131440aa9831edf2f4bf508506dfcc45f Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 25 Sep 2024 11:58:13 +0900 Subject: [PATCH 01/25] Fix the incorrect computation of self collision for grabbed bodies. - Issue - This commit tries to improve the reported issue1, issue2, and issue3 : https://github.com/rdiankov/openrave/issues/1436 - Previously, _listNonCollidingLinksWhenGrabbed is asymmetric : the results obtained from grabbedBody1 is not same as the results obtained from grabbedBody2. - However, many codes in CheckSelfCollision assumes it's symmetric. The assumption was broken. In addition, such breakage was amplified after https://github.com/rdiankov/openrave/pull/1421. - Resolution - Instead of store the target link like the previous _listNonCollidingLinksWhenGrabbed_, store the information about the link pairs. This is more accurate and the same methodologies as non-adjacent-links in self colision checking. - Separate grabbed-grabber link pair and inter-grabbed link pair. - grabbed-grabber link pair, e.g. object-robot link pair, still exists in the Grabbed class as before. - inter-grabbed link pair now exists in the KinBody class, since if it's held in Grabbed, it becomes inconsistent and asymmetric. - Following the same methodologies in https://github.com/rdiankov/openrave/pull/1421, inter-grabbed link pairs are stored as unordered_map, and its key is combined env body indices of two grabbed bodies. --- include/openrave/kinbody.h | 36 +++++- src/libopenrave/kinbody.cpp | 59 ++++++--- src/libopenrave/kinbodygrab.cpp | 173 ++++++++++++++++++++------ src/libopenrave/kinbodystatesaver.cpp | 92 ++++++++++---- 4 files changed, 276 insertions(+), 84 deletions(-) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index b7e0ce871c..d636615ff6 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -2438,6 +2438,9 @@ class OPENRAVE_API KinBody : public InterfaceBase typedef boost::shared_ptr KinBodyInfoPtr; typedef boost::shared_ptr KinBodyInfoConstPtr; + /// \brief Alias for list of non-colliding link pairs, mostly used for Grabbed checking. + using ListNonCollidingLinkPairs = std::list >; + /// \brief Saved data for Grabbed used in KinBodyStateSaver and KinBodyStateSaverRef /// When KinBody::Grab, KinBody::Release, ...etc are called, new Grabbed instance is created in KinBody and the original ptr for the original Grabbed instance is swapped. /// Thus, the original information of Grabbed instance is unchanged and holding the ptr of it as pGrabbed is enough for the saver. @@ -2446,7 +2449,7 @@ class OPENRAVE_API KinBody : public InterfaceBase struct SavedGrabbedData { GrabbedPtr pGrabbed; ///< pointer of original Grabbed instance, which originally in KinBody's _grabbedBodiesByEnvironmentIndex. - std::list listNonCollidingLinksWhenGrabbed; ///< copied values of Grabbed's _listNonCollidingLinksWhenGrabbed. See also the documentation of Grabbed class. + ListNonCollidingLinkPairs listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed; ///< copied values of Grabbed's _listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed. See also the documentation of Grabbed class. std::set setGrabberLinkIndicesToIgnore; ///< copied values of Grabbed's _setGrabberLinkIndicesToIgnore. See also the documentation of Grabbed class. bool listNonCollidingIsValid = false; ///< copied values of Grabbed's _listNonCollidingIsValid. See also the documentation of Grabbed class. }; @@ -2485,6 +2488,7 @@ class OPENRAVE_API KinBody : public InterfaceBase std::vector _vdoflastsetvalues; std::vector _vMaxVelocities, _vMaxAccelerations, _vMaxJerks, _vDOFWeights, _vDOFLimits[2], _vDOFResolutions; std::unordered_map _grabbedDataByEnvironmentIndex; + std::unordered_map _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed; bool _bRestoreOnDestructor; private: virtual void _RestoreKinBody(boost::shared_ptr body); @@ -2531,6 +2535,7 @@ class OPENRAVE_API KinBody : public InterfaceBase std::vector _vdoflastsetvalues; std::vector _vMaxVelocities, _vMaxAccelerations, _vMaxJerks, _vDOFWeights, _vDOFLimits[2], _vDOFResolutions; std::unordered_map _grabbedDataByEnvironmentIndex; + std::unordered_map _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed; bool _bRestoreOnDestructor; bool _bReleased; ///< if true, then body should not be restored private: @@ -3700,16 +3705,32 @@ class OPENRAVE_API KinBody : public InterfaceBase /// \param[in] savedBody : saved KinBody inside of saver. /// \param[in] options : SaveParameters inside of saver. /// \param[in] savedGrabbedBodiesByEnvironmentIndex : _grabbedBodiesByEnvironmentIndex held in saver. + /// \param[in] savedMapListNonCollidingInterGrabbedLinkPairsWhenGrabbed : _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed held 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& savedGrabbedDataByEnvironmentIndex, + const std::unordered_map& savedMapListNonCollidingInterGrabbedLinkPairsWhenGrabbed, const bool bCalledFromClone = false); /// \brief Save this kinbody's information. /// \param[out] savedGrabbedDataByEnvironmentIndex : saved information about _grabbedBodiesByEnvironmentIndex. void _SaveKinBodySavedGrabbedData(std::unordered_map& savedGrabbedDataByEnvironmentIndex) const; + /// \brief Check if IsListNonCollidingLinksValid is true for the Grabbed instance with the given envBodyIndex. + /// \param[int] envBodyIndex : env body index. + bool _IsListNonCollidingLinksValidFromEnvironmentBodyIndex(const int envBodyIndex) const; + + /// \brief Compute environment body indices pair. pack the two bodies' envBodyIndices (32bit int) into one environment body indices pair (uint64_t). + /// Here, environment body indices pair is uint64_t, which higher 32bits are for body2 envBodyIndex, and which lower 32bits are for body1 envBodyIndex. + static uint64_t _ComputeEnvironmentBodyIndicesPair(const KinBody& body1, const KinBody& body2); + + /// \brief Extract the first body's environmentBodyIndex from environment body indices pair. + static int _GetFirstEnvironmentBodyIndexFromPair(const uint64_t pair); + + /// \brief Extract the first body's environmentBodyIndex from environment body indices pair. + static int _GetSecondEnvironmentBodyIndexFromPair(const uint64_t pair); + std::string _name; ///< name of body std::vector _vecjoints; ///< \see GetJoints @@ -3773,6 +3794,7 @@ class OPENRAVE_API KinBody : public InterfaceBase mutable std::string __hashKinematicsGeometryDynamics; ///< hash serializing kinematics, dynamics and geometry properties of the KinBody int64_t _lastModifiedAtUS=0; ///< us, linux epoch, last modified time of the kinbody when it was originally loaded from the environment. int64_t _revisionId = 0; ///< the webstack revision for this loaded kinbody + std::unordered_map _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed; ///< map of list of link pairs. This is computed when grabbed bodies are grabbed, and at taht time, two grabbed bodies are not touching each other. Since these links are not colliding at the time of grabbing, they should remain non-colliding with the grabbed body throughout. If, while grabbing, they collide with the grabbed body at some point, CheckSelfCollision should return true. It is important to note that the enable state of a link does *not* affect its membership of this list. Each pair in the list should be [Grabbed1-link, Grabbed2-link]. Note that this does not contain link pairs of [Grabbed-link, Grabber-link], c.f. Grabbed::_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed. Note that the key of this map is 'environment body indices pair', which higher 32bits are for the first KinBody's envBodyIndex, and which lower 32bits are for the second KinBody's envBodyIndex. Please also see _ComputeEnvironmentBodyIndicesPair. private: mutable std::vector _vTempJoints; @@ -3853,11 +3875,21 @@ class OPENRAVE_API Grabbed : public UserData, public boost::enable_shared_from_t // Member Variables KinBodyWeakPtr _pGrabbedBody; ///< the body being grabbed KinBody::LinkPtr _pGrabbingLink; ///< the link used for grabbing _pGrabbedBody. Its transform (as well as the transforms of other links rigidly attached to _pGrabbingLink) relative to the grabbed body remains constant until the grabbed body is released. - std::list _listNonCollidingLinksWhenGrabbed; ///< list of links of the grabber that are not touching the grabbed body *at the time of grabbing*. Since these links are not colliding with the grabbed body at the time of grabbing, they should remain non-colliding with the grabbed body throughout. If, while grabbing, they collide with the grabbed body at some point, CheckSelfCollision should return true. It is important to note that the enable state of a link does *not* affect its membership of this list. + KinBody::ListNonCollidingLinkPairs _listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed; ///< list of link pairs of the grabber that are not touching the grabbed body *at the time of grabbing*. Since these links are not colliding with the grabbed body at the time of grabbing, they should remain non-colliding with the grabbed body throughout. If, while grabbing, they collide with the grabbed body at some point, CheckSelfCollision should return true. It is important to note that the enable state of a link does *not* affect its membership of this list. Each pair in the list should be [Grabbed-link, Grabber-link]. Note that this does not contain link pairs from two grabbed bodies, c.f. KinBody::_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed. Transform _tRelative; ///< the relative transform between the grabbed body and the grabbing link. tGrabbingLink*tRelative = tGrabbedBody. std::set _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 private: + + /// \brief push inter-grabbed-bodies non colliding link pairs to grabber. + /// \param[out] pGrabber : updated grabber. + /// \param[out] pchecker : collision checker + /// \param[in] grabbedBody, otherGrabbedBody : grabbed body by this class, and other grabbed body to check. + void _PushNonCollidingLinkPairsForGrabbedBodies(KinBodyPtr& pGrabber, + CollisionCheckerBasePtr& pchecker, + const KinBody& grabbedBody, + const KinBody& otherGrabbedBody); + bool _listNonCollidingIsValid = false; ///< a flag indicating whether the current _listNonCollidingLinksWhenGrabbed is valid or not. std::vector _vAttachedToGrabbingLink; ///< vector of all links that are rigidly attached to _pGrabbingLink KinBody::KinBodyStateSaverPtr _pGrabberSaver; ///< statesaver that saves the snapshot of the grabber at the time Grab is called. The saved state will be used (i.e. restored) temporarily when computation of _listNonCollidingLinksWhenGrabbed is necessary. diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 440732093a..cabdf0d5e4 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -5791,6 +5791,7 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) // clone the grabbed bodies, note that this can fail if the new cloned environment hasn't added the bodies yet (check out Environment::Clone) _listAttachedBodies.clear(); // will be set in the environment _grabbedBodiesByEnvironmentIndex.clear(); + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.clear(); if ((cloningoptions & Clone_IgnoreGrabbedBodies) != Clone_IgnoreGrabbedBodies) { for (const MapGrabbedByEnvironmentIndex::value_type& otherGrabPair : r->_grabbedBodiesByEnvironmentIndex) { const GrabbedPtr& pgrabbedref = otherGrabPair.second; @@ -5819,25 +5820,12 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) pgrabbed->_setGrabberLinkIndicesToIgnore = pgrabbedref->_setGrabberLinkIndicesToIgnore; // can do this since link indices are the same CopyRapidJsonDoc(pgrabbedref->_rGrabbedUserData, pgrabbed->_rGrabbedUserData); if( pgrabbedref->IsListNonCollidingLinksValid() ) { - FOREACHC(itLinkRef, pgrabbedref->_listNonCollidingLinksWhenGrabbed) { - if( (*itLinkRef)->GetParent() == r ) { - pgrabbed->_listNonCollidingLinksWhenGrabbed.push_back(_veclinks.at((*itLinkRef)->GetIndex())); - } - else { - KinBodyPtr pOtherGrabbedBody = GetEnv()->GetKinBody((*itLinkRef)->GetParent()->GetName()); - if( !!pOtherGrabbedBody ) { - KinBody::LinkPtr plink = pOtherGrabbedBody->GetLink((*itLinkRef)->GetName()); - if( !!plink ) { - pgrabbed->_listNonCollidingLinksWhenGrabbed.push_back(plink); - } - else { - RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find non-colliding link %s in body %s.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%(*itLinkRef)->GetName()%(*itLinkRef)->GetParent()->GetName()); - } - } - else { - RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find body %s for non-colliding link %s.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%(*itLinkRef)->GetParent()->GetName()%(*itLinkRef)->GetName()); - } + FOREACHC(itLinkRef, pgrabbedref->_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed) { + if( (*itLinkRef).second->GetParent() != r ) { + // TODO : warning + continue; } + pgrabbed->_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed.emplace_back(pgrabbedbody->GetLink((*itLinkRef).first->GetName()), _veclinks.at((*itLinkRef).second->GetIndex())); } pgrabbed->_SetLinkNonCollidingIsValid(true); } @@ -5855,6 +5843,41 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) _grabbedBodiesByEnvironmentIndex[pgrabbedbody->GetEnvironmentBodyIndex()] = std::move(pgrabbed); } } // end for pgrabbedref + + // map list of inter grabbed. + FOREACHC(itInfo, r->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed) { + const ListNonCollidingLinkPairs& pairs = (*itInfo).second; + if( pairs.empty() ) { + continue; + } + if( !_IsListNonCollidingLinksValidFromEnvironmentBodyIndex(_GetFirstEnvironmentBodyIndexFromPair(itInfo->first)) || + !_IsListNonCollidingLinksValidFromEnvironmentBodyIndex(_GetSecondEnvironmentBodyIndexFromPair(itInfo->first)) ) { + continue; + } + const KinBodyPtr pFirstRef = pairs.front().first->GetParent(true); + const KinBodyPtr pSecondRef = pairs.front().second->GetParent(true); + if( !pFirstRef || !pSecondRef ) { + // TODO : warning + continue; // somehow, relevant code in the above does not show warning nor exception. so, follow it for now. + } + const KinBodyPtr pFirst = GetEnv()->GetKinBody(pFirstRef->GetName()); + const KinBodyPtr pSecond = GetEnv()->GetKinBody(pSecondRef->GetName()); + if( !pFirst || !pSecond ) { + // WARNING + //RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find body %s for non-colliding link %s.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%(*itLinkRef).second->GetParent()->GetName()%(*itLinkRef).second->GetName()); + continue; + } + ListNonCollidingLinkPairs listNonCollidingLinkPairs; + FOREACHC(itLinkPair, itInfo->second) { + listNonCollidingLinkPairs.emplace_back(pFirst->GetLink((*itLinkPair).first->GetName()), pSecond->GetLink((*itLinkPair).second->GetName())); + // TODO : warning + // RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find non-colliding link %s in body %s.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%(*itLinkRef).second->GetName()%(*itLinkRef).second->GetParent()->GetName()); + } + if( listNonCollidingLinkPairs.size() > 0 ){ + const uint64_t key = _ComputeEnvironmentBodyIndicesPair(*pFirst, *pSecond); + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(key, std::move(listNonCollidingLinkPairs)); + } + } } // end if not Clone_IgnoreGrabbedBodies // Clone self-collision checker diff --git a/src/libopenrave/kinbodygrab.cpp b/src/libopenrave/kinbodygrab.cpp index 197c9ba9d2..c39f267ece 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -18,32 +18,58 @@ namespace OpenRAVE { -/// \brief Push link to listNonCollidingLinksWhenGrabbed, only if it has no collision with the whole grabbedBody. -static void _PushLinkIfNonColliding(std::list& listNonCollidingLinksWhenGrabbed, +/// \brief Push link to listNonCollidingLinkPairs if the given links are not in collision each other. This is for grabbed-grabber link pairs. +static void _PushLinkIfNonColliding(std::list >& listNonCollidingLinkPairs, CollisionCheckerBasePtr& pchecker, - const KinBody::LinkPtr& pLinkToCheck, const KinBody& grabbedBody) + const KinBody::LinkPtr& pGrabberLinkToCheck, const KinBody& grabbedBody) { - KinBody::LinkConstPtr pLinkToCheckConst(pLinkToCheck); + KinBody::LinkConstPtr pGrabberLinkToCheckConst(pGrabberLinkToCheck); for (const KinBody::LinkPtr& pGrabbedBodylink : grabbedBody.GetLinks()) { - if( pchecker->CheckCollision(pLinkToCheckConst, KinBody::LinkConstPtr(pGrabbedBodylink)) ) { - return; // if colliding, do not push. + if( !pchecker->CheckCollision(pGrabberLinkToCheckConst, KinBody::LinkConstPtr(pGrabbedBodylink)) ) { + listNonCollidingLinkPairs.emplace_back(pGrabbedBodylink, pGrabberLinkToCheck); } } - // if not colliding with any of links in grabbedBody, push it. - listNonCollidingLinksWhenGrabbed.push_back(pLinkToCheck); } -/// \brief remove link from listNonCollidingLinksWhenGrabbed if its parent is same as the given body. -template -static void _RemoveLinkFromListNonCollidingLinksWhenGrabbed(std::list& listNonCollidingLinksWhenGrabbed, - const KinBodyPtrT& pGrabbedBody) +/// \brief Check if link pair is included in the list. The order of first/second does not matter. +static bool _IsLinkPairIncluded(const KinBody::Link* pLink1ToSearch, + const KinBody::Link* pLink2ToSearch, + const std::list >& listNonCollidingLinkPairs) { - for (std::list::iterator itlink = listNonCollidingLinksWhenGrabbed.begin(); itlink != listNonCollidingLinksWhenGrabbed.end();) { - if ((*itlink)->GetParent() == pGrabbedBody) { - itlink = listNonCollidingLinksWhenGrabbed.erase(itlink); + FOREACHC(itLinks, listNonCollidingLinkPairs) { + const KinBody::Link* const pLinkFirst = (*itLinks).first.get(); + const KinBody::Link* const pLinkSecond = (*itLinks).second.get(); + if ( (pLinkFirst == pLink1ToSearch && pLinkSecond == pLink2ToSearch) || (pLinkFirst == pLink2ToSearch && pLinkSecond == pLink1ToSearch) ) { + return true; } - else { - ++itlink; + } + return false; +} + +/// \brief Push link to listNonCollidingLinkPairs if the given grabbed bodies links are not in collision each other. This is inter-grabbed link pairs. +/// \param[in] bInvertFirstSecond : true if push the pair by inverting the first and the second. +static void _PushLinkPairsIfNonCollidingWithOtherGrabbedBody(std::list >& listNonCollidingLinkPairs, + CollisionCheckerBasePtr& pchecker, + const KinBody& grabbedBody, + const KinBody& otherGrabbedBody, + const bool bInvertFirstSecond = false) +{ + for (const KinBody::LinkPtr& pOtherGrabbedLink : otherGrabbedBody.GetLinks()) { + KinBody::LinkConstPtr pOtherGrabbedLinkToCheckConst(pOtherGrabbedLink); + for (const KinBody::LinkPtr& pGrabbedBodylink : grabbedBody.GetLinks()) { + // if already in the list, no need to check collision. + if( _IsLinkPairIncluded(pOtherGrabbedLinkToCheckConst.get(), pGrabbedBodylink.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); + } + } } } } @@ -115,7 +141,14 @@ void Grabbed::AddMoreIgnoreLinks(const std::set& setAdditionalGrabberLinksT if( _listNonCollidingIsValid ) { KinBody::LinkPtr pGrabberLink = pGrabber->GetLinks().at(*itLinkIndexToIgnore); - _listNonCollidingLinksWhenGrabbed.remove(pGrabberLink); + for (KinBody::ListNonCollidingLinkPairs::iterator itPair = _listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed.begin(); itPair != _listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed.end();/* nop */) { + if ((*itPair).second == pGrabberLink) { // in this link pair, the second should be grabber link. + itPair = _listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed.erase(itPair); + } + else { + ++itPair; + } + } } } } @@ -141,8 +174,8 @@ void Grabbed::ComputeListNonCollidingLinks() _pGrabbedSaver->Restore(); _pGrabberSaver->Restore(); // note that this Restore also updates other grabbed bodies. - // Actual computation of _listNonCollidingLinksWhenGrabbed - _listNonCollidingLinksWhenGrabbed.clear(); + // Actual computation of _listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed. + _listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed.clear(); // clear only this, and do not clear the contents of _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed here, since other Grabbed instance might have updated it. EnvironmentBasePtr penv = pGrabber->GetEnv(); // if( 1 ) { // std::stringstream ssdebug; @@ -180,14 +213,6 @@ void Grabbed::ComputeListNonCollidingLinks() for (const KinBody::MapGrabbedByEnvironmentIndex::value_type& otherGrabbedPair : pGrabber->_grabbedBodiesByEnvironmentIndex) { const GrabbedPtr& pOtherGrabbed = otherGrabbedPair.second; - // Remove this pGrabbedBody from _listNonCollidingLinksWhenGrabbed in other grabbed. - // The condition when pOtherGrabbedBody is checked with pGrabbedBody might be different from the condition when this pGrabbedBody is checked with pOtherGrabbedBody now. - // In such case, it's reasonable to respect the latest condition. - // To do so, remove pOtherGrabbed->_listNonCollidingLinksWhenGrabbed first and this function will add it at the end of this function if necessary to this Grabbed's _listNonCollidingLinksWhenGrabbed. - // Note that the _listNonCollidingLinksWhenGrabbed result might not be symmetric between pOtherGrabbed and this Grabbed, e.g. this Grabbed's _listNonCollidingLinksWhenGrabbed might contain pOtherGrabbedBody, but pOtherGrabbed->_listNonCollidingLinksWhenGrabbed does not contain pGrabbedBody. - // Even if there is such asymmetricity, KinBody::CheckSelfCollision will consider the collision checking pair correctly. - _RemoveLinkFromListNonCollidingLinksWhenGrabbed(pOtherGrabbed->_listNonCollidingLinksWhenGrabbed, pGrabbedBody); - // extract valid pointers KinBodyPtr pOtherGrabbedBody = pOtherGrabbed->_pGrabbedBody.lock(); if( !pOtherGrabbedBody ) { @@ -203,10 +228,11 @@ void Grabbed::ComputeListNonCollidingLinks() continue; } - if( pOtherGrabbedBody != pGrabbedBody ) { - vGrabbedBodies.emplace_back(pOtherGrabbed.get()); - vLockedGrabbedBodiesCache.push_back(pOtherGrabbedBody); + if( pOtherGrabbedBody == pGrabbedBody ) { + continue; } + vGrabbedBodies.emplace_back(pOtherGrabbed.get()); + vLockedGrabbedBodiesCache.push_back(pOtherGrabbedBody); } KinBody::KinBodyStateSaver grabbedEnableSaver(pGrabbedBody, KinBody::Save_LinkEnable); @@ -224,7 +250,7 @@ void Grabbed::ComputeListNonCollidingLinks() // This link (*itGrabberLink) is *not* rigidly attached to _pGrabbingLink. if( _setGrabberLinkIndicesToIgnore.find((*itGrabberLink)->GetIndex()) == _setGrabberLinkIndicesToIgnore.end() ) { // Not ignoring collisions between this link and the grabber body - _PushLinkIfNonColliding(_listNonCollidingLinksWhenGrabbed, pchecker, *itGrabberLink, grabbedBody); + _PushLinkIfNonColliding(_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed, pchecker, *itGrabberLink, grabbedBody); } } } @@ -240,9 +266,7 @@ void Grabbed::ComputeListNonCollidingLinks() if (!bTwoGrabbedBodiesHaveStaticRelativePose) { KinBody::KinBodyStateSaver otherGrabbedEnableSaver(pOtherGrabbedBody, KinBody::Save_LinkEnable); pOtherGrabbedBody->Enable(true); - for (const KinBody::LinkPtr& pOtherGrabbedLink : pOtherGrabbedBody->GetLinks()) { - _PushLinkIfNonColliding(_listNonCollidingLinksWhenGrabbed, pchecker, pOtherGrabbedLink, grabbedBody); - } + _PushNonCollidingLinkPairsForGrabbedBodies(pGrabber, pchecker, grabbedBody, *pOtherGrabbedBody); } } } @@ -260,6 +284,37 @@ void Grabbed::ComputeListNonCollidingLinks() // } } +void Grabbed::_PushNonCollidingLinkPairsForGrabbedBodies(KinBodyPtr& pGrabber, + CollisionCheckerBasePtr& pchecker, + const KinBody& grabbedBody, + const KinBody& otherGrabbedBody) +{ + // Find item by (grabbedBody, otherGrabbedBody) pair. + 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); + 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); + 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; + } + } +} + bool KinBody::Grab(KinBodyPtr pGrabbedBody, LinkPtr pGrabbingLink, const rapidjson::Value& rGrabbedUserData) { // always ignore links that are statically attached to plink (ie assume they are always colliding with the body) @@ -421,6 +476,9 @@ void KinBody::Release(KinBody &body) void KinBody::ReleaseAllGrabbed() { + // just in case, always clear the map of inter grabbed pairs. + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.clear(); + // If we have no grabbed bodies, do nothing if (_grabbedBodiesByEnvironmentIndex.empty()) { return; @@ -447,6 +505,8 @@ void KinBody::ReleaseAllGrabbedWithLink(const KinBody::Link& bodyLinkToReleaseWi // If we have no grabbed bodies, do nothing if (_grabbedBodiesByEnvironmentIndex.empty()) { + // just in case, make sure to clear the map of inter grabbed pairs. + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.clear(); return; } @@ -494,6 +554,9 @@ void KinBody::RegrabAll() MapGrabbedByEnvironmentIndex originalGrabbedBodiesByBodyName; originalGrabbedBodiesByBodyName.swap(_grabbedBodiesByEnvironmentIndex); + // Since all of grabbed instances are re-created, ok to disregard the previous cache. + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.clear(); + // Regrab all the bodies again for (MapGrabbedByEnvironmentIndex::value_type& grabPair : originalGrabbedBodiesByBodyName) { const int grabbedBodyEnvIndex = grabPair.first; @@ -830,6 +893,9 @@ void KinBody::ResetGrabbed(const std::vector& vGra // Now that we are done processing our old grabs, reset the set of grabbed bodies. // Any bodies that are still grabbed will be re-added in the next pass. _grabbedBodiesByEnvironmentIndex.clear(); + + // Since all of grabbed instances are re-created, ok to disregard the previous cache. + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.clear(); } // Ensure that we reset the collision checker options when done @@ -966,13 +1032,42 @@ KinBody::MapGrabbedByEnvironmentIndex::iterator KinBody::_RemoveGrabbedBody(MapG return itGrabbed; } - // Scan through the other grabs we have and update the set of non-colliding links in those bodies to not include the links of the body we just removed - for (const MapGrabbedByEnvironmentIndex::value_type& otherGrabPair : _grabbedBodiesByEnvironmentIndex) { - const GrabbedPtr& pOtherGrabbed = otherGrabPair.second; - _RemoveLinkFromListNonCollidingLinksWhenGrabbed(pOtherGrabbed->_listNonCollidingLinksWhenGrabbed, pGrabbedBody); + // Scan through inter grabbed link pairs which contains the links of pGrabbedBody. + const int envBodyIndex = pGrabbedBody->GetEnvironmentBodyIndex(); + for (std::unordered_map::iterator itInfo = _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.begin(); itInfo != _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.end(); /* nop */) { + if ( (_GetFirstEnvironmentBodyIndexFromPair(itInfo->first) == envBodyIndex) || (_GetSecondEnvironmentBodyIndexFromPair(itInfo->first) == envBodyIndex) ) { + itInfo = _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.erase(itInfo); + } + else { + itInfo++; + } } return itGrabbed; } +bool KinBody::_IsListNonCollidingLinksValidFromEnvironmentBodyIndex(const int envBodyIndex) const +{ + MapGrabbedByEnvironmentIndex::const_iterator itGrab = _grabbedBodiesByEnvironmentIndex.find(envBodyIndex); + if( itGrab == _grabbedBodiesByEnvironmentIndex.end() ) { + return false; + } + return itGrab->second->IsListNonCollidingLinksValid(); +} + +uint64_t KinBody::_ComputeEnvironmentBodyIndicesPair(const KinBody& body1, const KinBody& body2) +{ + return static_cast(body1.GetEnvironmentBodyIndex()) + (static_cast(body2.GetEnvironmentBodyIndex()) << 32); +} + +int KinBody::_GetFirstEnvironmentBodyIndexFromPair(const uint64_t pair) +{ + return static_cast(pair & 0xffffffff); +} + +int KinBody::_GetSecondEnvironmentBodyIndexFromPair(const uint64_t pair) +{ + return static_cast(pair >> 32); +} + } // end namespace OpenRAVE diff --git a/src/libopenrave/kinbodystatesaver.cpp b/src/libopenrave/kinbodystatesaver.cpp index 2047e3a510..fb138ea4d8 100644 --- a/src/libopenrave/kinbodystatesaver.cpp +++ b/src/libopenrave/kinbodystatesaver.cpp @@ -19,18 +19,16 @@ namespace OpenRAVE { /// \brief push link to _listNonCollidingLinksWhenGrabbed of grabbed. -static void _PushLinkToListNonCollidingLinksWhenGrabbed(Grabbed& grabbed, - const int linkindex, +static bool _CheckLinkIndexForListNonCollidingLinkPairs(const int linkIndex, const std::string& linkName, const std::vector& vLinks, const EnvironmentBasePtr& pEnv) { - if( linkindex < 0 || linkindex >= (int)vLinks.size() ) { - RAVELOG_WARN_FORMAT("env=%s, could not restore link '%s' since its index %d is out of range (body num links is %d)", pEnv->GetNameId()%linkName%linkindex%vLinks.size()); - } - else { - grabbed._listNonCollidingLinksWhenGrabbed.push_back(vLinks[linkindex]); + if( linkIndex < 0 || linkIndex >= (int)vLinks.size() ) { + RAVELOG_WARN_FORMAT("env=%s, could not restore link '%s' since its index %d is out of range (body num links is %d)", pEnv->GetNameId()%linkName%linkIndex%vLinks.size()); + return false; } + return true; } void KinBody::_RestoreGrabbedBodiesForClone(const KinBody& originalBody) @@ -38,12 +36,13 @@ void KinBody::_RestoreGrabbedBodiesForClone(const KinBody& originalBody) std::unordered_map originalGrabbedDataByEnvironmentIndex; originalBody._SaveKinBodySavedGrabbedData(originalGrabbedDataByEnvironmentIndex); const int options = 0; // the following function works without Save_GrabbedBodies. also, the original code in Environment's Clone does not set Save_LinkTransformation, used in the following function. Thus, we don't need any options here and set it to 0. - _RestoreGrabbedBodiesFromSavedData(originalBody, options, originalGrabbedDataByEnvironmentIndex, true); + _RestoreGrabbedBodiesFromSavedData(originalBody, options, originalGrabbedDataByEnvironmentIndex, originalBody._mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed, true); } void KinBody::_RestoreGrabbedBodiesFromSavedData(const KinBody& savedBody, const int options, const std::unordered_map& savedGrabbedDataByEnvironmentIndex, + const std::unordered_map& savedMapListNonCollidingInterGrabbedLinkPairsWhenGrabbed, const bool bCalledFromClone) { const bool bIsFromSameEnv = GetEnv() == savedBody.GetEnv(); @@ -77,7 +76,7 @@ void KinBody::_RestoreGrabbedBodiesFromSavedData(const KinBody& savedBody, if( bIsFromSameEnv ) { // restore copied data for grabbed Grabbed& grabbed = *pGrabbed; - grabbed._listNonCollidingLinksWhenGrabbed = savedGrabbedData.listNonCollidingLinksWhenGrabbed; + grabbed._listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed = savedGrabbedData.listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed; grabbed._setGrabberLinkIndicesToIgnore = savedGrabbedData.setGrabberLinkIndicesToIgnore; grabbed._SetLinkNonCollidingIsValid(savedGrabbedData.listNonCollidingIsValid); @@ -111,24 +110,23 @@ void KinBody::_RestoreGrabbedBodiesFromSavedData(const KinBody& savedBody, pNewGrabbed->_tRelative = pGrabbed->_tRelative; pNewGrabbed->_setGrabberLinkIndicesToIgnore = savedGrabbedData.setGrabberLinkIndicesToIgnore; if( savedGrabbedData.listNonCollidingIsValid ) { - FOREACHC(itLinkSaved, savedGrabbedData.listNonCollidingLinksWhenGrabbed) { - const KinBodyPtr pParentSaved = (*itLinkSaved)->GetParent(); + FOREACHC(itLinkSaved, savedGrabbedData.listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed) { + const KinBodyPtr pParentSaved = (*itLinkSaved).second->GetParent(); if( !pParentSaved ) { - RAVELOG_WARN_FORMAT("env=%s, could not restore link '%s' since parent is not found.", GetEnv()->GetNameId()%(*itLinkSaved)->GetName()); + RAVELOG_WARN_FORMAT("env=%s, could not restore link '%s' since parent is not found.", GetEnv()->GetNameId()%(*itLinkSaved).second->GetName()); continue; } - const int linkindex = (*itLinkSaved)->GetIndex(); - if( pParentSaved.get() == &savedBody ) { - _PushLinkToListNonCollidingLinksWhenGrabbed(*pNewGrabbed, linkindex, (*itLinkSaved)->GetName(), GetLinks(), GetEnv()); + if( pParentSaved.get() != &savedBody ) { + RAVELOG_WARN_FORMAT("env=%s, could not restore link '%s' since parent is not same as saved body.", GetEnv()->GetNameId()%(*itLinkSaved).second->GetName()); + continue; } - else { - const KinBodyPtr pNewNonCollidingBody = GetEnv()->GetBodyFromEnvironmentBodyIndex(pParentSaved->GetEnvironmentBodyIndex()); - if( !pNewNonCollidingBody) { - RAVELOG_WARN_FORMAT("env=%s, could not restore link '%s' since could not not find body with id %d.", GetEnv()->GetNameId()%(*itLinkSaved)->GetName() % pParentSaved->GetEnvironmentBodyIndex()); - continue; - } - _PushLinkToListNonCollidingLinksWhenGrabbed(*pNewGrabbed, linkindex, (*itLinkSaved)->GetName(), pNewNonCollidingBody->GetLinks(), GetEnv()); + const int grabbedLinkIndex = (*itLinkSaved).first->GetIndex(); + const int grabberLinkIndex = (*itLinkSaved).second->GetIndex(); + if( !_CheckLinkIndexForListNonCollidingLinkPairs(grabberLinkIndex, (*itLinkSaved).second->GetName(), GetLinks(), GetEnv()) || + !_CheckLinkIndexForListNonCollidingLinkPairs(grabbedLinkIndex, (*itLinkSaved).first->GetName(), pNewGrabbedBody->GetLinks(), GetEnv()) ) { + continue; } + pNewGrabbed->_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed.emplace_back(pNewGrabbedBody->GetLinks()[grabbedLinkIndex], GetLinks()[grabberLinkIndex]); } pNewGrabbed->_SetLinkNonCollidingIsValid(true); } @@ -149,6 +147,48 @@ void KinBody::_RestoreGrabbedBodiesFromSavedData(const KinBody& savedBody, } } + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.clear(); + if( bIsFromSameEnv ) { + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed = savedMapListNonCollidingInterGrabbedLinkPairsWhenGrabbed; + } + else { + FOREACHC(itInfoSaved, savedMapListNonCollidingInterGrabbedLinkPairsWhenGrabbed) { + const ListNonCollidingLinkPairs& pairsSaved = itInfoSaved->second; + if( pairsSaved.empty() ) { + continue; + } + if( !_IsListNonCollidingLinksValidFromEnvironmentBodyIndex(_GetFirstEnvironmentBodyIndexFromPair(itInfoSaved->first)) || + !_IsListNonCollidingLinksValidFromEnvironmentBodyIndex(_GetSecondEnvironmentBodyIndexFromPair(itInfoSaved->first)) ) { + continue; + } + const KinBodyPtr pFirstSaved = pairsSaved.front().first->GetParent(true); + const KinBodyPtr pSecondSaved = pairsSaved.front().second->GetParent(true); + if( !pFirstSaved || !pSecondSaved ) { + continue; // somehow, relevant code in the above does not show warning nor exception. so, follow it for now. + } + const KinBodyPtr pFirst = GetEnv()->GetBodyFromEnvironmentBodyIndex(pFirstSaved->GetEnvironmentBodyIndex()); + const KinBodyPtr pSecond = GetEnv()->GetBodyFromEnvironmentBodyIndex(pSecondSaved->GetEnvironmentBodyIndex()); + if( !pFirst || !pSecond ) { + RAVELOG_WARN_FORMAT("env=%s, could not find bodies with envBodyIndex '%s' (%d) and '%s' (%d).", GetEnv()->GetNameId()%pFirstSaved->GetName()%pFirstSaved->GetEnvironmentBodyIndex()%pSecondSaved->GetName()%pSecondSaved->GetEnvironmentBodyIndex()); + continue; + } + KinBody::ListNonCollidingLinkPairs listNonCollidingLinkPairs; + FOREACHC(itLinkPairSaved, itInfoSaved->second) { + const int linkIndexFirst = (*itLinkPairSaved).first->GetIndex(); + const int linkIndexSecond = (*itLinkPairSaved).second->GetIndex(); + if( !_CheckLinkIndexForListNonCollidingLinkPairs(linkIndexFirst, (*itLinkPairSaved).first->GetName(), pFirst->GetLinks(), GetEnv()) || + !_CheckLinkIndexForListNonCollidingLinkPairs(linkIndexSecond, (*itLinkPairSaved).second->GetName(), pSecond->GetLinks(), GetEnv()) ) { + continue; + } + listNonCollidingLinkPairs.emplace_back(pFirst->GetLinks()[linkIndexFirst], pSecond->GetLinks()[linkIndexSecond]); + } + if( listNonCollidingLinkPairs.size() > 0 ){ + const uint64_t key = _ComputeEnvironmentBodyIndicesPair(*pFirst, *pSecond); + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(key, std::move(listNonCollidingLinkPairs)); + } + } + } + // if not calling SetLinkTransformations, then manually call _UpdateGrabbedBodies if( !(options & KinBody::Save_LinkTransformation ) ) { _UpdateGrabbedBodies(); @@ -163,7 +203,7 @@ void KinBody::_SaveKinBodySavedGrabbedData(std::unordered_mapsecond; data.pGrabbed = pGrabbed; - data.listNonCollidingLinksWhenGrabbed = pGrabbed->_listNonCollidingLinksWhenGrabbed; + data.listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed = pGrabbed->_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed; data.setGrabberLinkIndicesToIgnore = pGrabbed->_setGrabberLinkIndicesToIgnore; data.listNonCollidingIsValid = pGrabbed->IsListNonCollidingLinksValid(); } @@ -199,6 +239,7 @@ KinBody::KinBodyStateSaver::KinBodyStateSaver(KinBodyPtr pbody, int options) : _ } if( _options & Save_GrabbedBodies ) { _pbody->_SaveKinBodySavedGrabbedData(_grabbedDataByEnvironmentIndex); + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed = _pbody->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed; } } @@ -238,7 +279,7 @@ void KinBody::KinBodyStateSaver::_RestoreKinBody(boost::shared_ptr pbod } // restoring grabbed bodies has to happen first before link transforms can be restored since _UpdateGrabbedBodies can be called with the old grabbed bodies. if( _options & Save_GrabbedBodies ) { - pbody->_RestoreGrabbedBodiesFromSavedData(*_pbody, _options, _grabbedDataByEnvironmentIndex); + pbody->_RestoreGrabbedBodiesFromSavedData(*_pbody, _options, _grabbedDataByEnvironmentIndex, _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed); } if( _options & Save_LinkTransformation ) { pbody->SetLinkTransformations(_vLinkTransforms, _vdoflastsetvalues); @@ -312,6 +353,7 @@ KinBody::KinBodyStateSaverRef::KinBodyStateSaverRef(KinBody& body, int options) } if( _options & Save_GrabbedBodies ) { body._SaveKinBodySavedGrabbedData(_grabbedDataByEnvironmentIndex); + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed = body._mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed; } if( _options & Save_JointResolutions ) { body.GetDOFResolutions(_vDOFResolutions); @@ -358,7 +400,7 @@ void KinBody::KinBodyStateSaverRef::_RestoreKinBody(KinBody& body) } // restoring grabbed bodies has to happen first before link transforms can be restored since _UpdateGrabbedBodies can be called with the old grabbed bodies. if( _options & Save_GrabbedBodies ) { - body._RestoreGrabbedBodiesFromSavedData(_body, _options, _grabbedDataByEnvironmentIndex); + body._RestoreGrabbedBodiesFromSavedData(_body, _options, _grabbedDataByEnvironmentIndex, _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed); } if( _options & Save_LinkTransformation ) { body.SetLinkTransformations(_vLinkTransforms, _vdoflastsetvalues); From 0cd3346bb5462f8e77ab0bf9d64aec4f3feb08b2 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 25 Sep 2024 00:15:06 +0900 Subject: [PATCH 02/25] reduce the duplicated checking of pointer validity. check it when creating the vector. --- src/libopenrave/kinbodycollision.cpp | 38 ++++++++++------------------ 1 file changed, 13 insertions(+), 25 deletions(-) diff --git a/src/libopenrave/kinbodycollision.cpp b/src/libopenrave/kinbodycollision.cpp index ec7bff6550..db2e9d461a 100644 --- a/src/libopenrave/kinbodycollision.cpp +++ b/src/libopenrave/kinbodycollision.cpp @@ -72,17 +72,23 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase // Flatten the grabbed bodies so that we can zip our iteration with the cache of locked pointers // Use raw pointers to save overhead here since lifetime is guaranteed by _grabbedBodiesByEnvironmentIndex + // locking weak pointer is expensive, so do it N times and cache, where N is the number of grabbedBody instead of N^2 std::vector vGrabbedBodies; vGrabbedBodies.reserve(_grabbedBodiesByEnvironmentIndex.size()); + std::vector vLockedGrabbedBodiesCache; + vLockedGrabbedBodiesCache.reserve(_grabbedBodiesByEnvironmentIndex.size()); for (const MapGrabbedByEnvironmentIndex::value_type& grabPair : _grabbedBodiesByEnvironmentIndex) { + KinBodyPtr pGrabbedBody = grabPair.second.get()->_pGrabbedBody.lock(); + if( !pGrabbedBody ) { + RAVELOG_WARN_FORMAT("env=%s, grabbed body on %s has already been destroyed, ignoring.", GetEnv()->GetNameId()%GetName()); + continue; + } + const KinBody& grabbedBody = *pGrabbedBody; + if( !grabbedBody.IsEnabled() ) { + continue; + } vGrabbedBodies.emplace_back(grabPair.second.get()); - } - - // locking weak pointer is expensive, so do it N times and cache, where N is the number of grabbedBody instead of N^2 - std::vector vLockedGrabbedBodiesCache; - vLockedGrabbedBodiesCache.reserve(vGrabbedBodies.size()); - for (const Grabbed* grab : vGrabbedBodies) { - vLockedGrabbedBodiesCache.push_back(grab->_pGrabbedBody.lock()); + vLockedGrabbedBodiesCache.push_back(pGrabbedBody); } // check all grabbed bodies with (TODO: support CO_ActiveDOFs option) @@ -90,15 +96,6 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase // RAVELOG_INFO_FORMAT("env=%s, checking self collision for %s with grabbed bodies: numgrabbed=%d", GetEnv()->GetNameId()%GetName()%numGrabbed); for (size_t indexGrabbed1 = 0; indexGrabbed1 < numGrabbed; indexGrabbed1++) { const KinBodyPtr& pGrabbedBody1 = vLockedGrabbedBodiesCache[indexGrabbed1]; - if( !pGrabbedBody1 ) { - RAVELOG_WARN_FORMAT("env=%s, grabbed body on %s has already been destroyed, ignoring.", GetEnv()->GetNameId()%GetName()); - continue; - } - const KinBody& grabbedBody1 = *pGrabbedBody1; - if( !grabbedBody1.IsEnabled() ) { - continue; - } - vGrabbedBodies[indexGrabbed1]->ComputeListNonCollidingLinks(); const std::list& nonCollidingLinks1 = vGrabbedBodies[indexGrabbed1]->_listNonCollidingLinksWhenGrabbed; @@ -151,15 +148,6 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase // Since collision checking is commutative (i.e. CheckCollision(link1, link2) == CheckCollision(link2, link1)), checking it once per pair is sufficient. for( size_t indexGrabbed2 = indexGrabbed1 + 1; indexGrabbed2 < numGrabbed; ++indexGrabbed2 ) { const KinBodyPtr& pGrabbedBody2 = vLockedGrabbedBodiesCache[indexGrabbed2]; - if( !pGrabbedBody2 ) { - RAVELOG_WARN_FORMAT("env=%s, grabbed body on %s has already been destroyed, so ignoring.", GetEnv()->GetNameId()%GetName()); - continue; - } - const KinBody& grabbedBody2 = *pGrabbedBody2; - if( !grabbedBody2.IsEnabled() ) { - continue; - } - vGrabbedBodies[indexGrabbed2]->ComputeListNonCollidingLinks(); const std::list& nonCollidingLinks2 = vGrabbedBodies[indexGrabbed2]->_listNonCollidingLinksWhenGrabbed; From a942ee100e6655a0ad492db7335f4cfa7315cd4b Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 25 Sep 2024 00:52:53 +0900 Subject: [PATCH 03/25] use _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed and _listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed in CheckSelfCollisionChecking --- src/libopenrave/kinbodycollision.cpp | 102 +++++++++------------------ 1 file changed, 35 insertions(+), 67 deletions(-) diff --git a/src/libopenrave/kinbodycollision.cpp b/src/libopenrave/kinbodycollision.cpp index db2e9d461a..cd34b5da73 100644 --- a/src/libopenrave/kinbodycollision.cpp +++ b/src/libopenrave/kinbodycollision.cpp @@ -83,8 +83,7 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase RAVELOG_WARN_FORMAT("env=%s, grabbed body on %s has already been destroyed, ignoring.", GetEnv()->GetNameId()%GetName()); continue; } - const KinBody& grabbedBody = *pGrabbedBody; - if( !grabbedBody.IsEnabled() ) { + if( !pGrabbedBody->IsEnabled() ) { continue; } vGrabbedBodies.emplace_back(grabPair.second.get()); @@ -95,14 +94,13 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase const size_t numGrabbed = _grabbedBodiesByEnvironmentIndex.size(); // RAVELOG_INFO_FORMAT("env=%s, checking self collision for %s with grabbed bodies: numgrabbed=%d", GetEnv()->GetNameId()%GetName()%numGrabbed); for (size_t indexGrabbed1 = 0; indexGrabbed1 < numGrabbed; indexGrabbed1++) { - const KinBodyPtr& pGrabbedBody1 = vLockedGrabbedBodiesCache[indexGrabbed1]; vGrabbedBodies[indexGrabbed1]->ComputeListNonCollidingLinks(); - - const std::list& nonCollidingLinks1 = vGrabbedBodies[indexGrabbed1]->_listNonCollidingLinksWhenGrabbed; + const ListNonCollidingLinkPairs& nonCollidingLinkPairs = vGrabbedBodies[indexGrabbed1]->_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed; KinBodyPtr pLinkParent; - for (const KinBody::LinkConstPtr& probotlinkFromNonColliding : nonCollidingLinks1) { + FOREACHC(itNonCollidingLinkPairs, nonCollidingLinkPairs) { + const KinBody::LinkConstPtr& probotlinkFromNonColliding = (*itNonCollidingLinkPairs).second; const KinBody::Link& robotlinkFromNonColliding = *probotlinkFromNonColliding; pLinkParent = robotlinkFromNonColliding.GetParent(true); if( !pLinkParent ) { @@ -111,22 +109,16 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase const KinBody::LinkConstPtr& probotlink = (!!pLinkParent) ? probotlinkFromNonColliding : _veclinks.at(robotlinkFromNonColliding.GetIndex()); // have to use link/link collision since link/body checks attached bodies - for (const KinBody::LinkPtr& pGrabbedBodylink : grabbedBody1.GetLinks()) { - if( collisionchecker->CheckCollision(probotlink, pGrabbedBodylink, pusereport) ) { - bCollision = true; - if( !bAllLinkCollisions ) { // if checking all collisions, have to continue - break; - } - } - if( !!pusereport && pusereport->minDistance < report->minDistance ) { - *report = *pusereport; - } - } - if( bCollision ) { + const KinBody::LinkConstPtr& pGrabbedBodylink = (*itNonCollidingLinkPairs).first; + if( collisionchecker->CheckCollision(probotlink, pGrabbedBodylink, pusereport) ) { + bCollision = true; if( !bAllLinkCollisions ) { // if checking all collisions, have to continue break; } } + if( !!pusereport && pusereport->minDistance < report->minDistance ) { + *report = *pusereport; + } } if( bCollision ) { if( !bAllLinkCollisions ) { // if checking all collisions, have to continue @@ -134,6 +126,7 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase } } + const KinBody& grabbedBody1 = *vLockedGrabbedBodiesCache[indexGrabbed1]; if( grabbedBody1.CheckSelfCollision(pusereport, collisionchecker) ) { bCollision = true; if( !bAllLinkCollisions ) { // if checking all collisions, have to continue @@ -143,62 +136,37 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase if( !!pusereport && pusereport->minDistance < report->minDistance ) { *report = *pusereport; } + } // end for indexGrabbed1 - if( numGrabbed > 1 ) { - // Since collision checking is commutative (i.e. CheckCollision(link1, link2) == CheckCollision(link2, link1)), checking it once per pair is sufficient. - for( size_t indexGrabbed2 = indexGrabbed1 + 1; indexGrabbed2 < numGrabbed; ++indexGrabbed2 ) { - const KinBodyPtr& pGrabbedBody2 = vLockedGrabbedBodiesCache[indexGrabbed2]; - vGrabbedBodies[indexGrabbed2]->ComputeListNonCollidingLinks(); - - const std::list& nonCollidingLinks2 = vGrabbedBodies[indexGrabbed2]->_listNonCollidingLinksWhenGrabbed; - - for( const KinBody::LinkPtr& pGrabbedBody2Link : grabbedBody2.GetLinks() ) { - if( !pGrabbedBody2Link->IsEnabled() ) { - continue; - } - // See if these two links were initially colliding. If they are, then no further - // check is need (as this link pair should be skipped). - // if the link is in nonCollidingLinks1, collision has already been checked. - if( std::find(nonCollidingLinks1.begin(), nonCollidingLinks1.end(), pGrabbedBody2Link) != nonCollidingLinks1.end() ) { - continue; - } - for( const KinBody::LinkPtr& pGrabbedBody1Link : grabbedBody1.GetLinks() ) { - if( !pGrabbedBody1Link->IsEnabled() ) { - continue; - } - if( std::find(nonCollidingLinks2.begin(), nonCollidingLinks2.end(), pGrabbedBody1Link) != nonCollidingLinks2.end() ) { - if( collisionchecker->CheckCollision(KinBody::LinkConstPtr(pGrabbedBody1Link), - KinBody::LinkConstPtr(pGrabbedBody2Link), - pusereport) ) { - bCollision = true; - if( !bAllLinkCollisions ) { // if checking all collisions, have to continue - break; - } - } // end if CheckCollision - if( !!pusereport && pusereport->minDistance < report->minDistance ) { - *report = *pusereport; - } - } // end if pGrabbedBody1Link in nonCollidingLinks2 - } // end for pGrabbedBody1Link in nonCollidingLinks2 - if( bCollision ) { - if( !bAllLinkCollisions ) { // if checking all collisions, have to continue - break; - } - } - } // end for pGrabbedBody2Link - if( bCollision ) { - if( !bAllLinkCollisions ) { // if checking all collisions, have to continue - break; - } + // Check grabbed vs grabbed collision + // TODO + FOREACHC(itInfo, _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed) { + const ListNonCollidingLinkPairs& pairs = (*itInfo).second; + if( pairs.empty() ) { + continue; + } + const KinBodyPtr pLink1 = pairs.front().first->GetParent(true); + const KinBodyPtr pLink2 = pairs.front().second->GetParent(true); + if( !pLink1 || !pLink2 ) { + continue; + } + FOREACHC(itLinks, pairs) { + if( collisionchecker->CheckCollision((*itLinks).first, (*itLinks).second, pusereport) ) { + bCollision = true; + if( !bAllLinkCollisions ) { // if checking all collisions, have to continue + break; } - } // end for indexGrabbed2 - } // end if numGrabbed > 1 + } + if( !!pusereport && pusereport->minDistance < report->minDistance ) { + *report = *pusereport; + } + } if( bCollision ) { if( !bAllLinkCollisions ) { // if checking all collisions, have to continue break; } } - } // end for indexGrabbed1 + } if( bCollision && !!report ) { if( report != pusereport ) { From d7b510e8a9f45fcc5229db8f62b5f8555c23a388 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 25 Sep 2024 11:42:33 +0900 Subject: [PATCH 04/25] directly return from the function if collision is deteced when bAllLinkCollisions==false. also share the utilities for CollisioReport update and printing. --- src/libopenrave/kinbodycollision.cpp | 77 +++++++++++++--------------- 1 file changed, 35 insertions(+), 42 deletions(-) diff --git a/src/libopenrave/kinbodycollision.cpp b/src/libopenrave/kinbodycollision.cpp index cd34b5da73..58bca4e7fc 100644 --- a/src/libopenrave/kinbodycollision.cpp +++ b/src/libopenrave/kinbodycollision.cpp @@ -18,6 +18,31 @@ namespace OpenRAVE { +/// \brief print the status on check self collision. +static void _PrintStatusOnCheckSelfCollisoin(CollisionReportPtr& report, const KinBody& body) +{ + if( IS_DEBUGLEVEL(Level_Verbose) ) { + std::vector colvalues; + body.GetDOFValues(colvalues); + std::stringstream ss; ss << std::setprecision(std::numeric_limits::digits10+1); + FOREACHC(itval, colvalues) { + ss << *itval << ","; + } + RAVELOG_VERBOSE_FORMAT("env=%s, self collision report=%s; colvalues=[%s]", body.GetEnv()->GetNameId()%report->__str__()%ss.str()); + } +} + +/// \brief post process on the check self collision, mostly for grabbed bodies. +static void _PostProcessOnCheckSelfCollision(CollisionReportPtr& report, CollisionReportPtr& pusereport, const KinBody& body) +{ + if( !!report ) { + if( report != pusereport ) { + *report = *pusereport; + } + _PrintStatusOnCheckSelfCollisoin(report, body); + } +} + bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBasePtr collisionchecker) const { if( !collisionchecker ) { @@ -45,15 +70,7 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase bool bCollision = false; if( collisionchecker->CheckStandaloneSelfCollision(shared_kinbody_const(), report) ) { if( !!report ) { - if( IS_DEBUGLEVEL(Level_Verbose) ) { - std::vector colvalues; - GetDOFValues(colvalues); - std::stringstream ss; ss << std::setprecision(std::numeric_limits::digits10+1); - FOREACHC(itval, colvalues) { - ss << *itval << ","; - } - RAVELOG_VERBOSE_FORMAT("env=%s, self collision report=%s; colvalues=[%s]", GetEnv()->GetNameId()%report->__str__()%ss.str()); - } + _PrintStatusOnCheckSelfCollisoin(report, *this); } if( !bAllLinkCollisions ) { // if checking all collisions, have to continue return true; @@ -111,27 +128,24 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase // have to use link/link collision since link/body checks attached bodies const KinBody::LinkConstPtr& pGrabbedBodylink = (*itNonCollidingLinkPairs).first; if( collisionchecker->CheckCollision(probotlink, pGrabbedBodylink, pusereport) ) { - bCollision = true; if( !bAllLinkCollisions ) { // if checking all collisions, have to continue - break; + _PostProcessOnCheckSelfCollision(report, pusereport, *this); + return true; } + bCollision = true; } if( !!pusereport && pusereport->minDistance < report->minDistance ) { *report = *pusereport; } } - if( bCollision ) { - if( !bAllLinkCollisions ) { // if checking all collisions, have to continue - break; - } - } const KinBody& grabbedBody1 = *vLockedGrabbedBodiesCache[indexGrabbed1]; if( grabbedBody1.CheckSelfCollision(pusereport, collisionchecker) ) { - bCollision = true; if( !bAllLinkCollisions ) { // if checking all collisions, have to continue - break; + _PostProcessOnCheckSelfCollision(report, pusereport, *this); + return true; } + bCollision = true; } if( !!pusereport && pusereport->minDistance < report->minDistance ) { *report = *pusereport; @@ -139,7 +153,6 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase } // end for indexGrabbed1 // Check grabbed vs grabbed collision - // TODO FOREACHC(itInfo, _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed) { const ListNonCollidingLinkPairs& pairs = (*itInfo).second; if( pairs.empty() ) { @@ -152,37 +165,17 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase } FOREACHC(itLinks, pairs) { if( collisionchecker->CheckCollision((*itLinks).first, (*itLinks).second, pusereport) ) { - bCollision = true; if( !bAllLinkCollisions ) { // if checking all collisions, have to continue - break; + _PostProcessOnCheckSelfCollision(report, pusereport, *this); + return true; } + bCollision = true; } if( !!pusereport && pusereport->minDistance < report->minDistance ) { *report = *pusereport; } } - if( bCollision ) { - if( !bAllLinkCollisions ) { // if checking all collisions, have to continue - break; - } - } } - - if( bCollision && !!report ) { - if( report != pusereport ) { - *report = *pusereport; - } - if( IS_DEBUGLEVEL(Level_Verbose) ) { - std::vector colvalues; - GetDOFValues(colvalues); - std::stringstream ss; ss << std::setprecision(std::numeric_limits::digits10+1); - FOREACHC(itval, colvalues) { - ss << *itval << ","; - } - RAVELOG_VERBOSE_FORMAT("env=%s, self collision report=%s; colvalues=[%s]", GetEnv()->GetNameId()%report->__str__()%ss.str()); - } - } - return bCollision; } From 8fdf9ebbd738a85feaac25d2f8ac6b57f7a41219 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 25 Sep 2024 13:19:00 +0900 Subject: [PATCH 05/25] Improve warning message more --- src/libopenrave/kinbody.cpp | 38 ++++++++++++++++++++------- src/libopenrave/kinbodygrab.cpp | 1 + src/libopenrave/kinbodystatesaver.cpp | 16 +++++------ 3 files changed, 38 insertions(+), 17 deletions(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index cabdf0d5e4..6160a79db6 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -5822,10 +5822,20 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) if( pgrabbedref->IsListNonCollidingLinksValid() ) { FOREACHC(itLinkRef, pgrabbedref->_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed) { if( (*itLinkRef).second->GetParent() != r ) { - // TODO : warning + RAVELOG_WARN_FORMAT("env=%s, could not restore link '%s' since parent in the list is different from that in cloning reference.", GetEnv()->GetNameId()%(*itLinkRef).second->GetName()); continue; } - pgrabbed->_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed.emplace_back(pgrabbedbody->GetLink((*itLinkRef).first->GetName()), _veclinks.at((*itLinkRef).second->GetIndex())); + const int linkIndex = (*itLinkRef).second->GetIndex(); + if( linkIndex < 0 || linkIndex >= (int)_veclinks.size()) { + RAVELOG_WARN_FORMAT("env=%s, could not restore link '%s' since its index %d is out of range (body num links is %d)", GetEnv()->GetNameId()%(*itLinkRef).second->GetName()%linkIndex%_veclinks.size()); + continue; + } + const KinBody::LinkPtr pGrabbedBodyLink = pgrabbedbody->GetLink((*itLinkRef).first->GetName()); + if( !pGrabbedBodyLink ) { + RAVELOG_WARN_FORMAT("env=%s, could not restore link '%s' since it's not in the links of body '%s'.", GetEnv()->GetNameId()%(*itLinkRef).first->GetName()%pgrabbedbody->GetName()); + continue; + } + pgrabbed->_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed.emplace_back(pGrabbedBodyLink, _veclinks[linkIndex]); } pgrabbed->_SetLinkNonCollidingIsValid(true); } @@ -5857,21 +5867,31 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) const KinBodyPtr pFirstRef = pairs.front().first->GetParent(true); const KinBodyPtr pSecondRef = pairs.front().second->GetParent(true); if( !pFirstRef || !pSecondRef ) { - // TODO : warning continue; // somehow, relevant code in the above does not show warning nor exception. so, follow it for now. } const KinBodyPtr pFirst = GetEnv()->GetKinBody(pFirstRef->GetName()); + if( !pFirst ) { + RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find body %s for non-colliding link.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%pFirstRef->GetName()); + continue; + } const KinBodyPtr pSecond = GetEnv()->GetKinBody(pSecondRef->GetName()); - if( !pFirst || !pSecond ) { - // WARNING - //RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find body %s for non-colliding link %s.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%(*itLinkRef).second->GetParent()->GetName()%(*itLinkRef).second->GetName()); + if( !pSecond ) { + RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find body %s for non-colliding link.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%pSecondRef->GetName()); continue; } ListNonCollidingLinkPairs listNonCollidingLinkPairs; FOREACHC(itLinkPair, itInfo->second) { - listNonCollidingLinkPairs.emplace_back(pFirst->GetLink((*itLinkPair).first->GetName()), pSecond->GetLink((*itLinkPair).second->GetName())); - // TODO : warning - // RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find non-colliding link %s in body %s.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%(*itLinkRef).second->GetName()%(*itLinkRef).second->GetParent()->GetName()); + const KinBody::LinkPtr pFirstLink = pFirst->GetLink((*itLinkPair).first->GetName()); + if( !pFirstLink ) { + RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find non-colliding link %s in body %s.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%(*itLinkPair).first->GetName()%pFirst->GetName()); + continue; + } + const KinBody::LinkPtr pSecondLink = pSecond->GetLink((*itLinkPair).first->GetName()); + if( !pSecondLink ) { + RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find non-colliding link %s in body %s.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%(*itLinkPair).second->GetName()%pSecond->GetName()); + continue; + } + listNonCollidingLinkPairs.emplace_back(pFirstLink, pSecondLink); } if( listNonCollidingLinkPairs.size() > 0 ){ const uint64_t key = _ComputeEnvironmentBodyIndicesPair(*pFirst, *pSecond); diff --git a/src/libopenrave/kinbodygrab.cpp b/src/libopenrave/kinbodygrab.cpp index c39f267ece..20f91528dc 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -1050,6 +1050,7 @@ bool KinBody::_IsListNonCollidingLinksValidFromEnvironmentBodyIndex(const int en { MapGrabbedByEnvironmentIndex::const_iterator itGrab = _grabbedBodiesByEnvironmentIndex.find(envBodyIndex); if( itGrab == _grabbedBodiesByEnvironmentIndex.end() ) { + RAVELOG_WARN_FORMAT("env=%s, could not check the IsListNonCollidingLinksValid for body '%s', since there is no grabbed body with envBodyIndex=%d.", GetEnv()->GetNameId()%GetName() % envBodyIndex); return false; } return itGrab->second->IsListNonCollidingLinksValid(); diff --git a/src/libopenrave/kinbodystatesaver.cpp b/src/libopenrave/kinbodystatesaver.cpp index fb138ea4d8..9ec67848ce 100644 --- a/src/libopenrave/kinbodystatesaver.cpp +++ b/src/libopenrave/kinbodystatesaver.cpp @@ -19,10 +19,10 @@ namespace OpenRAVE { /// \brief push link to _listNonCollidingLinksWhenGrabbed of grabbed. -static bool _CheckLinkIndexForListNonCollidingLinkPairs(const int linkIndex, - const std::string& linkName, - const std::vector& vLinks, - const EnvironmentBasePtr& pEnv) +static bool _IsValidLinkIndexForListNonCollidingLinkPairs(const int linkIndex, + const std::string& linkName, + const std::vector& vLinks, + const EnvironmentBasePtr& pEnv) { if( linkIndex < 0 || linkIndex >= (int)vLinks.size() ) { RAVELOG_WARN_FORMAT("env=%s, could not restore link '%s' since its index %d is out of range (body num links is %d)", pEnv->GetNameId()%linkName%linkIndex%vLinks.size()); @@ -122,8 +122,8 @@ void KinBody::_RestoreGrabbedBodiesFromSavedData(const KinBody& savedBody, } const int grabbedLinkIndex = (*itLinkSaved).first->GetIndex(); const int grabberLinkIndex = (*itLinkSaved).second->GetIndex(); - if( !_CheckLinkIndexForListNonCollidingLinkPairs(grabberLinkIndex, (*itLinkSaved).second->GetName(), GetLinks(), GetEnv()) || - !_CheckLinkIndexForListNonCollidingLinkPairs(grabbedLinkIndex, (*itLinkSaved).first->GetName(), pNewGrabbedBody->GetLinks(), GetEnv()) ) { + if( !_IsValidLinkIndexForListNonCollidingLinkPairs(grabberLinkIndex, (*itLinkSaved).second->GetName(), GetLinks(), GetEnv()) || + !_IsValidLinkIndexForListNonCollidingLinkPairs(grabbedLinkIndex, (*itLinkSaved).first->GetName(), pNewGrabbedBody->GetLinks(), GetEnv()) ) { continue; } pNewGrabbed->_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed.emplace_back(pNewGrabbedBody->GetLinks()[grabbedLinkIndex], GetLinks()[grabberLinkIndex]); @@ -176,8 +176,8 @@ void KinBody::_RestoreGrabbedBodiesFromSavedData(const KinBody& savedBody, FOREACHC(itLinkPairSaved, itInfoSaved->second) { const int linkIndexFirst = (*itLinkPairSaved).first->GetIndex(); const int linkIndexSecond = (*itLinkPairSaved).second->GetIndex(); - if( !_CheckLinkIndexForListNonCollidingLinkPairs(linkIndexFirst, (*itLinkPairSaved).first->GetName(), pFirst->GetLinks(), GetEnv()) || - !_CheckLinkIndexForListNonCollidingLinkPairs(linkIndexSecond, (*itLinkPairSaved).second->GetName(), pSecond->GetLinks(), GetEnv()) ) { + if( !_IsValidLinkIndexForListNonCollidingLinkPairs(linkIndexFirst, (*itLinkPairSaved).first->GetName(), pFirst->GetLinks(), GetEnv()) || + !_IsValidLinkIndexForListNonCollidingLinkPairs(linkIndexSecond, (*itLinkPairSaved).second->GetName(), pSecond->GetLinks(), GetEnv()) ) { continue; } listNonCollidingLinkPairs.emplace_back(pFirst->GetLinks()[linkIndexFirst], pSecond->GetLinks()[linkIndexSecond]); From 042243f511d4d3878516fe27cb2fcc22a7c435c1 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 25 Sep 2024 14:27:34 +0900 Subject: [PATCH 06/25] fix wrong size --- src/libopenrave/kinbodycollision.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/libopenrave/kinbodycollision.cpp b/src/libopenrave/kinbodycollision.cpp index 58bca4e7fc..d111e28516 100644 --- a/src/libopenrave/kinbodycollision.cpp +++ b/src/libopenrave/kinbodycollision.cpp @@ -108,7 +108,7 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase } // check all grabbed bodies with (TODO: support CO_ActiveDOFs option) - const size_t numGrabbed = _grabbedBodiesByEnvironmentIndex.size(); + const size_t numGrabbed = vGrabbedBodies.size(); // RAVELOG_INFO_FORMAT("env=%s, checking self collision for %s with grabbed bodies: numgrabbed=%d", GetEnv()->GetNameId()%GetName()%numGrabbed); for (size_t indexGrabbed1 = 0; indexGrabbed1 < numGrabbed; indexGrabbed1++) { vGrabbedBodies[indexGrabbed1]->ComputeListNonCollidingLinks(); From a8839a2bf7fadac5504a31a6a6ab4d5340099bd4 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 25 Sep 2024 15:43:29 +0900 Subject: [PATCH 07/25] fix typo of pSecondLink. in addition, rename variable to contain 'Ref' to clarify meaning. --- src/libopenrave/kinbody.cpp | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 6160a79db6..70fdacc4be 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -5855,17 +5855,18 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) } // end for pgrabbedref // map list of inter grabbed. - FOREACHC(itInfo, r->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed) { - const ListNonCollidingLinkPairs& pairs = (*itInfo).second; - if( pairs.empty() ) { + FOREACHC(itInfoRef, r->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed) { + const ListNonCollidingLinkPairs& pairsRef = itInfoRef->second; + const uint64_t envBodyIndicesPair = itInfoRef->first; + if( pairsRef.empty() ) { continue; } - if( !_IsListNonCollidingLinksValidFromEnvironmentBodyIndex(_GetFirstEnvironmentBodyIndexFromPair(itInfo->first)) || - !_IsListNonCollidingLinksValidFromEnvironmentBodyIndex(_GetSecondEnvironmentBodyIndexFromPair(itInfo->first)) ) { + if( !_IsListNonCollidingLinksValidFromEnvironmentBodyIndex(_GetFirstEnvironmentBodyIndexFromPair(envBodyIndicesPair)) || + !_IsListNonCollidingLinksValidFromEnvironmentBodyIndex(_GetSecondEnvironmentBodyIndexFromPair(envBodyIndicesPair)) ) { continue; } - const KinBodyPtr pFirstRef = pairs.front().first->GetParent(true); - const KinBodyPtr pSecondRef = pairs.front().second->GetParent(true); + const KinBodyPtr pFirstRef = pairsRef.front().first->GetParent(true); + const KinBodyPtr pSecondRef = pairsRef.front().second->GetParent(true); if( !pFirstRef || !pSecondRef ) { continue; // somehow, relevant code in the above does not show warning nor exception. so, follow it for now. } @@ -5880,15 +5881,15 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) continue; } ListNonCollidingLinkPairs listNonCollidingLinkPairs; - FOREACHC(itLinkPair, itInfo->second) { - const KinBody::LinkPtr pFirstLink = pFirst->GetLink((*itLinkPair).first->GetName()); + FOREACHC(itLinkPairRef, itInfoRef->second) { + const KinBody::LinkPtr pFirstLink = pFirst->GetLink((*itLinkPairRef).first->GetName()); if( !pFirstLink ) { - RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find non-colliding link %s in body %s.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%(*itLinkPair).first->GetName()%pFirst->GetName()); + RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find non-colliding link %s in body %s.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%(*itLinkPairRef).first->GetName()%pFirst->GetName()); continue; } - const KinBody::LinkPtr pSecondLink = pSecond->GetLink((*itLinkPair).first->GetName()); + const KinBody::LinkPtr pSecondLink = pSecond->GetLink((*itLinkPairRef).second->GetName()); if( !pSecondLink ) { - RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find non-colliding link %s in body %s.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%(*itLinkPair).second->GetName()%pSecond->GetName()); + RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find non-colliding link %s in body %s.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%(*itLinkPairRef).second->GetName()%pSecond->GetName()); continue; } listNonCollidingLinkPairs.emplace_back(pFirstLink, pSecondLink); From 7841dafee36a53dd738055245a6b891ab6e8c81c Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 25 Sep 2024 16:05:49 +0900 Subject: [PATCH 08/25] improve comments --- src/libopenrave/kinbodycollision.cpp | 1 + src/libopenrave/kinbodystatesaver.cpp | 8 ++++++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/libopenrave/kinbodycollision.cpp b/src/libopenrave/kinbodycollision.cpp index d111e28516..ae45ce4675 100644 --- a/src/libopenrave/kinbodycollision.cpp +++ b/src/libopenrave/kinbodycollision.cpp @@ -161,6 +161,7 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase const KinBodyPtr pLink1 = pairs.front().first->GetParent(true); const KinBodyPtr pLink2 = pairs.front().second->GetParent(true); if( !pLink1 || !pLink2 ) { + RAVELOG_WARN_FORMAT("env=%s, _listNonCollidingLinks has invalid link %s, %s", GetEnv()->GetNameId() % pairs.front().first->GetName() % pairs.front().second->GetName()); continue; } FOREACHC(itLinks, pairs) { diff --git a/src/libopenrave/kinbodystatesaver.cpp b/src/libopenrave/kinbodystatesaver.cpp index 9ec67848ce..e05c546995 100644 --- a/src/libopenrave/kinbodystatesaver.cpp +++ b/src/libopenrave/kinbodystatesaver.cpp @@ -168,8 +168,12 @@ void KinBody::_RestoreGrabbedBodiesFromSavedData(const KinBody& savedBody, } const KinBodyPtr pFirst = GetEnv()->GetBodyFromEnvironmentBodyIndex(pFirstSaved->GetEnvironmentBodyIndex()); const KinBodyPtr pSecond = GetEnv()->GetBodyFromEnvironmentBodyIndex(pSecondSaved->GetEnvironmentBodyIndex()); - if( !pFirst || !pSecond ) { - RAVELOG_WARN_FORMAT("env=%s, could not find bodies with envBodyIndex '%s' (%d) and '%s' (%d).", GetEnv()->GetNameId()%pFirstSaved->GetName()%pFirstSaved->GetEnvironmentBodyIndex()%pSecondSaved->GetName()%pSecondSaved->GetEnvironmentBodyIndex()); + if( !pFirst ) { + RAVELOG_WARN_FORMAT("env=%s, could not find bodies with envBodyIndex '%s' (%d).", GetEnv()->GetNameId()%pFirstSaved->GetName()%pFirstSaved->GetEnvironmentBodyIndex()); + continue; + } + if( !pSecond ) { + RAVELOG_WARN_FORMAT("env=%s, could not find bodies with envBodyIndex '%s' (%d).", GetEnv()->GetNameId()%pSecondSaved->GetName()%pSecondSaved->GetEnvironmentBodyIndex()); continue; } KinBody::ListNonCollidingLinkPairs listNonCollidingLinkPairs; From 2bdbdb3deda496511c1bbef5ee218d5bc1f46dc3 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Mon, 30 Sep 2024 19:09:32 +0900 Subject: [PATCH 09/25] Add missing post processing of check self collision at the very end. --- src/libopenrave/kinbodycollision.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/libopenrave/kinbodycollision.cpp b/src/libopenrave/kinbodycollision.cpp index ae45ce4675..4cb58e266a 100644 --- a/src/libopenrave/kinbodycollision.cpp +++ b/src/libopenrave/kinbodycollision.cpp @@ -177,6 +177,10 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase } } } + + if( bCollision ) { + _PostProcessOnCheckSelfCollision(report, pusereport, *this); + } return bCollision; } From 2856b8dc03ddb2d5132cb82ece9b82c3043dbd4b Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Sat, 19 Oct 2024 11:08:02 +0900 Subject: [PATCH 10/25] Fix compile --- src/libopenrave/robot.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/libopenrave/robot.cpp b/src/libopenrave/robot.cpp index d392be3618..6861403c44 100644 --- a/src/libopenrave/robot.cpp +++ b/src/libopenrave/robot.cpp @@ -489,7 +489,7 @@ void RobotBase::_RestoreStateForClone(const RobotBasePtr& pOriginalRobot, const // KinBody::Save_GrabbedBodies const int options = 0; // the following function works without Save_GrabbedBodies. also, the original code in Environment's Clone does not set Save_LinkTransformation, used in the following function. Thus, we don't need any options here and set it to 0. - _RestoreGrabbedBodiesFromSavedData(*pOriginalRobot, options, originalGrabbedDataByEnvironmentIndex, /*bCalledFromClone*/ true); + _RestoreGrabbedBodiesFromSavedData(*pOriginalRobot, options, originalGrabbedDataByEnvironmentIndex, pOriginalRobot->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed, /*bCalledFromClone*/ true); // KinBody::Save_LinkVelocities if( !bRestoreGrabbedBodiesOnly ) { From f5a663df8ba88de663cdf75a62d27dcd6d84a074 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Sat, 19 Oct 2024 14:32:35 +0900 Subject: [PATCH 11/25] use | --- src/libopenrave/kinbodygrab.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/libopenrave/kinbodygrab.cpp b/src/libopenrave/kinbodygrab.cpp index 0d6cccd4e7..9bc70ef841 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -1055,7 +1055,7 @@ bool KinBody::_IsListNonCollidingLinksValidFromEnvironmentBodyIndex(const int en uint64_t KinBody::_ComputeEnvironmentBodyIndicesPair(const KinBody& body1, const KinBody& body2) { - return static_cast(body1.GetEnvironmentBodyIndex()) + (static_cast(body2.GetEnvironmentBodyIndex()) << 32); + return static_cast(body1.GetEnvironmentBodyIndex()) | (static_cast(body2.GetEnvironmentBodyIndex()) << 32); } int KinBody::_GetFirstEnvironmentBodyIndexFromPair(const uint64_t pair) From 0ae0e0e5fccba6933c58ea5e3cd5826485a99a29 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Tue, 22 Oct 2024 14:02:18 +0900 Subject: [PATCH 12/25] 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 13/25] 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 14/25] 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 15/25] 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 e7cd2917c4083d90dfa6684f484a6d68db3f6c39 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Fri, 25 Oct 2024 17:41:14 +0900 Subject: [PATCH 16/25] return value from here is unused --- src/libopenrave/kinbodygrab.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/libopenrave/kinbodygrab.cpp b/src/libopenrave/kinbodygrab.cpp index 21180a43ea..82456af83b 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -302,7 +302,7 @@ void Grabbed::_PushNonCollidingLinkPairsForGrabbedBodies(KinBodyPtr& pGrabber, KinBody::ListNonCollidingLinkPairs listNonCollidingLinkPairs; _PushLinkPairsIfNonCollidingWithOtherGrabbedBody(listNonCollidingLinkPairs, pchecker, grabbedBody, otherGrabbedBody); if( listNonCollidingLinkPairs.size() > 0 ) { - itInfo = pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(indexForTest0, std::move(listNonCollidingLinkPairs)).first; + pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(indexForTest0, std::move(listNonCollidingLinkPairs)).first; } } From 05c94414b81d28825b180c8a0fd61ea28eb6a728 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Fri, 25 Oct 2024 17:44:55 +0900 Subject: [PATCH 17/25] Update warning message clearer. --- src/libopenrave/kinbody.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 7b622c960b..a3db719bfb 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -5891,7 +5891,7 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) if( pgrabbedref->IsListNonCollidingLinksValid() ) { FOREACHC(itLinkRef, pgrabbedref->_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed) { if( (*itLinkRef).second->GetParent() != r ) { - RAVELOG_WARN_FORMAT("env=%s, could not restore link '%s' since parent in the list is different from that in cloning reference.", GetEnv()->GetNameId()%(*itLinkRef).second->GetName()); + RAVELOG_WARN_FORMAT("env=%s, could not restore link '%s' since parent in the list is different from cloning reference.", GetEnv()->GetNameId()%(*itLinkRef).second->GetName()); continue; } const int linkIndex = (*itLinkRef).second->GetIndex(); From d9f6275f2a6461c9ef284acb2b627d455896764e Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Fri, 25 Oct 2024 18:37:50 +0900 Subject: [PATCH 18/25] Introduce the rule of indices pair, which is key of map. the first envBodyIndex should be always smaller than the second envBodyIndex to simplify the searching. --- include/openrave/kinbody.h | 5 ++- src/libopenrave/kinbody.cpp | 18 +++++++-- src/libopenrave/kinbodygrab.cpp | 54 +++++++++++++-------------- src/libopenrave/kinbodystatesaver.cpp | 11 +++++- 4 files changed, 54 insertions(+), 34 deletions(-) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index 7d1eef8873..a211562642 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -3736,7 +3736,8 @@ class OPENRAVE_API KinBody : public InterfaceBase /// \brief Compute environment body indices pair. pack the two bodies' envBodyIndices (32bit int) into one environment body indices pair (uint64_t). /// Here, environment body indices pair is uint64_t, which higher 32bits are for body2 envBodyIndex, and which lower 32bits are for body1 envBodyIndex. - static uint64_t _ComputeEnvironmentBodyIndicesPair(const KinBody& body1, const KinBody& body2); + /// Note that index1 < index2. + static uint64_t _ComputeEnvironmentBodyIndicesPair(const uint64_t index1, const uint64_t index2); /// \brief Extract the first body's environmentBodyIndex from environment body indices pair. static int _GetFirstEnvironmentBodyIndexFromPair(const uint64_t pair); @@ -3813,7 +3814,7 @@ class OPENRAVE_API KinBody : public InterfaceBase mutable std::string __hashKinematicsGeometryDynamics; ///< hash serializing kinematics, dynamics and geometry properties of the KinBody int64_t _lastModifiedAtUS=0; ///< us, linux epoch, last modified time of the kinbody when it was originally loaded from the environment. int64_t _revisionId = 0; ///< the webstack revision for this loaded kinbody - std::unordered_map _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed; ///< map of list of link pairs. This is computed when grabbed bodies are grabbed, and at taht time, two grabbed bodies are not touching each other. Since these links are not colliding at the time of grabbing, they should remain non-colliding with the grabbed body throughout. If, while grabbing, they collide with the grabbed body at some point, CheckSelfCollision should return true. It is important to note that the enable state of a link does *not* affect its membership of this list. Each pair in the list should be [Grabbed1-link, Grabbed2-link]. Note that this does not contain link pairs of [Grabbed-link, Grabber-link], c.f. Grabbed::_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed. Note that the key of this map is 'environment body indices pair', which higher 32bits are for the first KinBody's envBodyIndex, and which lower 32bits are for the second KinBody's envBodyIndex. Please also see _ComputeEnvironmentBodyIndicesPair. + std::unordered_map _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed; ///< map of list of link pairs. This is computed when grabbed bodies are grabbed, and at taht time, two grabbed bodies are not touching each other. Since these links are not colliding at the time of grabbing, they should remain non-colliding with the grabbed body throughout. If, while grabbing, they collide with the grabbed body at some point, CheckSelfCollision should return true. It is important to note that the enable state of a link does *not* affect its membership of this list. Each pair in the list should be [Grabbed1-link, Grabbed2-link]. Note that this does not contain link pairs of [Grabbed-link, Grabber-link], c.f. Grabbed::_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed. Note that the key of this map is 'environment body indices pair', which lower 32bits are for the first KinBody's envBodyIndex, and which higher 32bits are for the second KinBody's envBodyIndex. The first envBodyIndex should be always smaller than the second envBodyIndex to simplify searching. Please also see _ComputeEnvironmentBodyIndicesPair. private: mutable std::vector _vTempJoints; diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index a3db719bfb..d0afffa1b7 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -5950,6 +5950,9 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) continue; } ListNonCollidingLinkPairs listNonCollidingLinkPairs; + const int envBodyIndexFirst = pFirst->GetEnvironmentBodyIndex(); + const int envBodyIndexSecond = pSecond->GetEnvironmentBodyIndex(); + const bool bHasSmallerFirstIndex = envBodyIndexFirst < envBodyIndexSecond; FOREACHC(itLinkPairRef, itInfoRef->second) { const KinBody::LinkPtr pFirstLink = pFirst->GetLink((*itLinkPairRef).first->GetName()); if( !pFirstLink ) { @@ -5961,11 +5964,20 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find non-colliding link %s in body %s.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%(*itLinkPairRef).second->GetName()%pSecond->GetName()); continue; } - listNonCollidingLinkPairs.emplace_back(pFirstLink, pSecondLink); + if( bHasSmallerFirstIndex ) { + listNonCollidingLinkPairs.emplace_back(pFirstLink, pSecondLink); + } + else { + listNonCollidingLinkPairs.emplace_back(pSecondLink, pFirstLink); + } } if( listNonCollidingLinkPairs.size() > 0 ){ - const uint64_t key = _ComputeEnvironmentBodyIndicesPair(*pFirst, *pSecond); - _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(key, std::move(listNonCollidingLinkPairs)); + if( bHasSmallerFirstIndex ) { + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(_ComputeEnvironmentBodyIndicesPair(envBodyIndexFirst, envBodyIndexSecond), std::move(listNonCollidingLinkPairs)); + } + else { + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(_ComputeEnvironmentBodyIndicesPair(envBodyIndexSecond, envBodyIndexFirst), std::move(listNonCollidingLinkPairs)); + } } } } // end if not Clone_IgnoreGrabbedBodies diff --git a/src/libopenrave/kinbodygrab.cpp b/src/libopenrave/kinbodygrab.cpp index 82456af83b..70e7f18066 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -31,15 +31,15 @@ static void _PushLinkIfNonColliding(std::list >& listNonCollidingLinkPairs) { FOREACHC(itLinks, listNonCollidingLinkPairs) { - const KinBody::Link* const pLinkFirst = (*itLinks).first.get(); - const KinBody::Link* const pLinkSecond = (*itLinks).second.get(); - if ( (pLinkFirst == pLink1ToSearch && pLinkSecond == pLink2ToSearch) || (pLinkFirst == pLink2ToSearch && pLinkSecond == pLink1ToSearch) ) { + if ( ((*itLinks).first.get() == pLink1ToSearch && (*itLinks).second.get() == pLink2ToSearch) ) { return true; } } @@ -47,21 +47,21 @@ static bool _IsLinkPairIncluded(const KinBody::Link* pLink1ToSearch, } /// \brief Push link to listNonCollidingLinkPairs if the given grabbed bodies links are not in collision each other. This is inter-grabbed link pairs. -/// \param[in] bInvertFirstSecond : true if push the pair by inverting the first and the second. +/// Note that envBodyIndex of grabbedBody1 should be smaller than envBodyIndex of grabbedBody2. static void _PushLinkPairsIfNonCollidingWithOtherGrabbedBody(std::list >& listNonCollidingLinkPairs, CollisionCheckerBasePtr& pchecker, - const KinBody& grabbedBody, - const KinBody& otherGrabbedBody) + const KinBody& grabbedBody1, + const KinBody& grabbedBody2) { - for (const KinBody::LinkPtr& pGrabbedBodyLink : grabbedBody.GetLinks()) { - for (const KinBody::LinkPtr& pOtherGrabbedLink : otherGrabbedBody.GetLinks()) { + for (const KinBody::LinkPtr& pGrabbedBody1Link : grabbedBody1.GetLinks()) { + for (const KinBody::LinkPtr& pGrabbedBody2Link : grabbedBody2.GetLinks()) { // if already in the list, no need to check collision. - if( _IsLinkPairIncluded(pGrabbedBodyLink.get(), pOtherGrabbedLink.get(), listNonCollidingLinkPairs) ) { + if( _IsLinkPairIncluded(pGrabbedBody1Link.get(), pGrabbedBody2Link.get(), listNonCollidingLinkPairs) ) { continue; } // if not colliding, push. - if( !pchecker->CheckCollision(KinBody::LinkConstPtr(pGrabbedBodyLink), KinBody::LinkConstPtr(pOtherGrabbedLink)) ) { - listNonCollidingLinkPairs.emplace_back(pGrabbedBodyLink, pOtherGrabbedLink); + if( !pchecker->CheckCollision(KinBody::LinkConstPtr(pGrabbedBody1Link), KinBody::LinkConstPtr(pGrabbedBody2Link)) ) { + listNonCollidingLinkPairs.emplace_back(pGrabbedBody1Link, pGrabbedBody2Link); } } } @@ -282,27 +282,24 @@ void Grabbed::_PushNonCollidingLinkPairsForGrabbedBodies(KinBodyPtr& pGrabber, const KinBody& grabbedBody, const KinBody& otherGrabbedBody) { - // Find item by (grabbedBody, otherGrabbedBody) pair. - const uint64_t indexForTest0 = KinBody::_ComputeEnvironmentBodyIndicesPair(grabbedBody, otherGrabbedBody); - std::unordered_map::iterator itInfo = pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.find(indexForTest0); + // Find item by indices pair. + const bool bNoInvert = grabbedBody.GetEnvironmentBodyIndex() < otherGrabbedBody.GetEnvironmentBodyIndex(); + + // here, grabbedBody1's envBodyIndex should be smaller than grabbedBody2's envBodyIndex. + const KinBody& grabbedBody1 = bNoInvert ? grabbedBody : otherGrabbedBody; + const KinBody& grabbedBody2 = bNoInvert ? otherGrabbedBody : grabbedBody; + const uint64_t key = KinBody::_ComputeEnvironmentBodyIndicesPair(grabbedBody1.GetEnvironmentBodyIndex(), grabbedBody2.GetEnvironmentBodyIndex()); + std::unordered_map::iterator itInfo = pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.find(key); if( itInfo != pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.end() ) { - _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, otherGrabbedBody, grabbedBody); + _PushLinkPairsIfNonCollidingWithOtherGrabbedBody((*itInfo).second, pchecker, grabbedBody1, grabbedBody2); 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); + _PushLinkPairsIfNonCollidingWithOtherGrabbedBody(listNonCollidingLinkPairs, pchecker, grabbedBody1, grabbedBody2); if( listNonCollidingLinkPairs.size() > 0 ) { - pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(indexForTest0, std::move(listNonCollidingLinkPairs)).first; + pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(key, std::move(listNonCollidingLinkPairs)).first; } } @@ -1044,9 +1041,10 @@ bool KinBody::_IsListNonCollidingLinksValidFromEnvironmentBodyIndex(const int en return itGrab->second->IsListNonCollidingLinksValid(); } -uint64_t KinBody::_ComputeEnvironmentBodyIndicesPair(const KinBody& body1, const KinBody& body2) +uint64_t KinBody::_ComputeEnvironmentBodyIndicesPair(const uint64_t index1, const uint64_t index2) { - return static_cast(body1.GetEnvironmentBodyIndex()) | (static_cast(body2.GetEnvironmentBodyIndex()) << 32); + OPENRAVE_ASSERT_OP(index1, <, index2); + return index1 | (index2 << 32); } int KinBody::_GetFirstEnvironmentBodyIndexFromPair(const uint64_t pair) diff --git a/src/libopenrave/kinbodystatesaver.cpp b/src/libopenrave/kinbodystatesaver.cpp index 0a84f78828..2ff3e5d30c 100644 --- a/src/libopenrave/kinbodystatesaver.cpp +++ b/src/libopenrave/kinbodystatesaver.cpp @@ -193,6 +193,15 @@ void KinBody::_RestoreGrabbedBodiesFromSavedData(const KinBody& savedBody, 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; } + + const int envBodyIndexFirst = pFirst->GetEnvironmentBodyIndex(); + const int envBodyIndexSecond = pSecond->GetEnvironmentBodyIndex(); + + // savedMapListNonCollidingInterGrabbedLinkPairsWhenGrabbed satisfies the rule of indices pair, e.g. pFirstSaved->GetEnvironmentBodyIndex() < pSecond->GetEnvironmentBodyIndex(). Thus, same rule should be satisfied to the restoring scene. + OPENRAVE_ASSERT_OP_FORMAT(envBodyIndexFirst, <, envBodyIndexSecond, + "env=%s, envBodyIndex for '%s'(%d) and '%s'(%d) are invalid. The former one should be smaller. from env=%s", GetEnv()->GetNameId()%pFirst->GetName()%pFirst->GetEnvironmentBodyIndex()%pSecond->GetName()%pSecond->GetEnvironmentBodyIndex()%savedBody.GetEnv()->GetNameId(), ORE_Failed); + + // compute and push KinBody::ListNonCollidingLinkPairs listNonCollidingLinkPairs; FOREACHC(itLinkPairSaved, itInfoSaved->second) { const int linkIndexFirst = (*itLinkPairSaved).first->GetIndex(); @@ -204,7 +213,7 @@ void KinBody::_RestoreGrabbedBodiesFromSavedData(const KinBody& savedBody, listNonCollidingLinkPairs.emplace_back(pFirst->GetLinks()[linkIndexFirst], pSecond->GetLinks()[linkIndexSecond]); } if( listNonCollidingLinkPairs.size() > 0 ){ - const uint64_t key = _ComputeEnvironmentBodyIndicesPair(*pFirst, *pSecond); + const uint64_t key = _ComputeEnvironmentBodyIndicesPair(envBodyIndexFirst, envBodyIndexSecond); _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(key, std::move(listNonCollidingLinkPairs)); } } From d8a7d4ffee5960632b538256ce677aa539c486ab Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Fri, 25 Oct 2024 19:33:06 +0900 Subject: [PATCH 19/25] Bump minor version to fix bug of _listNonColidingLinksWhenGrabbed asymmetricity. --- CMakeLists.txt | 2 +- docs/source/changelog.rst | 7 +++++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9a9b59d0a0..172a257f10 100644 --- a/CMakeLists.txt +++ b/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 156) +set (OPENRAVE_VERSION_MINOR 157) 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}) diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst index 43a03ed7eb..7cd0fc65c0 100644 --- a/docs/source/changelog.rst +++ b/docs/source/changelog.rst @@ -3,6 +3,13 @@ ChangeLog ######### +Version 0.157.0 +=============== + +- Fix bug of `_listNonColidingLinksWhenGrabbed` asymmetricity which might cause false positive/negative self collision checking and might make it less deterministic. + - Store the link pair for grabbed-grabber collision in `Grabbed` class. + - Store the link pair for inter-grabbed collision in `KinBody` class. + Version 0.156.0 =============== From f6f4f50d45c0c47f40f890ba2652ab302db7e60f Mon Sep 17 00:00:00 2001 From: Puttichai Date: Mon, 4 Nov 2024 15:51:50 +0900 Subject: [PATCH 20/25] Changes from using macro to range-based for for clarity; Improves log messages --- src/libopenrave/kinbody.cpp | 42 ++++++++++++++++----------------- src/libopenrave/kinbodygrab.cpp | 6 ++--- 2 files changed, 24 insertions(+), 24 deletions(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 0e4332bf3b..5e198d6a84 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -5906,19 +5906,19 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) pgrabbed->_setGrabberLinkIndicesToIgnore = pgrabbedref->_setGrabberLinkIndicesToIgnore; // can do this since link indices are the same CopyRapidJsonDoc(pgrabbedref->_rGrabbedUserData, pgrabbed->_rGrabbedUserData); if( pgrabbedref->IsListNonCollidingLinksValid() ) { - FOREACHC(itLinkRef, pgrabbedref->_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed) { - if( (*itLinkRef).second->GetParent() != r ) { - RAVELOG_WARN_FORMAT("env=%s, could not restore link '%s' since parent in the list is different from cloning reference.", GetEnv()->GetNameId()%(*itLinkRef).second->GetName()); + for( const std::pair& grabbedGrabberLinkPair: pgrabbedref->_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed ) { + if( grabbedGrabberLinkPair.second->GetParent() != r ) { + RAVELOG_WARN_FORMAT("env=%s, could not restore grabber link '%s' since the parent in the list is different from cloning reference.", GetEnv()->GetNameId() % grabbedGrabberLinkPair.second->GetName()); continue; } - const int linkIndex = (*itLinkRef).second->GetIndex(); + const int linkIndex = grabbedGrabberLinkPair.second->GetIndex(); if( linkIndex < 0 || linkIndex >= (int)_veclinks.size()) { - RAVELOG_WARN_FORMAT("env=%s, could not restore link '%s' since its index %d is out of range (body num links is %d)", GetEnv()->GetNameId()%(*itLinkRef).second->GetName()%linkIndex%_veclinks.size()); + RAVELOG_WARN_FORMAT("env=%s, could not restore grabber link '%s' since its index %d is out of range (num links in grabber is %d)", GetEnv()->GetNameId() % grabbedGrabberLinkPair.second->GetName() % linkIndex % _veclinks.size()); continue; } - const KinBody::LinkPtr pGrabbedBodyLink = pgrabbedbody->GetLink((*itLinkRef).first->GetName()); + const KinBody::LinkPtr pGrabbedBodyLink = pgrabbedbody->GetLink(grabbedGrabberLinkPair.first->GetName()); if( !pGrabbedBodyLink ) { - RAVELOG_WARN_FORMAT("env=%s, could not restore link '%s' since it's not in the links of body '%s'.", GetEnv()->GetNameId()%(*itLinkRef).first->GetName()%pgrabbedbody->GetName()); + RAVELOG_WARN_FORMAT("env=%s, could not restore grabbed link '%s' since it's not in the links of body '%s'.", GetEnv()->GetNameId() % grabbedGrabberLinkPair.first->GetName() % pgrabbedbody->GetName()); continue; } pgrabbed->_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed.emplace_back(pGrabbedBodyLink, _veclinks[linkIndex]); @@ -5941,44 +5941,44 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) } // end for pgrabbedref // map list of inter grabbed. - FOREACHC(itInfoRef, r->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed) { - const ListNonCollidingLinkPairs& pairsRef = itInfoRef->second; - const uint64_t envBodyIndicesPair = itInfoRef->first; - if( pairsRef.empty() ) { + for( const std::pair& keyValuePair: r->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed ) { + const ListNonCollidingLinkPairs& interGrabbedLinkPairs = keyValuePair.second; + const uint64_t envBodyIndicesPair = keyValuePair.first; + if( interGrabbedLinkPairs.empty() ) { continue; } if( !_IsListNonCollidingLinksValidFromEnvironmentBodyIndex(_GetFirstEnvironmentBodyIndexFromPair(envBodyIndicesPair)) || !_IsListNonCollidingLinksValidFromEnvironmentBodyIndex(_GetSecondEnvironmentBodyIndexFromPair(envBodyIndicesPair)) ) { continue; } - const KinBodyPtr pFirstRef = pairsRef.front().first->GetParent(true); - const KinBodyPtr pSecondRef = pairsRef.front().second->GetParent(true); + const KinBodyPtr pFirstRef = interGrabbedLinkPairs.front().first->GetParent(true); + const KinBodyPtr pSecondRef = interGrabbedLinkPairs.front().second->GetParent(true); if( !pFirstRef || !pSecondRef ) { continue; // somehow, relevant code in the above does not show warning nor exception. so, follow it for now. } const KinBodyPtr pFirst = GetEnv()->GetKinBody(pFirstRef->GetName()); if( !pFirst ) { - RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find body %s for non-colliding link.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%pFirstRef->GetName()); + RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find grabbed body %s for non-colliding link.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%pFirstRef->GetName()); continue; } const KinBodyPtr pSecond = GetEnv()->GetKinBody(pSecondRef->GetName()); if( !pSecond ) { - RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find body %s for non-colliding link.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%pSecondRef->GetName()); + RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find grabbed body %s for non-colliding link.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%pSecondRef->GetName()); continue; } ListNonCollidingLinkPairs listNonCollidingLinkPairs; const int envBodyIndexFirst = pFirst->GetEnvironmentBodyIndex(); const int envBodyIndexSecond = pSecond->GetEnvironmentBodyIndex(); const bool bHasSmallerFirstIndex = envBodyIndexFirst < envBodyIndexSecond; - FOREACHC(itLinkPairRef, itInfoRef->second) { - const KinBody::LinkPtr pFirstLink = pFirst->GetLink((*itLinkPairRef).first->GetName()); + for( const std::pair& interGrabbedLinkPair: interGrabbedLinkPairs ) { + const KinBody::LinkPtr pFirstLink = pFirst->GetLink(interGrabbedLinkPair.first->GetName()); if( !pFirstLink ) { - RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find non-colliding link %s in body %s.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%(*itLinkPairRef).first->GetName()%pFirst->GetName()); + RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find non-colliding link %s in grabbed body %s.", GetEnv()->GetNameId() % GetName() % r->GetEnv()->GetNameId() % interGrabbedLinkPair.first->GetName() % pFirst->GetName()); continue; } - const KinBody::LinkPtr pSecondLink = pSecond->GetLink((*itLinkPairRef).second->GetName()); + const KinBody::LinkPtr pSecondLink = pSecond->GetLink(interGrabbedLinkPair.second->GetName()); if( !pSecondLink ) { - RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find non-colliding link %s in body %s.", GetEnv()->GetNameId()%GetName()%r->GetEnv()->GetNameId()%(*itLinkPairRef).second->GetName()%pSecond->GetName()); + RAVELOG_WARN_FORMAT("env=%s, When cloning body '%s' from env=%s, could not find non-colliding link %s in grabbed body %s.", GetEnv()->GetNameId() % GetName() % r->GetEnv()->GetNameId() % interGrabbedLinkPair.second->GetName() % pSecond->GetName()); continue; } if( bHasSmallerFirstIndex ) { @@ -5988,7 +5988,7 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) listNonCollidingLinkPairs.emplace_back(pSecondLink, pFirstLink); } } - if( listNonCollidingLinkPairs.size() > 0 ){ + if( listNonCollidingLinkPairs.size() > 0 ) { if( bHasSmallerFirstIndex ) { _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(_ComputeEnvironmentBodyIndicesPair(envBodyIndexFirst, envBodyIndexSecond), std::move(listNonCollidingLinkPairs)); } diff --git a/src/libopenrave/kinbodygrab.cpp b/src/libopenrave/kinbodygrab.cpp index 6d5a147a63..8cf47a5f85 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -38,8 +38,8 @@ static bool _IsLinkPairIncluded(const KinBody::Link* pLink1ToSearch, const KinBody::Link* pLink2ToSearch, const std::list >& listNonCollidingLinkPairs) { - FOREACHC(itLinks, listNonCollidingLinkPairs) { - if ( ((*itLinks).first.get() == pLink1ToSearch && (*itLinks).second.get() == pLink2ToSearch) ) { + for( const std::pair& linkPair: listNonCollidingLinkPairs ) { + if ( linkPair.first.get() == pLink1ToSearch && linkPair.second.get() == pLink2ToSearch ) { return true; } } @@ -1035,7 +1035,7 @@ bool KinBody::_IsListNonCollidingLinksValidFromEnvironmentBodyIndex(const int en { MapGrabbedByEnvironmentIndex::const_iterator itGrab = _grabbedBodiesByEnvironmentIndex.find(envBodyIndex); if( itGrab == _grabbedBodiesByEnvironmentIndex.end() ) { - RAVELOG_WARN_FORMAT("env=%s, could not check the IsListNonCollidingLinksValid for body '%s', since there is no grabbed body with envBodyIndex=%d.", GetEnv()->GetNameId()%GetName() % envBodyIndex); + RAVELOG_WARN_FORMAT("env=%s, could not check the IsListNonCollidingLinksValid for body '%s', since there is no grabbed body with envBodyIndex=%d.", GetEnv()->GetNameId() % GetName() % envBodyIndex); return false; } return itGrab->second->IsListNonCollidingLinksValid(); From 2dc435f714fe36c0b3c633626b231f9104c46378 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 7 Nov 2024 17:59:06 +0900 Subject: [PATCH 21/25] Rename variables/functions to distinguish intergrabbed link pairs vs grabbed-grabber link pairs. : https://github.com/rdiankov/openrave/pull/1438#issuecomment-2453878411 --- include/openrave/kinbody.h | 10 +++---- src/libopenrave/kinbodygrab.cpp | 46 ++++++++++++++++----------------- 2 files changed, 28 insertions(+), 28 deletions(-) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index 896a2989a7..772c5ec5b0 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -3902,14 +3902,14 @@ class OPENRAVE_API Grabbed : public UserData, public boost::enable_shared_from_t rapidjson::Document _rGrabbedUserData; ///< user-defined data to be updated when kinbody grabs and releases objects private: - /// \brief push inter-grabbed-bodies non colliding link pairs to grabber. + /// \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. /// \param[out] pGrabber : updated grabber. /// \param[out] pchecker : collision checker /// \param[in] grabbedBody, otherGrabbedBody : grabbed body by this class, and other grabbed body to check. - void _PushNonCollidingLinkPairsForGrabbedBodies(KinBodyPtr& pGrabber, - CollisionCheckerBasePtr& pchecker, - const KinBody& grabbedBody, - const KinBody& otherGrabbedBody); + void _UpdateMapListNonCollidingInterGrabbedLinkPairs(KinBodyPtr& pGrabber, + CollisionCheckerBasePtr& pchecker, + const KinBody& grabbedBody, + const KinBody& otherGrabbedBody); bool _listNonCollidingIsValid = false; ///< a flag indicating whether the current _listNonCollidingLinksWhenGrabbed is valid or not. std::vector _vAttachedToGrabbingLink; ///< vector of all links that are rigidly attached to _pGrabbingLink diff --git a/src/libopenrave/kinbodygrab.cpp b/src/libopenrave/kinbodygrab.cpp index 8cf47a5f85..3f4a203bd0 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -18,15 +18,15 @@ namespace OpenRAVE { -/// \brief Push link to listNonCollidingLinkPairs if the given links are not in collision each other. This is for grabbed-grabber link pairs. -static void _PushLinkIfNonColliding(std::list >& listNonCollidingLinkPairs, - CollisionCheckerBasePtr& pchecker, - const KinBody::LinkPtr& pGrabberLinkToCheck, const KinBody& grabbedBody) +/// \brief Push link to listNonCollidingGrabbedGrabberLinkPairs if the given links are not in collision each other. This is for grabbed-grabber link pairs. +static void _PushGrabbedGrabberLinkPairsIfNonColliding(std::list >& listNonCollidingGrabbedGrabberLinkPairs, + CollisionCheckerBasePtr& pchecker, + const KinBody::LinkPtr& pGrabberLinkToCheck, const KinBody& grabbedBody) { KinBody::LinkConstPtr pGrabberLinkToCheckConst(pGrabberLinkToCheck); for (const KinBody::LinkPtr& pGrabbedBodylink : grabbedBody.GetLinks()) { if( !pchecker->CheckCollision(pGrabberLinkToCheckConst, KinBody::LinkConstPtr(pGrabbedBodylink)) ) { - listNonCollidingLinkPairs.emplace_back(pGrabbedBodylink, pGrabberLinkToCheck); + listNonCollidingGrabbedGrabberLinkPairs.emplace_back(pGrabbedBodylink, pGrabberLinkToCheck); } } } @@ -34,11 +34,11 @@ static void _PushLinkIfNonColliding(std::list >& listNonCollidingLinkPairs) +static bool _IsInterGrabbedLinkPairIncluded(const KinBody::Link* pLink1ToSearch, + const KinBody::Link* pLink2ToSearch, + const std::list >& listNonCollidingInterGrabbedLinkPairs) { - for( const std::pair& linkPair: listNonCollidingLinkPairs ) { + for( const std::pair& linkPair: listNonCollidingInterGrabbedLinkPairs ) { if ( linkPair.first.get() == pLink1ToSearch && linkPair.second.get() == pLink2ToSearch ) { return true; } @@ -48,20 +48,20 @@ static bool _IsLinkPairIncluded(const KinBody::Link* pLink1ToSearch, /// \brief Push link to listNonCollidingLinkPairs if the given grabbed bodies links are not in collision each other. This is inter-grabbed link pairs. /// Note that envBodyIndex of grabbedBody1 should be smaller than envBodyIndex of grabbedBody2. -static void _PushLinkPairsIfNonCollidingWithOtherGrabbedBody(std::list >& listNonCollidingLinkPairs, - CollisionCheckerBasePtr& pchecker, - const KinBody& grabbedBody1, - const KinBody& grabbedBody2) +static void _PushInterGrabbedLinkPairsIfNonColliding(std::list >& listNonCollidingInterGrabbedLinkPairs, + CollisionCheckerBasePtr& pchecker, + const KinBody& grabbedBody1, + const KinBody& grabbedBody2) { for (const KinBody::LinkPtr& pGrabbedBody1Link : grabbedBody1.GetLinks()) { for (const KinBody::LinkPtr& pGrabbedBody2Link : grabbedBody2.GetLinks()) { // if already in the list, no need to check collision. - if( _IsLinkPairIncluded(pGrabbedBody1Link.get(), pGrabbedBody2Link.get(), listNonCollidingLinkPairs) ) { + if( _IsInterGrabbedLinkPairIncluded(pGrabbedBody1Link.get(), pGrabbedBody2Link.get(), listNonCollidingInterGrabbedLinkPairs) ) { continue; } // if not colliding, push. if( !pchecker->CheckCollision(KinBody::LinkConstPtr(pGrabbedBody1Link), KinBody::LinkConstPtr(pGrabbedBody2Link)) ) { - listNonCollidingLinkPairs.emplace_back(pGrabbedBody1Link, pGrabbedBody2Link); + listNonCollidingInterGrabbedLinkPairs.emplace_back(pGrabbedBody1Link, pGrabbedBody2Link); } } } @@ -243,7 +243,7 @@ void Grabbed::ComputeListNonCollidingLinks() // This link (*itGrabberLink) is *not* rigidly attached to _pGrabbingLink. if( _setGrabberLinkIndicesToIgnore.find((*itGrabberLink)->GetIndex()) == _setGrabberLinkIndicesToIgnore.end() ) { // Not ignoring collisions between this link and the grabber body - _PushLinkIfNonColliding(_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed, pchecker, *itGrabberLink, grabbedBody); + _PushGrabbedGrabberLinkPairsIfNonColliding(_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed, pchecker, *itGrabberLink, grabbedBody); } } } @@ -259,7 +259,7 @@ void Grabbed::ComputeListNonCollidingLinks() if (!bTwoGrabbedBodiesHaveStaticRelativePose) { KinBody::KinBodyStateSaver otherGrabbedEnableSaver(pOtherGrabbedBody, KinBody::Save_LinkEnable); pOtherGrabbedBody->Enable(true); - _PushNonCollidingLinkPairsForGrabbedBodies(pGrabber, pchecker, grabbedBody, *pOtherGrabbedBody); + _UpdateMapListNonCollidingInterGrabbedLinkPairs(pGrabber, pchecker, grabbedBody, *pOtherGrabbedBody); } } } @@ -277,10 +277,10 @@ void Grabbed::ComputeListNonCollidingLinks() // } } -void Grabbed::_PushNonCollidingLinkPairsForGrabbedBodies(KinBodyPtr& pGrabber, - CollisionCheckerBasePtr& pchecker, - const KinBody& grabbedBody, - const KinBody& otherGrabbedBody) +void Grabbed::_UpdateMapListNonCollidingInterGrabbedLinkPairs(KinBodyPtr& pGrabber, + CollisionCheckerBasePtr& pchecker, + const KinBody& grabbedBody, + const KinBody& otherGrabbedBody) { // Find item by indices pair. const bool bNoInvert = grabbedBody.GetEnvironmentBodyIndex() < otherGrabbedBody.GetEnvironmentBodyIndex(); @@ -291,13 +291,13 @@ void Grabbed::_PushNonCollidingLinkPairsForGrabbedBodies(KinBodyPtr& pGrabber, const uint64_t key = KinBody::_ComputeEnvironmentBodyIndicesPair(grabbedBody1.GetEnvironmentBodyIndex(), grabbedBody2.GetEnvironmentBodyIndex()); std::unordered_map::iterator itInfo = pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.find(key); if( itInfo != pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.end() ) { - _PushLinkPairsIfNonCollidingWithOtherGrabbedBody((*itInfo).second, pchecker, grabbedBody1, grabbedBody2); + _PushInterGrabbedLinkPairsIfNonColliding((*itInfo).second, pchecker, grabbedBody1, grabbedBody2); 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, grabbedBody1, grabbedBody2); + _PushInterGrabbedLinkPairsIfNonColliding(listNonCollidingLinkPairs, pchecker, grabbedBody1, grabbedBody2); if( listNonCollidingLinkPairs.size() > 0 ) { pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(key, std::move(listNonCollidingLinkPairs)).first; } From 1e918f78f9949653f2ef8e323e6b4a760671a367 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 7 Nov 2024 18:02:28 +0900 Subject: [PATCH 22/25] Improve the comment of ListNonCollidingLinkPairs Co-authored-by: Puttichai Lertkultanon --- include/openrave/kinbody.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index 772c5ec5b0..95764630df 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -2438,7 +2438,7 @@ class OPENRAVE_API KinBody : public InterfaceBase typedef boost::shared_ptr KinBodyInfoPtr; typedef boost::shared_ptr KinBodyInfoConstPtr; - /// \brief Alias for list of non-colliding link pairs, mostly used for Grabbed checking. + /// \brief Alias for list of non-colliding link pairs, mainly used for collision checking for Grabbed. using ListNonCollidingLinkPairs = std::list >; /// \brief Saved data for Grabbed used in KinBodyStateSaver and KinBodyStateSaverRef From 455ed5f9b2d381cefefa5e79543e592feae0b8b9 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 7 Nov 2024 18:12:32 +0900 Subject: [PATCH 23/25] Improve comments : https://github.com/rdiankov/openrave/pull/1438#discussion_r1827214163, https://github.com/rdiankov/openrave/pull/1438#discussion_r1827213718 Co-authored-by: Puttichai Lertkultanon --- src/libopenrave/kinbodygrab.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/libopenrave/kinbodygrab.cpp b/src/libopenrave/kinbodygrab.cpp index 3f4a203bd0..b53b2bcb25 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -18,7 +18,9 @@ namespace OpenRAVE { -/// \brief Push link to listNonCollidingGrabbedGrabberLinkPairs if the given links are not in collision each other. This is for grabbed-grabber link pairs. +/// \brief Push (grabbedBodyLink, grabberLink) pairs to the given listNonCollidingGrabbedGrabberLinkPairs if they are not colliding +/// with each other. Note that the order of pair will always be such that the first element is the grabbed +/// body's link and the second element is the grabber's link. static void _PushGrabbedGrabberLinkPairsIfNonColliding(std::list >& listNonCollidingGrabbedGrabberLinkPairs, CollisionCheckerBasePtr& pchecker, const KinBody::LinkPtr& pGrabberLinkToCheck, const KinBody& grabbedBody) @@ -31,7 +33,10 @@ static void _PushGrabbedGrabberLinkPairsIfNonColliding(std::list Date: Thu, 7 Nov 2024 18:15:26 +0900 Subject: [PATCH 24/25] Improve comments : https://github.com/rdiankov/openrave/pull/1438#discussion_r1827214422 Co-authored-by: Puttichai Lertkultanon --- src/libopenrave/kinbodygrab.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/libopenrave/kinbodygrab.cpp b/src/libopenrave/kinbodygrab.cpp index b53b2bcb25..77f79786b7 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -51,8 +51,9 @@ static bool _IsInterGrabbedLinkPairIncluded(const KinBody::Link* pLink1ToSearch, return false; } -/// \brief Push link to listNonCollidingLinkPairs if the given grabbed bodies links are not in collision each other. This is inter-grabbed link pairs. -/// Note that envBodyIndex of grabbedBody1 should be smaller than envBodyIndex of grabbedBody2. +/// \brief Push to the given listNonCollidingInterGrabbedLinkPairs the pairs of non-colliding links between grabbedBody1 and grabbedBody2. +/// +/// This function assumes that grabbedBody1.GetEnvironmentBodyIndex() < grabbedBody2.GetEnvironmentBodyIndex(). static void _PushInterGrabbedLinkPairsIfNonColliding(std::list >& listNonCollidingInterGrabbedLinkPairs, CollisionCheckerBasePtr& pchecker, const KinBody& grabbedBody1, From 3d4efe2a1be0f1979ece99e90926d2615b4619b9 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 7 Nov 2024 18:22:04 +0900 Subject: [PATCH 25/25] Improve the comment of _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed Co-authored-by: Puttichai Lertkultanon --- include/openrave/kinbody.h | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index 95764630df..f8791b02e9 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -3820,7 +3820,18 @@ class OPENRAVE_API KinBody : public InterfaceBase mutable std::string __hashKinematicsGeometryDynamics; ///< hash serializing kinematics, dynamics and geometry properties of the KinBody int64_t _lastModifiedAtUS=0; ///< us, linux epoch, last modified time of the kinbody when it was originally loaded from the environment. int64_t _revisionId = 0; ///< the webstack revision for this loaded kinbody - std::unordered_map _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed; ///< map of list of link pairs. This is computed when grabbed bodies are grabbed, and at taht time, two grabbed bodies are not touching each other. Since these links are not colliding at the time of grabbing, they should remain non-colliding with the grabbed body throughout. If, while grabbing, they collide with the grabbed body at some point, CheckSelfCollision should return true. It is important to note that the enable state of a link does *not* affect its membership of this list. Each pair in the list should be [Grabbed1-link, Grabbed2-link]. Note that this does not contain link pairs of [Grabbed-link, Grabber-link], c.f. Grabbed::_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed. Note that the key of this map is 'environment body indices pair', which lower 32bits are for the first KinBody's envBodyIndex, and which higher 32bits are for the second KinBody's envBodyIndex. The first envBodyIndex should be always smaller than the second envBodyIndex to simplify searching. Please also see _ComputeEnvironmentBodyIndicesPair. + /// _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed maps a pair of envBodyIndices of two grabbed bodies + /// (encoded into one uint64_t via _ComputeEnvironmentBodyIndicesPair) to a list of initially non-colliding link + /// pairs between the two. The ListNonCollidingLinkPairs for body1 and body2 is computed from state when the latest + /// body between body1 and body2 has been grabbed. Since these links in each pair are not colliding with each other + /// at the time of grabbing, they should remain non-colliding throughout (i.e. until either of them is released). + /// Notes: + /// - The enable states of links do *not* affect the membership of this ListNonCollidingLinkPair. + /// - ListNonCollidingLinkPair, which is the values of this map, only contains link pairs of *grabbed* bodies (i.e. + /// not grabber's links). + /// - 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 _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed; private: mutable std::vector _vTempJoints;