diff --git a/CMakeLists.txt b/CMakeLists.txt
index 15bafb7fa8191f30cf1f3d7cb453963c7af03e9e..490c2768f875123e5730278bf89d4b35e8e17525 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -68,6 +68,7 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/graph/node.hh
include/hpp/manipulation/graph/edge.hh
include/hpp/manipulation/graph/node-selector.hh
+ include/hpp/manipulation/graph/guided-node-selector.hh
include/hpp/manipulation/graph/graph.hh
include/hpp/manipulation/graph/statistics.hh
include/hpp/manipulation/graph/graph-component.hh
diff --git a/include/hpp/manipulation/graph/fwd.hh b/include/hpp/manipulation/graph/fwd.hh
index ff2f0529ebd3dd02f93d044ffefd6355ea54b7d9..baf21abbc5feccc8f1f6d4b09a6b735a5ed69f4c 100644
--- a/include/hpp/manipulation/graph/fwd.hh
+++ b/include/hpp/manipulation/graph/fwd.hh
@@ -30,6 +30,7 @@ namespace hpp {
HPP_PREDEF_CLASS (WaypointEdge);
HPP_PREDEF_CLASS (LevelSetEdge);
HPP_PREDEF_CLASS (NodeSelector);
+ HPP_PREDEF_CLASS (GuidedNodeSelector);
HPP_PREDEF_CLASS (GraphComponent);
typedef boost::shared_ptr < Graph > GraphPtr_t;
typedef boost::shared_ptr < Node > NodePtr_t;
@@ -37,6 +38,7 @@ namespace hpp {
typedef boost::shared_ptr < WaypointEdge > WaypointEdgePtr_t;
typedef boost::shared_ptr < LevelSetEdge > LevelSetEdgePtr_t;
typedef boost::shared_ptr < NodeSelector > NodeSelectorPtr_t;
+ typedef boost::shared_ptr < GuidedNodeSelector > GuidedNodeSelectorPtr_t;
typedef boost::shared_ptr < GraphComponent > GraphComponentPtr_t;
typedef std::vector < NodePtr_t > Nodes_t;
typedef std::vector < EdgePtr_t > Edges_t;
diff --git a/include/hpp/manipulation/graph/guided-node-selector.hh b/include/hpp/manipulation/graph/guided-node-selector.hh
new file mode 100644
index 0000000000000000000000000000000000000000..0d3ca92bd2b5e071f2ee9ce7e0feb51c11d8691d
--- /dev/null
+++ b/include/hpp/manipulation/graph/guided-node-selector.hh
@@ -0,0 +1,70 @@
+// Copyright (c) 2014, LAAS-CNRS
+// 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/>.
+
+#ifndef HPP_MANIPULATION_GRAPH_GUIDED_NODE_SELECTOR_HH
+# define HPP_MANIPULATION_GRAPH_GUIDED_NODE_SELECTOR_HH
+
+#include "hpp/manipulation/fwd.hh"
+#include "hpp/manipulation/graph/fwd.hh"
+#include "hpp/manipulation/graph/node-selector.hh"
+
+namespace hpp {
+ namespace manipulation {
+ namespace graph {
+ class HPP_MANIPULATION_DLLAPI GuidedNodeSelector : public NodeSelector
+ {
+ public:
+ /// Create a new GuidedNodeSelector.
+ static GuidedNodeSelectorPtr_t create(const std::string& name,
+ const core::RoadmapPtr_t& roadmap);
+
+ /// Set the target
+ void setNodeList (const Nodes_t& nodeList);
+
+ /// Select randomly an outgoing edge of the given node.
+ virtual EdgePtr_t chooseEdge(const core::NodePtr_t& from) const;
+
+ /// Print the object in a stream.
+ std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
+
+ protected:
+ /// Initialization of the object.
+ void init (const GuidedNodeSelectorPtr_t& weak);
+
+ /// Constructor
+ GuidedNodeSelector (const std::string& name,
+ const core::RoadmapPtr_t roadmap) :
+ NodeSelector (name), roadmap_ (roadmap)
+ {}
+
+ /// Print the object in a stream.
+ std::ostream& print (std::ostream& os) const;
+
+ private:
+ /// The target
+ Nodes_t nodeList_;
+
+ /// The roadmap
+ core::RoadmapPtr_t roadmap_;
+
+ /// Weak pointer to itself.
+ GuidedNodeSelectorWkPtr_t wkPtr_;
+ }; // Class NodeSelector
+ } // namespace graph
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_GRAPH_GUIDED_NODE_SELECTOR_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 9d0bd2eb2a6932345994ab4313d4b7e2a3df8f14..a8eb9efa733b34dd9a705f0fd067618f110235fb 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -34,6 +34,7 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED
graph/graph.cc
graph/graph-component.cc
graph/node-selector.cc
+ graph/guided-node-selector.cc
graph/statistics.cc
graph/dot.cc
diff --git a/src/astar.hh b/src/astar.hh
new file mode 100644
index 0000000000000000000000000000000000000000..e1f43ffb325304319e7c3bdcc9ff62a615ef074e
--- /dev/null
+++ b/src/astar.hh
@@ -0,0 +1,164 @@
+//
+// Copyright (c) 2015 CNRS
+// Authors: Florent Lamiraux, Joseph Mirabel
+//
+// This file is part of hpp-core
+// hpp-core 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-core 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-core If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef HPP_CORE_ASTAR_HH
+# define HPP_CORE_ASTAR_HH
+
+# include <limits>
+# include <hpp/core/distance.hh>
+# include <hpp/core/node.hh>
+# include <hpp/core/edge.hh>
+
+# include <hpp/manipulation/fwd.hh>
+# include <hpp/manipulation/graph/node-selector.hh>
+//# include <hpp/core/path-vector.hh>
+
+namespace hpp {
+ namespace manipulation {
+ class Astar
+ {
+ public:
+ typedef std::map <core::NodePtr_t, value_type> CostMap_t;
+ struct CostMapCompFunctor {
+ CostMap_t& cost_;
+ CostMapCompFunctor (CostMap_t& cost) : cost_ (cost) {}
+ bool operator () (const core::NodePtr_t& n1, const core::NodePtr_t& n2)
+ { return cost_ [n1] < cost_ [n2]; }
+ bool operator () (const core::NodePtr_t& n1, const value_type& val)
+ { return cost_ [n1] < val; }
+ }; // struc CostMapCompFunctor
+
+ typedef std::list <graph::NodePtr_t> Nodes_t;
+ typedef std::list <core::NodePtr_t> RoadmapNodes_t;
+ typedef std::list <core::EdgePtr_t> RoadmapEdges_t;
+ typedef std::map <core::NodePtr_t, core::EdgePtr_t> Parent_t;
+
+ Astar (const core::DistancePtr_t distance,
+ const graph::NodeSelectorPtr_t& nodeSelector, const core::NodePtr_t& from) :
+ distance_ (distance), selector_ (nodeSelector),
+ from_ (from)
+ {
+ open_.push_back (from);
+ costFromStart_ [from] = 0;
+ }
+
+ Nodes_t solution (const core::NodePtr_t to)
+ {
+ if (parent_.find (to) != parent_.end () ||
+ findPath (to))
+ {
+ core::NodePtr_t node = to;
+ Nodes_t nodes;
+
+ while (node) {
+ Parent_t::const_iterator itNode = parent_.find (node);
+ if (itNode != parent_.end ()) {
+ node = itNode->second->from ();
+ nodes.push_front (selector_->getNode (*node->configuration()));
+ }
+ else node = core::NodePtr_t ();
+ }
+ // We may want to clean it a little
+ // std::unique (nodes.begin(), nodes.end ());
+ return nodes;
+ }
+ return Nodes_t();
+ }
+
+ private:
+ bool findPath (const core::NodePtr_t& to)
+ {
+ // Recompute the estimated cost to goal
+ for (CostMap_t::iterator it = estimatedCostToGoal_.begin ();
+ it != estimatedCostToGoal_.end (); ++it) {
+ it->second = getCostFromStart (it->first) + heuristic (it->first, to);
+ }
+ open_.sort (CostMapCompFunctor (estimatedCostToGoal_));
+
+ while (!open_.empty ()) {
+ RoadmapNodes_t::iterator itv = open_.begin ();
+ core::NodePtr_t current (*itv);
+ if (current == to) {
+ return true;
+ }
+ open_.erase (itv);
+ closed_.push_back (current);
+ for (RoadmapEdges_t::const_iterator itEdge = current->outEdges ().begin ();
+ itEdge != current->outEdges ().end (); ++itEdge) {
+ core::NodePtr_t child ((*itEdge)->to ());
+ if (std::find (closed_.begin(), closed_.end(),
+ child) == closed_.end ()) {
+ // node is not in closed set
+ value_type transitionCost = edgeCost (*itEdge);
+ value_type tmpCost = getCostFromStart (current) + transitionCost;
+ bool childNotInOpenSet = (std::find (open_.begin (),
+ open_.end (),
+ child) == open_.end ());
+ if ((childNotInOpenSet) || (tmpCost < getCostFromStart (child))) {
+ parent_ [child] = *itEdge;
+ costFromStart_ [child] = tmpCost;
+ value_type estimatedCost = tmpCost + heuristic (child, to);
+ estimatedCostToGoal_ [child] = estimatedCost;
+ if (childNotInOpenSet) {
+ // Find the first element not strictly smaller than child
+ RoadmapNodes_t::iterator pos =
+ std::lower_bound (open_.begin (), open_.end (),
+ estimatedCost, CostMapCompFunctor (estimatedCostToGoal_));
+ open_.insert (pos, child);
+ }
+ }
+ }
+ }
+ }
+ return false;
+ }
+
+ inline value_type heuristic (const core::NodePtr_t node, const core::NodePtr_t to) const
+ {
+ const ConfigurationPtr_t& config = node->configuration ();
+ return (*distance_) (*config, *to->configuration ());
+ }
+
+ inline value_type edgeCost (const core::EdgePtr_t& edge) const
+ {
+ return edge->path ()->length ();
+ }
+
+ value_type getCostFromStart (const core::NodePtr_t& to) const
+ {
+ CostMap_t::const_iterator it = costFromStart_.find (to);
+ if (it == costFromStart_.end())
+ return std::numeric_limits <value_type>::max();
+ return it->second;
+ }
+
+ RoadmapNodes_t closed_;
+ RoadmapNodes_t open_;
+ std::map <core::NodePtr_t, value_type> costFromStart_;
+ std::map <core::NodePtr_t, value_type> estimatedCostToGoal_;
+ Parent_t parent_;
+ core::DistancePtr_t distance_;
+ graph::NodeSelectorPtr_t selector_;
+ core::NodePtr_t from_;
+
+ }; // class Astar
+ } // namespace manipulation
+} // namespace hpp
+
+
+#endif // HPP_CORE_ASTAR_HH
diff --git a/src/graph/guided-node-selector.cc b/src/graph/guided-node-selector.cc
new file mode 100644
index 0000000000000000000000000000000000000000..39cef5847372de997f3bf4b5765d3076e232dfda
--- /dev/null
+++ b/src/graph/guided-node-selector.cc
@@ -0,0 +1,148 @@
+// Copyright (c) 2014, LAAS-CNRS
+// 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/graph/guided-node-selector.hh"
+
+#include <hpp/util/assertion.hh>
+#include <hpp/model/configuration.hh>
+#include <hpp/core/steering-method.hh>
+
+#include "../astar.hh"
+#include "hpp/manipulation/roadmap.hh"
+
+#include <cstdlib>
+
+namespace hpp {
+ namespace manipulation {
+ namespace graph {
+ GuidedNodeSelectorPtr_t GuidedNodeSelector::create(const std::string& name,
+ const core::RoadmapPtr_t& roadmap)
+ {
+ GuidedNodeSelector* ptr = new GuidedNodeSelector (name, roadmap);
+ GuidedNodeSelectorPtr_t shPtr (ptr);
+ ptr->init (shPtr);
+ return shPtr;
+ }
+
+ void GuidedNodeSelector::init (const GuidedNodeSelectorPtr_t& weak)
+ {
+ NodeSelector::init (weak);
+ wkPtr_ = weak;
+ }
+
+ void GuidedNodeSelector::setNodeList (const Nodes_t& nodeList)
+ {
+ nodeList_ = nodeList;
+ }
+
+ EdgePtr_t GuidedNodeSelector::chooseEdge(const core::NodePtr_t& from) const
+ {
+ if (nodeList_.empty ()) return NodeSelector::chooseEdge (from);
+ Astar::Nodes_t list;
+ bool reverse = false;
+ if (from->connectedComponent () == roadmap_->initNode ()->connectedComponent ()) {
+ Astar alg (roadmap_->distance (), wkPtr_.lock(), roadmap_->initNode ());
+ list = alg.solution (from);
+ } else {
+ core::Nodes_t::const_iterator itg = roadmap_->goalNodes ().begin ();
+ for (; itg != roadmap_->goalNodes ().end (); ++itg)
+ if ((*itg)->connectedComponent () == from->connectedComponent ())
+ break;
+ if (itg == roadmap_->goalNodes ().end ()) {
+ hppDout (error, "This configuration can reach neither the initial "
+ "configuration nor any of the goal configurations.");
+ return EdgePtr_t ();
+ }
+ reverse = true;
+ Astar alg (roadmap_->distance (), wkPtr_.lock(), from);
+ list = alg.solution (*itg);
+ }
+ std::unique (list.begin(), list.end ());
+ // Check if the beginning of nodeList is list
+ if (list.size() <= nodeList_.size()) {
+ Neighbors_t nn;
+ if (reverse) {
+ Nodes_t::const_reverse_iterator it1 = nodeList_.rbegin ();
+ Astar::Nodes_t::const_reverse_iterator it2 = list.rbegin();
+ Astar::Nodes_t::const_reverse_iterator itEnd2 = list.rend();
+ do {
+ if (*it1 != *it2) {
+ hppDout (error, "The target sequence of nodes does not end with "
+ "the sequence of nodes to reach this configuration.");
+ return EdgePtr_t ();
+ }
+ ++it1;
+ } while (++it2 != itEnd2);
+ NodePtr_t node = getNode (*from->configuration ());
+ HPP_ASSERT (node == list.front ());
+ const Neighbors_t& n = node->neighbors();
+ /// You stay in the same node
+ for (Neighbors_t::const_iterator it = n.begin (); it != n.end (); ++it)
+ if (it->second->to () == node)
+ nn.insert (it->second, it->first);
+ const Neighbors_t& n1 = (*it1)->neighbors ();
+ /// Go from node it1 to node
+ for (Neighbors_t::const_iterator it = n1.begin (); it != n1.end (); ++it)
+ if (it->second->to () == node)
+ nn.insert (it->second, it->first);
+ } else {
+ Nodes_t::const_iterator it1 = nodeList_.begin ();
+ Astar::Nodes_t::const_iterator it2 = list.begin();
+ Astar::Nodes_t::const_iterator itEnd2 = list.end();
+ do {
+ if (*it1 != *it2) {
+ hppDout (error, "The target sequence of nodes does not start with "
+ "the sequence of nodes to reach this configuration.");
+ return EdgePtr_t ();
+ }
+ ++it1;
+ } while (++it2 != itEnd2);
+ NodePtr_t node = getNode (*from->configuration ());
+ HPP_ASSERT (node == list.back ());
+ const Neighbors_t& n = node->neighbors();
+ for (Neighbors_t::const_iterator it = n.begin (); it != n.end (); ++it)
+ /// You stay in the same node
+ /// or go from node to node it1
+ if (it->second->to () == node || it->second->to () == *it1)
+ nn.insert (it->second, it->first);
+ }
+ if (nn.size () > 0)
+ return nn ();
+ hppDout (error, "This node has no neighbors to get to an admissible states.");
+ }
+ return EdgePtr_t ();
+ }
+
+ std::ostream& GuidedNodeSelector::dotPrint (std::ostream& os, dot::DrawingAttributes) const
+ {
+ for (Nodes_t::const_iterator it = orderedStates_.begin();
+ orderedStates_.end() != it; ++it)
+ (*it)->dotPrint (os);
+ return os;
+ }
+
+ std::ostream& GuidedNodeSelector::print (std::ostream& os) const
+ {
+ os << "|-- ";
+ GraphComponent::print (os) << std::endl;
+ for (Nodes_t::const_iterator it = orderedStates_.begin();
+ orderedStates_.end() != it; ++it)
+ os << *(*it);
+ return os;
+ }
+ } // namespace graph
+ } // namespace manipulation
+} // namespace hpp