From 37bdf57595304e14d6a9fac70f46882e54d7e50f Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 8 Oct 2015 16:58:31 +0200
Subject: [PATCH] Fix ConfigOptimizationTraits
---
src/path-optimization/config-optimization.cc | 12 ++++++++----
1 file changed, 8 insertions(+), 4 deletions(-)
diff --git a/src/path-optimization/config-optimization.cc b/src/path-optimization/config-optimization.cc
index a654d84..f93149d 100644
--- a/src/path-optimization/config-optimization.cc
+++ b/src/path-optimization/config-optimization.cc
@@ -42,13 +42,17 @@ namespace hpp {
n2 = graph->getNode (after->initial ()),
n3 = graph->getNode (after->end ());
/// Find if path were computed from init or goal config
+ /// There is a nasty case: when build a path fails partially, then you
+ /// end up where the edge->to() do not correspond to the node of
+ /// path->end(). In that case, path->end () should be in edge->node().
+ /// (obviously, in this case, we have edge->isInNodeFrom_ = true)
assert ((n0 == setB->edge()->from () && n1 == setB->edge()->to ())
|| (n1 == setB->edge()->from () && n0 == setB->edge()->to ()));
assert ((n2 == setA->edge()->from () && n3 == setA->edge()->to ())
|| (n3 == setA->edge()->from () && n2 == setA->edge()->to ()));
- bool reverseB =
+ bool reverseB = (n0 != n1) &&
(n1 == setB->edge()->from () && n0 == setB->edge()->to ());
- bool reverseA =
+ bool reverseA = (n2 != n3) &&
(n3 == setA->edge()->from () && n2 == setA->edge()->to ());
reverse = reverseB;
@@ -71,10 +75,10 @@ namespace hpp {
vector_t rhsB = p->rightHandSideFromConfig (before->initial ());
setA->edge()->insertNumericalConstraints (p);
- if (nodeB_Eq_nodeA)
+ if (!nodeB_Eq_nodeA)
setA->edge()->node()->insertNumericalConstraints (p);
setA->edge()->insertLockedJoints (p);
- if (nodeB_Eq_nodeA)
+ if (!nodeB_Eq_nodeA)
setA->edge()->node()->insertLockedJoints (p);
p->rightHandSideFromConfig (before->end ());
--
GitLab