From 893645bef070104e4e66b05b1917fde5d452ed15 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 19 Sep 2014 15:56:20 +0200
Subject: [PATCH] Graph can only have one NodeSelector.
---
include/hpp/manipulation/graph/graph.hh | 50 ++---
include/hpp/manipulation/graph/statistics.hh | 27 +--
.../hpp/manipulation/manipulation-planner.hh | 10 +-
src/graph-path-validation.cc | 12 +-
src/graph-steering-method.cc | 4 +-
src/graph/edge.cc | 6 +-
src/graph/graph.cc | 197 ++++--------------
src/graph/node.cc | 6 +-
src/manipulation-planner.cc | 33 ++-
tests/test-constraintgraph.cc | 106 ++++------
10 files changed, 129 insertions(+), 322 deletions(-)
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index b178033..e55b92a 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -42,45 +42,31 @@ namespace hpp {
static GraphPtr_t create(RobotPtr_t robot);
/// Create and insert a NodeSelector inside the graph.
- NodeSelectorPtr_t createNodeSelector();
+ NodeSelectorPtr_t createNodeSelector ();
/// Returns the states of a configuration.
- Nodes_t getNode(const Configuration_t config) const;
+ NodePtr_t getNode (const Configuration_t config) const;
/// Get possible edges between two nodes.
- std::vector <Edges_t> getEdge(const Nodes_t& from, const Nodes_t& to) const;
+ Edges_t getEdges (const NodePtr_t& from, const NodePtr_t& to) const;
- /// Select randomly outgoing edges of the given nodes.
- Edges_t chooseEdge(const Nodes_t& node) const;
+ /// Select randomly outgoing edge of the given node.
+ EdgePtr_t chooseEdge(const NodePtr_t& node) const;
- /// Constraint to project onto the Nodes_t.
- /// \param the Nodes_t on which to project.
+ /// Constraint to project onto the Node.
+ /// \param the Node_t on which to project.
/// \return The initialized projector.
- ConstraintSetPtr_t configConstraint (const Nodes_t& nodes);
-
- /// Kept for compatibility. Do not use. Use configConstraint(const Edges_t&) instead
- /// \param config Configuration that will initialize the projector.
- /// \return The initialized constraint.
- ConstraintSetPtr_t configConstraint (const Edges_t& edges, ConfigurationIn_t config) __attribute__ ((deprecated));
+ ConstraintSetPtr_t configConstraint (const NodePtr_t& node);
/// Constraint to project onto the same leaf as config.
/// \param edges a list of edges defining the foliation.
/// \return The constraint.
- ConstraintSetPtr_t configConstraint (const Edges_t& edges);
+ ConstraintSetPtr_t configConstraint (const EdgePtr_t& edge);
/// Constraint to project a path.
- /// \param edges a list of edges defining the foliation.
+ /// \param edge a list of edges defining the foliation.
/// \return The constraint.
- ConstraintSetPtr_t pathConstraint (const Edges_t& edges);
-
- /// Kept for compatibility. Do not use. Use pathConstraint(const Edges_t&) instead
- /// \param config Configuration that will initialize the constraint.
- /// \return The initialized constraint.
- ConstraintSetPtr_t pathConstraint (const Edges_t& edges, ConfigurationIn_t config) __attribute__ ((deprecated));
-
- /// Return the NodeSelector with the given name if any,
- /// NULL pointer if not found.
- NodeSelectorPtr_t getNodeSelectorByName (const std::string& name);
+ ConstraintSetPtr_t pathConstraint (const EdgePtr_t& edge);
/// Print the object in a stream.
std::ostream& print (std::ostream& os) const;
@@ -110,7 +96,7 @@ namespace hpp {
private:
/// This list contains a node selector for each end-effector.
- NodeSelectors_t nodeSelectors_;
+ NodeSelectorPtr_t nodeSelector_;
/// A set of constraints that will always be used, for example
/// stability constraints.
@@ -122,14 +108,14 @@ namespace hpp {
/// Weak pointer to itself.
GraphWkPtr_t wkPtr_;
- /// Map of constraint sets (from Nodes).
- typedef std::map < Nodes_t, ConstraintSetPtr_t > MapFromNode;
- typedef std::pair < Nodes_t, ConstraintSetPtr_t > PairNodesConstraints;
+ /// Map of constraint sets (from Node).
+ typedef std::map < NodePtr_t, ConstraintSetPtr_t > MapFromNode;
+ typedef std::pair < NodePtr_t, ConstraintSetPtr_t > PairNodeConstraints;
MapFromNode constraintSetMapFromNode_;
- /// Map of constraint sets (from Edges).
- typedef std::map < Edges_t, ConstraintSetPtr_t > MapFromEdge;
- typedef std::pair < Edges_t, ConstraintSetPtr_t > PairEdgesConstraints;
+ /// Map of constraint sets (from Edge).
+ typedef std::map < EdgePtr_t, ConstraintSetPtr_t > MapFromEdge;
+ typedef std::pair < EdgePtr_t, ConstraintSetPtr_t > PairEdgeConstraints;
MapFromEdge cfgConstraintSetMapFromEdge_, pathConstraintSetMapFromEdge_;
value_type errorThreshold_;
diff --git a/include/hpp/manipulation/graph/statistics.hh b/include/hpp/manipulation/graph/statistics.hh
index ee22ed9..e92fa69 100644
--- a/include/hpp/manipulation/graph/statistics.hh
+++ b/include/hpp/manipulation/graph/statistics.hh
@@ -116,32 +116,23 @@ namespace hpp {
{
public :
typedef ::hpp::statistics::Bin Parent;
- NodeBin(const Nodes_t& ns): nodes_(ns), roadmapNodes_() {}
+ NodeBin(const NodePtr_t& n): node_(n), roadmapNodes_() {}
void push_back(const core::NodePtr_t& n) {
roadmapNodes_.push_back(n);
}
bool operator<(const NodeBin& rhs) const {
- Nodes_t::const_iterator it1,
- it2 = rhs.nodes ().begin();
- for (it1 = nodes_.begin(); it1 != nodes_.end(); it1++) {
- if ((*it1)->id () < (*it2)->id())
- return true;
- if ((*it1)->id () > (*it2)->id())
- return false;
- it2++;
- }
- return false;
+ return node_->id () < rhs.node ()->id ();
}
bool operator==(const NodeBin& rhs) const {
- return nodes_ == rhs.nodes ();
+ return node_ == rhs.node ();
}
- const Nodes_t& nodes () const
+ const NodePtr_t& node () const
{
- return nodes_;
+ return node_;
}
std::ostream& print (std::ostream& os) const
@@ -173,18 +164,14 @@ namespace hpp {
}
private:
- Nodes_t nodes_;
+ NodePtr_t node_;
typedef std::list <core::NodePtr_t> RoadmapNodes_t;
RoadmapNodes_t roadmapNodes_;
std::ostream& printValue (std::ostream& os) const
{
- os << "NodeBin (";
- Nodes_t::const_iterator it1;
- for (it1 = nodes_.begin(); it1 != nodes_.end(); it1++)
- os << (*it1)->name () << " / ";
- return os << ")";
+ return os << "NodeBin (" << node_->name () << ")";
}
};
diff --git a/include/hpp/manipulation/manipulation-planner.hh b/include/hpp/manipulation/manipulation-planner.hh
index 48e213a..f6b6aff 100644
--- a/include/hpp/manipulation/manipulation-planner.hh
+++ b/include/hpp/manipulation/manipulation-planner.hh
@@ -81,7 +81,7 @@ namespace hpp {
typedef ::hpp::statistics::SuccessBin SuccessBin;
SuccessStatistics extendStatistics_;
- /// A Reason is associated to each Edges_t that generated a failure.
+ /// A Reason is associated to each EdgePtr_t that generated a failure.
enum TypeOfFailure {
PROJECTION,
STEERING_METHOD,
@@ -106,11 +106,11 @@ namespace hpp {
return ::hpp::statistics::SuccessBin::REASON_UNKNOWN;
}
};
- typedef std::pair < graph::Edges_t, Reasons > EdgesReasonPair;
- typedef std::map < graph::Edges_t, Reasons > EdgesReasonMap;
- EdgesReasonMap failureReasons_;
+ typedef std::pair < graph::EdgePtr_t, Reasons > EdgeReasonPair;
+ typedef std::map < graph::EdgePtr_t, Reasons > EdgeReasonMap;
+ EdgeReasonMap failureReasons_;
- void addFailure (TypeOfFailure t, const graph::Edges_t& edges);
+ void addFailure (TypeOfFailure t, const graph::EdgePtr_t& edge);
mutable Configuration_t qProj_;
};
diff --git a/src/graph-path-validation.cc b/src/graph-path-validation.cc
index 6aa09cf..6b92f36 100644
--- a/src/graph-path-validation.cc
+++ b/src/graph-path-validation.cc
@@ -91,13 +91,13 @@ namespace hpp {
newTmax = pathNoCollision->timeRange ().second,
oldTmin = path->timeRange ().first,
oldTmax = path->timeRange ().second;
- graph::Nodes_t origNodes, destNodes;
+ graph::NodePtr_t origNode, destNode;
try {
- origNodes = constraintGraph_->getNode (newPath (newTmin));
- destNodes = constraintGraph_->getNode (newPath (newTmax));
+ origNode = constraintGraph_->getNode (newPath (newTmin));
+ destNode = constraintGraph_->getNode (newPath (newTmax));
- if (origNodes == constraintGraph_->getNode ((*path) (oldTmin))
- && destNodes == constraintGraph_->getNode ((*path) (oldTmax))) {
+ if (origNode == constraintGraph_->getNode ((*path) (oldTmin))
+ && destNode == constraintGraph_->getNode ((*path) (oldTmax))) {
validPart = pathNoCollision;
return false;
}
@@ -113,7 +113,7 @@ namespace hpp {
// - Use the steering method to create a new path and validate it.
// - Return a null path.
assert (!reverse && "This has never been tested with reverse path");
- std::vector <graph::Edges_t> possibleEdges (constraintGraph_->getEdge (origNodes, destNodes));
+ graph::Edges_t possibleEdges (constraintGraph_->getEdges (origNode, destNode));
// We check for all of them if both nodes are on the same leaf.
ConstraintSetPtr_t constraints;
while (!possibleEdges.empty ()) {
diff --git a/src/graph-steering-method.cc b/src/graph-steering-method.cc
index f085af9..4b61201 100644
--- a/src/graph-steering-method.cc
+++ b/src/graph-steering-method.cc
@@ -31,9 +31,9 @@ namespace hpp {
PathPtr_t GraphSteeringMethod::impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const
{
value_type length = (*distance_) (q1,q2);
- std::vector< graph::Edges_t > possibleEdges;
+ graph::Edges_t possibleEdges;
try {
- possibleEdges = graph_->getEdge (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 ();
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 447ef5d..c5eb4a6 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -47,8 +47,7 @@ namespace hpp {
ConstraintPtr_t Edge::configConstraint(ConfigurationIn_t config)
{
if (!configConstraints_) {
- Edges_t thisEdge; thisEdge.push_back (wkPtr_.lock ());
- configConstraints_ = graph_.lock ()->configConstraint (thisEdge);
+ configConstraints_ = graph_.lock ()->configConstraint (wkPtr_.lock ());
}
configConstraints_->offsetFromConfig (config);
return configConstraints_;
@@ -57,8 +56,7 @@ namespace hpp {
ConstraintPtr_t Edge::pathConstraint(ConfigurationIn_t config)
{
if (!pathConstraints_) {
- Edges_t thisEdge; thisEdge.push_back (wkPtr_.lock ());
- pathConstraints_ = graph_.lock ()->pathConstraint (thisEdge);
+ pathConstraints_ = graph_.lock ()->pathConstraint (wkPtr_.lock ());
}
pathConstraints_->offsetFromConfig (config);
return pathConstraints_;
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index 0bc2e23..8f5a8d6 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -25,21 +25,13 @@
namespace hpp {
namespace manipulation {
namespace graph {
- static std::string toString (const Nodes_t& n) {
- std::string nodesStr = "(";
- for (graph::Nodes_t::const_iterator itNode = n.begin();
- itNode != n.end (); itNode++)
- nodesStr += (*itNode)->name () + " / ";
- nodesStr += ")";
+ static std::string toString (const NodePtr_t& n) {
+ std::string nodesStr = "(" + n->name () + ")";
return nodesStr;
}
- static std::string toString (const Edges_t& e) {
- std::string edgesStr = "(";
- for (graph::Edges_t::const_iterator itEdge = e.begin();
- itEdge != e.end (); itEdge++)
- edgesStr += (*itEdge)->name () + " / ";
- edgesStr += ")";
+ static std::string toString (const EdgePtr_t& e) {
+ std::string edgesStr = "(" + e->name () + ")";
return edgesStr;
}
@@ -60,10 +52,9 @@ namespace hpp {
NodeSelectorPtr_t Graph::createNodeSelector()
{
- NodeSelectorPtr_t newNodeSelector = NodeSelector::create();
- nodeSelectors_.push_back(newNodeSelector);
- newNodeSelector->parentGraph (wkPtr_);
- return newNodeSelector;
+ nodeSelector_ = NodeSelector::create();
+ nodeSelector_->parentGraph (wkPtr_);
+ return nodeSelector_;
}
void Graph::maxIterations (size_type iterations)
@@ -91,158 +82,67 @@ namespace hpp {
return robot_;
}
- Nodes_t Graph::getNode(const Configuration_t config) const
+ NodePtr_t Graph::getNode (const Configuration_t config) const
{
- Nodes_t nodes;
- for (NodeSelectors_t::const_iterator it = nodeSelectors_.begin();
- it != nodeSelectors_.end(); it++)
- nodes.push_back( (*it)->getNode(config) );
- return nodes;
+ return nodeSelector_->getNode (config);
}
- static void buildPossibleEdges (Edges_t current,
- const std::vector <Edges_t>& edgesPerNodeSelector,
- std::vector <Edges_t>& possibleEdges)
- {
- size_t d = current.size();
- if (d == edgesPerNodeSelector.size()) {
- possibleEdges.push_back (current);
- return;
- }
- Edges_t::const_iterator it = edgesPerNodeSelector[d].begin();
- while (it != edgesPerNodeSelector[d].end()) {
- current.push_back (*it);
- buildPossibleEdges (current, edgesPerNodeSelector, possibleEdges);
- current.pop_back ();
- it++;
- }
- }
-
- std::vector <Edges_t> Graph::getEdge(const Nodes_t& from, const Nodes_t& to) const
- {
- assert (from.size() == to.size());
- assert (nodeSelectors_.size() == to.size());
- size_t numberPossibleEdges = 1;
- std::vector < Edges_t > edgesPerNodeSelector (nodeSelectors_.size());
- std::vector < Edges_t >::iterator itEdgePerNodeSelector = edgesPerNodeSelector.begin();
- Nodes_t::const_iterator itFrom = from.begin (),
- itTo = to.begin ();
- // We first iterate through from. For each element of from,
- // we look for all edges between this element of from and its corresponding
- // element in to. The resulting set of Edges_t is stored in
- // edgesPerNodeSelector.
-
- // Temporary variable.
- Edges_t edgesInNodeSelector;
- const Neighbors_t* neighbors;
- Neighbors_t::const_iterator itEdge;
- while (itFrom != from.end()) {
- edgesInNodeSelector.clear ();
- neighbors = &((*itFrom)->neighbors ());
- itEdge = neighbors->begin();
- // Find the edges between *itFrom and *itTo
- while (itEdge != neighbors->end()) {
- if (itEdge->second->to() == (*itTo) )
- edgesInNodeSelector.push_back (itEdge->second);
- itEdge++;
- }
- /// If no Edge is found, the two Node are not connected.
- if (edgesInNodeSelector.empty ())
- return std::vector <Edges_t>(0);
-
- // Store the Edges.
- numberPossibleEdges *= edgesInNodeSelector.size();
- *itEdgePerNodeSelector = edgesInNodeSelector;
-
- itFrom++; itTo++; itEdgePerNodeSelector++;
- }
- assert (itTo == to.end());
- assert (itEdgePerNodeSelector == edgesPerNodeSelector.end());
-
- // Now, we can create the list of possible Edges_t
- // between from and to.
- std::vector <Edges_t> possibleEdges;
- buildPossibleEdges (Edges_t(), edgesPerNodeSelector, possibleEdges);
- assert (possibleEdges.size() == numberPossibleEdges);
- return possibleEdges;
- }
-
- Edges_t Graph::chooseEdge(const Nodes_t& nodes) const
+ Edges_t Graph::getEdges (const NodePtr_t& from, const NodePtr_t& to) const
{
Edges_t edges;
- for (Nodes_t::const_iterator it = nodes.begin();
- it != nodes.end(); it++)
- edges.push_back( (*it)->nodeSelector().lock()->chooseEdge(*it) );
+ for (Neighbors_t::const_iterator it = from->neighbors ().begin ();
+ it != from->neighbors ().end (); it++) {
+ if (it->second->to () == to)
+ edges.push_back (it->second);
+ }
return edges;
}
- NodeSelectorPtr_t Graph::getNodeSelectorByName (const std::string& name)
+ EdgePtr_t Graph::chooseEdge (const NodePtr_t& node) const
{
- for (NodeSelectors_t::iterator it = nodeSelectors_.begin();
- it != nodeSelectors_.end(); it++) {
- if (name == (*it)->name())
- return *it;
- }
- return NodeSelectorPtr_t();
+ return nodeSelector_->chooseEdge (node);
}
- ConstraintSetPtr_t Graph::configConstraint (const Nodes_t& nodes)
+ ConstraintSetPtr_t Graph::configConstraint (const NodePtr_t& node)
{
ConstraintSetPtr_t constraint;
- MapFromNode::const_iterator it = constraintSetMapFromNode_.find (nodes);
+ MapFromNode::const_iterator it = constraintSetMapFromNode_.find (node);
if (it == constraintSetMapFromNode_.end ()) {
- std::string name = toString (nodes);
+ std::string name = toString (node);
constraint = ConstraintSet::create (robot (), "Set " + name);
ConfigProjectorPtr_t proj = ConfigProjector::create(robot(), "proj " + name, errorThreshold(), maxIterations());
insertNumericalConstraints (proj);
- for (Nodes_t::const_iterator it = nodes.begin();
- it != nodes.end(); it++)
- (*it)->insertNumericalConstraints (proj);
+ node->insertNumericalConstraints (proj);
constraint->addConstraint (HPP_DYNAMIC_PTR_CAST(Constraint, proj));
insertLockedDofs (constraint);
- for (Nodes_t::const_iterator it = nodes.begin();
- it != nodes.end(); it++)
- (*it)->insertLockedDofs (constraint);
- constraintSetMapFromNode_.insert (PairNodesConstraints(nodes, constraint));
+ node->insertLockedDofs (constraint);
+ constraintSetMapFromNode_.insert (PairNodeConstraints(node, constraint));
} else {
constraint = it->second;
}
return constraint;
}
- ConstraintSetPtr_t Graph::configConstraint (const Edges_t& edges, ConfigurationIn_t config)
- {
- ConstraintSetPtr_t constraint = configConstraint (edges);
- constraint->offsetFromConfig (config);
- return constraint;
- }
-
- ConstraintSetPtr_t Graph::configConstraint (const Edges_t& edges)
+ ConstraintSetPtr_t Graph::configConstraint (const EdgePtr_t& edge)
{
ConstraintSetPtr_t constraint;
- MapFromEdge::const_iterator it = cfgConstraintSetMapFromEdge_.find (edges);
+ MapFromEdge::const_iterator it = cfgConstraintSetMapFromEdge_.find (edge);
if (it == cfgConstraintSetMapFromEdge_.end ()) {
- std::string name = toString (edges);
+ std::string name = toString (edge);
constraint = ConstraintSet::create (robot (), "Set " + name);
ConfigProjectorPtr_t proj = ConfigProjector::create(robot(), "proj " + name, errorThreshold(), maxIterations());
insertNumericalConstraints (proj);
- for (Edges_t::const_iterator it = edges.begin();
- it != edges.end(); it++) {
- (*it)->insertNumericalConstraints (proj);
- (*it)->to ()->insertNumericalConstraints (proj);
- }
+ edge->insertNumericalConstraints (proj);
+ edge->to ()->insertNumericalConstraints (proj);
constraint->addConstraint (HPP_DYNAMIC_PTR_CAST(Constraint, proj));
insertLockedDofs (constraint);
- for (Edges_t::const_iterator it = edges.begin();
- it != edges.end(); it++) {
- (*it)->insertLockedDofs (constraint);
- (*it)->to ()->insertLockedDofs (constraint);
- }
- cfgConstraintSetMapFromEdge_.insert (PairEdgesConstraints(edges, constraint));
+ edge->insertLockedDofs (constraint);
+ edge->to ()->insertLockedDofs (constraint);
+ cfgConstraintSetMapFromEdge_.insert (PairEdgeConstraints(edge, constraint));
} else {
constraint = it->second;
}
@@ -250,37 +150,24 @@ namespace hpp {
return constraint;
}
- ConstraintSetPtr_t Graph::pathConstraint (const Edges_t& edges, ConfigurationIn_t config)
- {
- ConstraintSetPtr_t constraint = pathConstraint (edges);
- constraint->offsetFromConfig (config);
- return constraint;
- }
-
- ConstraintSetPtr_t Graph::pathConstraint (const Edges_t& edges)
+ ConstraintSetPtr_t Graph::pathConstraint (const EdgePtr_t& edge)
{
ConstraintSetPtr_t constraint;
- MapFromEdge::const_iterator it = pathConstraintSetMapFromEdge_.find (edges);
+ MapFromEdge::const_iterator it = pathConstraintSetMapFromEdge_.find (edge);
if (it == pathConstraintSetMapFromEdge_.end ()) {
- std::string name = toString (edges);
+ std::string name = toString (edge);
constraint = ConstraintSet::create (robot (), "Set " + name);
ConfigProjectorPtr_t proj = ConfigProjector::create(robot(), "proj " + name, errorThreshold(), maxIterations());
insertNumericalConstraints (proj);
- for (Edges_t::const_iterator it = edges.begin();
- it != edges.end(); it++) {
- (*it)->insertNumericalConstraints (proj);
- (*it)->node ()->insertNumericalConstraintsForPath (proj);
- }
+ edge->insertNumericalConstraints (proj);
+ edge->node ()->insertNumericalConstraintsForPath (proj);
constraint->addConstraint (HPP_DYNAMIC_PTR_CAST(Constraint, proj));
insertLockedDofs (constraint);
- for (Edges_t::const_iterator it = edges.begin();
- it != edges.end(); it++) {
- (*it)->insertLockedDofs (constraint);
- (*it)->node ()->insertLockedDofs (constraint);
- }
- pathConstraintSetMapFromEdge_.insert (PairEdgesConstraints (edges, constraint));
+ edge->insertLockedDofs (constraint);
+ edge->node ()->insertLockedDofs (constraint);
+ pathConstraintSetMapFromEdge_.insert (PairEdgeConstraints (edge, constraint));
} else {
constraint = it->second;
}
@@ -290,11 +177,7 @@ namespace hpp {
std::ostream& Graph::print (std::ostream& os) const
{
- GraphComponent::print (os) << std::endl;
- for (NodeSelectors_t::const_iterator it = nodeSelectors_.begin();
- it != nodeSelectors_.end(); it++)
- os << *(*it);
- return os;
+ return GraphComponent::print (os) << std::endl << nodeSelector_;
}
} // namespace graph
} // namespace manipulation
diff --git a/src/graph/node.cc b/src/graph/node.cc
index 914f76a..c01a7e9 100644
--- a/src/graph/node.cc
+++ b/src/graph/node.cc
@@ -47,9 +47,6 @@ namespace hpp {
bool Node::contains (const Configuration_t config)
{
- // TODO: This is not the most efficient way. We should
- // compute the value of the constraint instead of apllying
- // the constraint.
return configConstraint()->isSatisfied (config);
}
@@ -65,8 +62,7 @@ namespace hpp {
ConstraintPtr_t Node::configConstraint()
{
if (!configConstraints_) {
- Nodes_t thisNode; thisNode.push_back (wkPtr_.lock ());
- configConstraints_ = graph_.lock ()->configConstraint (thisNode);
+ configConstraints_ = graph_.lock ()->configConstraint (wkPtr_.lock ());
}
return configConstraints_;
}
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 08cee2c..639f4ef 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -102,13 +102,13 @@ namespace hpp {
{
graph::GraphPtr_t graph = problem_.constraintGraph ();
// Select next node in the constraint graph.
- graph::Nodes_t nodes = graph->getNode (*q_near);
- graph::Edges_t edges = graph->chooseEdge (nodes);
- ConstraintSetPtr_t constraint = graph->configConstraint (edges);
+ graph::NodePtr_t node = graph->getNode (*q_near);
+ graph::EdgePtr_t edge = graph->chooseEdge (node);
+ ConstraintSetPtr_t constraint = graph->configConstraint (edge);
constraint->offsetFromConfig (*q_near);
qProj_ = *q_rand;
if (!constraint->apply (qProj_)) {
- addFailure (PROJECTION, edges);
+ addFailure (PROJECTION, edge);
SuccessStatistics& ss = constraint->configProjector ()->statistics ();
if (ss.nbFailure () > ss.nbSuccess ()) {
hppDout (warning, constraint->name () << " fails often." << std::endl << ss);
@@ -120,31 +120,27 @@ namespace hpp {
core::SteeringMethodPtr_t sm (problem().steeringMethod());
core::PathPtr_t path = (*sm) (*q_near, qProj_);
if (!path) {
- addFailure (STEERING_METHOD, edges);
+ addFailure (STEERING_METHOD, edge);
return false;
}
core::PathValidationPtr_t pathValidation (problem ().pathValidation ());
pathValidation->validate (path, false, validPath);
if (validPath->length () == 0)
- addFailure (PATH_VALIDATION, edges);
+ addFailure (PATH_VALIDATION, edge);
else
extendStatistics_.addSuccess ();
return true;
}
- void ManipulationPlanner::addFailure (TypeOfFailure t, const graph::Edges_t& edges)
+ void ManipulationPlanner::addFailure (TypeOfFailure t, const graph::EdgePtr_t& edge)
{
- EdgesReasonMap::iterator it = failureReasons_.find (edges);
+ EdgeReasonMap::iterator it = failureReasons_.find (edge);
if (it == failureReasons_.end ()) {
- std::string edgesStr = "(";
- for (graph::Edges_t::const_iterator itEdge = edges.begin();
- itEdge != edges.end (); itEdge++)
- edgesStr += (*itEdge)->name () + " / ";
- edgesStr += ")";
- Reasons r (SuccessBin::createReason ("Projection for " + edgesStr),
- SuccessBin::createReason ("SteeringMethod for " + edgesStr),
- SuccessBin::createReason ("PathValidation returned length 0 for " + edgesStr));
- failureReasons_.insert (EdgesReasonPair (edges, r));
+ std::string edgeStr = "(" + edge->name () + ")";
+ Reasons r (SuccessBin::createReason ("Projection for " + edgeStr),
+ SuccessBin::createReason ("SteeringMethod for " + edgeStr),
+ SuccessBin::createReason ("PathValidation returned length 0 for " + edgeStr));
+ failureReasons_.insert (EdgeReasonPair (edge, r));
extendStatistics_.addFailure (r.get (t));
return;
}
@@ -160,10 +156,7 @@ namespace hpp {
core::PathValidationPtr_t pathValidation (problem ().pathValidation ());
core::PathPtr_t path, validPath;
graph::GraphPtr_t graph = problem_.constraintGraph ();
- graph::Nodes_t n1, n2;
- graph::Edges_t edges;
bool connectSucceed = false;
- std::vector< graph::Edges_t > possibleEdges;
for (core::Nodes_t::const_iterator itn1 = nodes.begin ();
itn1 != nodes.end (); ++itn1) {
ConfigurationPtr_t q1 ((*itn1)->configuration ());
diff --git a/tests/test-constraintgraph.cc b/tests/test-constraintgraph.cc
index 18750c8..0af0e28 100644
--- a/tests/test-constraintgraph.cc
+++ b/tests/test-constraintgraph.cc
@@ -68,20 +68,13 @@ namespace hpp_test {
GraphComponents components;
GraphPtr_t graph_;
- NodeSelectorPtr_t ns1;
- NodeSelectorPtr_t ns2;
- NodePtr_t n11;
- NodePtr_t n12;
- NodePtr_t n21;
- NodePtr_t n22;
- EdgePtr_t e111;
- EdgePtr_t e121;
- EdgePtr_t e112;
- EdgePtr_t e122;
- EdgePtr_t e211;
- EdgePtr_t e221;
- EdgePtr_t e212;
- EdgePtr_t e222;
+ NodeSelectorPtr_t ns;
+ NodePtr_t n1;
+ NodePtr_t n2;
+ EdgePtr_t e11;
+ EdgePtr_t e21;
+ EdgePtr_t e12;
+ EdgePtr_t e22;
void initialize (bool ur5)
{
@@ -100,25 +93,13 @@ namespace hpp_test {
graph_ = Graph::create (robot); components.push_back(graph_);
graph_->maxIterations (20);
graph_->errorThreshold (1e-4);
- ns1 = graph_->createNodeSelector(); components.push_back(ns1);
- if (!ur5)
- ns2 = graph_->createNodeSelector(); components.push_back(ns2);
- n11 = ns1->createNode (); components.push_back(n11);
- n12 = ns1->createNode (); components.push_back(n12);
- if (!ur5) {
- n21 = ns2->createNode (); components.push_back(n21);
- n22 = ns2->createNode (); components.push_back(n22);
- }
- e111 = n11->linkTo (n11); components.push_back(e111);
- e121 = n12->linkTo (n11); components.push_back(e121);
- e112 = n11->linkTo (n12); components.push_back(e112);
- e122 = n12->linkTo (n12); components.push_back(e122);
- if (!ur5) {
- e211 = n21->linkTo (n21); components.push_back(e211);
- e221 = n22->linkTo (n21); components.push_back(e221);
- e212 = n21->linkTo (n22); components.push_back(e212);
- e222 = n22->linkTo (n22); components.push_back(e222);
- }
+ ns = graph_->createNodeSelector(); components.push_back(ns);
+ n1 = ns->createNode (); components.push_back(n1);
+ n2 = ns->createNode (); components.push_back(n2);
+ e11 = n1->linkTo (n1); components.push_back(e11);
+ e21 = n2->linkTo (n1); components.push_back(e21);
+ e12 = n1->linkTo (n2); components.push_back(e12);
+ e22 = n2->linkTo (n2); components.push_back(e22);
q1 = Configuration_t::Zero(6);
q2 = Configuration_t::Zero(6);
@@ -143,34 +124,19 @@ BOOST_AUTO_TEST_CASE (GraphStructure)
}
// Test function Graph::getEdge
- Nodes_t from, to;
- from.push_back (n11);
- from.push_back (n22);
- to.push_back (n12);
- to.push_back (n22);
- std::vector <Edges_t> checkPossibleEdges,
- possibleEdges = graph_->getEdge (from, to);
- do {
- Edges_t edges;
- edges.push_back (e112);
- edges.push_back (e222);
- checkPossibleEdges.push_back (edges);
- } while (0);
- for (size_t i = 0; i < possibleEdges.size(); i++) {
- for (size_t j = 0; j < possibleEdges[i].size(); j++)
- BOOST_CHECK_MESSAGE (possibleEdges[i][j] == checkPossibleEdges[i][j],
- "i = " << i << " and j = " << j);
- }
+ NodePtr_t from(n1), to(n2);
+ Edges_t checkPossibleEdges,
+ possibleEdges = graph_->getEdges (from, to);
+ checkPossibleEdges.push_back (e12);
+ for (size_t j = 0; j < possibleEdges.size(); j++)
+ BOOST_CHECK_MESSAGE (possibleEdges[j] == checkPossibleEdges[j],
+ "Possible edge j = " << j);
Configuration_t cfg;
- Nodes_t nodes = graph_->getNode (cfg);
- BOOST_CHECK (nodes.size() == 2);
- BOOST_CHECK (nodes[0] == n11);
- BOOST_CHECK (nodes[1] == n21);
- Edges_t edges = graph_->chooseEdge (nodes);
- BOOST_CHECK (edges.size() == 2);
- BOOST_CHECK (edges[0]->from() == n11);
- BOOST_CHECK (edges[1]->from() == n21);
+ NodePtr_t node = graph_->getNode (cfg);
+ BOOST_CHECK (node == n1);
+ EdgePtr_t edge = graph_->chooseEdge (node);
+ BOOST_CHECK (edge->from() == n1);
}
#ifdef TEST_UR5
@@ -201,7 +167,7 @@ BOOST_AUTO_TEST_CASE (ConstraintSets)
BOOST_CHECK (res == expectedRes);
//graph_->addNumericalConstraint (com);
- n11->addNumericalConstraint (pos, Equality::create ());
+ n1->addNumericalConstraint (pos, Equality::create ());
}
BOOST_AUTO_TEST_CASE (PathValidationTest)
@@ -211,19 +177,17 @@ BOOST_AUTO_TEST_CASE (PathValidationTest)
ProblemPtr_t pb = new Problem (robot);
BOOST_CHECK(robot->configSize() == 6);
pb->constraintGraph (graph_);
- Nodes_t nodes11; nodes11.push_back (n11);
- Nodes_t nodes12; nodes12.push_back (n12);
- ConstraintSetPtr_t constraintn11 = graph_->configConstraint (nodes11);
- ConstraintSetPtr_t constraintn12 = graph_->configConstraint (nodes12);
- BOOST_CHECK ( constraintn11->isSatisfied (q1));
- BOOST_CHECK (!constraintn11->isSatisfied (q2));
- BOOST_CHECK ( constraintn12->isSatisfied (q2));
+ ConstraintSetPtr_t constraintn1 = graph_->configConstraint (n1);
+ ConstraintSetPtr_t constraintn2 = graph_->configConstraint (n2);
+ BOOST_CHECK ( constraintn1->isSatisfied (q1));
+ BOOST_CHECK (!constraintn1->isSatisfied (q2));
+ BOOST_CHECK ( constraintn2->isSatisfied (q2));
PathPtr_t p = (*pb->steeringMethod ())(ConfigurationIn_t(q1),ConfigurationIn_t(q2)),
validp;
- Nodes_t nq1 = graph_->getNode (q1);
- Nodes_t nq2 = graph_->getNode (q2);
- BOOST_CHECK (nq1.size() == 1); BOOST_CHECK (nq1[0] == n11);
- BOOST_CHECK (nq2.size() == 1); BOOST_CHECK (nq2[0] == n12);
+ NodePtr_t nq1 = graph_->getNode (q1);
+ NodePtr_t nq2 = graph_->getNode (q2);
+ BOOST_CHECK (nq1 == n1);
+ BOOST_CHECK (nq2 == n2);
GraphPathValidationPtr_t pv = pb->pathValidation ();
BOOST_CHECK (pv);
if (!pv->validate(p, false, validp)) {
--
GitLab