Fix bugs for grabbed self col checking due to asymmetricity and unordered-ness.

[snozawa]

Summary

• Fix the bugs in CheckSelfCollision about grabbed bodies, which is caused by asymmetricity of internal information, and thus, amplified after Optimize IsGrabbing / ResetGrabbed #1421.
• This PR resolves the Issue1, Issue2, and Issue3 in Grabbed, CheckSelfCollision, and unordered grabbed bodies issues. #1436

Issue

Asymmetricity of the data

• CheckSelfCollision assumes that the internal data like _listNonCollidingLinksWhenGrabbed is kind of symmetric. Let's say, there are two grabbed bodies A and B. If we check collision by using A->_listNonCollidingLinksWhenGrabbed, CheckSelfCollision assumes that it should be same as B->_listNonCollidingLinksWhenGrabbe. But, actually, it's not symmetric. In addition, such assumption is more broken after we lose the order of grabbed bodies due to unordered_map.
• There are several symptoms, and described in Grabbed, CheckSelfCollision, and unordered grabbed bodies issues. #1436

Resolution 1

• We need to make data symmetric. To do so, instead of storing the target link like _listNonColidingLinksWhenGrabbed, this PR introduces the list of link pairs.

• There might be several ways for the link pairs.

  • Here are examples. When ComputeListNonCollidingLinks is called, the red is considered as colliding. the blue is considered as not colliding. A and B are both two-linked grabbed bodies. In the following explanation, we discuss the case when we compute ComputeListNonCollindngLinks for Grabbed for A.

  	policy1	policy2	policy3
  policy

  • policy1 is the one in production branch now. B's links are checked with the whole A. It cannot be symmetric, so we need link pairs as data.
  • If we want to introduce the link pairs, there are policy2 and policy3.
    • policy2 ignores the whole links in B if at least one link is colliding with A. Since A2 and B2 are collided, any link pairs between A and B are ignored in CheckSelfCollision.
    • policy3 ignores the only link pair which collides. The link pair (A2, B2) is only ignored, and other pairs are still checked in CheckSelfCollision.
  • By comparing policy2 and policy3, this PR goes for the policy3, because:
    • policy2 is too optimistic, and it might ignore dangerous physical collision.
    • policy3 is actually same methodologies with non-adjacent-link pairs. so, it should be fine.
  • Note that this will also change the behavior of grabbedbody-robotlink collisions.
  • Before introducing the link pairs, CheckSelfCollision consists of complex nested for loop. in addition, it caused unnecessary iteration and checking if the pairs are already checked or not. By introducing the link pairs, i hope CheckSelfCollision code is more readable, predictable, and reducing the overhead.

Resolution 2

• Since we introduced the link pairs in Resolution 1, it's bit odd that Grabbed class holds such information. Actually, Grabbed for A and Grabbed for B need to hold it.
• Instead, this PR splits link pairs as follows:
  • list of pairs between grabbed and grabber (e.g. robot) : still in Grabbed::_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed.
  • list of pairs between one grabbed and another grabbed : now in KinBody::_mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed.
• Similar to recent optimization Optimize IsGrabbing / ResetGrabbed #1421, this PR uses unordered_map for the data. Note that this PR computes the key of the unordered_map by combining two environment body indices into one uint64_t.
• This splitting also makes CheckSelfCollision for loop simpler.

[rdiankov]

@Puttichai can you review? Thanks

[kanbouchou]

Looks good to me.

[Puttichai]

@snozawa I have one concern regarding the conventions of ordering of elements of KinBody::ListNonCollidingLinkPairs. In particular, the ordering of the pair of links (plink1, plink2) depends on the context.

From the codes, there are two cases:

1. KinBody::ListNonCollidingLinkPairs contains pairs of grabbed-grabber links. In this case, the convention is (grabbedLink, grabberLink).
2. KinBody::ListNonCollidingLinkPairs contains pairs of grabbed-grabbed links (i.e. inter-grabbed links). In this case, the convention is (grabbed1Link, grabbed2Link) where grabbedBody1.GetEnvironmentBodyIndex() < grabbedBody2.GetEnvironmentBodyIndex().

Functions like _PushLinkIfNonColliding seem to operate entirely with grabbed-grabber relations while others like _IsLinkPairIncluded operate with inter-grabbed relations. From the names alone, we can't really tell which is which.

I think there are no problems with the current codes. But I wonder what we can do to make sure that in the future, people don't mistakenly use _IsLinkPairIncluded on listNonCollidingLinkPairs that contains grabbed-grabber link pairs, for example. Maybe at least we can improve the names of the functions as well as the input arguments?

How do you think?

[Puttichai]

Suggested change

	/// \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.
	/// \brief Returns a uint64_t that encodes the two given environment body indices. Body1's envBodyIndex is the lower
	/// 32 bits. Body2's envBodyIndex is the higher 32 bits. This function is used to produce a key to
	/// _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed, which is used for managing non-colliding link pairs
	/// information between two grabbed bodies.
	///
	/// The inputs must be such that index1 < index2. Raises an exception if index1 < index2 is not satisfied.

[Puttichai]

@snozawa Are there any particular reasons why you chose to force the caller to give correct ordering of indices instead of letting the function _ComputeEnvironmentBodyIndicesPair take care of ordering?

[snozawa]

Thanks!

this is because:

• some of the code path can assume always index1 < index2, and no need of reordering in that function.
• The user code of this function is not just computing the indices pair. It most likely needs to compute the link pairs as well. in such case, such computation also requires correct ordering, so automatic reordering in _ComputeEnvironmentBodyIndicesPair might not help that much. c.f. KinBody::Clone.

[snozawa]

d on listNonCollidingLinkPairs that contains grabbed-grabber link pairs, for example. Maybe at least we can improve

@Puttichai

Thanks for your commet!

Totally agree with it!
I renamed the variables/functions to distinguish intergrabbed vs grabbedgrabber:
2dc435f

[rdiankov]

thanks~