Optimize link checkCollision not to synchronize unnecessary links

CMakeLists.txt

@@ -5,7 +5,7 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE )
 # Define here the needed parameters
 set (OPENRAVE_VERSION_MAJOR 0)
 set (OPENRAVE_VERSION_MINOR 167)
-set (OPENRAVE_VERSION_PATCH 2)
+set (OPENRAVE_VERSION_PATCH 3)
 set (OPENRAVE_VERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR}.${OPENRAVE_VERSION_PATCH})
 set (OPENRAVE_SOVERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR})
 message(STATUS "Compiling OpenRAVE Version ${OPENRAVE_VERSION}, soversion=${OPENRAVE_SOVERSION}")

docs/source/changelog.rst

@@ -3,6 +3,11 @@
 ChangeLog
 #########
 
+Version 0.167.3
+===============
+
+- Optimize link checkCollision not to synchronize unnecessary links.
+
 Version 0.167.2
 ===============
 

plugins/fclrave/fclcollision.cpp

@@ -341,19 +341,8 @@ bool FCLCollisionChecker::CheckCollision(LinkConstPtr plink1, LinkConstPtr plink
         return false;
     }
 
-    KinBodyPtr plink1parent = plink1->GetParent(true);
-    if( !plink1parent ) {
-        throw OPENRAVE_EXCEPTION_FORMAT("Failed to get link %s parent", plink1parent->GetName(), OpenRAVE::ORE_InvalidArguments);
-    }
-    KinBodyPtr plink2parent = plink2->GetParent(true);
-    if( !plink2parent ) {
-        throw OPENRAVE_EXCEPTION_FORMAT("Failed to get link %s parent", plink2parent->GetName(), OpenRAVE::ORE_InvalidArguments);
-    }
-
-    _fclspace->SynchronizeWithAttached(*plink1parent);
-    if( plink1parent != plink2parent ) {
-        _fclspace->SynchronizeWithAttached(*plink2parent);
-    }
+    _fclspace->SynchronizeLink(*plink1);
+    _fclspace->SynchronizeLink(*plink2);
 
     CollisionObjectPtr pcollLink1 = _fclspace->GetLinkBV(*plink1), pcollLink2 = _fclspace->GetLinkBV(*plink2);
 
@@ -400,7 +389,7 @@ bool FCLCollisionChecker::CheckCollision(LinkConstPtr plink, KinBodyConstPtr pbo
         return false;
     }
 
-    _fclspace->SynchronizeWithAttached(*plink->GetParent());
+    _fclspace->SynchronizeLink(*plink);
     _fclspace->SynchronizeWithAttached(*pbody);
     CollisionObjectPtr pcollLink = _fclspace->GetLinkBV(*plink);
 
@@ -437,7 +426,8 @@ bool FCLCollisionChecker::CheckCollision(LinkConstPtr plink, std::vector<KinBody
         return false;
     }
 
-    _fclspace->Synchronize();
+    _fclspace->SynchronizeLink(*plink);
+    _fclspace->SynchronizeExcluded(plink->GetParent());
     CollisionObjectPtr pcollLink = _fclspace->GetLinkBV(*plink);
 
     if( !pcollLink ) {

plugins/fclrave/fclcollision.h

@@ -174,6 +174,7 @@ class FCLCollisionChecker : public OpenRAVE::CollisionCheckerBase
 
     bool CheckCollision(LinkConstPtr plink, KinBodyConstPtr pbody,CollisionReportPtr report = CollisionReportPtr()) override;
 
+    /// \brief check link/env collision. collision between link and its body is not checked
     bool CheckCollision(LinkConstPtr plink, std::vector<KinBodyConstPtr> const &vbodyexcluded, std::vector<LinkConstPtr> const &vlinkexcluded, CollisionReportPtr report = CollisionReportPtr()) override;
 
     bool CheckCollision(KinBodyConstPtr pbody, std::vector<KinBodyConstPtr> const &vbodyexcluded, std::vector<LinkConstPtr> const &vlinkexcluded, CollisionReportPtr report = CollisionReportPtr()) override;

plugins/fclrave/fclspace.cpp

@@ -165,7 +165,7 @@ void FCLSpace::ReloadKinBodyLinks(KinBodyConstPtr pbody, FCLKinBodyInfoPtr pinfo
             linkinfo->linkBV = std::make_pair(trans, pfclcollBV);
         }
 
-        //link->nLastStamp = pinfo->nLastStamp;
+        linkinfo->nLastStamp = pinfo->nLastStamp;
         linkinfo->bodylinkname = pbody->GetName() + "/" + plink->GetName();
         pinfo->vlinks.push_back(linkinfo);
 #ifdef FCLRAVE_COLLISION_OBJECTS_STATISTICS
@@ -392,6 +392,30 @@ void FCLSpace::SynchronizeWithAttached(const KinBody &body)
     }
 }
 
+void FCLSpace::SynchronizeExcluded(const KinBodyConstPtr& pbodyexcluded)
+{
+    // We synchronize only the initialized bodies, which differs from oderave
+    for (const KinBodyConstPtr& pbody : _vecInitializedBodies) {
+        if (!pbody) {
+            continue;
+        }
+        if (pbody == pbodyexcluded) {
+            continue;
+        }
+        Synchronize(*pbody);
+    }
+}
+
+void FCLSpace::SynchronizeLink(const KinBody::Link &link)
+{
+    const KinBody &body = *link.GetParent();
+    FCLKinBodyInfoPtr& pinfo = GetInfo(body);
+    if( !pinfo ) {
+        return;
+    }
+    _SynchronizeLink(*pinfo, body, link.GetIndex());
+}
+
 FCLSpace::FCLKinBodyInfoPtr& FCLSpace::GetInfo(const KinBody &body)
 {
     int envId = body.GetEnvironmentBodyIndex();
@@ -600,6 +624,40 @@ CollisionGeometryPtr FCLSpace::_CreateFCLGeomFromGeometryInfo(const KinBody::Geo
     }
 }
 
+void FCLSpace::_SynchronizeLink(FCLKinBodyInfo& info, const KinBody& body, int linkIndex)
+{
+    FCLSpace::FCLKinBodyInfo::LinkInfo& linkInfo = *info.vlinks[linkIndex];
+    if( linkInfo.nLastStamp != body.GetUpdateStamp() ) {
+        linkInfo.nLastStamp = body.GetUpdateStamp();
+        CollisionObjectPtr& pcoll = linkInfo.linkBV.second; // avoid copying shared pointer for performance
+        if( !pcoll ) {
+            return;
+        }
+        const Transform& linkTransform = body.GetLinks()[linkIndex]->GetTransform();
+        Transform pose = linkTransform;
+        pose.trans += pose.rotate(linkInfo.linkBV.first);
+        const fcl::Vec3f newPosition = ConvertVectorToFCL(pose.trans);
+        const fcl::Quaternion3f newOrientation = ConvertQuaternionToFCL(pose.rot);
+
+        pcoll->setTranslation(newPosition);
+        pcoll->setQuatRotation(newOrientation);
+        // Do not forget to recompute the AABB otherwise getAABB won't give an up to date AABB
+        pcoll->computeAABB();
+
+        for (const TransformCollisionPair& pgeom : linkInfo.vgeoms) {
+            fcl::CollisionObject& coll = *pgeom.second;
+            const Transform pose1 = linkTransform * pgeom.first;
+            const fcl::Vec3f newPosition1 = ConvertVectorToFCL(pose1.trans);
+            const fcl::Quaternion3f newOrientation1 = ConvertQuaternionToFCL(pose1.rot);
+
+            coll.setTranslation(newPosition1);
+            coll.setQuatRotation(newOrientation1);
+            // Do not forget to recompute the AABB otherwise getAABB won't give an up to date AABB
+            coll.computeAABB();
+        }
+    }
+}
+
 void FCLSpace::_Synchronize(FCLKinBodyInfo& info, const KinBody& body)
 {
     //KinBodyPtr pbody = info.GetBody();
@@ -610,34 +668,7 @@ void FCLSpace::_Synchronize(FCLKinBodyInfo& info, const KinBody& body)
         }
 
         for(size_t i = 0; i < body.GetLinks().size(); ++i) {
-            FCLSpace::FCLKinBodyInfo::LinkInfo& linkInfo = *info.vlinks[i];
-            CollisionObjectPtr& pcoll = linkInfo.linkBV.second; // avoid copying shared pointer for performance
-            if( !pcoll ) {
-                continue;
-            }
-            const Transform& linkTransform = body.GetLinks()[i]->GetTransform();
-            Transform pose = linkTransform;
-            pose.trans += pose.rotate(linkInfo.linkBV.first);
-            const fcl::Vec3f newPosition = ConvertVectorToFCL(pose.trans);
-            const fcl::Quaternion3f newOrientation = ConvertQuaternionToFCL(pose.rot);
-
-            pcoll->setTranslation(newPosition);
-            pcoll->setQuatRotation(newOrientation);
-            // Do not forget to recompute the AABB otherwise getAABB won't give an up to date AABB
-            pcoll->computeAABB();
-
-            //info.vlinks[i]->nLastStamp = info.nLastStamp;
-            for (const TransformCollisionPair& pgeom : linkInfo.vgeoms) {
-                fcl::CollisionObject& coll = *pgeom.second;
-                const Transform pose1 = linkTransform * pgeom.first;
-                const fcl::Vec3f newPosition1 = ConvertVectorToFCL(pose1.trans);
-                const fcl::Quaternion3f newOrientation1 = ConvertQuaternionToFCL(pose1.rot);
-
-                coll.setTranslation(newPosition1);
-                coll.setQuatRotation(newOrientation1);
-                // Do not forget to recompute the AABB otherwise getAABB won't give an up to date AABB
-                coll.computeAABB();
-            }
+            _SynchronizeLink(info, body, i);
         }
 
         // Does this have any use ?

plugins/fclrave/fclspace.h

@@ -124,7 +124,7 @@ class FCLSpace : public boost::enable_shared_from_this<FCLSpace>
             KinBody::LinkWeakPtr _plink;
             vector< boost::shared_ptr<FCLGeometryInfo> > vgeominfos; ///< info for every geometry of the link
 
-            //int nLastStamp; ///< Tracks if the collision geometries are up to date wrt the body update stamp. This is for narrow phase collision
+            int nLastStamp = 0; ///< Tracks if the collision geometries are up to date wrt the body update stamp. This is for narrow phase collision. This should be the same as FCLKinBodyInfo.nLastStamp or newer
             TranslationCollisionPair linkBV; ///< pair of the translation and collision object corresponding to a bounding OBB for the link
             std::vector<TransformCollisionPair> vgeoms; ///< vector of transformations and collision object; one per geometries
             std::string bodylinkname; // for debugging purposes
@@ -206,6 +206,15 @@ class FCLSpace : public boost::enable_shared_from_this<FCLSpace>
 
     void SynchronizeWithAttached(const KinBody &body);
 
+    /// \brief Synchronize all bodies except the specified body.
+    ///
+    /// Useful to avoid redundant synchronization
+    /// while ensuring the rest of the bodies is up to date.
+    void SynchronizeExcluded(const KinBodyConstPtr& pbodyexcluded);
+
+    /// \brief Synchronize only the specified link's collision geometry.
+    void SynchronizeLink(const KinBody::Link &link);
+
     FCLKinBodyInfoPtr& GetInfo(const KinBody &body);
 
     const FCLKinBodyInfoPtr& GetInfo(const KinBody &body) const;
@@ -262,6 +271,16 @@ class FCLSpace : public boost::enable_shared_from_this<FCLSpace>
     // what about the tests on non-zero size (eg. box extents) ?
     CollisionGeometryPtr _CreateFCLGeomFromGeometryInfo(const KinBody::GeometryInfo &info);
 
+    /// \brief Synchronize one link.
+    ///
+    /// If the link’s lastStamp differs from the body’s updateStamp,
+    /// its collision geometry is synchronized
+    ///
+    /// \param info The FCLKinBodyInfo for the parent body.
+    /// \param body The KinBody of the parent body.
+    /// \param linkIndex The integer index of the link within `body`.
+    void _SynchronizeLink(FCLKinBodyInfo& info, const KinBody& body, int linkIndex);
+
     /// \brief pass in info.GetBody() as a reference to avoid dereferencing the weak pointer in FCLKinBodyInfo
     void _Synchronize(FCLKinBodyInfo& info, const KinBody& body);