From 55a34b3d9aef67d22b59603e902d0b748c028b2f Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 26 Oct 2017 14:37:59 +0200
Subject: [PATCH] Fix usage of cache in WaypointEdge
---
include/hpp/manipulation/graph/edge.hh | 4 +++-
src/graph/edge.cc | 27 +++++++++++++++-----------
2 files changed, 19 insertions(+), 12 deletions(-)
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 4a1cc5bf..9ce1e109 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -258,7 +258,8 @@ namespace hpp {
protected:
WaypointEdge (const std::string& name) :
- Edge (name)
+ Edge (name),
+ lastSucceeded_ (false)
{
}
/// Initialization of the object.
@@ -273,6 +274,7 @@ namespace hpp {
States_t states_;
mutable matrix_t configs_;
+ mutable bool lastSucceeded_;
WaypointEdgeWkPtr_t wkPtr_;
}; // class WaypointEdge
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index edfa06bb..2066b85c 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -336,32 +336,35 @@ namespace hpp {
graph_.lock ()->robot ()->numberDof ());
// Many times, this will be called rigth after WaypointEdge::applyConstraints so config_
// already satisfies the constraints.
- size_type n = edges_.size() - 1;
- bool useCache = configs_.col(0).isApprox (q1)
- && configs_.col(n).isApprox (q2);
- if (!useCache) {
- configs_.col(0) = q1;
- configs_.col(n) = q2;
- }
-
- for (std::size_t i = 0; i < edges_.size (); ++i) {
- if (!useCache) configs_.col (i+1) = q2;
- if (!edges_[i]->applyConstraints (configs_.col(i), configs_.col (i+1))) {
+ size_type n = edges_.size();
+ assert (configs_.cols() == n + 1);
+ bool useCache = lastSucceeded_
+ && configs_.col(0).isApprox (q1)
+ && configs_.col(n).isApprox (q2);
+ configs_.col(0) = q1;
+ configs_.col(n) = q2;
+
+ for (size_type i = 0; i < n; ++i) {
+ if (i < (n-1) && !useCache) configs_.col (i+1) = q2;
+ if (i < (n-1) && !edges_[i]->applyConstraints (configs_.col(i), configs_.col (i+1))) {
hppDout (info, "Waypoint edge " << name() << ": applyConstraints failed at waypoint " << i << "."
<< "\nUse cache: " << useCache
);
+ lastSucceeded_ = false;
return false;
}
if (!edges_[i]->build (p, configs_.col(i), configs_.col (i+1))) {
hppDout (info, "Waypoint edge " << name() << ": build failed at waypoint " << i << "."
<< "\nUse cache: " << useCache
);
+ lastSucceeded_ = false;
return false;
}
pv->appendPath (p);
}
path = pv;
+ lastSucceeded_ = true;
return true;
}
@@ -373,10 +376,12 @@ namespace hpp {
configs_.col (i+1) = q;
if (!edges_[i]->applyConstraints (configs_.col(i), configs_.col (i+1))) {
q = configs_.col(i+1);
+ lastSucceeded_ = false;
return false;
}
}
q = configs_.col(edges_.size());
+ lastSucceeded_ = true;
return true;
}
--
GitLab