diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index bc36ccce7f9e05f8c4779169f46f0d56c00cff5e..be27b5f8675bb91865bd133d6f9b6fa47051d25f 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -100,6 +100,15 @@ namespace hpp {
return steeringMethod_;
}
+ /// Get direction of the path compare to the edge
+ /// \return true is reverse
+ virtual bool direction (const core::PathPtr_t& path) const;
+
+ /// Populate a ConfigProjector with constraints required to generate
+ /// a path at the intersection of two edges.
+ virtual bool intersectionConstraint (const EdgePtr_t& other,
+ ConfigProjectorPtr_t projector) const;
+
/// Print the object in a stream.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
@@ -188,6 +197,8 @@ namespace hpp {
const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
+ virtual bool direction (const core::PathPtr_t& path) const;
+
virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2, const core::WeighedDistance& d) const;
virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index ddc519b1a6fa9a21c1f3a4283f8071e80b11f59a..cf709af8cc828b7fafee93310c5cca84fbe6bc59 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -61,6 +61,60 @@ namespace hpp {
else return to ();
}
+ bool Edge::direction (const core::PathPtr_t& path) const
+ {
+ Configuration_t q0 = path->initial (),
+ q1 = path->end ();
+ const bool src_contains_q0 = from()->contains (q0);
+ const bool dst_contains_q0 = to ()->contains (q0);
+ const bool src_contains_q1 = from()->contains (q1);
+ const bool dst_contains_q1 = to ()->contains (q1);
+ assert ((src_contains_q0 && dst_contains_q1)
+ || (src_contains_q1 && dst_contains_q0));
+ /// true if reverse
+ return !dst_contains_q1;
+ }
+
+ bool WaypointEdge::direction (const core::PathPtr_t& path) const
+ {
+ Configuration_t q0 = path->initial (),
+ q1 = path->end ();
+ const bool src_contains_q0 = waypoint_.second->contains (q0);
+ const bool dst_contains_q0 = to ()->contains (q0);
+ const bool src_contains_q1 = waypoint_.second->contains (q1);
+ const bool dst_contains_q1 = to ()->contains (q1);
+ assert ((src_contains_q0 && dst_contains_q1)
+ || (src_contains_q1 && dst_contains_q0));
+ /// true if reverse
+ return !dst_contains_q1;
+ }
+
+ bool Edge::intersectionConstraint (const EdgePtr_t& other,
+ ConfigProjectorPtr_t proj) const
+ {
+ GraphPtr_t g = graph_.lock ();
+
+ g->insertNumericalConstraints (proj);
+ insertNumericalConstraints (proj);
+ node ()->insertNumericalConstraints (proj);
+
+ g->insertLockedJoints (proj);
+ insertLockedJoints (proj);
+ node ()->insertLockedJoints (proj);
+
+ if (wkPtr_.lock() == other) // No intersection to be computed.
+ return false;
+
+ bool nodeB_Eq_nodeA = (node() == other->node());
+
+ other->insertNumericalConstraints (proj);
+ if (!nodeB_Eq_nodeA) other->node()->insertNumericalConstraints (proj);
+ other->insertLockedJoints (proj);
+ if (!nodeB_Eq_nodeA) other->node()->insertLockedJoints (proj);
+
+ return true;
+ }
+
EdgePtr_t Edge::create (const std::string& name,
const core::SteeringMethodPtr_t& steeringMethod,
const GraphWkPtr_t& graph,
diff --git a/src/path-optimization/config-optimization.cc b/src/path-optimization/config-optimization.cc
index f93149d93adf9e07260c8f61a2a15f19c8e067bf..74f026602c5e1926d2058dc3c2aa5fc6181df90e 100644
--- a/src/path-optimization/config-optimization.cc
+++ b/src/path-optimization/config-optimization.cc
@@ -37,23 +37,13 @@ namespace hpp {
HPP_STATIC_PTR_CAST (ConstraintSet, after->constraints ());
assert (setA->edge () && setB->edge());
graph::GraphPtr_t graph = setA->edge()->parentGraph ();
- graph::NodePtr_t n0 = graph->getNode (before->initial ()),
- n1 = graph->getNode (before->end ()),
- 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 = (n0 != n1) &&
- (n1 == setB->edge()->from () && n0 == setB->edge()->to ());
- bool reverseA = (n2 != n3) &&
- (n3 == setA->edge()->from () && n2 == setA->edge()->to ());
+ const bool reverseB = setB->edge ()->direction(before);
+ const bool reverseA = setA->edge ()->direction(after);
reverse = reverseB;
@@ -61,25 +51,7 @@ namespace hpp {
"intersect_" + setB->edge()->name() + "_" + setA->edge()->name(),
graph->errorThreshold (), graph->maxIterations ());
- graph->insertNumericalConstraints (p);
- // TODO: Is reverse case different ?
- bool nodeB_Eq_nodeA = (setB->edge()->node() == setA->edge()->node());
-
- setB->edge()->insertNumericalConstraints (p);
- setB->edge()->node ()->insertNumericalConstraints (p);
-
- graph->insertLockedJoints (p);
- setB->edge()->insertLockedJoints (p);
- setB->edge()->node ()->insertLockedJoints (p);
-
- vector_t rhsB = p->rightHandSideFromConfig (before->initial ());
-
- setA->edge()->insertNumericalConstraints (p);
- if (!nodeB_Eq_nodeA)
- setA->edge()->node()->insertNumericalConstraints (p);
- setA->edge()->insertLockedJoints (p);
- if (!nodeB_Eq_nodeA)
- setA->edge()->node()->insertLockedJoints (p);
+ setB->edge()->intersectionConstraint (setA->edge (), p);
p->rightHandSideFromConfig (before->end ());
return p;