diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4d4d385a76..b363024019 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 157)
-set (OPENRAVE_VERSION_PATCH 1)
+set (OPENRAVE_VERSION_MINOR 158)
+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 f1ad18eeda..911c6c76b0 100644
--- a/docs/source/changelog.rst
+++ b/docs/source/changelog.rst
@@ -3,6 +3,11 @@
 ChangeLog
 #########
 
+Version 0.158.0
+===============
+
+- Add print message related to non adjacent links to track the self collision issue.
+
 Version 0.157.1
 ===============
 
diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h
index 6daa8158bd..e0810c7eaf 100644
--- a/include/openrave/kinbody.h
+++ b/include/openrave/kinbody.h
@@ -3729,6 +3729,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
diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp
index c4fbb4c0df..45f0159e53 100644
--- a/src/libopenrave/kinbody.cpp
+++ b/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:
@@ -5113,6 +5128,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;
     }
 
@@ -5585,6 +5601,7 @@ void KinBody::SetNonCollidingConfiguration()
     _ResetInternalCollisionCache();
     vector<dReal> vdoflastsetvalues;
     GetLinkTransformations(_vInitialLinkTransformations, vdoflastsetvalues);
+    _PrintDOFValuesForInitialLinkTransformations(*this, vdoflastsetvalues, __FUNCTION__);
 }
 
 void KinBody::_ResetInternalCollisionCache()
@@ -5615,6 +5632,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
@@ -5663,6 +5694,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;