diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 360ce8e9ae01d5d5e96810237cf04e6f59e9fcd1..16cb9a31c44c3f8532cfa8e03fa2367a98e1dd44 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -87,9 +87,7 @@ namespace hpp {
HPP_PREDEF_CLASS (GraphPathValidation);
typedef boost::shared_ptr < GraphPathValidation > GraphPathValidationPtr_t;
HPP_PREDEF_CLASS (SteeringMethod);
- HPP_PREDEF_CLASS (GraphSteeringMethod);
typedef boost::shared_ptr < SteeringMethod > SteeringMethodPtr_t;
- typedef boost::shared_ptr < GraphSteeringMethod > GraphSteeringMethodPtr_t;
typedef core::PathOptimizer PathOptimizer;
typedef core::PathOptimizerPtr_t PathOptimizerPtr_t;
HPP_PREDEF_CLASS (GraphOptimizer);
diff --git a/include/hpp/manipulation/graph-steering-method.hh b/include/hpp/manipulation/graph-steering-method.hh
deleted file mode 100644
index d21d7a46184d737c5c77a5ede6e303c35754a6c7..0000000000000000000000000000000000000000
--- a/include/hpp/manipulation/graph-steering-method.hh
+++ /dev/null
@@ -1,124 +0,0 @@
-// Copyright (c) 2014, LAAS-CNRS
-// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
-//
-// This file is part of hpp-manipulation.
-// hpp-manipulation is free software: you can redistribute it
-// and/or modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation, either version
-// 3 of the License, or (at your option) any later version.
-//
-// hpp-manipulation is distributed in the hope that it will be
-// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
-// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-// General Lesser Public License for more details. You should have
-// received a copy of the GNU Lesser General Public License along with
-// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
-
-
-#ifndef HPP_MANIPULATION_GRAPH_STEERING_METHOD_HH
-# define HPP_MANIPULATION_GRAPH_STEERING_METHOD_HH
-
-# include <hpp/core/problem-solver.hh> // SteeringMethodBuilder_t
-# include <hpp/core/steering-method.hh>
-
-# include <hpp/manipulation/config.hh>
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/graph/fwd.hh>
-
-namespace hpp {
- namespace manipulation {
- using core::PathPtr_t;
- /// \addtogroup steering_method
- /// \{
-
- class HPP_MANIPULATION_DLLAPI SteeringMethod : public core::SteeringMethod
- {
- public:
- const core::SteeringMethodPtr_t& innerSteeringMethod () const
- {
- return steeringMethod_;
- }
-
- void innerSteeringMethod (const core::SteeringMethodPtr_t& sm)
- {
- steeringMethod_ = sm;
- }
-
- protected:
- /// Constructor
- SteeringMethod (const Problem& problem);
-
- /// Copy constructor
- SteeringMethod (const SteeringMethod& other);
-
- void init (SteeringMethodWkPtr_t weak)
- {
- core::SteeringMethod::init (weak);
- }
-
- /// A pointer to the manipulation problem
- const Problem& problem_;
- /// The encapsulated steering method
- core::SteeringMethodPtr_t steeringMethod_;
- };
-
- class HPP_MANIPULATION_DLLAPI GraphSteeringMethod : public SteeringMethod
- {
- typedef core::SteeringMethodBuilder_t SteeringMethodBuilder_t;
-
- public:
- /// Create instance and return shared pointer
- /// \warning core::Problem will be casted to Problem
- static GraphSteeringMethodPtr_t create
- (const core::Problem& problem);
-
- template <typename T>
- static GraphSteeringMethodPtr_t create
- (const core::Problem& problem);
-
- /// Create instance and return shared pointer
- static GraphSteeringMethodPtr_t create (const Problem& problem);
-
- /// Create copy and return shared pointer
- static GraphSteeringMethodPtr_t createCopy
- (const GraphSteeringMethodPtr_t& other);
-
- /// Copy instance and return shared pointer
- virtual core::SteeringMethodPtr_t copy () const
- {
- return createCopy (weak_.lock ());
- }
-
- protected:
- /// Constructor
- GraphSteeringMethod (const Problem& problem);
-
- /// Copy constructor
- GraphSteeringMethod (const GraphSteeringMethod&);
-
- virtual PathPtr_t impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
-
- void init (GraphSteeringMethodWkPtr_t weak)
- {
- SteeringMethod::init (weak);
- weak_ = weak;
- }
-
- private:
- /// Weak pointer to itself
- GraphSteeringMethodWkPtr_t weak_;
- };
-
- template <typename T>
- GraphSteeringMethodPtr_t GraphSteeringMethod::create
- (const core::Problem& problem)
- {
- GraphSteeringMethodPtr_t gsm = GraphSteeringMethod::create (problem);
- gsm->innerSteeringMethod (T::create (problem));
- return gsm;
- }
- /// \}
- } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_GRAPH_STEERING_METHOD_HH
diff --git a/include/hpp/manipulation/steering-method/fwd.hh b/include/hpp/manipulation/steering-method/fwd.hh
index 3b245f6bf4d74a71de57110f81f8078aaba51dd2..467557d9d37ee8684783ecdeab090c12ee5b19bc 100644
--- a/include/hpp/manipulation/steering-method/fwd.hh
+++ b/include/hpp/manipulation/steering-method/fwd.hh
@@ -26,6 +26,8 @@
namespace hpp {
namespace manipulation {
namespace steeringMethod {
+ HPP_PREDEF_CLASS (Graph);
+ typedef boost::shared_ptr < Graph > GraphPtr_t;
HPP_PREDEF_CLASS (CrossStateOptimization);
typedef boost::shared_ptr < CrossStateOptimization > CrossStateOptimizationPtr_t;
} // namespace steeringMethod
diff --git a/include/hpp/manipulation/steering-method/graph.hh b/include/hpp/manipulation/steering-method/graph.hh
new file mode 100644
index 0000000000000000000000000000000000000000..5374e8099e06eedaa4cb0296e70af87d83504804
--- /dev/null
+++ b/include/hpp/manipulation/steering-method/graph.hh
@@ -0,0 +1,127 @@
+// Copyright (c) 2014, LAAS-CNRS
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+
+#ifndef HPP_MANIPULATION_STEERING_METHOD_GRAPH_HH
+# define HPP_MANIPULATION_STEERING_METHOD_GRAPH_HH
+
+# include <hpp/core/problem-solver.hh> // SteeringMethodBuilder_t
+# include <hpp/core/steering-method.hh>
+
+# include <hpp/manipulation/config.hh>
+# include <hpp/manipulation/fwd.hh>
+# include <hpp/manipulation/graph/fwd.hh>
+# include <hpp/manipulation/steering-method/fwd.hh>
+
+namespace hpp {
+ namespace manipulation {
+ class HPP_MANIPULATION_DLLAPI SteeringMethod : public core::SteeringMethod
+ {
+ public:
+ const core::SteeringMethodPtr_t& innerSteeringMethod () const
+ {
+ return steeringMethod_;
+ }
+
+ void innerSteeringMethod (const core::SteeringMethodPtr_t& sm)
+ {
+ steeringMethod_ = sm;
+ }
+
+ protected:
+ /// Constructor
+ SteeringMethod (const Problem& problem);
+
+ /// Copy constructor
+ SteeringMethod (const SteeringMethod& other);
+
+ void init (SteeringMethodWkPtr_t weak)
+ {
+ core::SteeringMethod::init (weak);
+ }
+
+ /// A pointer to the manipulation problem
+ const Problem& problem_;
+ /// The encapsulated steering method
+ core::SteeringMethodPtr_t steeringMethod_;
+ };
+
+ namespace steeringMethod {
+ using core::PathPtr_t;
+ /// \addtogroup steering_method
+ /// \{
+
+ class HPP_MANIPULATION_DLLAPI Graph : public SteeringMethod
+ {
+ typedef core::SteeringMethodBuilder_t SteeringMethodBuilder_t;
+
+ public:
+ /// Create instance and return shared pointer
+ /// \warning core::Problem will be casted to Problem
+ static GraphPtr_t create
+ (const core::Problem& problem);
+
+ template <typename T>
+ static GraphPtr_t create
+ (const core::Problem& problem);
+
+ /// Create instance and return shared pointer
+ static GraphPtr_t create (const Problem& problem);
+
+ /// Create copy and return shared pointer
+ static GraphPtr_t createCopy
+ (const GraphPtr_t& other);
+
+ /// Copy instance and return shared pointer
+ virtual core::SteeringMethodPtr_t copy () const
+ {
+ return createCopy (weak_.lock ());
+ }
+
+ protected:
+ /// Constructor
+ Graph (const Problem& problem);
+
+ /// Copy constructor
+ Graph (const Graph&);
+
+ virtual PathPtr_t impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
+
+ void init (GraphWkPtr_t weak)
+ {
+ SteeringMethod::init (weak);
+ weak_ = weak;
+ }
+
+ private:
+ /// Weak pointer to itself
+ GraphWkPtr_t weak_;
+ };
+
+ template <typename T>
+ GraphPtr_t Graph::create
+ (const core::Problem& problem)
+ {
+ GraphPtr_t gsm = Graph::create (problem);
+ gsm->innerSteeringMethod (T::create (problem));
+ return gsm;
+ }
+ /// \}
+ } // namespace steeringMethod
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_STEERING_METHOD_GRAPH_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 66d6b9eed3168cbd25eb1d8faac2a71c5e1eece2..ecb8644bfc75c921001c9f865d9beb7ffdcc2381 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -34,7 +34,6 @@ SET(SOURCES
weighed-distance.cc
problem.cc
graph-path-validation.cc
- graph-steering-method.cc
graph-optimizer.cc
graph/state.cc
@@ -55,6 +54,7 @@ SET(SOURCES
problem-target/state.cc
steering-method/cross-state-optimization.cc
+ steering-method/graph.cc
)
IF(HPP_WHOLEBODY_STEP_FOUND)
diff --git a/src/graph-steering-method.cc b/src/graph-steering-method.cc
deleted file mode 100644
index 88fbfb143c75e838595e084eb1910fd0f85112cc..0000000000000000000000000000000000000000
--- a/src/graph-steering-method.cc
+++ /dev/null
@@ -1,104 +0,0 @@
-// Copyright (c) 2014, LAAS-CNRS
-// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
-//
-// This file is part of hpp-manipulation.
-// hpp-manipulation is free software: you can redistribute it
-// and/or modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation, either version
-// 3 of the License, or (at your option) any later version.
-//
-// hpp-manipulation is distributed in the hope that it will be
-// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
-// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-// General Lesser Public License for more details. You should have
-// received a copy of the GNU Lesser General Public License along with
-// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
-
-#include "hpp/manipulation/graph-steering-method.hh"
-
-#include <hpp/util/pointer.hh>
-
-#include <hpp/core/straight-path.hh>
-#include <hpp/core/steering-method/straight.hh>
-
-#include <hpp/manipulation/problem.hh>
-#include <hpp/manipulation/graph/graph.hh>
-#include <hpp/manipulation/graph/edge.hh>
-
-namespace hpp {
- namespace manipulation {
- SteeringMethod::SteeringMethod (const Problem& problem) :
- core::SteeringMethod (problem), problem_ (problem),
- steeringMethod_ (core::SteeringMethodStraight::create (problem))
- {
- }
-
- SteeringMethod::SteeringMethod (const SteeringMethod& other):
- core::SteeringMethod (other), problem_ (other.problem_), steeringMethod_
- (other.steeringMethod_)
- {
- }
-
- GraphSteeringMethodPtr_t GraphSteeringMethod::create
- (const core::Problem& problem)
- {
- HPP_STATIC_CAST_REF_CHECK (const Problem, problem);
- const Problem& p = static_cast <const Problem&> (problem);
- return create (p);
- }
-
- GraphSteeringMethodPtr_t GraphSteeringMethod::create
- (const Problem& problem)
- {
- GraphSteeringMethod* ptr = new GraphSteeringMethod (problem);
- GraphSteeringMethodPtr_t shPtr (ptr);
- ptr->init (shPtr);
- return shPtr;
- }
-
- GraphSteeringMethodPtr_t GraphSteeringMethod::createCopy
- (const GraphSteeringMethodPtr_t& other)
- {
- GraphSteeringMethod* ptr = new GraphSteeringMethod (*other);
- GraphSteeringMethodPtr_t shPtr (ptr);
- ptr->init (shPtr);
- return shPtr;
- }
-
- GraphSteeringMethod::GraphSteeringMethod (const Problem& problem) :
- SteeringMethod (problem), weak_ ()
- {
- }
-
- GraphSteeringMethod::GraphSteeringMethod (const GraphSteeringMethod& other):
- SteeringMethod (other)
- {
- }
-
- PathPtr_t GraphSteeringMethod::impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const
- {
- graph::Edges_t possibleEdges;
- if (!problem_.constraintGraph())
- throw std::invalid_argument ("The constraint graph should be set to use the GraphSteeringMethod");
- const graph::Graph& graph = *problem_.constraintGraph ();
- try {
- possibleEdges = graph.getEdges
- (graph.getState (q1), graph.getState (q2));
- } catch (const std::logic_error& e) {
- hppDout (error, e.what ());
- return PathPtr_t ();
- }
- PathPtr_t path;
- if (possibleEdges.empty()) {
- hppDout (info, "No edge found.");
- }
- while (!possibleEdges.empty()) {
- if (possibleEdges.back ()->build (path, q1, q2)) {
- return path;
- }
- possibleEdges.pop_back ();
- }
- return PathPtr_t ();
- }
- } // namespace manipulation
-} // namespace hpp
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 9f14011c8d60a27637dfd3ba6e90dd110043d030..28258129d01964e3ed275d4853e53ac19d7fb7cf 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -52,12 +52,12 @@
#include "hpp/manipulation/graph-optimizer.hh"
#include "hpp/manipulation/graph-path-validation.hh"
#include "hpp/manipulation/graph-node-optimizer.hh"
-#include "hpp/manipulation/graph-steering-method.hh"
#include "hpp/manipulation/path-optimization/config-optimization.hh"
#include "hpp/manipulation/path-optimization/keypoints.hh"
#include "hpp/manipulation/path-optimization/spline-gradient-based.hh"
#include "hpp/manipulation/problem-target/state.hh"
#include "hpp/manipulation/steering-method/cross-state-optimization.hh"
+#include "hpp/manipulation/steering-method/graph.hh"
#if HPP_MANIPULATION_HAS_WHOLEBODY_STEP
#include <hpp/wholebody-step/small-steps.hh>
@@ -81,23 +81,23 @@ namespace hpp {
}
};
- template <typename SteeringMethodType>
- core::SteeringMethodPtr_t createSteeringMethodWithGuess
+ template <typename ParentSM_t, typename ChildSM_t>
+ core::SteeringMethodPtr_t createSMWithGuess
(const core::Problem& problem)
{
- GraphSteeringMethodPtr_t gsm = GraphSteeringMethod::create (problem);
- gsm->innerSteeringMethod (SteeringMethodType::createWithGuess (problem));
- return gsm;
+ boost::shared_ptr<ParentSM_t> sm = ParentSM_t::create (problem);
+ sm->innerSteeringMethod (ChildSM_t::createWithGuess (problem));
+ return sm;
}
template <typename PathProjectorType>
core::PathProjectorPtr_t createPathProjector
(const core::Problem& problem, const value_type& step)
{
- GraphSteeringMethodPtr_t gsm =
- HPP_DYNAMIC_PTR_CAST (GraphSteeringMethod, problem.steeringMethod());
+ steeringMethod::GraphPtr_t gsm =
+ HPP_DYNAMIC_PTR_CAST (steeringMethod::Graph, problem.steeringMethod());
if (!gsm) throw std::logic_error ("The steering method should be of type"
- " GraphSteeringMethod");
+ " steeringMethod::Graph");
return PathProjectorType::create (problem.distance(),
gsm->innerSteeringMethod(), step);
}
@@ -149,19 +149,19 @@ namespace hpp {
pathOptimizers.add ("SplineGradientBased_bezier3",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 3>::createFromCore);
steeringMethods.add ("Graph-SteeringMethodStraight",
- GraphSteeringMethod::create <core::SteeringMethodStraight>);
+ steeringMethod::Graph::create <core::SteeringMethodStraight>);
steeringMethods.add ("Graph-Straight",
- GraphSteeringMethod::create <core::steeringMethod::Straight>);
+ steeringMethod::Graph::create <core::steeringMethod::Straight>);
steeringMethods.add ("Graph-Hermite",
- GraphSteeringMethod::create <core::steeringMethod::Hermite>);
+ steeringMethod::Graph::create <core::steeringMethod::Hermite>);
steeringMethods.add ("CrossStateOptimization",
steeringMethod::CrossStateOptimization::createFromCore);
steeringMethods.add ("Graph-ReedsShepp",
- createSteeringMethodWithGuess <core::steeringMethod::ReedsShepp>);
+ createSMWithGuess <steeringMethod::Graph, core::steeringMethod::ReedsShepp>);
steeringMethods.add ("Graph-Dubins",
- createSteeringMethodWithGuess <core::steeringMethod::Dubins>);
+ createSMWithGuess <steeringMethod::Graph, core::steeringMethod::Dubins>);
steeringMethods.add ("Graph-Snibud",
- createSteeringMethodWithGuess <core::steeringMethod::Snibud>);
+ createSMWithGuess <steeringMethod::Graph, core::steeringMethod::Snibud>);
pathOptimizers.add ("KeypointsShortcut",
pathOptimization::Keypoints::create);
diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc
index e5cf48bebe466616b0d01a66f826bdc8458dd3c3..28fbf13f936ed91240bca44bfc88d767442901be 100644
--- a/src/steering-method/cross-state-optimization.cc
+++ b/src/steering-method/cross-state-optimization.cc
@@ -39,10 +39,18 @@
namespace hpp {
namespace manipulation {
namespace steeringMethod {
- using namespace graph;
using Eigen::RowBlockIndices;
using Eigen::ColBlockIndices;
+ using graph::StatePtr_t;
+ using graph::States_t;
+ using graph::EdgePtr_t;
+ using graph::Edges_t;
+ using graph::Neighbors_t;
+ using graph::NumericalConstraints_t;
+ using graph::LockedJoints_t;
+ using graph::segments_t;
+
CrossStateOptimizationPtr_t CrossStateOptimization::create (
const Problem& problem)
{
@@ -218,7 +226,7 @@ namespace hpp {
solver.integration (boost::bind(&OptimizationData::_integrate, this, _1, _2, _3));
}
- void addGraphConstraints (const GraphPtr_t& graph)
+ void addGraphConstraints (const graph::GraphPtr_t& graph)
{
for (std::size_t i = 0; i < N; ++i) {
_add (graph->lockedJoints(), i);
@@ -541,7 +549,7 @@ namespace hpp {
core::PathPtr_t CrossStateOptimization::impl_compute (
ConfigurationIn_t q1, ConfigurationIn_t q2) const
{
- const Graph& graph = *problem_.constraintGraph ();
+ const graph::Graph& graph = *problem_.constraintGraph ();
GraphSearchData d;
d.s1 = graph.getState (q1);
d.s2 = graph.getState (q2);
diff --git a/src/steering-method/graph.cc b/src/steering-method/graph.cc
new file mode 100644
index 0000000000000000000000000000000000000000..107baac9bfc62da4668d1bcd758e5509fa2c1cd7
--- /dev/null
+++ b/src/steering-method/graph.cc
@@ -0,0 +1,107 @@
+// Copyright (c) 2014, LAAS-CNRS
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+#include "hpp/manipulation/steering-method/graph.hh"
+
+#include <hpp/util/pointer.hh>
+
+#include <hpp/core/straight-path.hh>
+#include <hpp/core/steering-method/straight.hh>
+
+#include <hpp/manipulation/problem.hh>
+#include <hpp/manipulation/graph/graph.hh>
+#include <hpp/manipulation/graph/edge.hh>
+
+namespace hpp {
+ namespace manipulation {
+ SteeringMethod::SteeringMethod (const Problem& problem) :
+ core::SteeringMethod (problem), problem_ (problem),
+ steeringMethod_ (core::SteeringMethodStraight::create (problem))
+ {
+ }
+
+ SteeringMethod::SteeringMethod (const SteeringMethod& other):
+ core::SteeringMethod (other), problem_ (other.problem_), steeringMethod_
+ (other.steeringMethod_)
+ {
+ }
+
+ namespace steeringMethod {
+
+ GraphPtr_t Graph::create
+ (const core::Problem& problem)
+ {
+ HPP_STATIC_CAST_REF_CHECK (const Problem, problem);
+ const Problem& p = static_cast <const Problem&> (problem);
+ return create (p);
+ }
+
+ GraphPtr_t Graph::create
+ (const Problem& problem)
+ {
+ Graph* ptr = new Graph (problem);
+ GraphPtr_t shPtr (ptr);
+ ptr->init (shPtr);
+ return shPtr;
+ }
+
+ GraphPtr_t Graph::createCopy
+ (const GraphPtr_t& other)
+ {
+ Graph* ptr = new Graph (*other);
+ GraphPtr_t shPtr (ptr);
+ ptr->init (shPtr);
+ return shPtr;
+ }
+
+ Graph::Graph (const Problem& problem) :
+ SteeringMethod (problem), weak_ ()
+ {
+ }
+
+ Graph::Graph (const Graph& other):
+ SteeringMethod (other)
+ {
+ }
+
+ PathPtr_t Graph::impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const
+ {
+ graph::Edges_t possibleEdges;
+ if (!problem_.constraintGraph())
+ throw std::invalid_argument ("The constraint graph should be set to use the steeringMethod::Graph");
+ const graph::Graph& graph = *problem_.constraintGraph ();
+ try {
+ possibleEdges = graph.getEdges
+ (graph.getState (q1), graph.getState (q2));
+ } catch (const std::logic_error& e) {
+ hppDout (error, e.what ());
+ return PathPtr_t ();
+ }
+ PathPtr_t path;
+ if (possibleEdges.empty()) {
+ hppDout (info, "No edge found.");
+ }
+ while (!possibleEdges.empty()) {
+ if (possibleEdges.back ()->build (path, q1, q2)) {
+ return path;
+ }
+ possibleEdges.pop_back ();
+ }
+ return PathPtr_t ();
+ }
+ } // namespace steeringMethod
+ } // namespace manipulation
+} // namespace hpp