diff --git a/src/steering-method/graph.cc b/src/steering-method/graph.cc index 107baac9bfc62da4668d1bcd758e5509fa2c1cd7..cd07999c696eb6599e60d1c6d2942beb4514d84f 100644 --- a/src/steering-method/graph.cc +++ b/src/steering-method/graph.cc @@ -80,6 +80,13 @@ namespace hpp { PathPtr_t Graph::impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const { graph::Edges_t possibleEdges; + // If q1 and q2 are the same, call the problem steering method between + // them + if (q1 == q2) { + core::SteeringMethodPtr_t sm + (problem_.manipulationSteeringMethod()->innerSteeringMethod()); + return (*sm) (q1, q2); + } if (!problem_.constraintGraph()) throw std::invalid_argument ("The constraint graph should be set to use the steeringMethod::Graph"); const graph::Graph& graph = *problem_.constraintGraph ();