From 5d44f3a8fa881a8759965283873a0904759af3cb Mon Sep 17 00:00:00 2001 From: "yuta.kojio" Date: Fri, 2 May 2025 17:37:27 +0900 Subject: [PATCH 1/7] optimize link checkCollision not to synchronize unnecessary links --- plugins/fclrave/fclcollision.cpp | 20 ++----- plugins/fclrave/fclcollision.h | 1 + plugins/fclrave/fclspace.cpp | 93 ++++++++++++++++++++++---------- plugins/fclrave/fclspace.h | 8 ++- 4 files changed, 77 insertions(+), 45 deletions(-) diff --git a/plugins/fclrave/fclcollision.cpp b/plugins/fclrave/fclcollision.cpp index 9d42876866..73b56dae04 100644 --- a/plugins/fclrave/fclcollision.cpp +++ b/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::vectorSynchronize(); + _fclspace->SynchronizeLink(*plink); + _fclspace->SynchronizeExcluded(plink->GetParent()); CollisionObjectPtr pcollLink = _fclspace->GetLinkBV(*plink); if( !pcollLink ) { diff --git a/plugins/fclrave/fclcollision.h b/plugins/fclrave/fclcollision.h index 4f47c430a5..9b22a96e44 100644 --- a/plugins/fclrave/fclcollision.h +++ b/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 const &vbodyexcluded, std::vector const &vlinkexcluded, CollisionReportPtr report = CollisionReportPtr()) override; bool CheckCollision(KinBodyConstPtr pbody, std::vector const &vbodyexcluded, std::vector const &vlinkexcluded, CollisionReportPtr report = CollisionReportPtr()) override; diff --git a/plugins/fclrave/fclspace.cpp b/plugins/fclrave/fclspace.cpp index d5df89019e..cf81d7d710 100644 --- a/plugins/fclrave/fclspace.cpp +++ b/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,34 @@ 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; + } + if( pinfo->nLastStamp != body.GetUpdateStamp() ) { + // do not update info->nLastStamp + // since other links are not updated + _SynchronizeLink(*pinfo, body, link.GetIndex()); + } +} + FCLSpace::FCLKinBodyInfoPtr& FCLSpace::GetInfo(const KinBody &body) { int envId = body.GetEnvironmentBodyIndex(); @@ -600,6 +628,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 +672,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 ? diff --git a/plugins/fclrave/fclspace.h b/plugins/fclrave/fclspace.h index 7f2adde27a..6e3d9631a1 100644 --- a/plugins/fclrave/fclspace.h +++ b/plugins/fclrave/fclspace.h @@ -124,7 +124,7 @@ class FCLSpace : public boost::enable_shared_from_this KinBody::LinkWeakPtr _plink; vector< boost::shared_ptr > 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 TranslationCollisionPair linkBV; ///< pair of the translation and collision object corresponding to a bounding OBB for the link std::vector vgeoms; ///< vector of transformations and collision object; one per geometries std::string bodylinkname; // for debugging purposes @@ -206,6 +206,10 @@ class FCLSpace : public boost::enable_shared_from_this void SynchronizeWithAttached(const KinBody &body); + void SynchronizeExcluded(const KinBodyConstPtr& pbodyexcluded); + + void SynchronizeLink(const KinBody::Link &link); + FCLKinBodyInfoPtr& GetInfo(const KinBody &body); const FCLKinBodyInfoPtr& GetInfo(const KinBody &body) const; @@ -262,6 +266,8 @@ class FCLSpace : public boost::enable_shared_from_this // what about the tests on non-zero size (eg. box extents) ? CollisionGeometryPtr _CreateFCLGeomFromGeometryInfo(const KinBody::GeometryInfo &info); + 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); From d0c6abebb157060c05209babe3bfa7536aa38b6f Mon Sep 17 00:00:00 2001 From: "yuta.kojio" Date: Mon, 12 May 2025 11:36:28 +0900 Subject: [PATCH 2/7] update changelog and minor version --- CMakeLists.txt | 4 ++-- docs/source/changelog.rst | 5 +++++ 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d178978a97..775449971d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,8 +4,8 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE ) # Define here the needed parameters set (OPENRAVE_VERSION_MAJOR 0) -set (OPENRAVE_VERSION_MINOR 166) -set (OPENRAVE_VERSION_PATCH 1) +set (OPENRAVE_VERSION_MINOR 167) +set (OPENRAVE_VERSION_PATCH 0) 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}") diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst index 199226a2de..b55dda96b5 100644 --- a/docs/source/changelog.rst +++ b/docs/source/changelog.rst @@ -3,6 +3,11 @@ ChangeLog ######### +Version 0.167.0 +=============== + +- Optimize link checkCollision not to synchronize unnecessary links. + Version 0.166.1 =============== - Add missing enum to `PlannerStatusCode` python binding From d6bec875775b9a22f6e175fea27c06fdce8a0fdd Mon Sep 17 00:00:00 2001 From: "yuta.kojio" Date: Wed, 14 May 2025 18:00:55 +0900 Subject: [PATCH 3/7] no need to check pinfo->nLastStamp since linkInfo.nLastStamp is checked inside _SynchronizeLink --- plugins/fclrave/fclspace.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/plugins/fclrave/fclspace.cpp b/plugins/fclrave/fclspace.cpp index cf81d7d710..185b6a511e 100644 --- a/plugins/fclrave/fclspace.cpp +++ b/plugins/fclrave/fclspace.cpp @@ -413,11 +413,7 @@ void FCLSpace::SynchronizeLink(const KinBody::Link &link) if( !pinfo ) { return; } - if( pinfo->nLastStamp != body.GetUpdateStamp() ) { - // do not update info->nLastStamp - // since other links are not updated - _SynchronizeLink(*pinfo, body, link.GetIndex()); - } + _SynchronizeLink(*pinfo, body, link.GetIndex()); } FCLSpace::FCLKinBodyInfoPtr& FCLSpace::GetInfo(const KinBody &body) From 2f120edbaa9b3dae302969ff29a2a83549b2d00c Mon Sep 17 00:00:00 2001 From: "yuta.kojio" Date: Thu, 15 May 2025 10:05:28 +0900 Subject: [PATCH 4/7] add docs --- plugins/fclrave/fclspace.h | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/plugins/fclrave/fclspace.h b/plugins/fclrave/fclspace.h index 6e3d9631a1..d7e487800e 100644 --- a/plugins/fclrave/fclspace.h +++ b/plugins/fclrave/fclspace.h @@ -206,8 +206,13 @@ class FCLSpace : public boost::enable_shared_from_this 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); @@ -266,6 +271,14 @@ class FCLSpace : public boost::enable_shared_from_this // 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 From 5f32fa11288a551a88c972c5e493cd69fcc4c310 Mon Sep 17 00:00:00 2001 From: "yuta.kojio" Date: Wed, 21 May 2025 14:26:44 +0900 Subject: [PATCH 5/7] update patch version instead of minor since there are no abi changes --- CMakeLists.txt | 4 ++-- docs/source/changelog.rst | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c3ca448c10..d9bfac4473 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,8 +4,8 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE ) # Define here the needed parameters set (OPENRAVE_VERSION_MAJOR 0) -set (OPENRAVE_VERSION_MINOR 168) -set (OPENRAVE_VERSION_PATCH 0) +set (OPENRAVE_VERSION_MINOR 167) +set (OPENRAVE_VERSION_PATCH 2) 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}") diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst index c0dd93626d..a893492c60 100644 --- a/docs/source/changelog.rst +++ b/docs/source/changelog.rst @@ -3,7 +3,7 @@ ChangeLog ######### -Version 0.168.0 +Version 0.167.2 =============== - Optimize link checkCollision not to synchronize unnecessary links. From 7224a5c86017d017704e42e0f96dd1854cf26473 Mon Sep 17 00:00:00 2001 From: "yuta.kojio" Date: Thu, 22 May 2025 11:08:29 +0900 Subject: [PATCH 6/7] bump patch version --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d9bfac4473..bc929e8c12 100644 --- a/CMakeLists.txt +++ b/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}") From f6ea72b9084dd197e0d4256d705dcda84b6d3ec7 Mon Sep 17 00:00:00 2001 From: "yuta.kojio" Date: Fri, 23 May 2025 10:58:53 +0900 Subject: [PATCH 7/7] improve comment --- plugins/fclrave/fclspace.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plugins/fclrave/fclspace.h b/plugins/fclrave/fclspace.h index d7e487800e..760f595e6d 100644 --- a/plugins/fclrave/fclspace.h +++ b/plugins/fclrave/fclspace.h @@ -124,7 +124,7 @@ class FCLSpace : public boost::enable_shared_from_this KinBody::LinkWeakPtr _plink; vector< boost::shared_ptr > vgeominfos; ///< info for every geometry of the link - int nLastStamp = 0; ///< 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 vgeoms; ///< vector of transformations and collision object; one per geometries std::string bodylinkname; // for debugging purposes