[Minor] Make debug and error messages more readable.

include/hpp/core/collision-validation-report.hh

@@ -19,6 +19,8 @@
 #ifndef HPP_CORE_COLLISION_VALIDATION_REPORT_HH
 # define HPP_CORE_COLLISION_VALIDATION_REPORT_HH
 
+# include <hpp/util/indent.hh>
+
 # include <hpp/pinocchio/collision-object.hh>
 # include <hpp/core/validation-report.hh>
 # include <hpp/fcl/collision_data.h>
@@ -56,11 +58,11 @@ namespace hpp {
       std::vector<CollisionValidationReportPtr_t> collisionReports;
       virtual std::ostream& print (std::ostream& os) const
       {
-        os <<" Number of collisions : "<<collisionReports.size()<<".";
+        os <<" Number of collisions : "<<collisionReports.size()<<"."<<incendl;
         for(std::vector<CollisionValidationReportPtr_t>::const_iterator it = collisionReports.begin() ; it != collisionReports.end() ; ++it){
-          (*it)->print(os);
+          os << **it << iendl;
         }
-        return os;
+        return os << decindent;
       }
     }; // class AllCollisionValidationReport
     /// \}

include/hpp/core/relative-motion.hh

@@ -31,12 +31,12 @@ namespace hpp {
       enum RelativeMotionType {
         /// The relative motion is fully constrained and the constraint cannot be
         /// parameterized (has constant right-hand side)
-        Constrained,
+        Constrained = 0,
         /// The relative motion is fully constrained but the constraint can be
         /// parameterized (has non-constant right-hand side)
-        Parameterized,
+        Parameterized = 1,
         /// The relative motion is not constrained
-        Unconstrained
+        Unconstrained = 2
       };
 
       typedef Eigen::Matrix<RelativeMotionType, Eigen::Dynamic, Eigen::Dynamic> matrix_type;

src/collision-validation.cc

@@ -208,7 +208,7 @@ namespace hpp {
               if (fcl::collide (_colPair->first ->fcl (), _colPair->second->fcl (),
                     collisionRequest_, unused) != 0) {
                 hppDout(warning, "Disabling collision detection between two "
-                    "body in collision.");
+                    "bodies in collision.");
               }
               disabledPairs_.push_back (*_colPair);
               _colPair = collisionPairs_.erase (_colPair);

src/relative-motion.cc

@@ -42,6 +42,16 @@ namespace hpp {
         m(i0,i1) = m(i1,i0) = t;
       }
 
+      /// Considering the order Constrained(0) < Parameterized(1) < Unconstrained(2)
+      /// Change m(i0,i1) only if it decreases.
+      inline void symSetNoDowngrade (RelativeMotion::matrix_type& m, size_type i0, size_type i1, RelativeMotion::RelativeMotionType t)
+      {
+        assert (RelativeMotion::Constrained < RelativeMotion::Parameterized
+            && RelativeMotion::Parameterized < RelativeMotion::Unconstrained);
+        assert (m(i0,i1) == m(i1,i0));
+        if (t < m(i0,i1)) m(i0,i1) = m(i1,i0) = t;
+      }
+
       /// Check that a differentiable function defines a constraint of
       /// relative pose between two joints
       template <typename T > struct check {
@@ -169,9 +179,12 @@ namespace hpp {
         const size_type& i1, const size_type& i2,
         const RelativeMotion::RelativeMotionType& type)
     {
+      assert (Constrained < Parameterized && Parameterized < Unconstrained);
       bool param = (type == Parameterized);
       RelativeMotionType t = Unconstrained;
-      if (type == Unconstrained) return;
+      // Constrained(0) < Parameterized(1) < Unconstrained(2)
+      // If the current value is more constraining, then do not change it.
+      if (matrix(i1,i2) <= type) return;
       symSet(matrix, i1, i2, type);
 
       // i1 to i3
@@ -180,7 +193,7 @@ namespace hpp {
         if (i3 == i2) continue;
         if (matrix(i2,i3) != Unconstrained) {
           t = (!param && matrix(i2,i3) == Constrained) ? Constrained : Parameterized;
-          symSet(matrix, i1, i3, t);
+          symSetNoDowngrade(matrix, i1, i3, t);
         }
       }
       for (size_type i0 = 0; i0 < matrix.rows(); ++i0) {
@@ -189,7 +202,7 @@ namespace hpp {
         // i0 to i2
         if (matrix(i0,i1) != Unconstrained) {
           t = (!param && matrix(i0,i1) == Constrained) ? Constrained : Parameterized;
-          symSet (matrix, i0, i2 ,t);
+          symSetNoDowngrade (matrix, i0, i2 ,t);
         }
 
         // from i0 to i3
@@ -199,9 +212,10 @@ namespace hpp {
           if (i3 == i1) continue;
           if (i3 == i0) continue;
           if (matrix(i2,i3) == Unconstrained) continue;
+          // If motion is already constrained, continue.
           t = (!param && matrix(i0,i1) == Constrained && matrix(i2,i3) == Constrained)
             ? Constrained : Parameterized;
-          symSet (matrix, i0, i3, t);
+          symSetNoDowngrade (matrix, i0, i3, t);
         }
       }
     }