From 889c56fcaef28fcd7f6661650fb9ec5ffac1b132 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 3 Dec 2014 10:30:30 +0100
Subject: [PATCH] Fix TODO regarding projection with WaypointEdge
---
include/hpp/manipulation/graph/edge.hh | 2 +-
src/graph/edge.cc | 13 ++++++++-----
2 files changed, 9 insertions(+), 6 deletions(-)
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 1d636aff..7eea322a 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -202,7 +202,7 @@ namespace hpp {
typedef std::pair < EdgePtr_t, NodePtr_t > Waypoint;
Waypoint waypoint_;
- mutable Configuration_t config_;
+ mutable Configuration_t config_, result_;
}; // class WaypointEdge
/// Edge that find intersection of level set.
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 7b27926f..0757f090 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -204,9 +204,9 @@ namespace hpp {
{
assert (waypoint_.first);
core::PathPtr_t pathToWaypoint;
- // TO DO: Many times, this will be called rigth after WaypointEdge::applyConstraints so config_
+ // Many times, this will be called rigth after WaypointEdge::applyConstraints so config_
// already satisfies the constraints.
- config_ = q2;
+ if (!result_.isApprox (q2)) config_ = q2;
if (!waypoint_.first->applyConstraints (q1, config_))
return false;
if (!waypoint_.first->build (pathToWaypoint, q1, config_, d))
@@ -230,10 +230,12 @@ namespace hpp {
bool WaypointEdge::applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const
{
assert (waypoint_.first);
- if (!waypoint_.first->applyConstraints (qoffset, q))
- return false;
config_ = q;
- return Edge::applyConstraints (config_, q);
+ if (!waypoint_.first->applyConstraints (qoffset, config_))
+ return false;
+ bool success = Edge::applyConstraints (config_, q);
+ result_ = q;
+ return success;
}
void WaypointEdge::createWaypoint (const unsigned d, const std::string& bname)
@@ -258,6 +260,7 @@ namespace hpp {
edge->name (ss.str ());
waypoint_ = Waypoint (edge, node);
config_ = Configuration_t(graph_.lock ()->robot ()->configSize ());
+ result_ = Configuration_t(graph_.lock ()->robot ()->configSize ());
}
NodePtr_t WaypointEdge::node () const
--
GitLab