From 242126f924d6d5cbf7a4294d57ba578f56b8e444 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 31 Jul 2014 10:35:48 +0200
Subject: [PATCH] ProblemSolver contains a Problem and not a Graph
---
include/hpp/manipulation/problem-solver.hh | 26 ++++++++--------
src/problem-solver.cc | 35 ++++++++--------------
2 files changed, 26 insertions(+), 35 deletions(-)
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 3e7869d4..b8594b3c 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -23,6 +23,7 @@
# include <hpp/model/device.hh>
# include "hpp/manipulation/object.hh"
# include "hpp/manipulation/robot.hh"
+# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/fwd.hh"
namespace hpp {
@@ -95,19 +96,6 @@ namespace hpp {
graph::GraphPtr_t constraintGraph () const;
/// \}
- /// \name Solve problem
- /// \{
-
- /// Prepare the solver for a step by step planning.
- /// and try to make direct connections (call PathPlanner::tryDirectPath)
- /// \return the return value of PathPlanner::pathExists
- virtual bool prepareSolveStepByStep ();
-
- /// Set and solve the problem
- virtual void solve ();
-
- /// \}
-
/// Add grasp
void addGrasp( const DifferentiableFunctionPtr_t& constraint,
const model::GripperPtr_t& gripper,
@@ -160,9 +148,21 @@ namespace hpp {
/// Objects are detected by dynamic cast.
void buildCompositeRobot (const std::string& robotName,
const Names_t& robotNames);
+
+ /// Create new problem.
+ virtual void resetProblem ();
+
+ /// Get pointer to problem
+ ProblemPtr_t problem ()
+ {
+ return problem_;
+ }
+
private:
typedef std::map <const std::string, DevicePtr_t> RobotsandObjects_t;
RobotPtr_t robot_;
+ /// The pointer should point to the same object as core::Problem.
+ ProblemPtr_t problem_;
graph::GraphPtr_t constraintGraph_;
/// Map of single robots to store before building a composite robot.
RobotsandObjects_t robotsAndObjects_;
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index b0f55286..fdc68109 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -18,11 +18,13 @@
#include <hpp/util/pointer.hh>
#include <hpp/util/debug.hh>
#include <hpp/model/gripper.hh>
+#include <hpp/core/roadmap.hh>
#include "hpp/manipulation/object.hh"
#include "hpp/manipulation/robot.hh"
#include "hpp/manipulation/graph/graph.hh"
#include "hpp/manipulation/manipulation-planner.hh"
+#include "hpp/manipulation/problem.hh"
#include "hpp/manipulation/problem-solver.hh"
@@ -72,6 +74,17 @@ namespace hpp {
return object;
}
+ void ProblemSolver::resetProblem ()
+ {
+ if (problem_)
+ delete (problem_);
+ problem_ = new Problem (robot_);
+ roadmap (core::Roadmap::create (problem_->distance (), problem_->robot()));
+ problem_->constraints ();
+ problem_->constraintGraph (constraintGraph_);
+ core::ProblemSolver::problem (problem_);
+ }
+
void ProblemSolver::constraintGraph (const graph::GraphPtr_t& graph)
{
constraintGraph_ = graph;
@@ -145,27 +158,5 @@ namespace hpp {
}
}
}
-
- bool ProblemSolver::prepareSolveStepByStep ()
- {
- ManipulationPlannerPtr_t manipPlanner =
- HPP_DYNAMIC_PTR_CAST (ManipulationPlanner, pathPlanner ());
- if (manipPlanner)
- manipPlanner->constraintGraph (constraintGraph_);
- else
- hppDout (warning, "The planner is not a manipulation planner.");
- return core::ProblemSolver::prepareSolveStepByStep ();
- }
-
- void ProblemSolver::solve ()
- {
- ManipulationPlannerPtr_t manipPlanner =
- HPP_DYNAMIC_PTR_CAST (ManipulationPlanner, pathPlanner ());
- if (manipPlanner)
- manipPlanner->constraintGraph (constraintGraph_);
- else
- hppDout (warning, "The planner is not a manipulation planner.");
- core::ProblemSolver::solve ();
- }
} // namespace manipulation
} // namespace hpp
--
GitLab