From 04d872620bd69042440161d93513906171114559 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 28 Nov 2014 15:08:14 +0100
Subject: [PATCH] Fix bug in class PathProjector::Progressive
---
src/path-projector/progressive.cc | 11 +++++------
1 file changed, 5 insertions(+), 6 deletions(-)
diff --git a/src/path-projector/progressive.cc b/src/path-projector/progressive.cc
index 8d34fb9..906dbb7 100644
--- a/src/path-projector/progressive.cc
+++ b/src/path-projector/progressive.cc
@@ -61,21 +61,20 @@ namespace hpp {
timeRange = toSplitRef.timeRange ();
const Configuration_t& qb = toSplitRef (timeRange.first);
curStep = step_;
+ curLength = std::numeric_limits <value_type>::max();
bool stop = false;
/// Find the good length.
/// Here, it would be good to have an upper bound of the Hessian
/// of the constraint.
do {
- toSplitRef (qi, curStep);
- if (!constraints->apply (qi)) {
- stop = true;
- break;
- }
- curLength = d (qb, qi);
if (curStep < 0.02) {
stop = true;
break;
}
+ toSplitRef (qi, curStep);
+ if (constraints->apply (qi)) {
+ curLength = d (qb, qi);
+ }
curStep /= 2;
} while (curLength > step_);
if (stop) break;
--
GitLab