Add some assertions on configuration normalization.

src/diffusing-planner.cc

@@ -32,6 +32,7 @@
 #include <tuple>
 #include <iterator>
 
+#include <pinocchio/math/quaternion.hpp>
 #include <hpp/util/debug.hh>
 #include <hpp/util/timer.hh>
 #include <hpp/pinocchio/configuration.hh>
@@ -112,8 +113,11 @@ namespace hpp {
       if (constraints) {
 	ConfigProjectorPtr_t configProjector (constraints->configProjector ());
 	if (configProjector) {
+    assert (isNormalized(problem()->robot(), target, PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE));
+    assert (isNormalized(problem()->robot(), *(near->configuration()), PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE));
 	  configProjector->projectOnKernel (*(near->configuration ()), target,
 					    qProj_);
+    assert (isNormalized(problem()->robot(), qProj_, PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE));
 	} else {
 	  qProj_ = target;
 	}

src/straight-path.cc

@@ -111,6 +111,8 @@ namespace hpp {
       }
       value_type u = (param - paramRange().first) / L;
       if (L == 0) u = 0;
+      assert(hpp::pinocchio::checkNormalized(hpp::pinocchio::LiegroupElement(initial_, space_)));
+      assert(hpp::pinocchio::checkNormalized(hpp::pinocchio::LiegroupElement(end_, space_)));
       space_->interpolate(initial_, end_, u, result);
       return true;
     }