From ae1d86391f41e1ab3aac5dafbcdf2436f2f960a4 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Tue, 10 Jul 2018 10:53:27 +0200
Subject: [PATCH] Remove assert in WaypointEdge::build.
- when building edges by hand, this assert may fail eventhough the problem
is well posed.
---
src/graph/edge.cc | 7 +++++--
1 file changed, 5 insertions(+), 2 deletions(-)
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index df04b97c..5b557535 100644
--- a/src/graph/edge.cc
+++ b/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 << "."
--
GitLab