WIP: Fix grabbed self collision check in other APIs #1443
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
added 30 commits
September 25, 2024 11:58
- Issue - This commit tries to improve the reported issue1, issue2, and issue3 : #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 #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 #1421, inter-grabbed link pairs are stored as unordered_map, and its key is combined env body indices of two grabbed bodies.
…ating the vector.
…CollidingGrabbedGrabberLinkPairsWhenGrabbed in CheckSelfCollisionChecking
…nkCollisions==false. also share the utilities for CollisioReport update and printing.
…' to clarify meaning.
…fixGrabbedSelfColCheckAsym
…fixGrabbedSelfColCheckAsym
…ed variables. 2) remove unused kinbodysaver from the first overload function. 3) remove linksaver, since it will not be necessary when _CheckGrabbedBodiesSelfCollision will be used.
…fCollsionCheck overload. In addition, keep the plink transform for grabbed bodies checking.
…ion checking context. So far, we supply vindependentlinks. Although we can do that by both inclusive links and exclusive links, introduce inclusive links here because we can just use vindependent links in CheckEndEffectorSelfCollisionCheck function
…fixGrabbedSelfColCheckAsym
… fixGrabbedSelfCollisionCheckInOtherAPIs
…ollisionCheckInOtherAPIs
…fixGrabbedSelfColCheckAsym
… fixGrabbedSelfCollisionCheckInOtherAPIs
…fixGrabbedSelfColCheckAsym
… fixGrabbedSelfCollisionCheckInOtherAPIs
…inbodystatesaver.cpp.
added 14 commits
October 19, 2024 12:30
… fixGrabbedSelfCollisionCheckInOtherAPIs
- Issue - Previously, KinBody remembers the order when the grabbed bodies are grabbed. - However, it's broken since we introduced unordered_map. Especially, listNonCollidingLinksWhenGrabbed computation becomes less deterministic along with its lazy computatoin. - Resolution - Since unordered_map is introduced to improve the performance, not good idea to revive std::vector as its representation. - Instead, introduce the unique id to remember the order when 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 #1436.
…abbedSelfCollisionCheckInOtherAPIs
… reviveOrderOfGrabbed
… reviveOrderOfGrabbed
- Issue - When the new Grabbed instance is created in saver or Clone, the _pGrabbedSaver and _pGrabberSaver of the new Grabbed are different from those of the original Grabbed. - Thus, if isListNonCollidingLinksValid=false, wrong state was used in ComputeListNonCollidingLinks. - Resolution - Copy the consistent states from the original Grabbed to the new Grabbed. - To do so, introduce the constructor of Grabbed to copy the saver states from the original Grabbed. In addition, introduce the constructor of StateSavers to copy the states from the original savers. - If the states are not copy-able, throw exception. Also, this API is very advanced and not expected to be used from the usual users. Thus, make such functionality as the constructor.
…olCache' into fixGrabbedSelfCollisionCheckInOtherAPIs
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Summary
Bug fix 1
KinBody::CheckLinkSelfCollisionfunctions have a code trying to handle grabbed bodies.CheckCollisionaboutKinBody, and it ignores the collision checking between two bodies attached each other. Thus, it does not check the collision at all.CheckSelfCollision.CheckSelfCollisionandCheckLinkSelfCollision.CheckLinkSelfCollision.Bug fix 2
Manipulator::CheckEndEffectorSelfCollisiondoes not check the self collision with grabbed bodies ifbIgnoreManipulatorLinks=trueis specified.