WIP: Add print message for non adjacent links

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}")

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
 ===============
 

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<std::vector<int>, 4>& vNonAdjacentLinks, const size_t nonAdjacentMask, const std::string& envNameId, const std::string& bodyName);
+
     std::string _name; ///< name of body
 
     std::vector<JointPtr> _vecjoints; ///< \see GetJoints

src/libopenrave/kinbody.cpp

@@ -65,6 +65,21 @@ inline void _ResizeVectorFor2DTable(std::vector<int8_t>& vec, size_t vectorSize)
     }
 }
 
+static void _PrintDOFValuesForInitialLinkTransformations(const KinBody& body, const std::vector<dReal>& 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<dReal> vdoflastsetvalues;
     GetLinkTransformations(_vInitialLinkTransformations, vdoflastsetvalues);
+    _PrintDOFValuesForInitialLinkTransformations(*this, vdoflastsetvalues, __FUNCTION__);
 }
 
 void KinBody::_ResetInternalCollisionCache()
@@ -5598,6 +5615,20 @@ bool CompareNonAdjacentFarthest(int pair0, int pair1)
     return dist0 > dist1;
 }
 
+void KinBody::_PrintNonAdjacentLinks(const boost::array<std::vector<int>, 4>& vNonAdjacentLinks, const size_t nonAdjacentMask, const std::string& envNameId, const std::string& bodyName)
+{
+    std::stringstream ssLinks;
+    const std::vector<int>& 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='%s', body '%s' computes the cache for GetNonAdjacentLinks(%d). linkPairs=[%s]", envNameId%bodyName%nonAdjacentMask%ssLinks.str());
+}
+
 const std::vector<int>& KinBody::GetNonAdjacentLinks(int adjacentoptions) const
 {
     class TransformsSaver
@@ -5646,6 +5677,7 @@ const std::vector<int>& 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 +5692,7 @@ const std::vector<int>& 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);

src/libopenrave/robot.cpp

@@ -2091,6 +2091,7 @@ const std::vector<int>& 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<int>& 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<int>& 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;
     }