Merge pull request #41 from florent-lamiraux/devel

Remove assert in WaypointEdge::build.

src/graph/edge.cc

@@ -430,8 +430,11 @@ namespace hpp {
             lastSucceeded_ = false;
             return false;
           }
-          if (configs_.col(i) != configs_.col (i+1)) {
-            assert ((configs_.col(i) - configs_.col (i+1)).norm () > 1e-8);
+          assert (configConstraint ());
+          assert (configConstraint ()->configProjector ());
+          value_type eps
+            (configConstraint ()->configProjector ()->errorThreshold ());
+          if ((configs_.col(i) - configs_.col (i+1)).squaredNorm () > eps*eps) {
             if (!edges_[i]->build (p, configs_.col(i), configs_.col (i+1))) {
               hppDout (info, "Waypoint edge " << name()
                        << ": build failed at waypoint " << i << "."