diff --git a/CMakeLists.txt b/CMakeLists.txt index 172a257f10..b6be13e028 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 157) +set (OPENRAVE_VERSION_MINOR 159) 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 3cf53508db..6baf5cdc96 100644 --- a/docs/source/changelog.rst +++ b/docs/source/changelog.rst @@ -3,6 +3,18 @@ ChangeLog ######### +Version 0.159.0 +=============== + +- Revive the order of grabbed bodies to avoid less deterministic grabbed self collision checking. + +Version 0.158.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.157.0 =============== diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index 6daa8158bd..3485fc1b9b 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,8 @@ 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; + uint64_t _nextGrabbedBodyUniqueId = 0; bool _bRestoreOnDestructor; private: virtual void _RestoreKinBody(boost::shared_ptr body); @@ -2531,6 +2536,8 @@ 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; + uint64_t _nextGrabbedBodyUniqueId = 0; bool _bRestoreOnDestructor; bool _bReleased; ///< if true, then body should not be restored private: @@ -3716,10 +3723,14 @@ 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] savedNextGrabbedBodyUniqueId : _nextGrabbedBodyUniqueId in saver. /// \param[in] bCalledFromClone : true this is called from clone, e.g. called from _RestoreGrabbedBodiesForClone. false if Assumes that this is called from _RestoreKinBody of saver classes. void _RestoreGrabbedBodiesFromSavedData(const KinBody& savedBody, const int options, const std::unordered_map& savedGrabbedDataByEnvironmentIndex, + const std::unordered_map& savedMapListNonCollidingInterGrabbedLinkPairsWhenGrabbed, + const uint64_t savedNextGrabbedBodyUniqueId, const bool bCalledFromClone = false); /// \brief Save this kinbody's information. @@ -3729,6 +3740,21 @@ class OPENRAVE_API KinBody : public InterfaceBase /// Ensures that _vAllPairsShortestPaths is initialized if it is not already void _EnsureAllPairsShortestPaths() 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. + /// 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); + + /// \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 @@ -3798,6 +3824,8 @@ 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. + uint64_t _nextGrabbedBodyUniqueId = 0; ///< This indicates the unique id of the next grabbed body. Monotonically increasing, except for the resetting when all grabbed bodies are released, ...etc. Lower unique id means grabbed earlier, and we can identify the order of grabbed bodies through this Id. private: mutable std::vector _vTempJoints; @@ -3833,7 +3861,7 @@ class OPENRAVE_API KinBody : public InterfaceBase class OPENRAVE_API Grabbed : public UserData, public boost::enable_shared_from_this { public: - Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink); + Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink, const uint64_t uniqueId); virtual ~Grabbed() { } @@ -3873,11 +3901,22 @@ 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 + const uint64_t _uniqueId = 0; ///< The unique id of this Grabbed instance. Lower unique id means grabbed earlier, and we can identify the order of grabbed bodies through this Id. private: + + /// \brief 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 c4fbb4c0df..5093c35abc 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -5877,6 +5877,8 @@ 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(); + _nextGrabbedBodyUniqueId = r->_nextGrabbedBodyUniqueId; if ((cloningoptions & Clone_IgnoreGrabbedBodies) != Clone_IgnoreGrabbedBodies) { for (const MapGrabbedByEnvironmentIndex::value_type& otherGrabPair : r->_grabbedBodiesByEnvironmentIndex) { const GrabbedPtr& pgrabbedref = otherGrabPair.second; @@ -5900,30 +5902,27 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) } //BOOST_ASSERT(pgrabbedbody->GetName() == pbodyref->GetName()); - GrabbedPtr pgrabbed(new Grabbed(pgrabbedbody,_veclinks.at(KinBody::LinkPtr(pgrabbedref->_pGrabbingLink)->GetIndex()))); + GrabbedPtr pgrabbed(new Grabbed(pgrabbedbody,_veclinks.at(KinBody::LinkPtr(pgrabbedref->_pGrabbingLink)->GetIndex()), pgrabbedref->_uniqueId)); pgrabbed->_tRelative = pgrabbedref->_tRelative; 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())); + 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()); + continue; } - 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()); - } + 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); } @@ -5941,6 +5940,64 @@ void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions) _grabbedBodiesByEnvironmentIndex[pgrabbedbody->GetEnvironmentBodyIndex()] = std::move(pgrabbed); } } // 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() ) { + 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); + 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()); + 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()); + 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 ) { + 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((*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()%(*itLinkPairRef).second->GetName()%pSecond->GetName()); + continue; + } + if( bHasSmallerFirstIndex ) { + listNonCollidingLinkPairs.emplace_back(pFirstLink, pSecondLink); + } + else { + listNonCollidingLinkPairs.emplace_back(pSecondLink, pFirstLink); + } + } + if( listNonCollidingLinkPairs.size() > 0 ){ + if( bHasSmallerFirstIndex ) { + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(_ComputeEnvironmentBodyIndicesPair(envBodyIndexFirst, envBodyIndexSecond), std::move(listNonCollidingLinkPairs)); + } + else { + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(_ComputeEnvironmentBodyIndicesPair(envBodyIndexSecond, envBodyIndexFirst), std::move(listNonCollidingLinkPairs)); + } + } + } } // end if not Clone_IgnoreGrabbedBodies // Clone self-collision checker diff --git a/src/libopenrave/kinbodycollision.cpp b/src/libopenrave/kinbodycollision.cpp index ec7bff6550..4cb58e266a 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; @@ -72,40 +89,35 @@ 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()); - for (const MapGrabbedByEnvironmentIndex::value_type& grabPair : _grabbedBodiesByEnvironmentIndex) { - 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()); - } - - // check all grabbed bodies with (TODO: support CO_ActiveDOFs option) - 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]; - if( !pGrabbedBody1 ) { + 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& grabbedBody1 = *pGrabbedBody1; - if( !grabbedBody1.IsEnabled() ) { + if( !pGrabbedBody->IsEnabled() ) { continue; } + vGrabbedBodies.emplace_back(grabPair.second.get()); + vLockedGrabbedBodiesCache.push_back(pGrabbedBody); + } + // check all grabbed bodies with (TODO: support CO_ActiveDOFs option) + 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(); - - 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 ) { @@ -114,119 +126,61 @@ 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) ) { if( !bAllLinkCollisions ) { // if checking all collisions, have to continue - break; + _PostProcessOnCheckSelfCollision(report, pusereport, *this); + return true; } + bCollision = true; } - } - if( bCollision ) { - if( !bAllLinkCollisions ) { // if checking all collisions, have to continue - break; + if( !!pusereport && pusereport->minDistance < report->minDistance ) { + *report = *pusereport; } } + 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; } - - 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]; - 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; - - 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; - } - } - } // end for indexGrabbed2 - } // end if numGrabbed > 1 - if( bCollision ) { - if( !bAllLinkCollisions ) { // if checking all collisions, have to continue - break; - } - } } // end for indexGrabbed1 - if( bCollision && !!report ) { - if( report != pusereport ) { - *report = *pusereport; + // Check grabbed vs grabbed collision + 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 ) { + RAVELOG_WARN_FORMAT("env=%s, _listNonCollidingLinks has invalid link %s, %s", GetEnv()->GetNameId() % pairs.front().first->GetName() % pairs.front().second->GetName()); + continue; } - 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 << ","; + FOREACHC(itLinks, pairs) { + if( collisionchecker->CheckCollision((*itLinks).first, (*itLinks).second, pusereport) ) { + if( !bAllLinkCollisions ) { // if checking all collisions, have to continue + _PostProcessOnCheckSelfCollision(report, pusereport, *this); + return true; + } + bCollision = true; + } + if( !!pusereport && pusereport->minDistance < report->minDistance ) { + *report = *pusereport; } - RAVELOG_VERBOSE_FORMAT("env=%s, self collision report=%s; colvalues=[%s]", GetEnv()->GetNameId()%report->__str__()%ss.str()); } } + if( bCollision ) { + _PostProcessOnCheckSelfCollision(report, pusereport, *this); + } return bCollision; } diff --git a/src/libopenrave/kinbodygrab.cpp b/src/libopenrave/kinbodygrab.cpp index 23e8c5caab..e0f95eb188 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -18,32 +18,51 @@ 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. Note that envBodyIndex for pLink1ToSearch's parent KinBody should be smaller than envBodyIndex for pLink2ToSearch's parent KinBody. +/// \param[in] pLink1ToSearch, pLink2ToSearch : ptr of links to check. +/// \param[in] listNonCollidingLinkPairs : taret list. +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) { + if ( ((*itLinks).first.get() == pLink1ToSearch && (*itLinks).second.get() == pLink2ToSearch) ) { + 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. +/// 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) +{ + 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) ) { + continue; + } + // if not colliding, push. + if( !pchecker->CheckCollision(KinBody::LinkConstPtr(pGrabbedBody1Link), KinBody::LinkConstPtr(pGrabbedBody2Link)) ) { + listNonCollidingLinkPairs.emplace_back(pGrabbedBody1Link, pGrabbedBody2Link); + } } } } @@ -88,7 +107,8 @@ static void _CreateSaverForGrabbedAndGrabber(KinBody::KinBodyStateSaverPtr& pSav } } -Grabbed::Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink) +Grabbed::Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink, const uint64_t uniqueId) + : _uniqueId(uniqueId) { _pGrabbedBody = pGrabbedBody; _pGrabbingLink = pGrabbingLink; @@ -115,7 +135,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 +168,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 +207,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 +222,19 @@ void Grabbed::ComputeListNonCollidingLinks() continue; } - if( pOtherGrabbedBody != pGrabbedBody ) { - vGrabbedBodies.emplace_back(pOtherGrabbed.get()); - vLockedGrabbedBodiesCache.push_back(pOtherGrabbedBody); + if( pOtherGrabbedBody == pGrabbedBody ) { + continue; + } + + // If the unique id of pOtherGrabbed is >= than the one of this grabbed, it means pOtherGrabbed is grabbed later than 'this'. Thus, pOtherGrabbed did not exist when 'this' was grabbed. + // The results of lazy computation should be same as the result of non-lazy computation. Therefore, we should not compute the inter-grabbed collision checking with pOtherGrabbed. + // This resolves Issue4 in https://github.com/rdiankov/openrave/issues/1436. + if( pOtherGrabbed->_uniqueId >= _uniqueId ) { + continue; } + + vGrabbedBodies.emplace_back(pOtherGrabbed.get()); + vLockedGrabbedBodiesCache.push_back(pOtherGrabbedBody); } KinBody::KinBodyStateSaver grabbedEnableSaver(pGrabbedBody, KinBody::Save_LinkEnable); @@ -224,7 +252,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 +268,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 +286,32 @@ void Grabbed::ComputeListNonCollidingLinks() // } } +void Grabbed::_PushNonCollidingLinkPairsForGrabbedBodies(KinBodyPtr& pGrabber, + CollisionCheckerBasePtr& pchecker, + const KinBody& grabbedBody, + const KinBody& otherGrabbedBody) +{ + // 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, 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); + if( listNonCollidingLinkPairs.size() > 0 ) { + pGrabber->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(key, 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) @@ -348,7 +400,7 @@ bool KinBody::Grab(KinBodyPtr pGrabbedBody, LinkPtr pGrabbingLink, const std::se _RemoveGrabbedBody(itPreviouslyGrabbed); } - GrabbedPtr pGrabbed(new Grabbed(pGrabbedBody, pGrabbingLink)); + GrabbedPtr pGrabbed(new Grabbed(pGrabbedBody, pGrabbingLink, _nextGrabbedBodyUniqueId++)); pGrabbed->_tRelative = tGrabbingLink.inverse() * tGrabbedBody; pGrabbed->_setGrabberLinkIndicesToIgnore = setGrabberLinksToIgnore; @@ -421,8 +473,12 @@ 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()) { + _nextGrabbedBodyUniqueId = 0; // just in case return; } @@ -436,6 +492,7 @@ void KinBody::ReleaseAllGrabbed() // Clear our set of grabs _grabbedBodiesByEnvironmentIndex.clear(); + _nextGrabbedBodyUniqueId = 0; // Execute any hooks registered for grabs _PostprocessChangedParameters(Prop_RobotGrabbed); @@ -447,6 +504,9 @@ 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(); + _nextGrabbedBodyUniqueId = 0; 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; @@ -506,7 +569,7 @@ void KinBody::RegrabAll() continue; } - GrabbedPtr pNewGrabbed(new Grabbed(pBody, pGrabbed->_pGrabbingLink)); + GrabbedPtr pNewGrabbed(new Grabbed(pBody, pGrabbed->_pGrabbingLink, _nextGrabbedBodyUniqueId++)); pNewGrabbed->_tRelative = pGrabbed->_tRelative; pNewGrabbed->_setGrabberLinkIndicesToIgnore = pGrabbed->_setGrabberLinkIndicesToIgnore; CopyRapidJsonDoc(pGrabbed->_rGrabbedUserData, pNewGrabbed->_rGrabbedUserData); @@ -827,6 +890,10 @@ 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(); + _nextGrabbedBodyUniqueId = 0; + + // 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 @@ -879,7 +946,7 @@ void KinBody::ResetGrabbed(const std::vector& vGra // Even if this is an existing grabbed body, re-allocate our Grabbed record for two reasons: // - We need to re-save the current state of the grabbed object (e.g. the relative transform) // - References to the old Grabbed infos for this body may be held by state savers somewhere outside the body, so we can't mutate them without invalidating those checkpoints - MapGrabbedByEnvironmentIndex::iterator existingGrabIt = _grabbedBodiesByEnvironmentIndex.emplace(pBody->GetEnvironmentBodyIndex(), new Grabbed(pBody, pGrabbingLink)).first; + MapGrabbedByEnvironmentIndex::iterator existingGrabIt = _grabbedBodiesByEnvironmentIndex.emplace(pBody->GetEnvironmentBodyIndex(), new Grabbed(pBody, pGrabbingLink, _nextGrabbedBodyUniqueId++)).first; // Update the grab object with the ancillary grab info GrabbedPtr& pGrabbed = existingGrabIt->second; @@ -957,19 +1024,53 @@ KinBody::MapGrabbedByEnvironmentIndex::iterator KinBody::_RemoveGrabbedBody(MapG // Remove the body from our set of grabs itGrabbed = _grabbedBodiesByEnvironmentIndex.erase(itGrabbed); + if( _grabbedBodiesByEnvironmentIndex.empty() ) { + _nextGrabbedBodyUniqueId = 0; + } // If the grabbed body wasn't real, skip updating the link collision states if (!pGrabbedBody) { 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() ) { + 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(); +} + +uint64_t KinBody::_ComputeEnvironmentBodyIndicesPair(const uint64_t index1, const uint64_t index2) +{ + OPENRAVE_ASSERT_OP(index1, <, index2); + return index1 | (index2 << 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 968b448a0d..2da045c08d 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, - 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()); - } - 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::_RestoreStateForClone(const KinBodyPtr& pOriginalBody) @@ -43,7 +41,7 @@ void KinBody::_RestoreStateForClone(const KinBodyPtr& pOriginalBody) std::unordered_map originalGrabbedDataByEnvironmentIndex; pOriginalBody->_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(*pOriginalBody, options, originalGrabbedDataByEnvironmentIndex, /*bCalledFromClone*/ true); + _RestoreGrabbedBodiesFromSavedData(*pOriginalBody, options, originalGrabbedDataByEnvironmentIndex, pOriginalBody->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed, pOriginalBody->_nextGrabbedBodyUniqueId, /*bCalledFromClone*/ true); // KinBodyStateSaver::Save_LinkVelocities KinBody::KinBodyStateSaver saver(pOriginalBody, KinBody::Save_LinkVelocities); // all the others should have been saved? @@ -53,6 +51,8 @@ void KinBody::_RestoreStateForClone(const KinBodyPtr& pOriginalBody) void KinBody::_RestoreGrabbedBodiesFromSavedData(const KinBody& savedBody, const int options, const std::unordered_map& savedGrabbedDataByEnvironmentIndex, + const std::unordered_map& savedMapListNonCollidingInterGrabbedLinkPairsWhenGrabbed, + const uint64_t savedNextGrabbedBodyUniqueId, const bool bCalledFromClone) { const bool bIsFromSameEnv = GetEnv() == savedBody.GetEnv(); @@ -65,6 +65,7 @@ void KinBody::_RestoreGrabbedBodiesFromSavedData(const KinBody& savedBody, // have to release all grabbed first ReleaseAllGrabbed(); OPENRAVE_ASSERT_OP(_grabbedBodiesByEnvironmentIndex.size(),==,0); + _nextGrabbedBodyUniqueId = savedNextGrabbedBodyUniqueId; for (const std::unordered_map::value_type& grabPair : savedGrabbedDataByEnvironmentIndex) { const SavedGrabbedData& savedGrabbedData = grabPair.second; const GrabbedPtr& pGrabbed = savedGrabbedData.pGrabbed; @@ -86,7 +87,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); @@ -116,28 +117,27 @@ void KinBody::_RestoreGrabbedBodiesFromSavedData(const KinBody& savedBody, // initialized Grabbed objects will save the current state of pbody for later computation of // _listNonCollidingLinksWhenGrabbed (in case it is not yet computed). KinBody::LinkPtr pNewGrabbingLink = GetLinks().at(pGrabbingLink->GetIndex()); - GrabbedPtr pNewGrabbed(new Grabbed(pNewGrabbedBody, pNewGrabbingLink)); + GrabbedPtr pNewGrabbed(new Grabbed(pNewGrabbedBody, pNewGrabbingLink, pGrabbed->_uniqueId)); 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( !_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]); } pNewGrabbed->SetLinkNonCollidingIsValid(true); } @@ -158,6 +158,69 @@ 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 ) { + 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; + } + 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; + } + + 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(); + const int linkIndexSecond = (*itLinkPairSaved).second->GetIndex(); + 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]); + } + if( listNonCollidingLinkPairs.size() > 0 ){ + const uint64_t key = _ComputeEnvironmentBodyIndicesPair(envBodyIndexFirst, envBodyIndexSecond); + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.emplace(key, std::move(listNonCollidingLinkPairs)); + } + } + } + // if not calling SetLinkTransformations, then manually call _UpdateGrabbedBodies if( !(options & KinBody::Save_LinkTransformation ) ) { _UpdateGrabbedBodies(); @@ -172,7 +235,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(); } @@ -208,6 +271,8 @@ KinBody::KinBodyStateSaver::KinBodyStateSaver(KinBodyPtr pbody, int options) : _ } if( _options & Save_GrabbedBodies ) { _pbody->_SaveKinBodySavedGrabbedData(_grabbedDataByEnvironmentIndex); + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed = _pbody->_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed; + _nextGrabbedBodyUniqueId = _pbody->_nextGrabbedBodyUniqueId; } } @@ -247,7 +312,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, _nextGrabbedBodyUniqueId); } if( _options & Save_LinkTransformation ) { pbody->SetLinkTransformations(_vLinkTransforms, _vdoflastsetvalues); @@ -321,6 +386,8 @@ KinBody::KinBodyStateSaverRef::KinBodyStateSaverRef(KinBody& body, int options) } if( _options & Save_GrabbedBodies ) { body._SaveKinBodySavedGrabbedData(_grabbedDataByEnvironmentIndex); + _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed = body._mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed; + _nextGrabbedBodyUniqueId = body._nextGrabbedBodyUniqueId; } if( _options & Save_JointResolutions ) { body.GetDOFResolutions(_vDOFResolutions); @@ -367,7 +434,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, _nextGrabbedBodyUniqueId); } if( _options & Save_LinkTransformation ) { body.SetLinkTransformations(_vLinkTransforms, _vdoflastsetvalues); diff --git a/src/libopenrave/robot.cpp b/src/libopenrave/robot.cpp index 6badcae4a6..ccb7b73a8e 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, pOriginalRobot->_nextGrabbedBodyUniqueId, /*bCalledFromClone*/ true); // KinBody::Save_LinkVelocities if( !bRestoreGrabbedBodiesOnly ) {