diff --git a/src/libopenrave/kinbodycollision.cpp b/src/libopenrave/kinbodycollision.cpp
index db2e9d461a..cd34b5da73 100644
--- a/src/libopenrave/kinbodycollision.cpp
+++ b/src/libopenrave/kinbodycollision.cpp
@@ -83,8 +83,7 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase
             RAVELOG_WARN_FORMAT("env=%s, grabbed body on %s has already been destroyed, ignoring.", GetEnv()->GetNameId()%GetName());
             continue;
         }
-        const KinBody& grabbedBody = *pGrabbedBody;
-        if( !grabbedBody.IsEnabled() ) {
+        if( !pGrabbedBody->IsEnabled() ) {
             continue;
         }
         vGrabbedBodies.emplace_back(grabPair.second.get());
@@ -95,14 +94,13 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase
     const size_t numGrabbed = _grabbedBodiesByEnvironmentIndex.size();
     // RAVELOG_INFO_FORMAT("env=%s, checking self collision for %s with grabbed bodies: numgrabbed=%d", GetEnv()->GetNameId()%GetName()%numGrabbed);
     for (size_t indexGrabbed1 = 0; indexGrabbed1 < numGrabbed; indexGrabbed1++) {
-        const KinBodyPtr& pGrabbedBody1 = vLockedGrabbedBodiesCache[indexGrabbed1];
         vGrabbedBodies[indexGrabbed1]->ComputeListNonCollidingLinks();
-
-        const std::list<KinBody::LinkConstPtr>& nonCollidingLinks1 = vGrabbedBodies[indexGrabbed1]->_listNonCollidingLinksWhenGrabbed;
+        const ListNonCollidingLinkPairs& nonCollidingLinkPairs = vGrabbedBodies[indexGrabbed1]->_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed;
 
         KinBodyPtr pLinkParent;
 
-        for (const KinBody::LinkConstPtr& probotlinkFromNonColliding : nonCollidingLinks1) {
+        FOREACHC(itNonCollidingLinkPairs, nonCollidingLinkPairs) {
+            const KinBody::LinkConstPtr& probotlinkFromNonColliding = (*itNonCollidingLinkPairs).second;
             const KinBody::Link& robotlinkFromNonColliding = *probotlinkFromNonColliding;
             pLinkParent = robotlinkFromNonColliding.GetParent(true);
             if( !pLinkParent ) {
@@ -111,22 +109,16 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase
             const KinBody::LinkConstPtr& probotlink = (!!pLinkParent) ? probotlinkFromNonColliding : _veclinks.at(robotlinkFromNonColliding.GetIndex());
 
             // have to use link/link collision since link/body checks attached bodies
-            for (const KinBody::LinkPtr& pGrabbedBodylink : grabbedBody1.GetLinks()) {
-                if( collisionchecker->CheckCollision(probotlink, pGrabbedBodylink, pusereport) ) {
-                    bCollision = true;
-                    if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
-                        break;
-                    }
-                }
-                if( !!pusereport && pusereport->minDistance < report->minDistance ) {
-                    *report = *pusereport;
-                }
-            }
-            if( bCollision ) {
+            const KinBody::LinkConstPtr& pGrabbedBodylink = (*itNonCollidingLinkPairs).first;
+            if( collisionchecker->CheckCollision(probotlink, pGrabbedBodylink, pusereport) ) {
+                bCollision = true;
                 if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
                     break;
                 }
             }
+            if( !!pusereport && pusereport->minDistance < report->minDistance ) {
+                *report = *pusereport;
+            }
         }
         if( bCollision ) {
             if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
@@ -134,6 +126,7 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase
             }
         }
 
+        const KinBody& grabbedBody1 = *vLockedGrabbedBodiesCache[indexGrabbed1];
         if( grabbedBody1.CheckSelfCollision(pusereport, collisionchecker) ) {
             bCollision = true;
             if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
@@ -143,62 +136,37 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase
         if( !!pusereport && pusereport->minDistance < report->minDistance ) {
             *report = *pusereport;
         }
+    } // end for indexGrabbed1
 
-        if( numGrabbed > 1 ) {
-            // Since collision checking is commutative (i.e. CheckCollision(link1, link2) == CheckCollision(link2, link1)), checking it once per pair is sufficient.
-            for( size_t indexGrabbed2 = indexGrabbed1 + 1; indexGrabbed2 < numGrabbed; ++indexGrabbed2 ) {
-                const KinBodyPtr& pGrabbedBody2 = vLockedGrabbedBodiesCache[indexGrabbed2];
-                vGrabbedBodies[indexGrabbed2]->ComputeListNonCollidingLinks();
-
-                const std::list<KinBody::LinkConstPtr>& nonCollidingLinks2 = vGrabbedBodies[indexGrabbed2]->_listNonCollidingLinksWhenGrabbed;
-
-                for( const KinBody::LinkPtr& pGrabbedBody2Link : grabbedBody2.GetLinks() ) {
-                    if( !pGrabbedBody2Link->IsEnabled() ) {
-                        continue;
-                    }
-                    // See if these two links were initially colliding. If they are, then no further
-                    // check is need (as this link pair should be skipped).
-                    // if the link is in nonCollidingLinks1, collision has already been checked.
-                    if( std::find(nonCollidingLinks1.begin(), nonCollidingLinks1.end(), pGrabbedBody2Link) != nonCollidingLinks1.end() ) {
-                        continue;
-                    }
-                    for( const KinBody::LinkPtr& pGrabbedBody1Link : grabbedBody1.GetLinks() ) {
-                        if( !pGrabbedBody1Link->IsEnabled() ) {
-                            continue;
-                        }
-                        if( std::find(nonCollidingLinks2.begin(), nonCollidingLinks2.end(), pGrabbedBody1Link) != nonCollidingLinks2.end() ) {
-                            if( collisionchecker->CheckCollision(KinBody::LinkConstPtr(pGrabbedBody1Link),
-                                                                 KinBody::LinkConstPtr(pGrabbedBody2Link),
-                                                                 pusereport) ) {
-                                bCollision = true;
-                                if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
-                                    break;
-                                }
-                            } // end if CheckCollision
-                            if( !!pusereport && pusereport->minDistance < report->minDistance ) {
-                                *report = *pusereport;
-                            }
-                        } // end if pGrabbedBody1Link in nonCollidingLinks2
-                    } // end for pGrabbedBody1Link in nonCollidingLinks2
-                    if( bCollision ) {
-                        if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
-                            break;
-                        }
-                    }
-                } // end for pGrabbedBody2Link
-                if( bCollision ) {
-                    if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
-                        break;
-                    }
+    // Check grabbed vs grabbed collision
+    // TODO
+    FOREACHC(itInfo, _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed) {
+        const ListNonCollidingLinkPairs& pairs = (*itInfo).second;
+        if( pairs.empty() ) {
+            continue;
+        }
+        const KinBodyPtr pLink1 = pairs.front().first->GetParent(true);
+        const KinBodyPtr pLink2 = pairs.front().second->GetParent(true);
+        if( !pLink1 || !pLink2 ) {
+            continue;
+        }
+        FOREACHC(itLinks, pairs) {
+            if( collisionchecker->CheckCollision((*itLinks).first, (*itLinks).second, pusereport) ) {
+                bCollision = true;
+                if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
+                    break;
                 }
-            } // end for indexGrabbed2
-        } // end if numGrabbed > 1
+            }
+            if( !!pusereport && pusereport->minDistance < report->minDistance ) {
+                *report = *pusereport;
+            }
+        }
         if( bCollision ) {
             if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
                 break;
             }
         }
-    } // end for indexGrabbed1
+    }
 
     if( bCollision && !!report ) {
         if( report != pusereport ) {