diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index cfa463221fc7df00b571caf229714d856f2bb006..9bb19e3dff07f8f5bb0f7e2bc646c584946fa4ee 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -63,10 +63,12 @@ namespace hpp {
~Edge ();
/// Create a new empty Edge.
- static EdgePtr_t create (const std::string& name,
- const GraphWkPtr_t& graph,
- const NodeWkPtr_t& from,
- const NodeWkPtr_t& to);
+ 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);
virtual bool applyConstraints (core::NodePtr_t nnear, ConfigurationOut_t q) const;
@@ -92,7 +94,11 @@ namespace hpp {
{
return isInNodeFrom_;
}
-
+ /// Get steering method associated to the edge.
+ const core::SteeringMethodPtr_t& steeringMethod () const
+ {
+ return steeringMethod_;
+ }
/// Print the object in a stream.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
@@ -102,7 +108,8 @@ namespace hpp {
const NodeWkPtr_t& to);
/// Constructor
- Edge (const std::string& name);
+ Edge (const std::string& name,
+ const core::SteeringMethodPtr_t& steeringMethod);
/// Constraint to project onto the same leaf as config.
/// \return The initialized projector.
@@ -135,6 +142,9 @@ namespace hpp {
/// True if this path is in node from, False if in node to
bool isInNodeFrom_;
+ /// Steering method used to create paths associated to the edge
+ core::SteeringMethodPtr_t steeringMethod_;
+
/// Weak pointer to itself.
EdgeWkPtr_t wkPtr_;
@@ -171,10 +181,11 @@ namespace hpp {
{
public:
/// Create a new WaypointEdge.
- static WaypointEdgePtr_t create (const std::string& name,
- const GraphWkPtr_t& graph,
- const NodeWkPtr_t& from,
- const NodeWkPtr_t& to);
+ 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 build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2, const core::WeighedDistance& d) const;
@@ -200,7 +211,9 @@ namespace hpp {
NodePtr_t node () const;
protected:
- WaypointEdge (const std::string& name) : Edge (name)
+ WaypointEdge (const std::string& name,
+ const core::SteeringMethodPtr_t& steeringMethod) :
+ Edge (name, steeringMethod)
{
}
/// Initialization of the object.
@@ -224,10 +237,11 @@ namespace hpp {
~LevelSetEdge ();
/// Create a new LevelSetEdge.
- static LevelSetEdgePtr_t create (const std::string& name,
- const GraphWkPtr_t& graph,
- const NodeWkPtr_t& from,
- const NodeWkPtr_t& to);
+ 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);
virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
@@ -252,7 +266,8 @@ namespace hpp {
void init (const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
const NodeWkPtr_t& to);
- LevelSetEdge (const std::string& name);
+ LevelSetEdge (const std::string& name,
+ const core::SteeringMethodPtr_t& steeringMethod);
/// 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 cb09de29bff5d00ddf6916fe5c68b86a794b81ff..459372b1492ebc54ad17ec977c630a2ec4964d31 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -43,7 +43,11 @@ namespace hpp {
{
public:
/// Create a new Graph.
- static GraphPtr_t create(const std::string& name, DevicePtr_t robot);
+ ///
+ /// \param robot a manipulation robot
+ /// \param sm a steering method to create paths from edges
+ static GraphPtr_t create(const std::string& name, DevicePtr_t robot,
+ const core::SteeringMethodPtr_t& sm);
/// Create and insert a NodeSelector inside the graph.
NodeSelectorPtr_t createNodeSelector (const std::string& name);
@@ -99,6 +103,9 @@ namespace hpp {
/// Get the robot.
const DevicePtr_t& robot () const;
+ /// Get the steering Method
+ const core::SteeringMethodPtr_t& steeringMethod () const;
+
/// Print the component in DOT language.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
@@ -107,7 +114,9 @@ namespace hpp {
void init (const GraphWkPtr_t& weak, DevicePtr_t robot);
/// Constructor
- Graph (const std::string& name) : GraphComponent (name)
+ /// \param sm a steering method to create paths from edges
+ Graph (const std::string& name, const core::SteeringMethodPtr_t& sm) :
+ GraphComponent (name), steeringMethod_ (sm)
{}
/// Print the object in a stream.
@@ -136,7 +145,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_;
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 4213e6a9d68b5ac308549130e1b301969c6dda58..c59d0280724d528d9a04ff722290ef954691d5e5 100644
--- a/include/hpp/manipulation/graph/node.hh
+++ b/include/hpp/manipulation/graph/node.hh
@@ -44,7 +44,9 @@ namespace hpp {
{
public:
typedef boost::function < EdgePtr_t
- (const std::string&, const GraphWkPtr_t&,
+ (const std::string&,
+ const core::SteeringMethodPtr_t&,
+ const GraphWkPtr_t&,
const NodeWkPtr_t&, const NodeWkPtr_t&) >
EdgeFactory;
/// Destructor
diff --git a/include/hpp/manipulation/problem.hh b/include/hpp/manipulation/problem.hh
index 54610fed0f8f910cab927d14d92509d572961872..3860f3afbf672ee7df7ad61d2cb59a52cff7e065 100644
--- a/include/hpp/manipulation/problem.hh
+++ b/include/hpp/manipulation/problem.hh
@@ -37,7 +37,7 @@ namespace hpp {
/// Constructor
Problem (DevicePtr_t robot) : Parent (robot),
- graph_(), steeringMethod_ (new GraphSteeringMethod (robot))
+ graph_(), steeringMethod_ (GraphSteeringMethod::create (robot))
{
Parent::steeringMethod (steeringMethod_);
}
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index e1e1fb923439c284dc2f737e375333b4f69f617f..e8e22e767f4fee63f46942ec5b570e4c1109757a 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -18,7 +18,7 @@
#include <sstream>
-#include <hpp/core/straight-path.hh>
+#include <hpp/core/steering-method.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/constraints/differentiable-function.hh>
@@ -31,9 +31,11 @@
namespace hpp {
namespace manipulation {
namespace graph {
- Edge::Edge (const std::string& name) :
+ Edge::Edge (const std::string& name,
+ const core::SteeringMethodPtr_t& steeringMethod) :
GraphComponent (name), pathConstraints_ (new Constraint_t()),
- configConstraints_ (new Constraint_t())
+ configConstraints_ (new Constraint_t()),
+ steeringMethod_ (steeringMethod->copy ())
{}
Edge::~Edge ()
@@ -59,10 +61,11 @@ 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);
+ Edge* ptr = new Edge (name, steeringMethod);
EdgePtr_t shPtr (ptr);
ptr->init(shPtr, graph, from, to);
return shPtr;
@@ -129,7 +132,9 @@ namespace hpp {
ConstraintSetPtr_t Edge::pathConstraint() const
{
if (!*pathConstraints_) {
- pathConstraints_->set (buildPathConstraint ());
+ ConstraintSetPtr_t pathConstraints (buildPathConstraint ());
+ pathConstraints_->set (pathConstraints);
+ steeringMethod_->constraints (pathConstraints);
}
return pathConstraints_->get ();
}
@@ -153,15 +158,16 @@ namespace hpp {
return constraint;
}
- bool Edge::build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2, const core::WeighedDistance& d) const
+ bool Edge::build (core::PathPtr_t& path, ConfigurationIn_t q1,
+ ConfigurationIn_t q2, const core::WeighedDistance& d)
+ const
{
ConstraintSetPtr_t constraints = pathConstraint ();
constraints->configProjector ()->rightHandSideFromConfig(q1);
if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
return false;
}
- path = core::StraightPath::create (graph_.lock ()->robot (), q1, q2,
- d (q1, q2), constraints);
+ path = (*steeringMethod_) (q1, q2);
return true;
}
@@ -192,10 +198,11 @@ namespace hpp {
}
WaypointEdgePtr_t WaypointEdge::create
- (const std::string& name, const GraphWkPtr_t& graph,
- const NodeWkPtr_t& from, const NodeWkPtr_t& to)
+ (const std::string& name, const core::SteeringMethodPtr_t& steeringMethod,
+ const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
+ const NodeWkPtr_t& to)
{
- WaypointEdge* ptr = new WaypointEdge (name);
+ WaypointEdge* ptr = new WaypointEdge (name, steeringMethod);
WaypointEdgePtr_t shPtr (ptr);
ptr->init(shPtr, graph, from, to);
return shPtr;
@@ -255,11 +262,12 @@ namespace hpp {
ss.str (std::string ()); ss.clear ();
ss << bname << "_e" << d;
if (d == 0) {
- edge = Edge::create (ss.str (), graph_, from (), node);
+ edge = Edge::create (ss.str (), steeringMethod (), graph_, from (),
+ node);
edge->isInNodeFrom (isInNodeFrom ());
} else {
- WaypointEdgePtr_t we = WaypointEdge::create (ss.str (), graph_,
- from (), node);
+ WaypointEdgePtr_t we = WaypointEdge::create
+ (ss.str (), steeringMethod (), graph_, from (), node);
we->createWaypoint (d-1, bname);
edge = we;
edge->isInNodeFrom (isInNodeFrom ());
@@ -393,10 +401,11 @@ namespace hpp {
}
LevelSetEdgePtr_t LevelSetEdge::create
- (const std::string& name, const GraphWkPtr_t& graph,
- const NodeWkPtr_t& from, const NodeWkPtr_t& to)
+ (const std::string& name, const core::SteeringMethodPtr_t& steeringMethod,
+ const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
+ const NodeWkPtr_t& to)
{
- LevelSetEdge* ptr = new LevelSetEdge (name);
+ LevelSetEdge* ptr = new LevelSetEdge (name, steeringMethod);
LevelSetEdgePtr_t shPtr (ptr);
ptr->init(shPtr, graph, from, to);
return shPtr;
@@ -481,8 +490,10 @@ namespace hpp {
extraLockedJoints_.push_back (lockedJoint);
}
- LevelSetEdge::LevelSetEdge (const std::string& name) :
- Edge (name), extraConstraints_ (new Constraint_t())
+ LevelSetEdge::LevelSetEdge
+ (const std::string& name,
+ const core::SteeringMethodPtr_t& steeringMethod) :
+ Edge (name, steeringMethod), extraConstraints_ (new Constraint_t())
{
}
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index 927b89e706dc5a0efea865ed974b577d687e7bca..efa980360c337f6823a030c77a0d4f2ed112913d 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -25,9 +25,10 @@
namespace hpp {
namespace manipulation {
namespace graph {
- GraphPtr_t Graph::create(const std::string& name, DevicePtr_t robot)
+ GraphPtr_t Graph::create(const std::string& name, DevicePtr_t robot,
+ const core::SteeringMethodPtr_t& sm)
{
- Graph* ptr = new Graph (name);
+ Graph* ptr = new Graph (name, sm);
GraphPtr_t shPtr (ptr);
ptr->init (shPtr, robot);
return shPtr;
@@ -72,6 +73,11 @@ namespace hpp {
return robot_;
}
+ const core::SteeringMethodPtr_t& Graph::steeringMethod () const
+ {
+ return steeringMethod_;
+ }
+
NodePtr_t Graph::getNode (ConfigurationIn_t config) const
{
return nodeSelector_->getNode (config);
diff --git a/src/graph/node.cc b/src/graph/node.cc
index 37be2aa18ef35359a462700dd38eab85df41aa9b..9c9f90847299130a1cbd41a196a3e8519e65c806 100644
--- a/src/graph/node.cc
+++ b/src/graph/node.cc
@@ -52,7 +52,8 @@ namespace hpp {
const Weight_t& w, const bool& isInNodeFrom,
EdgeFactory create)
{
- EdgePtr_t newEdge = create(name, graph_, wkPtr_, to);
+ EdgePtr_t newEdge = create(name, graph_.lock ()->steeringMethod (),
+ graph_, wkPtr_, to);
neighbors_.insert (newEdge, w);
newEdge->isInNodeFrom (isInNodeFrom);
return newEdge;
diff --git a/tests/path-projection.cc b/tests/path-projection.cc
index 4c54abecfff2b7bcd411d974949fc4d396a932bd..095db5d9993963b31000820378e4e4f422afb55c 100644
--- a/tests/path-projection.cc
+++ b/tests/path-projection.cc
@@ -15,6 +15,7 @@
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
#include <hpp/core/path-projector/progressive.hh>
+#include <hpp/core/steering-method-straight.hh>
#include <hpp/core/numerical-constraint.hh>
#define ARM_LENGTH 1
@@ -61,6 +62,8 @@ using hpp::core::Path;
using hpp::core::PathPtr_t;
using hpp::core::WeighedDistance;
using hpp::core::WeighedDistancePtr_t;
+using hpp::core::SteeringMethodStraight;
+using hpp::core::SteeringMethodPtr_t;
using hpp::core::ConstraintSet;
using hpp::core::ConstraintSetPtr_t;
using hpp::core::ConfigProjector;
@@ -175,8 +178,9 @@ int main (int , char**) {
proj->add (NumericalConstraint::create (c));
cs->addConstraint (proj);
WeighedDistancePtr_t dist = WeighedDistance::create (r, list_of (1)(1));
+ SteeringMethodPtr_t sm (SteeringMethodStraight::create (r, dist));
const WeighedDistance& d = *dist;
- ProgressivePtr_t pp_ptr = Progressive::create (dist, 0.1);
+ ProgressivePtr_t pp_ptr = Progressive::create (dist, sm, 0.1);
Progressive pp = *pp_ptr;
Configuration_t qinit (2), qgoal (2);
diff --git a/tests/test-constraintgraph.cc b/tests/test-constraintgraph.cc
index 3f8e5c856d03f99d7572fab9bf52cd49f957f1ea..24440dc4e7435d405e66c95234cc3a1a9753c6e7 100644
--- a/tests/test-constraintgraph.cc
+++ b/tests/test-constraintgraph.cc
@@ -17,6 +17,8 @@
#include <hpp/util/pointer.hh>
#include <hpp/model/urdf/util.hh>
+#include <hpp/core/steering-method-straight.hh>
+
#include <hpp/constraints/position.hh>
#include <hpp/constraints/relative-com.hh>
@@ -32,6 +34,8 @@
using namespace ::hpp::manipulation;
using namespace ::hpp::manipulation::graph;
+using hpp::core::SteeringMethodStraight;
+using hpp::core::SteeringMethodPtr_t;
typedef std::vector <GraphComponentPtr_t> GraphComponents;
@@ -62,7 +66,8 @@ namespace hpp_test {
} else {
robot = Device::create ("test-robot");
}
- graph_ = Graph::create ("manpulation-graph", robot);
+ SteeringMethodPtr_t sm (SteeringMethodStraight::create (robot));
+ graph_ = Graph::create ("manpulation-graph", robot, sm);
components.push_back(graph_);
graph_->maxIterations (20);
graph_->errorThreshold (1e-4);