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