diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh index 9d1b67c7916c827b621c566eb6ee1e6d642f5077..c432ccfee926b2db8547915850c0373576e95802 100644 --- a/include/hpp/manipulation/graph/graph.hh +++ b/include/hpp/manipulation/graph/graph.hh @@ -235,9 +235,12 @@ namespace hpp { /// Get the robot. const DevicePtr_t& robot () const; - /// Get the steering Method + /// Get the problem const ProblemPtr_t& problem () const; + /// Set the problem + void problem (const ProblemPtr_t& problem); + /// Register an histogram representing a foliation void insertHistogram (const graph::HistogramPtr_t& hist) { diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh index 3dddc4ebaea1675bfbb7c6d1d2511859435a66fe..cd63fe1f47f26d67a32673b58655ae29ef81f1b9 100644 --- a/include/hpp/manipulation/problem-solver.hh +++ b/include/hpp/manipulation/problem-solver.hh @@ -63,7 +63,7 @@ namespace hpp { /// \{ /// Set the constraint graph - void constraintGraph (const graph::GraphPtr_t& graph); + void constraintGraph (const std::string& graph); /// Get the constraint graph graph::GraphPtr_t constraintGraph () const; @@ -134,6 +134,8 @@ namespace hpp { void setTargetState (const graph::StatePtr_t state); + core::Container <graph::GraphPtr_t> graphs; + protected: virtual void initializeProblem (ProblemPtr_t problem); diff --git a/src/graph/graph.cc b/src/graph/graph.cc index f50d5af9ecc974bcf573adcd467c6b7759a3b7c2..97c8458c6b472c0bd0d84d9bda372ec8083ad4e2 100644 --- a/src/graph/graph.cc +++ b/src/graph/graph.cc @@ -108,6 +108,14 @@ namespace hpp { return robot_; } + void Graph::problem (const ProblemPtr_t& problem) + { + if (problem_ != problem) { + problem_ = problem; + setDirty(); + } + } + const ProblemPtr_t& Graph::problem () const { return problem_; diff --git a/src/graph/helper.cc b/src/graph/helper.cc index 224c71d802ecf9ad3f0f0f231f6a2c181e56ef24..1dd96373cd357d20312635dba1e0dc81423b65dd 100644 --- a/src/graph/helper.cc +++ b/src/graph/helper.cc @@ -1113,7 +1113,8 @@ namespace hpp { } GraphPtr_t graph = Graph::create (graphName, ps->robot(), ps->problem()); - ps->constraintGraph (graph); + ps->graphs.add (graphName, graph); + ps->constraintGraph (graphName); graph->stateSelector ( GuidedStateSelector::create ("stateSelector", ps->roadmap ())); @@ -1121,7 +1122,6 @@ namespace hpp { graph->errorThreshold (ps->errorThreshold ()); graphBuilder (ps, objects, grippers, graph, rules); - ps->constraintGraph (graph); return graph; } } // namespace helper diff --git a/src/problem-solver.cc b/src/problem-solver.cc index 6016b9f50e4e2db209e34505850c606418b3b048..f70e31766e7a0021306745bf7701c21f8e074de1 100644 --- a/src/problem-solver.cc +++ b/src/problem-solver.cc @@ -197,15 +197,19 @@ namespace hpp { { problem_ = problem; core::ProblemSolver::initializeProblem (problem_); + problem_->constraintGraph (constraintGraph_); if (problem_->pathValidation ()) problem_->pathValidation ()->constraintGraph (constraintGraph_); } - void ProblemSolver::constraintGraph (const graph::GraphPtr_t& graph) + void ProblemSolver::constraintGraph (const std::string& graphName) { - constraintGraph_ = graph; + if (!graphs.has (graphName)) + throw std::invalid_argument ("ProblemSolver has no graph named " + graphName); + constraintGraph_ = graphs.get(graphName); RoadmapPtr_t r = HPP_DYNAMIC_PTR_CAST (Roadmap, roadmap()); - if (r) r->constraintGraph (graph); + if (r) r->constraintGraph (constraintGraph_); + if (problem_) problem_->constraintGraph (constraintGraph_); } graph::GraphPtr_t ProblemSolver::constraintGraph () const diff --git a/src/problem.cc b/src/problem.cc index 883f4a9d7b12e5bf0e6f59adb4222193ee506aea..ca00798e1eec559857eba20893c57f60f78be203 100644 --- a/src/problem.cc +++ b/src/problem.cc @@ -38,6 +38,7 @@ namespace hpp { void Problem::constraintGraph (const graph::GraphPtr_t& graph) { graph_ = graph; + graph_->problem (this); if (pathValidation ()) pathValidation ()->constraintGraph (graph); WeighedDistancePtr_t d = HPP_DYNAMIC_PTR_CAST (WeighedDistance,