From 50198d70dd620c36fc49fe140c98347306a6458a Mon Sep 17 00:00:00 2001 From: Ross Schlaikjer Date: Tue, 7 Jan 2025 10:38:06 +0900 Subject: [PATCH 1/4] We don't actually need the velocities for ComputeListNonCollidingLinks --- 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 77f79786b7..f14e1f56fa 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -120,15 +120,16 @@ Grabbed::Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink) _pGrabbingLink->GetRigidlyAttachedLinks(_vAttachedToGrabbingLink); _listNonCollidingIsValid = false; const bool bDisableRestoreOnDestructor = true; // This is very important! These saver are used only in ComputeListNonCollidingLinks and we don't want to restore on destructor. + static const int saverOptions = KinBody::Save_LinkTransformation | KinBody::Save_LinkEnable | KinBody::Save_JointLimits | KinBody::Save_ConnectedBodies; _CreateSaverForGrabbedAndGrabber(_pGrabbedSaver, pGrabbedBody, - KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_JointLimits, + saverOptions, bDisableRestoreOnDestructor); KinBodyPtr pGrabber = RaveInterfaceCast(_pGrabbingLink->GetParent()); _CreateSaverForGrabbedAndGrabber(_pGrabberSaver, pGrabber, - KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_JointLimits|KinBody::Save_LinkVelocities, // Need to save link velocities of the grabber since will be used for computing link velocities of the grabbed bodies. + saverOptions, bDisableRestoreOnDestructor); } // end Grabbed From 6535e9c5e0504763cc5bae6af5bd6a3b5db26857 Mon Sep 17 00:00:00 2001 From: Ross Schlaikjer Date: Tue, 7 Jan 2025 10:53:41 +0900 Subject: [PATCH 2/4] Skip saver for fully rigid bodies --- src/libopenrave/kinbodygrab.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/libopenrave/kinbodygrab.cpp b/src/libopenrave/kinbodygrab.cpp index f14e1f56fa..2aab7320b4 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -118,6 +118,19 @@ Grabbed::Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink) _pGrabbedBody = pGrabbedBody; _pGrabbingLink = pGrabbingLink; _pGrabbingLink->GetRigidlyAttachedLinks(_vAttachedToGrabbingLink); + + // If the grabbing body has no joints, then it is effectively a rigid body. + // When calculating non-colliding links, we exclude all links rigidly attached to the grabbing link. + // Since for fully rigid bodies _all_ links are attached, we effectively exclude all links. + // We can therefore skip the non-colliding calculation, along with generating the associated state savers. + KinBodyPtr pGrabber(pGrabbingLink->GetParent()); + if (pGrabber->GetJoints().size() + pGrabber->GetPassiveJoints().size() == 0) { + // This body is fully rigid: all links are excluded from the non colliding link set, so just flag that the list is valid (empty) and return. + _listNonCollidingIsValid = true; + return; + } + + // if this body is not rigid, then we need to save the current state of the grabber/grabbee so that we can roll back to it when computing the non-colliding links _listNonCollidingIsValid = false; const bool bDisableRestoreOnDestructor = true; // This is very important! These saver are used only in ComputeListNonCollidingLinks and we don't want to restore on destructor. static const int saverOptions = KinBody::Save_LinkTransformation | KinBody::Save_LinkEnable | KinBody::Save_JointLimits | KinBody::Save_ConnectedBodies; @@ -125,8 +138,6 @@ Grabbed::Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink) pGrabbedBody, saverOptions, bDisableRestoreOnDestructor); - - KinBodyPtr pGrabber = RaveInterfaceCast(_pGrabbingLink->GetParent()); _CreateSaverForGrabbedAndGrabber(_pGrabberSaver, pGrabber, saverOptions, From 992028f3ffb45d5ced23be018f5c207c8398014c Mon Sep 17 00:00:00 2001 From: Ross Schlaikjer Date: Tue, 7 Jan 2025 13:23:16 +0900 Subject: [PATCH 3/4] Remove a couple shared ptr copies --- src/libopenrave/kinbody.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 8fe16c72de..46bc973c93 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -4787,7 +4787,7 @@ void KinBody::_ComputeInternalInformation() else { // NOTE: possibly try to choose roots that do not involve mimic joints. ikfast might have problems // dealing with very complex formulas - LinkPtr plink0 = joint.GetFirstAttached(), plink1 = joint.GetSecondAttached(); + const LinkPtr& plink0 = joint.GetFirstAttached(), plink1 = joint.GetSecondAttached(); if( vlinkdepths[plink0->GetIndex()] < vlinkdepths[plink1->GetIndex()] ) { parentlinkindex = plink0->GetIndex(); } @@ -4909,7 +4909,7 @@ void KinBody::_ComputeInternalInformation() stringstream ss; ss << GetName() << " closedloop found: "; FOREACH(itlinkindex,*itclosedloop) { - LinkPtr plink = _veclinks.at(*itlinkindex); + const LinkPtr& plink = _veclinks.at(*itlinkindex); ss << plink->GetName() << "(" << plink->GetIndex() << ") "; } RAVELOG_VERBOSE(ss.str()); @@ -4954,7 +4954,7 @@ void KinBody::_ComputeInternalInformation() // breadth first search for rigid links for(size_t icurlink = 0; icurlinkIsStatic() ) { if(((*itjoint)->GetFirstAttached() == plink)&& !!(*itjoint)->GetSecondAttached() &&(find(vattachedlinks.begin(),vattachedlinks.end(),(*itjoint)->GetSecondAttached()->GetIndex()) == vattachedlinks.end())) { From fba6a6f487740c9d509302c35f3908b43e5ace27 Mon Sep 17 00:00:00 2001 From: Ross Schlaikjer Date: Wed, 29 Jan 2025 08:55:50 -0500 Subject: [PATCH 4/4] Don't need connected bodies in grab saver, don't need velocity in compute non colliding --- src/libopenrave/kinbodygrab.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/libopenrave/kinbodygrab.cpp b/src/libopenrave/kinbodygrab.cpp index b327902164..4008480bd0 100644 --- a/src/libopenrave/kinbodygrab.cpp +++ b/src/libopenrave/kinbodygrab.cpp @@ -133,7 +133,7 @@ Grabbed::Grabbed(KinBodyPtr pGrabbedBody, KinBody::LinkPtr pGrabbingLink) // if this body is not rigid, then we need to save the current state of the grabber/grabbee so that we can roll back to it when computing the non-colliding links _listNonCollidingIsValid = false; const bool bDisableRestoreOnDestructor = true; // This is very important! These saver are used only in ComputeListNonCollidingLinks and we don't want to restore on destructor. - static const int saverOptions = KinBody::Save_LinkTransformation | KinBody::Save_LinkEnable | KinBody::Save_JointLimits | KinBody::Save_ConnectedBodies; + static const int saverOptions = KinBody::Save_LinkTransformation | KinBody::Save_LinkEnable | KinBody::Save_JointLimits; _CreateSaverForGrabbedAndGrabber(_pGrabbedSaver, pGrabbedBody, saverOptions, @@ -174,7 +174,7 @@ void Grabbed::ComputeListNonCollidingLinks() KinBodyPtr pGrabbedBody(_pGrabbedBody); KinBodyPtr pGrabber = RaveInterfaceCast(_pGrabbingLink->GetParent()); KinBody::KinBodyStateSaverPtr pCurrentGrabbedSaver, pCurrentGrabberSaver; - const int defaultSaveOptions = KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_LinkVelocities|KinBody::Save_JointLimits; + const int defaultSaveOptions = KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_JointLimits; const bool bDisableRestoreOnDestructor = false; _CreateSaverForGrabbedAndGrabber(pCurrentGrabbedSaver, pGrabbedBody, defaultSaveOptions, bDisableRestoreOnDestructor); _CreateSaverForGrabbedAndGrabber(pCurrentGrabberSaver, pGrabber, defaultSaveOptions, bDisableRestoreOnDestructor);