From b7dcb786a2ce22319e93daac51733b831276327f Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 31 Jul 2014 10:38:07 +0200
Subject: [PATCH] ManipulationPlanner has a pointer to Problem and no Graph
---
.../hpp/manipulation/manipulation-planner.hh | 32 ++++++-------------
src/manipulation-planner.cc | 19 ++++++++---
2 files changed, 23 insertions(+), 28 deletions(-)
diff --git a/include/hpp/manipulation/manipulation-planner.hh b/include/hpp/manipulation/manipulation-planner.hh
index 566f43c..2cc6eaf 100644
--- a/include/hpp/manipulation/manipulation-planner.hh
+++ b/include/hpp/manipulation/manipulation-planner.hh
@@ -20,7 +20,6 @@
#include <hpp/model/configuration.hh>
#include <hpp/core/basic-configuration-shooter.hh>
#include <hpp/core/path-planner.hh>
-#include <hpp/core/problem.hh>
#include <hpp/core/roadmap.hh>
#include "hpp/manipulation/config.hh"
@@ -37,12 +36,6 @@ namespace hpp {
static ManipulationPlannerPtr_t create (const core::Problem& problem,
const core::RoadmapPtr_t& roadmap);
- /// Set the constraint graph
- void constraintGraph (const graph::GraphPtr_t& graph)
- {
- constraintGraph_ = graph;
- }
-
/// One step of extension.
///
/// A set of constraints is choosen using the graph of constraints.
@@ -50,14 +43,6 @@ namespace hpp {
///
virtual void oneStep ();
- protected:
- /// Protected constructor
- ManipulationPlanner (const core::Problem& problem,
- const core::RoadmapPtr_t& roadmap);
-
- /// Store weak pointer to itself
- void init (const ManipulationPlannerWkPtr_t& weak);
-
/// Extend a the configuration q_near toward q_rand.
/// \param q_near the configuration to be extended.
/// \param q_rand the configuration toward extension is performed.
@@ -67,18 +52,19 @@ namespace hpp {
bool extend (const ConfigurationPtr_t &q_near,
const ConfigurationPtr_t &q_rand, core::PathPtr_t& validPath);
- /// Extend a node of the roadmap towards a configuration.
- /// near The node of the roadmap to be extended.
- /// target The configuration towards which extension is performed.
- core::PathPtr_t extend(const core::NodePtr_t& near,
- const ConfigurationPtr_t& target,
- const graph::Edges_t& edge);
+ protected:
+ /// Protected constructor
+ ManipulationPlanner (const Problem& problem,
+ const core::RoadmapPtr_t& roadmap);
+
+ /// Store weak pointer to itself
+ void init (const ManipulationPlannerWkPtr_t& weak);
private:
/// Configuration shooter
ConfigurationShooterPtr_t shooter_;
- /// The graph of constraints
- graph::GraphPtr_t constraintGraph_;
+ /// Pointer to the problem
+ const Problem& problem_;
/// weak pointer to itself
ManipulationPlannerWkPtr_t weakPtr_;
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 4612b0f..07f2cd9 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -18,6 +18,7 @@
#include <hpp/core/path-validation.hh>
#include "hpp/manipulation/robot.hh"
+#include "hpp/manipulation/problem.hh"
#include "hpp/manipulation/manipulation-planner.hh"
namespace hpp {
@@ -25,7 +26,13 @@ namespace hpp {
ManipulationPlannerPtr_t ManipulationPlanner::create (const core::Problem& problem,
const core::RoadmapPtr_t& roadmap)
{
- ManipulationPlanner* ptr = new ManipulationPlanner (problem, roadmap);
+ ManipulationPlanner* ptr;
+ try {
+ const Problem& p = dynamic_cast < const Problem& > (problem);
+ ptr = new ManipulationPlanner (p, roadmap);
+ } catch (std::exception&) {
+ throw std::invalid_argument ("The problem must be of type hpp::manipulation::Problem.");
+ }
ManipulationPlannerPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
@@ -107,10 +114,11 @@ namespace hpp {
const ConfigurationPtr_t& q_rand,
core::PathPtr_t& validPath)
{
+ graph::GraphPtr_t graph = problem_.constraintGraph ();
// Select next node in the constraint graph.
- graph::Nodes_t nodes = constraintGraph_->getNode (*q_near);
- graph::Edges_t edges = constraintGraph_->chooseEdge (nodes);
- ConstraintPtr_t constraint = constraintGraph_->configConstraint (edges, *q_near);
+ graph::Nodes_t nodes = graph->getNode (*q_near);
+ graph::Edges_t edges = graph->chooseEdge (nodes);
+ ConstraintPtr_t constraint = graph->configConstraint (edges, *q_near);
qProj_ = *q_rand;
if (!constraint->apply (qProj_))
return core::PathPtr_t();
@@ -122,10 +130,11 @@ namespace hpp {
return pathValidation->validate (path, false, validPath);
}
- ManipulationPlanner::ManipulationPlanner (const core::Problem& problem,
+ ManipulationPlanner::ManipulationPlanner (const Problem& problem,
const core::RoadmapPtr_t& roadmap) :
core::PathPlanner (problem, roadmap),
shooter_ (new core::BasicConfigurationShooter (problem.robot ())),
+ problem_ (problem),
qProj_ (problem.robot ()->configSize ())
{}
--
GitLab