From e15cb09ae9636a4a1cc758f5aea4fbf68f691547 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Fri, 24 Apr 2015 16:46:04 +0200
Subject: [PATCH] Add steering method member in Graph and Edge classes.
The steering method of the graph is copied in each edge.
The steering method is used to create path related to an edge of the
manipulation graph.
In the current version, a SteeringMethodStraight instance is created by the
ProblemSolver, but in the future, this will be parameterizable.
---
include/hpp/manipulation/graph/edge.hh | 47 ++++++++++++++++--------
include/hpp/manipulation/graph/graph.hh | 15 ++++++--
include/hpp/manipulation/graph/node.hh | 4 +-
include/hpp/manipulation/problem.hh | 2 +-
src/graph/edge.cc | 49 +++++++++++++++----------
src/graph/graph.cc | 10 ++++-
src/graph/node.cc | 3 +-
tests/path-projection.cc | 6 ++-
tests/test-constraintgraph.cc | 7 +++-
9 files changed, 98 insertions(+), 45 deletions(-)
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index cfa46322..9bb19e3d 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 cb09de29..459372b1 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 4213e6a9..c59d0280 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 54610fed..3860f3af 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 e1e1fb92..e8e22e76 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 927b89e7..efa98036 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 37be2aa1..9c9f9084 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 4c54abec..095db5d9 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 3f8e5c85..24440dc4 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);
--
GitLab