Fix CheckLinkSelfCollision and CheckEndEffectorCollision

[snozawa]

Summary

• Fix bugs that several of KinBody::CheckLinkSelfCollision and Manipulator::CheckEndEffectorCollision, which are mostly used from IK related code path.

Bug fix 1

• Issue
  • KinBody::CheckLinkSelfCollision functions have a code trying to handle grabbed bodies.
  • However, it uses the overload of CheckCollision(KinBody, KinBody), and it ignores the collision checking between two bodies attached each other.

    openrave/src/libopenrave/kinbodycollision.cpp

    Line 464 in d1b75e6

    if( pchecker->CheckCollision(shared_kinbody_const(), KinBodyConstPtr(pgrabbedbody),report) ) {

  • Thus, it does not check the collision at all.
• Resolution
  • Re-use the same logic in CheckSelfCollision.
  • Separate the self collision checking functionality about grabbed bodies. Use it both from CheckSelfCollision and CheckLinkSelfCollision.
  • In addition, add functionalities to fit with CheckLinkSelfCollision use cases:
    • Enable to check only specific grabber's link :

      openrave/src/libopenrave/kinbodycollision.cpp

      Line 454 in d1b75e6

      if( pgrabbed->_pGrabbingLink == plink ) {

    • Enable to change the link transform temporarily :

      openrave/src/libopenrave/kinbodycollision.cpp

      Line 463 in d1b75e6

      pgrabbedbody->SetTransform(tlinktrans * pgrabbed->_tRelative);

Bug fix 2

• Issue
  • does not check the self collision with grabbed bodies at all, if bIgnoreManipulatorLinks=true is specified :

    openrave/src/libopenrave/robotmanipulator.cpp

    Lines 1147 to 1155 in d1b75e6

    	FOREACHC(itindependentlink,vindependentinks) {
    	if( *itlink != *itindependentlink && (*itindependentlink)->IsEnabled() ) {
    	if( pselfchecker->CheckCollision(*itlink, *itindependentlink,report) ) {
    	if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
    	RAVELOG_VERBOSE_FORMAT("gripper link self collision with link %s", (*itlink)->GetName());
    	return true;
    	}
    	bincollision = true;
    	}

• Resolution
  • Resolve this by calling CheckLinkSelfCollision as other code path.

Bug fix 3

• Issue
  • Manipulator::CheckEndEffectorSelfCollision is implemented hand-made link-link collision checking if bIgnoreManipulatorLinks=true is specified :

    openrave/src/libopenrave/robotmanipulator.cpp

    Lines 1147 to 1155 in d1b75e6

    	FOREACHC(itindependentlink,vindependentinks) {
    	if( *itlink != *itindependentlink && (*itindependentlink)->IsEnabled() ) {
    	if( pselfchecker->CheckCollision(*itlink, *itindependentlink,report) ) {
    	if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
    	RAVELOG_VERBOSE_FORMAT("gripper link self collision with link %s", (*itlink)->GetName());
    	return true;
    	}
    	bincollision = true;
    	}

  • But, it does not consider the self-collision specific notion like isSelfCollisionIgnored and adjacent links.
  • Thus, it can result in false-positive results.
• Resolution
  • Added the vIncludedLinks arguments to one of the CollisionCheckerBase::CheckStandaloneSelfCollision overload, which takes plink from the argument. Accordingly, add the same argument to KinBody::CheckLinkSelfCollision as well.
  • Notes for this change
    • We can add the code to handle self-collision-specific notion in CheckEndEffectorSelfCollision side. But, it's hard to maintain. So, better to support included links in KinBody::CheckLinkSelfCollision.
    • In terms of the argument, we can implement both excluded links and included links, since both are used in the existing code of CollisionCheckerBase. In these two, I used included links in this PR, since the included link in this situation can be easily computed from GetIndependentLinks of manipulator.

Misc improvements

• CheckEndEffectorSelfCollision and CheckLinkSelfCollision have duplicated codes. Reduce it by introducing common functions.