From c6c17cbc4d57834a298e25fe666f30a36ba528e6 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Fri, 1 Nov 2024 13:34:03 +0900 Subject: [PATCH 1/5] Print _vNonAdjacentLinks when it's lazily computed, for better debugging. --- include/openrave/kinbody.h | 6 ++++++ src/libopenrave/kinbody.cpp | 16 ++++++++++++++++ src/libopenrave/robot.cpp | 3 +++ 3 files changed, 25 insertions(+) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index 3a4c0da518..311f68a25d 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -3723,6 +3723,12 @@ class OPENRAVE_API KinBody : public InterfaceBase /// Ensures that _vAllPairsShortestPaths is initialized if it is not already void _EnsureAllPairsShortestPaths() const; + /// \brief print the computed _vNonAdjacentLinks contents. + /// \param[in] vNonAdjacentLinks : from KinBody::_vNonAdjacentLinks + /// \param[in] nonAdjacentMask : index of element to print, which is mask. _vNonAdjacentLinks[nonAdjacentMask] is printed. + /// \param[in] envNameId, bodyName : for print message. + static void _PrintNonAdjacentLinks(const boost::array, 4>& vNonAdjacentLinks, const size_t nonAdjacentMask, const std::string& envNameId, const std::string& bodyName); + std::string _name; ///< name of body std::vector _vecjoints; ///< \see GetJoints diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index e081798073..800e36d7be 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -5598,6 +5598,20 @@ bool CompareNonAdjacentFarthest(int pair0, int pair1) return dist0 > dist1; } +void KinBody::_PrintNonAdjacentLinks(const boost::array, 4>& vNonAdjacentLinks, const size_t nonAdjacentMask, const std::string& envNameId, const std::string& bodyName) +{ + std::stringstream ssLinks; + const std::vector& vSelectedNonAdjacentLinks = vNonAdjacentLinks[nonAdjacentMask]; + for(size_t iLinks = 0; iLinks < vSelectedNonAdjacentLinks.size(); ++iLinks) { + const int value = vSelectedNonAdjacentLinks[iLinks]; + if( iLinks > 0 ) { + ssLinks << ","; + } + ssLinks << "(" << (value & 0xffff) << "," << (value>>16) << ")"; + } + RAVELOG_INFO_FORMAT("env=%d, body %s computes the cache for GetNonAdjacentLinks(%d). links=[%s]", envNameId%bodyName%nonAdjacentMask%ssLinks.str()); +} + const std::vector& KinBody::GetNonAdjacentLinks(int adjacentoptions) const { class TransformsSaver @@ -5646,6 +5660,7 @@ const std::vector& KinBody::GetNonAdjacentLinks(int adjacentoptions) const std::sort(_vNonAdjacentLinks[0].begin(), _vNonAdjacentLinks[0].end(), CompareNonAdjacentFarthest); _nUpdateStampId++; // because transforms were modified _nNonAdjacentLinkCache = 0; + KinBody::_PrintNonAdjacentLinks(_vNonAdjacentLinks, 0, GetEnv()->GetNameId(), GetName()); } if( (_nNonAdjacentLinkCache&adjacentoptions) != adjacentoptions ) { int requestedoptions = (~_nNonAdjacentLinkCache)&adjacentoptions; @@ -5660,6 +5675,7 @@ const std::vector& KinBody::GetNonAdjacentLinks(int adjacentoptions) const } _nNonAdjacentLinkCache |= AO_Enabled; std::sort(_vNonAdjacentLinks[AO_Enabled].begin(), _vNonAdjacentLinks[AO_Enabled].end(), CompareNonAdjacentFarthest); + KinBody::_PrintNonAdjacentLinks(_vNonAdjacentLinks, AO_Enabled, GetEnv()->GetNameId(), GetName()); } else { throw OPENRAVE_EXCEPTION_FORMAT(_("no support for adjacentoptions %d"), adjacentoptions,ORE_InvalidArguments); diff --git a/src/libopenrave/robot.cpp b/src/libopenrave/robot.cpp index d392be3618..29959806df 100644 --- a/src/libopenrave/robot.cpp +++ b/src/libopenrave/robot.cpp @@ -2091,6 +2091,7 @@ const std::vector& RobotBase::GetNonAdjacentLinks(int adjacentoptions) cons } } std::sort(_vNonAdjacentLinks[AO_Enabled].begin(), _vNonAdjacentLinks[AO_Enabled].end(), CompareNonAdjacentFarthest); + KinBody::_PrintNonAdjacentLinks(_vNonAdjacentLinks, AO_Enabled, GetEnv()->GetNameId(), GetName()); } if( compute.at(AO_ActiveDOFs) ) { _vNonAdjacentLinks.at(AO_ActiveDOFs).resize(0); @@ -2103,6 +2104,7 @@ const std::vector& RobotBase::GetNonAdjacentLinks(int adjacentoptions) cons } } std::sort(_vNonAdjacentLinks[AO_ActiveDOFs].begin(), _vNonAdjacentLinks[AO_ActiveDOFs].end(), CompareNonAdjacentFarthest); + KinBody::_PrintNonAdjacentLinks(_vNonAdjacentLinks, AO_ActiveDOFs, GetEnv()->GetNameId(), GetName()); } if( compute.at(AO_Enabled|AO_ActiveDOFs) ) { _vNonAdjacentLinks.at(AO_Enabled|AO_ActiveDOFs).resize(0); @@ -2113,6 +2115,7 @@ const std::vector& RobotBase::GetNonAdjacentLinks(int adjacentoptions) cons } } std::sort(_vNonAdjacentLinks[AO_Enabled|AO_ActiveDOFs].begin(), _vNonAdjacentLinks[AO_Enabled|AO_ActiveDOFs].end(), CompareNonAdjacentFarthest); + KinBody::_PrintNonAdjacentLinks(_vNonAdjacentLinks, AO_Enabled|AO_ActiveDOFs, GetEnv()->GetNameId(), GetName()); } _nNonAdjacentLinkCache |= requestedoptions; } From 26d6fee6090e649c76edcfb68bdd10a157a70b7c Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Fri, 1 Nov 2024 14:38:44 +0900 Subject: [PATCH 2/5] Improve print message. add dof value print message when _vInitialLinkTransformations is initialized. --- src/libopenrave/kinbody.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 800e36d7be..5eff9a1b47 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -65,6 +65,21 @@ inline void _ResizeVectorFor2DTable(std::vector& vec, size_t vectorSize) } } +static void _PrintDOFValuesForInitialLinkTransformations(const KinBody& body, const std::vector& vdoflastsetvalues, const char* context) +{ + if( body.GetDOF() == 0 ) { + return; + } + std::stringstream ssJoints; + for(size_t iDOF = 0; iDOF < vdoflastsetvalues.size(); ++iDOF) { + if( iDOF > 0 ) { + ssJoints << ","; + } + ssJoints << vdoflastsetvalues[iDOF]; + } + RAVELOG_INFO_FORMAT("env='%s', body '%s' _vInitialLinkTransformations is updated in %s by dofValues=[%s]", body.GetEnv()->GetNameId()%body.GetName()%context%ssJoints.str()); +} + class ChangeCallbackData : public UserData { public: @@ -5111,6 +5126,7 @@ void KinBody::_ComputeInternalInformation() RAVELOG_VERBOSE(str(boost::format("dof %d has different values after SetDOFValues %d!=%d, this could be due to mimic joint equations kicking into effect.")%i%vprevdoflastsetvalues.at(i)%vnewdoflastsetvalues.at(i))); } } + _PrintDOFValuesForInitialLinkTransformations(*this, vnewdoflastsetvalues, __FUNCTION__); _vInitialLinkTransformations = vnewtrans; } @@ -5568,6 +5584,7 @@ void KinBody::SetNonCollidingConfiguration() _ResetInternalCollisionCache(); vector vdoflastsetvalues; GetLinkTransformations(_vInitialLinkTransformations, vdoflastsetvalues); + _PrintDOFValuesForInitialLinkTransformations(*this, vdoflastsetvalues, __FUNCTION__); } void KinBody::_ResetInternalCollisionCache() @@ -5609,7 +5626,7 @@ void KinBody::_PrintNonAdjacentLinks(const boost::array, 4>& vN } ssLinks << "(" << (value & 0xffff) << "," << (value>>16) << ")"; } - RAVELOG_INFO_FORMAT("env=%d, body %s computes the cache for GetNonAdjacentLinks(%d). links=[%s]", envNameId%bodyName%nonAdjacentMask%ssLinks.str()); + RAVELOG_INFO_FORMAT("env='%s', body '%s' computes the cache for GetNonAdjacentLinks(%d). linkPairs=[%s]", envNameId%bodyName%nonAdjacentMask%ssLinks.str()); } const std::vector& KinBody::GetNonAdjacentLinks(int adjacentoptions) const From 6903dbb6492c1e1c73f137e81d96c5a4cecd0102 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Fri, 1 Nov 2024 16:17:21 +0900 Subject: [PATCH 3/5] Bump minor version. Add print message related to non adjacent links to track the self collision issue. --- CMakeLists.txt | 4 ++-- docs/source/changelog.rst | 5 +++++ 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 80f13a4163..172a257f10 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 156) -set (OPENRAVE_VERSION_PATCH 1) +set (OPENRAVE_VERSION_MINOR 157) +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 bc4d3258c3..d7c94a0419 100644 --- a/docs/source/changelog.rst +++ b/docs/source/changelog.rst @@ -3,6 +3,11 @@ ChangeLog ######### +Version 0.157.0 +=============== + +- Add print message related to non adjacent links to track the self collision issue. + Version 0.156.1 =============== From 59c0c064d93bc6cbdb23ec59537bada8a25e906f Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Fri, 1 Nov 2024 21:54:28 +0900 Subject: [PATCH 4/5] do not use single quote for env printing --- src/libopenrave/kinbody.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 5eff9a1b47..6adf9bd385 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -77,7 +77,7 @@ static void _PrintDOFValuesForInitialLinkTransformations(const KinBody& body, co } ssJoints << vdoflastsetvalues[iDOF]; } - RAVELOG_INFO_FORMAT("env='%s', body '%s' _vInitialLinkTransformations is updated in %s by dofValues=[%s]", body.GetEnv()->GetNameId()%body.GetName()%context%ssJoints.str()); + RAVELOG_INFO_FORMAT("env=%s, body '%s' _vInitialLinkTransformations is updated in %s by dofValues=[%s]", body.GetEnv()->GetNameId()%body.GetName()%context%ssJoints.str()); } class ChangeCallbackData : public UserData @@ -5626,7 +5626,7 @@ void KinBody::_PrintNonAdjacentLinks(const boost::array, 4>& vN } ssLinks << "(" << (value & 0xffff) << "," << (value>>16) << ")"; } - RAVELOG_INFO_FORMAT("env='%s', body '%s' computes the cache for GetNonAdjacentLinks(%d). linkPairs=[%s]", envNameId%bodyName%nonAdjacentMask%ssLinks.str()); + RAVELOG_INFO_FORMAT("env=%s, body '%s' computes the cache for GetNonAdjacentLinks(%d). linkPairs=[%s]", envNameId%bodyName%nonAdjacentMask%ssLinks.str()); } const std::vector& KinBody::GetNonAdjacentLinks(int adjacentoptions) const From 231ea85a1adc0696bdec9afcdce3f4d17c5b2a97 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Mon, 11 Nov 2024 13:32:25 +0900 Subject: [PATCH 5/5] Do not print adjacent links on changing of AO_ActiveDOFs and/or AO_Enabled, since it's relatively noisy. --- src/libopenrave/kinbody.cpp | 1 - src/libopenrave/robot.cpp | 3 --- 2 files changed, 4 deletions(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 9158971bdf..45f0159e53 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -5709,7 +5709,6 @@ const std::vector& KinBody::GetNonAdjacentLinks(int adjacentoptions) const } _nNonAdjacentLinkCache |= AO_Enabled; std::sort(_vNonAdjacentLinks[AO_Enabled].begin(), _vNonAdjacentLinks[AO_Enabled].end(), CompareNonAdjacentFarthest); - KinBody::_PrintNonAdjacentLinks(_vNonAdjacentLinks, AO_Enabled, GetEnv()->GetNameId(), GetName()); } else { throw OPENRAVE_EXCEPTION_FORMAT(_("no support for adjacentoptions %d"), adjacentoptions,ORE_InvalidArguments); diff --git a/src/libopenrave/robot.cpp b/src/libopenrave/robot.cpp index e2783d4053..6badcae4a6 100644 --- a/src/libopenrave/robot.cpp +++ b/src/libopenrave/robot.cpp @@ -2093,7 +2093,6 @@ const std::vector& RobotBase::GetNonAdjacentLinks(int adjacentoptions) cons } } std::sort(_vNonAdjacentLinks[AO_Enabled].begin(), _vNonAdjacentLinks[AO_Enabled].end(), CompareNonAdjacentFarthest); - KinBody::_PrintNonAdjacentLinks(_vNonAdjacentLinks, AO_Enabled, GetEnv()->GetNameId(), GetName()); } if( compute.at(AO_ActiveDOFs) ) { _vNonAdjacentLinks.at(AO_ActiveDOFs).resize(0); @@ -2106,7 +2105,6 @@ const std::vector& RobotBase::GetNonAdjacentLinks(int adjacentoptions) cons } } std::sort(_vNonAdjacentLinks[AO_ActiveDOFs].begin(), _vNonAdjacentLinks[AO_ActiveDOFs].end(), CompareNonAdjacentFarthest); - KinBody::_PrintNonAdjacentLinks(_vNonAdjacentLinks, AO_ActiveDOFs, GetEnv()->GetNameId(), GetName()); } if( compute.at(AO_Enabled|AO_ActiveDOFs) ) { _vNonAdjacentLinks.at(AO_Enabled|AO_ActiveDOFs).resize(0); @@ -2117,7 +2115,6 @@ const std::vector& RobotBase::GetNonAdjacentLinks(int adjacentoptions) cons } } std::sort(_vNonAdjacentLinks[AO_Enabled|AO_ActiveDOFs].begin(), _vNonAdjacentLinks[AO_Enabled|AO_ActiveDOFs].end(), CompareNonAdjacentFarthest); - KinBody::_PrintNonAdjacentLinks(_vNonAdjacentLinks, AO_Enabled|AO_ActiveDOFs, GetEnv()->GetNameId(), GetName()); } _nNonAdjacentLinkCache |= requestedoptions; }