diff --git a/include/hpp/manipulation/graph-steering-method.hh b/include/hpp/manipulation/graph-steering-method.hh
index eb5a132b3201e8fc7d05b9838749eb267f768596..22e8a6d206373e365db2a27b8cfa832055b8da19 100644
--- a/include/hpp/manipulation/graph-steering-method.hh
+++ b/include/hpp/manipulation/graph-steering-method.hh
@@ -20,12 +20,14 @@
# include <hpp/manipulation/config.hh>
+# include "hpp/core/problem-solver.hh" // SteeringMethodBuilder_t
# include <hpp/core/steering-method.hh>
# include <hpp/core/weighed-distance.hh>
# include <hpp/model/device.hh>
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/fwd.hh"
+# include "hpp/manipulation/problem.hh"
namespace hpp {
namespace manipulation {
@@ -36,61 +38,73 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI GraphSteeringMethod : public SteeringMethod
{
+ typedef core::SteeringMethodBuilder_t SteeringMethodBuilder_t;
+
public:
- /// Create instance and return shared pointer
- static GraphSteeringMethodPtr_t create (const model::DevicePtr_t& robot);
-
- /// 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 ());
- }
- /// \name Graph of constraints applicable to the robot.
- /// \{
-
- /// Set constraint graph
- void constraintGraph (const graph::GraphPtr_t& graph)
+ /// Create instance and return shared pointer
+ /// \warning core::ProblemPtr_t will be casted to ProblemPtr_t
+ static GraphSteeringMethodPtr_t create
+ (const core::ProblemPtr_t& problem);
+
+ template <typename T>
+ static GraphSteeringMethodPtr_t create
+ (const core::ProblemPtr_t& problem);
+
+ /// Create instance and return shared pointer
+ static GraphSteeringMethodPtr_t create (const ProblemPtr_t& 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
{
- graph_ = graph;
+ return createCopy (weak_.lock ());
}
- /// Get constraint graph
- const graph::GraphPtr_t& constraintGraph () const
+ const core::SteeringMethodPtr_t& innerSteeringMethod () const
{
- return graph_;
+ return steeringMethod_;
}
- /// \}
- const core::WeighedDistancePtr_t& distance () const;
+ void innerSteeringMethod (const core::SteeringMethodPtr_t& sm)
+ {
+ steeringMethod_ = sm;
+ }
protected:
/// Constructor
- GraphSteeringMethod (const model::DevicePtr_t& robot);
+ GraphSteeringMethod (const ProblemPtr_t& problem);
/// Copy constructor
GraphSteeringMethod (const GraphSteeringMethod&);
virtual PathPtr_t impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
- void init (GraphSteeringMethodWkPtr_t weak)
- {
- core::SteeringMethod::init (weak);
- weak_ = weak;
- }
+ void init (GraphSteeringMethodWkPtr_t weak)
+ {
+ core::SteeringMethod::init (weak);
+ weak_ = weak;
+ }
private:
- /// A pointer to the graph of constraint.
- graph::GraphPtr_t graph_;
- /// Pointer to the Robot.
- core::DeviceWkPtr_t robot_;
- /// Metric in configuration space.
- core::WeighedDistancePtr_t distance_;
- /// Weak pointer to itself
- GraphSteeringMethodWkPtr_t weak_;
+ /// A pointer to the problem
+ ProblemPtr_t problem_;
+ /// Weak pointer to itself
+ GraphSteeringMethodWkPtr_t weak_;
+ /// The encapsulated steering method
+ core::SteeringMethodPtr_t steeringMethod_;
};
+
+ template <typename T>
+ GraphSteeringMethodPtr_t GraphSteeringMethod::create
+ (const core::ProblemPtr_t& problem)
+ {
+ GraphSteeringMethodPtr_t gsm = GraphSteeringMethod::create (problem);
+ gsm->innerSteeringMethod (T::create (problem));
+ return gsm;
+ }
/// \}
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 65098c2a962a1e0794f9a3e2a36b4fd881aea62d..ace1836c37725c39a0bee90ea300397392f6d204 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -18,7 +18,7 @@
# define HPP_MANIPULATION_GRAPH_EDGE_HH
#include <hpp/core/constraint-set.hh>
-#include <hpp/core/weighed-distance.hh>
+#include <hpp/core/steering-method.hh>
#include <hpp/core/path.hh>
#include "hpp/manipulation/config.hh"
@@ -65,7 +65,6 @@ namespace hpp {
/// Create a new empty Edge.
static EdgePtr_t create
(const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod,
const GraphWkPtr_t& graph,
const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
@@ -74,7 +73,8 @@ namespace hpp {
virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
- virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2, const core::WeighedDistance& d) const;
+ virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1,
+ ConfigurationIn_t q2) const;
/// Get the destination
NodePtr_t to () const;
@@ -97,7 +97,7 @@ namespace hpp {
/// Get steering method associated to the edge.
const core::SteeringMethodPtr_t& steeringMethod () const
{
- return steeringMethod_;
+ return steeringMethod_->get();
}
/// Get direction of the path compare to the edge
@@ -126,8 +126,7 @@ namespace hpp {
const NodeWkPtr_t& to);
/// Constructor
- Edge (const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod);
+ Edge (const std::string& name);
/// Constraint to project a path.
/// \return The initialized constraint.
@@ -144,6 +143,7 @@ namespace hpp {
private:
typedef Cache < ConstraintSetPtr_t > Constraint_t;
+ typedef Cache < core::SteeringMethodPtr_t > SteeringMethod_t;
/// See pathConstraint member function.
Constraint_t* pathConstraints_;
@@ -159,7 +159,7 @@ namespace hpp {
bool isInNodeFrom_;
/// Steering method used to create paths associated to the edge
- core::SteeringMethodPtr_t steeringMethod_;
+ SteeringMethod_t* steeringMethod_;
/// Weak pointer to itself.
EdgeWkPtr_t wkPtr_;
@@ -199,13 +199,12 @@ namespace hpp {
/// Create a new WaypointEdge.
static WaypointEdgePtr_t create
(const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod,
const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
virtual bool direction (const core::PathPtr_t& path) const;
- virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2, const core::WeighedDistance& d) const;
+ virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2) const;
virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
@@ -229,9 +228,8 @@ namespace hpp {
NodePtr_t node () const;
protected:
- WaypointEdge (const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod) :
- Edge (name, steeringMethod)
+ WaypointEdge (const std::string& name) :
+ Edge (name)
{
}
/// Initialization of the object.
@@ -259,7 +257,6 @@ namespace hpp {
/// Create a new LevelSetEdge.
static LevelSetEdgePtr_t create
(const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod,
const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
@@ -287,8 +284,7 @@ namespace hpp {
void init (const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
- LevelSetEdge (const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod);
+ LevelSetEdge (const std::string& name);
/// Print the object in a stream.
virtual std::ostream& print (std::ostream& os) const;
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index 3fb85bbefd7ea7f602876d023b50da494c09e274..86f1d0b5bd125a3598bac57e05a7b43e20247adb 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -45,9 +45,9 @@ namespace hpp {
/// Create a new Graph.
///
/// \param robot a manipulation robot
- /// \param sm a steering method to create paths from edges
+ /// \param problem a pointer to the problem
static GraphPtr_t create(const std::string& name, DevicePtr_t robot,
- const core::SteeringMethodPtr_t& sm);
+ const ProblemPtr_t& problem);
/// Create and insert a NodeSelector inside the graph.
NodeSelectorPtr_t createNodeSelector (const std::string& name);
@@ -119,7 +119,7 @@ namespace hpp {
const DevicePtr_t& robot () const;
/// Get the steering Method
- const core::SteeringMethodPtr_t& steeringMethod () const;
+ const ProblemPtr_t& problem () const;
/// Print the component in DOT language.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
@@ -130,8 +130,8 @@ namespace hpp {
/// Constructor
/// \param sm a steering method to create paths from edges
- Graph (const std::string& name, const core::SteeringMethodPtr_t& sm) :
- GraphComponent (name), steeringMethod_ (sm)
+ Graph (const std::string& name, const ProblemPtr_t& problem) :
+ GraphComponent (name), problem_ (problem)
{}
/// Print the object in a stream.
@@ -160,7 +160,7 @@ namespace hpp {
typedef std::map < EdgePtr_t, ConstraintSetPtr_t > MapFromEdge;
typedef std::pair < EdgePtr_t, ConstraintSetPtr_t > PairEdgeConstraints;
MapFromEdge cfgConstraintSetMapFromEdge_, pathConstraintSetMapFromEdge_;
- core::SteeringMethodPtr_t steeringMethod_;
+ ProblemPtr_t problem_;
value_type errorThreshold_;
size_type maxIterations_;
}; // Class Graph
diff --git a/include/hpp/manipulation/graph/node.hh b/include/hpp/manipulation/graph/node.hh
index c59d0280724d528d9a04ff722290ef954691d5e5..fb78a4caca5d31ee7dfa0e82ea3b8d779cc6c3f0 100644
--- a/include/hpp/manipulation/graph/node.hh
+++ b/include/hpp/manipulation/graph/node.hh
@@ -45,7 +45,6 @@ namespace hpp {
public:
typedef boost::function < EdgePtr_t
(const std::string&,
- const core::SteeringMethodPtr_t&,
const GraphWkPtr_t&,
const NodeWkPtr_t&, const NodeWkPtr_t&) >
EdgeFactory;
diff --git a/include/hpp/manipulation/problem.hh b/include/hpp/manipulation/problem.hh
index 3860f3afbf672ee7df7ad61d2cb59a52cff7e065..ec6ec64fde4916112c0a23f1d4cf8735960ccd7b 100644
--- a/include/hpp/manipulation/problem.hh
+++ b/include/hpp/manipulation/problem.hh
@@ -19,11 +19,10 @@
# include <hpp/core/problem.hh>
+# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/device.hh"
# include "hpp/manipulation/graph/graph.hh"
# include "hpp/manipulation/graph-path-validation.hh"
-# include "hpp/manipulation/graph-steering-method.hh"
-# include "hpp/manipulation/fwd.hh"
namespace hpp {
namespace manipulation {
@@ -36,17 +35,12 @@ namespace hpp {
typedef core::Problem Parent;
/// Constructor
- Problem (DevicePtr_t robot) : Parent (robot),
- graph_(), steeringMethod_ (GraphSteeringMethod::create (robot))
- {
- Parent::steeringMethod (steeringMethod_);
- }
+ Problem (DevicePtr_t robot);
/// Set the graph of constraints
void constraintGraph (const graph::GraphPtr_t& graph)
{
graph_ = graph;
- steeringMethod_->constraintGraph (graph);
if (pathValidation ())
pathValidation ()->constraintGraph (graph);
}
@@ -70,22 +64,14 @@ namespace hpp {
}
/// Get the path validation as a GraphPathValidation
- GraphPathValidationPtr_t pathValidation () const
- {
- return HPP_DYNAMIC_PTR_CAST (GraphPathValidation, Parent::pathValidation());
- }
+ GraphPathValidationPtr_t pathValidation () const;
/// Get the steering method as a GraphSteeringMethod
- GraphSteeringMethodPtr_t steeringMethod () const
- {
- return steeringMethod_;
- }
+ GraphSteeringMethodPtr_t steeringMethod () const;
private:
/// The graph of constraints
graph::GraphPtr_t graph_;
- /// Steering method
- GraphSteeringMethodPtr_t steeringMethod_;
}; // class Problem
/// \}
} // namespace manipulation
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index b15985e242af674bee8d3e497671ee1edfee77fb..1e1d9043935929fe67cd3c123ed252d2db3e506f 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -28,6 +28,7 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED
constraint-set.cc
roadmap-node.cc
device.cc
+ problem.cc
graph-path-validation.cc
graph-steering-method.cc
graph-optimizer.cc
diff --git a/src/graph-steering-method.cc b/src/graph-steering-method.cc
index 10f18719b287d18044f12201de1ac1049fc10100..381d301a69b2866fcb128ae008dd4d994d546023 100644
--- a/src/graph-steering-method.cc
+++ b/src/graph-steering-method.cc
@@ -16,6 +16,9 @@
#include "hpp/manipulation/graph-steering-method.hh"
+#include <hpp/util/pointer.hh>
+
+#include <hpp/core/distance.hh>
#include <hpp/core/straight-path.hh>
#include "hpp/manipulation/graph/graph.hh"
@@ -24,9 +27,18 @@
namespace hpp {
namespace manipulation {
GraphSteeringMethodPtr_t GraphSteeringMethod::create
- (const model::DevicePtr_t& robot)
+ (const core::ProblemPtr_t& problem)
+ {
+ assert (dynamic_cast <const ProblemPtr_t> (problem) != NULL
+ && "Cast to const ProblemPtr_t failed");
+ const ProblemPtr_t& p = static_cast <const ProblemPtr_t> (problem);
+ return create (p);
+ }
+
+ GraphSteeringMethodPtr_t GraphSteeringMethod::create
+ (const ProblemPtr_t& problem)
{
- GraphSteeringMethod* ptr = new GraphSteeringMethod (robot);
+ GraphSteeringMethod* ptr = new GraphSteeringMethod (problem);
GraphSteeringMethodPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
@@ -41,40 +53,34 @@ namespace hpp {
return shPtr;
}
- GraphSteeringMethod::GraphSteeringMethod (const model::DevicePtr_t& robot) :
- SteeringMethod (), graph_ (), robot_ (robot),
- distance_ (core::WeighedDistance::create (robot)), weak_ ()
+ GraphSteeringMethod::GraphSteeringMethod (const ProblemPtr_t& problem) :
+ SteeringMethod (problem), problem_ (problem), weak_ ()
{
}
GraphSteeringMethod::GraphSteeringMethod (const GraphSteeringMethod& other):
- SteeringMethod (other), graph_ (other.graph_), robot_ (other.robot_),
- distance_ (other.distance_)
+ SteeringMethod (other), problem_ (other.problem_)
{
}
PathPtr_t GraphSteeringMethod::impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const
{
graph::Edges_t possibleEdges;
+ graph::Graph& graph = *problem_->constraintGraph ();
try {
- possibleEdges = graph_->getEdges (graph_->getNode (q1), graph_->getNode (q2));
+ possibleEdges = graph.getEdges (graph.getNode (q1), graph.getNode (q2));
} catch (const std::logic_error& e) {
hppDout (error, e.what ());
return PathPtr_t ();
}
PathPtr_t path;
while (!possibleEdges.empty()) {
- if (possibleEdges.back ()->build (path, q1, q2, *distance_)) {
+ if (possibleEdges.back ()->build (path, q1, q2)) {
return path;
}
possibleEdges.pop_back ();
}
return PathPtr_t ();
}
-
- const core::WeighedDistancePtr_t& GraphSteeringMethod::distance () const
- {
- return distance_;
- }
} // namespace manipulation
} // namespace hpp
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 84c6e4b874bd16bcb10e292b6d4ad53ef8ecf88a..402857b7158042c323a70e29d44c4d9288ce84f0 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -18,7 +18,6 @@
#include <sstream>
-#include <hpp/core/steering-method.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/constraints/differentiable-function.hh>
@@ -26,24 +25,26 @@
#include <hpp/util/pointer.hh>
#include "hpp/manipulation/device.hh"
+#include "hpp/manipulation/problem.hh"
+#include "hpp/manipulation/graph-steering-method.hh"
#include "hpp/manipulation/graph/statistics.hh"
#include "hpp/manipulation/constraint-set.hh"
namespace hpp {
namespace manipulation {
namespace graph {
- Edge::Edge (const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod) :
+ Edge::Edge (const std::string& name) :
GraphComponent (name), isShort_ (false),
pathConstraints_ (new Constraint_t()),
configConstraints_ (new Constraint_t()),
- steeringMethod_ (steeringMethod->copy ())
+ steeringMethod_ (new SteeringMethod_t())
{}
Edge::~Edge ()
{
if (pathConstraints_ ) delete pathConstraints_;
if (configConstraints_) delete configConstraints_;
+ if (steeringMethod_ ) delete steeringMethod_;
}
NodePtr_t Edge::to () const
@@ -151,11 +152,10 @@ namespace hpp {
}
EdgePtr_t Edge::create (const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod,
const GraphWkPtr_t& graph,
const NodeWkPtr_t& from, const NodeWkPtr_t& to)
{
- Edge* ptr = new Edge (name, steeringMethod);
+ Edge* ptr = new Edge (name);
EdgePtr_t shPtr (ptr);
ptr->init(shPtr, graph, from, to);
return shPtr;
@@ -226,7 +226,6 @@ namespace hpp {
if (!*pathConstraints_) {
ConstraintSetPtr_t pathConstraints (buildPathConstraint ());
pathConstraints_->set (pathConstraints);
- steeringMethod_->constraints (pathConstraints);
}
return pathConstraints_->get ();
}
@@ -249,11 +248,16 @@ namespace hpp {
node ()->insertLockedJoints (proj);
constraint->edge (wkPtr_.lock ());
+
+ // Build steering method
+ steeringMethod_->set(g->problem()->steeringMethod()
+ ->innerSteeringMethod()->copy());
+ steeringMethod_->get()->constraints (constraint);
return constraint;
}
bool Edge::build (core::PathPtr_t& path, ConfigurationIn_t q1,
- ConfigurationIn_t q2, const core::WeighedDistance&)
+ ConfigurationIn_t q2)
const
{
ConstraintSetPtr_t constraints = pathConstraint ();
@@ -261,7 +265,7 @@ namespace hpp {
if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
return false;
}
- path = (*steeringMethod_) (q1, q2);
+ path = (*steeringMethod_->get()) (q1, q2);
return true;
}
@@ -290,12 +294,11 @@ namespace hpp {
return false;
}
- WaypointEdgePtr_t WaypointEdge::create
- (const std::string& name, const core::SteeringMethodPtr_t& steeringMethod,
+ WaypointEdgePtr_t WaypointEdge::create (const std::string& name,
const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to)
{
- WaypointEdge* ptr = new WaypointEdge (name, steeringMethod);
+ WaypointEdge* ptr = new WaypointEdge (name);
WaypointEdgePtr_t shPtr (ptr);
ptr->init(shPtr, graph, from, to);
return shPtr;
@@ -308,7 +311,8 @@ namespace hpp {
wkPtr_ = weak;
}
- bool WaypointEdge::build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2, const core::WeighedDistance& d) const
+ bool WaypointEdge::build (core::PathPtr_t& path, ConfigurationIn_t q1,
+ ConfigurationIn_t q2) const
{
assert (waypoint_.first);
core::PathPtr_t pathToWaypoint;
@@ -317,7 +321,7 @@ namespace hpp {
if (!result_.isApprox (q2)) config_ = q2;
if (!waypoint_.first->applyConstraints (q1, config_))
return false;
- if (!waypoint_.first->build (pathToWaypoint, q1, config_, d))
+ if (!waypoint_.first->build (pathToWaypoint, q1, config_))
return false;
core::PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST (core::PathVector, pathToWaypoint);
if (!pv) {
@@ -329,7 +333,7 @@ namespace hpp {
path = pv;
core::PathPtr_t end;
- if (!Edge::build (end, config_, q2, d))
+ if (!Edge::build (end, config_, q2))
return false;
pv->appendPath (end);
return true;
@@ -358,12 +362,12 @@ namespace hpp {
ss.str (std::string ()); ss.clear ();
ss << bname << "_e" << d;
if (d == 0) {
- edge = Edge::create (ss.str (), steeringMethod (), graph_, from (),
+ edge = Edge::create (ss.str (), graph_, from (),
node);
edge->isInNodeFrom (isInNodeFrom ());
} else {
WaypointEdgePtr_t we = WaypointEdge::create
- (ss.str (), steeringMethod (), graph_, from (), node);
+ (ss.str (), graph_, from (), node);
we->createWaypoint (d-1, bname);
edge = we;
edge->isInNodeFrom (isInNodeFrom ());
@@ -499,11 +503,10 @@ namespace hpp {
}
LevelSetEdgePtr_t LevelSetEdge::create
- (const std::string& name, const core::SteeringMethodPtr_t& steeringMethod,
- const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
- const NodeWkPtr_t& to)
+ (const std::string& name, const GraphWkPtr_t& graph,
+ const NodeWkPtr_t& from, const NodeWkPtr_t& to)
{
- LevelSetEdge* ptr = new LevelSetEdge (name, steeringMethod);
+ LevelSetEdge* ptr = new LevelSetEdge (name);
LevelSetEdgePtr_t shPtr (ptr);
ptr->init(shPtr, graph, from, to);
return shPtr;
@@ -592,9 +595,8 @@ namespace hpp {
}
LevelSetEdge::LevelSetEdge
- (const std::string& name,
- const core::SteeringMethodPtr_t& steeringMethod) :
- Edge (name, steeringMethod), extraConstraints_ (new Constraint_t())
+ (const std::string& name) :
+ Edge (name), extraConstraints_ (new Constraint_t())
{
}
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index db6fa6e16276bab2478bd20ab2c8fa71bf2debcd..fc8385ec35615090cfc8401ac919bf15f6919e5b 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -27,9 +27,9 @@ namespace hpp {
namespace manipulation {
namespace graph {
GraphPtr_t Graph::create(const std::string& name, DevicePtr_t robot,
- const core::SteeringMethodPtr_t& sm)
+ const ProblemPtr_t& problem)
{
- Graph* ptr = new Graph (name, sm);
+ Graph* ptr = new Graph (name, problem);
GraphPtr_t shPtr (ptr);
ptr->init (shPtr, robot);
return shPtr;
@@ -80,9 +80,9 @@ namespace hpp {
return robot_;
}
- const core::SteeringMethodPtr_t& Graph::steeringMethod () const
+ const ProblemPtr_t& Graph::problem () const
{
- return steeringMethod_;
+ return problem_;
}
NodePtr_t Graph::getNode (ConfigurationIn_t config) const
diff --git a/src/graph/node.cc b/src/graph/node.cc
index 6a1e3d855fe0b267351aa6a7d651d73d5adb1d37..598a43a755e9cf293eb8a25e79493a44ac5dbce2 100644
--- a/src/graph/node.cc
+++ b/src/graph/node.cc
@@ -53,8 +53,7 @@ namespace hpp {
const Weight_t& w, const bool& isInNodeFrom,
EdgeFactory create)
{
- EdgePtr_t newEdge = create(name, graph_.lock ()->steeringMethod (),
- graph_, wkPtr_, to);
+ EdgePtr_t newEdge = create(name, graph_, wkPtr_, to);
neighbors_.insert (newEdge, w);
newEdge->isInNodeFrom (isInNodeFrom);
return newEdge;
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index cdd1dfe0a38376a321816fcdb9c5d081f84e94bb..82daeb5b610eb696f53520f218ab86649e2c20a5 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -32,6 +32,7 @@
#include "hpp/manipulation/problem.hh"
#include "hpp/manipulation/roadmap.hh"
#include "hpp/manipulation/roadmap-node.hh"
+#include "hpp/manipulation/graph-steering-method.hh"
#include "hpp/manipulation/graph/edge.hh"
#include "hpp/manipulation/graph/node-selector.hh"
@@ -188,7 +189,7 @@ namespace hpp {
GraphSteeringMethodPtr_t sm = problem_.steeringMethod();
core::PathPtr_t path;
HPP_START_TIMECOUNTER (buildPath);
- if (!edge->build (path, *q_near, qProj_, *(sm->distance ()))) {
+ if (!edge->build (path, *q_near, qProj_)) {
HPP_STOP_TIMECOUNTER (buildPath);
addFailure (STEERING_METHOD, edge);
return false;
diff --git a/src/path-optimization/config-optimization.cc b/src/path-optimization/config-optimization.cc
index 74f026602c5e1926d2058dc3c2aa5fc6181df90e..ffa166338f9798c34c9ec8db5060efc20a2e417d 100644
--- a/src/path-optimization/config-optimization.cc
+++ b/src/path-optimization/config-optimization.cc
@@ -43,7 +43,7 @@ namespace hpp {
/// path->end(). In that case, path->end () should be in edge->node().
/// (obviously, in this case, we have edge->isInNodeFrom_ = true)
const bool reverseB = setB->edge ()->direction(before);
- const bool reverseA = setA->edge ()->direction(after);
+ //const bool reverseA = setA->edge ()->direction(after);
reverse = reverseB;
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 9a9ed0de25122f3967e886856cdee4211c65adad..8772e0097366fa19ecd4aa361f208f7fb5b7365f 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -17,6 +17,8 @@
#include "hpp/manipulation/problem-solver.hh"
+#include <boost/bind.hpp>
+
#include <hpp/util/pointer.hh>
#include <hpp/util/debug.hh>
@@ -29,17 +31,19 @@
#include <hpp/core/continuous-collision-checking/progressive.hh>
#include <hpp/core/path-optimization/partial-shortcut.hh>
#include <hpp/core/roadmap.hh>
+#include <hpp/core/steering-method-straight.hh>
+#include "hpp/manipulation/problem.hh"
#include "hpp/manipulation/device.hh"
#include "hpp/manipulation/handle.hh"
#include "hpp/manipulation/graph/graph.hh"
#include "hpp/manipulation/manipulation-planner.hh"
-#include "hpp/manipulation/problem.hh"
#include "hpp/manipulation/roadmap.hh"
#include "hpp/manipulation/constraint-set.hh"
#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"
namespace hpp {
@@ -90,8 +94,13 @@ namespace hpp {
GraphConfigOptimizationTraits
<pathOptimization::ConfigOptimizationTraits>
>);
+ using core::SteeringMethodBuilder_t;
+ add <SteeringMethodBuilder_t> ("Graph-SteeringMethodStraight",
+ GraphSteeringMethod::create <core::SteeringMethodStraight>);
+
pathPlannerType ("M-RRT");
pathValidationType ("Graph-Discretized", 0.05);
+ steeringMethodType ("Graph-SteeringMethodStraight");
}
ProblemSolverPtr_t ProblemSolver::create ()
diff --git a/src/problem.cc b/src/problem.cc
new file mode 100644
index 0000000000000000000000000000000000000000..ba00d23c82ec18675462efd4d674517478062f0a
--- /dev/null
+++ b/src/problem.cc
@@ -0,0 +1,40 @@
+// Copyright (c) 2015, Joseph Mirabel
+// 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/problem.hh>
+#include <hpp/manipulation/graph-steering-method.hh>
+
+namespace hpp {
+ namespace manipulation {
+ Problem::Problem (DevicePtr_t robot)
+ : Parent (robot), graph_()
+ {
+ Parent::steeringMethod (GraphSteeringMethod::create (this));
+ }
+
+ GraphPathValidationPtr_t Problem::pathValidation () const
+ {
+ return HPP_DYNAMIC_PTR_CAST (GraphPathValidation,
+ Parent::pathValidation());
+ }
+
+ GraphSteeringMethodPtr_t Problem::steeringMethod () const
+ {
+ return HPP_DYNAMIC_PTR_CAST (GraphSteeringMethod,
+ Parent::steeringMethod());
+ }
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/roadmap.cc b/src/roadmap.cc
index 0458686bd953e7ef02c86ea489621822eb846cc8..2bc24768495ea70b96d3d9ed08ad8252891aaea8 100644
--- a/src/roadmap.cc
+++ b/src/roadmap.cc
@@ -17,6 +17,7 @@
#include "hpp/manipulation/roadmap.hh"
#include <hpp/util/pointer.hh>
+#include <hpp/core/distance.hh>
#include <hpp/core/connected-component.hh>
#include <hpp/manipulation/roadmap-node.hh>