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}") diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst index be9c1da82a..ac432c21a8 100644 --- a/docs/source/changelog.rst +++ b/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 =============== 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..185b6a511e 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,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 ? diff --git a/plugins/fclrave/fclspace.h b/plugins/fclrave/fclspace.h index 7f2adde27a..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; ///< 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 @@ -206,6 +206,15 @@ 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); const FCLKinBodyInfoPtr& GetInfo(const KinBody &body) const; @@ -262,6 +271,16 @@ 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 void _Synchronize(FCLKinBodyInfo& info, const KinBody& body);