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 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.

[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?

[Puttichai]

Suggested change

	std::unordered_map<uint64_t, ListNonCollidingLinkPairs> _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<uint64_t, ListNonCollidingLinkPairs> _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed;

[Puttichai]

Suggested change

	/// \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<std::pair<KinBody::LinkConstPtr, KinBody::LinkConstPtr> >& listNonCollidingLinkPairs,
	/// \brief Push (grabbedBodyLink, grabberLink) pairs to the given listNonCollidingLinkPairs 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 _PushLinkIfNonColliding(KinBody::ListNonCollidingLinkPairs& listNonCollidingLinkPairs,
	CollisionCheckerBasePtr& pchecker,
	const KinBody::LinkPtr& pGrabberLinkToCheck, const KinBody& grabbedBody)

[Puttichai]

Suggested change

	/// \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<std::pair<KinBody::LinkConstPtr, KinBody::LinkConstPtr> >& listNonCollidingLinkPairs)
	/// \brief Return true if the link pair (pLink1ToSearch, pLink2ToSearch) is already in the given
	/// listNonCollidingLinkPairs. Note that the order of the links must be such that body1.GetEnvironmentBodyIndex()
	/// < body2.GetEnvironmentBodyIndex() where body1 is the kinbody pLink1ToSearch belongs to and body2 is the
	/// kinbody pLink2ToSearch belongs to.
	/// \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 KinBody::ListNonCollidingLinkPairs& listNonCollidingLinkPairs)

[Puttichai]

Suggested change

	/// \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<std::pair<KinBody::LinkConstPtr, KinBody::LinkConstPtr> >& listNonCollidingLinkPairs,
	/// \brief Push to the given listNonCollidingLinkPairs the pairs of non-colliding links between grabbedBody1 and grabbedBody2.
	///
	/// This function assumes that grabbedBody1.GetEnvironmentBodyIndex() < grabbedBody2.GetEnvironmentBodyIndex().
	static void _PushLinkPairsIfNonCollidingWithOtherGrabbedBody(KinBody::ListNonCollidingLinkPairs& listNonCollidingLinkPairs,