diff --git a/include/hpp/manipulation/graph/node-selector.hh b/include/hpp/manipulation/graph/node-selector.hh
index ae7a5f1cf3f685c0497d21c9c00b98debefcfc3a..300ef22ee88b0ea3cb1cf909982fa2316ee7f235 100644
--- a/include/hpp/manipulation/graph/node-selector.hh
+++ b/include/hpp/manipulation/graph/node-selector.hh
@@ -34,7 +34,7 @@ namespace hpp {
           static NodeSelectorPtr_t create(const std::string& name);
 
           /// Create an empty node
-          NodePtr_t createNode (const std::string& name);
+          NodePtr_t createNode (const std::string& name, bool waypoint = false);
 
           /// Returns the state of a configuration.
           NodePtr_t getNode(ConfigurationIn_t config) const;
@@ -82,6 +82,7 @@ namespace hpp {
 
           /// List of the states of one end-effector, ordered by priority.
           Nodes_t orderedStates_;
+          Nodes_t waypoints_;
 
         private:
           /// Weak pointer to itself.
diff --git a/include/hpp/manipulation/graph/node.hh b/include/hpp/manipulation/graph/node.hh
index fb78a4caca5d31ee7dfa0e82ea3b8d779cc6c3f0..e47b9248066a33a5335cb1350826e82aaac61dc4 100644
--- a/include/hpp/manipulation/graph/node.hh
+++ b/include/hpp/manipulation/graph/node.hh
@@ -69,6 +69,16 @@ namespace hpp {
           /// the constraints. Instead, use the class NodeSelector.
           virtual bool contains (ConfigurationIn_t config) const;
 
+          inline bool isWaypoint () const
+          {
+            return isWaypoint_;
+          }
+
+          inline void isWaypoint (bool isWaypoint)
+          {
+            isWaypoint_ = isWaypoint;
+          }
+
           /// Get the parent NodeSelector.
           NodeSelectorWkPtr_t nodeSelector () const
           {
@@ -160,6 +170,8 @@ namespace hpp {
 
           /// Weak pointer to itself.
           NodeWkPtr_t wkPtr_;
+
+          bool isWaypoint_;
       }; // class Node
 
       /// \}
diff --git a/src/graph/node-selector.cc b/src/graph/node-selector.cc
index ce29ad023c65cd31b4b6d09ff6d1f436e1e6289d..49a7787f0869d190436ace6461c94712f67b7ac5 100644
--- a/src/graph/node-selector.cc
+++ b/src/graph/node-selector.cc
@@ -39,12 +39,15 @@ namespace hpp {
         wkPtr_ = weak;
       }
 
-      NodePtr_t NodeSelector::createNode (const std::string& name)
+      NodePtr_t NodeSelector::createNode (const std::string& name,
+          bool waypoint)
       {
         NodePtr_t newNode = Node::create (name);
         newNode->nodeSelector(wkPtr_);
         newNode->parentGraph(graph_);
-        orderedStates_.push_back(newNode);
+        newNode->isWaypoint (waypoint);
+        if (waypoint) waypoints_.push_back(newNode);
+        else orderedStates_.push_back(newNode);
         return newNode;
       }
 
diff --git a/src/graph/node.cc b/src/graph/node.cc
index 598a43a755e9cf293eb8a25e79493a44ac5dbce2..c6e2429d612ac37a6f473ebcd56425572c72b3d7 100644
--- a/src/graph/node.cc
+++ b/src/graph/node.cc
@@ -27,7 +27,8 @@ namespace hpp {
   namespace manipulation {
     namespace graph {
       Node::Node (const std::string& name) :
-	GraphComponent (name), configConstraints_ (new Constraint_t())
+	GraphComponent (name), configConstraints_ (new Constraint_t()),
+        isWaypoint_ (false)
       {}
 
       Node::~Node ()