From ab205f3a3e41a07da8a7499a42be7c8d030f5a9a Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Fri, 24 Jan 2020 12:05:05 +0100
Subject: [PATCH] [steeringMethod::Graph] If q1 == q2, call problem inner
steering method.
---
src/steering-method/graph.cc | 7 +++++++
1 file changed, 7 insertions(+)
diff --git a/src/steering-method/graph.cc b/src/steering-method/graph.cc
index 107baac9..cd07999c 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 ();
--
GitLab