diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3f5a62a20e65ed4c8acb484f6d6f57d534b946f0..84a18a3857e025f4292e956ec173095e60055dfd 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -82,9 +82,12 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/graph-steering-method.hh
include/hpp/manipulation/graph-optimizer.hh
include/hpp/manipulation/graph/node.hh
+ include/hpp/manipulation/graph/state.hh
include/hpp/manipulation/graph/edge.hh
include/hpp/manipulation/graph/node-selector.hh
+ include/hpp/manipulation/graph/state-selector.hh
include/hpp/manipulation/graph/guided-node-selector.hh
+ include/hpp/manipulation/graph/guided-state-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/connected-component.hh b/include/hpp/manipulation/connected-component.hh
index 21251ab1046d5dd967df99b2d8cff51bf61845b2..654331c2ff007429a9424ca42fc01751a04a06ef 100644
--- a/include/hpp/manipulation/connected-component.hh
+++ b/include/hpp/manipulation/connected-component.hh
@@ -27,13 +27,13 @@
namespace hpp {
namespace manipulation {
/// Extension of hpp::core::connected-component. Adds a list of roadmap nodes for
- /// every contraint graph node within the connected component. Thus every roadmap
- /// node is assigned to a grahp node, which minimises computation time.
+ /// every contraint graph state within the connected component. Thus every roadmap
+ /// node is assigned to a grahp state, which minimises computation time.
class HPP_MANIPULATION_DLLAPI ConnectedComponent : public core::ConnectedComponent
{
public:
- /// Map of graph nodes within the connected component
- typedef std::map <graph::NodePtr_t, RoadmapNodes_t> GraphNodes_t;
+ /// Map of graph states within the connected component
+ typedef std::map <graph::StatePtr_t, RoadmapNodes_t> GraphStates_t;
ConnectedComponent() {}
@@ -49,11 +49,12 @@ class HPP_MANIPULATION_DLLAPI ConnectedComponent : public core::ConnectedCompone
/// \param roadmap node to be added
void addNode (const core::NodePtr_t& node);
- const RoadmapNodes_t& getRoadmapNodes (const graph::NodePtr_t graphNode) const;
+ const RoadmapNodes_t& getRoadmapNodes (const graph::StatePtr_t graphState) const;
protected:
private:
- GraphNodes_t graphNodeMap_;
+ bool check () const;
+ GraphStates_t graphStateMap_;
RoadmapPtr_t roadmap_;
static RoadmapNodes_t empty_;
}; // class ConnectedComponent
diff --git a/include/hpp/manipulation/graph-path-validation.hh b/include/hpp/manipulation/graph-path-validation.hh
index 7449f8e7e76db0a97a905d3b1f9e110782e7555a..f6015362f862562823b5fac9022afa8ed3122e26 100644
--- a/include/hpp/manipulation/graph-path-validation.hh
+++ b/include/hpp/manipulation/graph-path-validation.hh
@@ -24,7 +24,7 @@
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/graph.hh"
-# include "hpp/manipulation/graph/node.hh"
+# include "hpp/manipulation/graph/state.hh"
namespace hpp {
namespace manipulation {
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 44762a26bef923b16d1b7b7e3f1945a199f3e3c8..25b0b95a32b5064a821e14c8946443f942bd7c90 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -58,7 +58,7 @@ namespace hpp {
/// \addtogroup constraint_graph
/// \{
- /// Transition between two nodes of a constraint graph
+ /// Transition between two states of a constraint graph
///
/// An edge stores two types of constraints.
@@ -72,16 +72,16 @@ namespace hpp {
/// the object can be at different positions.
/// \sa method pathConstraint.
/// \li <b> Configuration constraints </b> are constraints that
- /// configurations in the destination node should satisfy and
+ /// configurations in the destination state should satisfy and
/// the constraints that paths should satisfy. For instance, if
- /// the edge links a node where the robot does not hold the
- /// object to a node where the robot holds the object, the
+ /// the edge links a state where the robot does not hold the
+ /// object to a state where the robot holds the object, the
/// configuration constraints represent a fixed relative
/// position of the object with respect to the gripper and a
/// stable position of the object. Configuration constraints are
/// necessary to generate a configuration in the destination
- /// node of the edge that is reachable from a given
- /// configuration in the start node by an admissible path.
+ /// state of the edge that is reachable from a given
+ /// configuration in the start state by an admissible path.
class HPP_MANIPULATION_DLLAPI Edge : public GraphComponent
{
public:
@@ -94,8 +94,8 @@ namespace hpp {
static EdgePtr_t create
(const std::string& name,
const GraphWkPtr_t& graph,
- const NodeWkPtr_t& from,
- const NodeWkPtr_t& to);
+ const StateWkPtr_t& from,
+ const StateWkPtr_t& to);
/// Apply edge constraint
///
@@ -122,33 +122,47 @@ namespace hpp {
ConfigurationIn_t q2) const;
/// Get the destination
- NodePtr_t to () const;
+ StatePtr_t to () const;
/// Get the origin
- NodePtr_t from () const;
+ StatePtr_t from () const;
- /// Get the node in which path is.
- NodePtr_t node () const
+ /// Get the state in which path is.
+ StatePtr_t state () const
{
- return node_.lock();
+ return state_.lock();
}
- void node (NodePtr_t node)
+ void state (StatePtr_t state)
{
- node_ = node;
+ state_ = state;
}
- /// \deprecated use node(NodePtr_t) instead.
+ /// Get the state in which path is.
+ /// \deprecated use StatePtr_t state () const instead
+ StatePtr_t node () const
+ {
+ return state_.lock();
+ }
+
+ /// Set state
+ /// \deprecated use void state (StatePtr_t state) instead
+ void node (StatePtr_t state)
+ {
+ state_ = state;
+ }
+
+ /// \deprecated use state(StatePtr_t) instead.
void isInNodeFrom (bool iinf) HPP_MANIPULATION_DEPRECATED
{
- if (iinf) node_ = from_;
- else node_ = to_;
+ if (iinf) state_ = from_;
+ else state_ = to_;
}
- /// \deprecated see NodePtr_t node() const
+ /// \deprecated see StatePtr_t state() const
bool isInNodeFrom () const HPP_MANIPULATION_DEPRECATED
{
- return node_.lock() == from_.lock();
+ return state_.lock() == from_.lock();
}
/// Get steering method associated to the edge.
@@ -180,7 +194,7 @@ namespace hpp {
/// Print the object in a stream.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
- /// Constraint of the destination node and of the path
+ /// Constraint of the destination state and of the path
ConstraintSetPtr_t configConstraint() const;
void setShort (bool isShort) {
@@ -193,8 +207,8 @@ namespace hpp {
protected:
/// Initialization of the object.
- void init (const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
- const NodeWkPtr_t& to);
+ void init (const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const StateWkPtr_t& from,
+ const StateWkPtr_t& to);
/// Constructor
Edge (const std::string& name);
@@ -225,10 +239,10 @@ namespace hpp {
Constraint_t* configConstraints_;
/// The two ends of the transition.
- NodeWkPtr_t from_, to_;
+ StateWkPtr_t from_, to_;
- /// True if this path is in node from, False if in node to
- NodeWkPtr_t node_;
+ /// True if this path is in state from, False if in state to
+ StateWkPtr_t state_;
/// Steering method used to create paths associated to the edge
SteeringMethod_t* steeringMethod_;
@@ -245,17 +259,17 @@ namespace hpp {
/// Edge with waypoint.
/// Waypoints are handled recursively, i.e.\ class WaypointEdge contains only a
- /// Node and an Edge, the second Edge being itself.
- /// In this package, the Node in a WaypointEdge is semantically different from other Node
+ /// State and an Edge, the second Edge being itself.
+ /// In this package, the State in a WaypointEdge is semantically different from other State
/// because it does not correspond to a state with different manipulation rules. It has
- /// the same rules as another Node (either Edge::from() or Edge::to()).
+ /// the same rules as another State (either Edge::from() or Edge::to()).
///
- /// Semantically, a waypoint Node is fully part of the WaypointEdge. When a corresponding path
+ /// Semantically, a waypoint State is fully part of the WaypointEdge. When a corresponding path
/// reaches it, no planning is required to know what to do next. To the contrary, when a path reaches
/// Edge::from() or Edge::to(), there may be several possibilities.
///
/// \note
- /// Implementation details: let's say, between the two nodes \f$N_f\f$ and \f$N_t\f$,
+ /// Implementation details: let's say, between the two states \f$N_f\f$ and \f$N_t\f$,
/// two waypoints are required:
/// \f$ N_f \xrightarrow{e_0} n_0 \xrightarrow{e_1} n_1 \xrightarrow{E} N_t\f$.
/// The outmost WaypointEdg contains:
@@ -275,8 +289,8 @@ namespace hpp {
/// Create a new WaypointEdge.
static WaypointEdgePtr_t create
(const std::string& name,
- const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
- const NodeWkPtr_t& to);
+ const GraphWkPtr_t& graph, const StateWkPtr_t& from,
+ const StateWkPtr_t& to);
virtual bool direction (const core::PathPtr_t& path) const;
@@ -303,11 +317,15 @@ namespace hpp {
}
/// Set waypoint index with wEdge and wTo.
- /// \param wTo is the destination node of wEdge
- void setWaypoint (const std::size_t index, const EdgePtr_t wEdge, const NodePtr_t wTo);
+ /// \param wTo is the destination state of wEdge
+ void setWaypoint (const std::size_t index, const EdgePtr_t wEdge, const StatePtr_t wTo);
+
+ /// Get the state in which path after the waypoint is.
+ /// \deprecated use StatePtr_t state () const instead
+ StatePtr_t node () const;
- /// Get the node in which path after the waypoint is.
- NodePtr_t node () const;
+ /// Get the state in which path after the waypoint is.
+ StatePtr_t state () const;
protected:
WaypointEdge (const std::string& name) :
@@ -315,14 +333,14 @@ namespace hpp {
{
}
/// Initialization of the object.
- void init (const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
- const NodeWkPtr_t& to);
+ void init (const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const StateWkPtr_t& from,
+ const StateWkPtr_t& to);
/// Print the object in a stream.
virtual std::ostream& print (std::ostream& os) const;
private:
- typedef std::pair < EdgePtr_t, NodePtr_t > Waypoint_t;
+ typedef std::pair < EdgePtr_t, StatePtr_t > Waypoint_t;
typedef std::vector <Waypoint_t> Waypoints_t;
Waypoints_t waypoints_;
@@ -342,8 +360,8 @@ namespace hpp {
/// Create a new LevelSetEdge.
static LevelSetEdgePtr_t create
(const std::string& name,
- const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
- const NodeWkPtr_t& to);
+ const GraphWkPtr_t& graph, const StateWkPtr_t& from,
+ const StateWkPtr_t& to);
virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
@@ -387,8 +405,8 @@ namespace hpp {
protected:
/// Initialization of the object.
- void init (const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
- const NodeWkPtr_t& to);
+ void init (const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const StateWkPtr_t& from,
+ const StateWkPtr_t& to);
LevelSetEdge (const std::string& name);
diff --git a/include/hpp/manipulation/graph/fwd.hh b/include/hpp/manipulation/graph/fwd.hh
index 0d0a56abe5b59db83509f23ea79cf4b7f04caa90..c9bf318bf9b361696d4b340bc6193b3962e93fb6 100644
--- a/include/hpp/manipulation/graph/fwd.hh
+++ b/include/hpp/manipulation/graph/fwd.hh
@@ -25,26 +25,27 @@ namespace hpp {
namespace manipulation {
namespace graph {
HPP_PREDEF_CLASS (Graph);
- HPP_PREDEF_CLASS (Node);
HPP_PREDEF_CLASS (Edge);
+ HPP_PREDEF_CLASS (State);
HPP_PREDEF_CLASS (WaypointEdge);
HPP_PREDEF_CLASS (LevelSetEdge);
- HPP_PREDEF_CLASS (NodeSelector);
- HPP_PREDEF_CLASS (GuidedNodeSelector);
+ HPP_PREDEF_CLASS (StateSelector);
HPP_PREDEF_CLASS (GraphComponent);
+ HPP_PREDEF_CLASS (GuidedStateSelector);
typedef boost::shared_ptr < Graph > GraphPtr_t;
- typedef boost::shared_ptr < Node > NodePtr_t;
+ typedef boost::shared_ptr < State > StatePtr_t;
typedef boost::shared_ptr < Edge > EdgePtr_t;
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 < StateSelector > StateSelectorPtr_t;
+ typedef boost::shared_ptr < GuidedStateSelector >
+ GuidedStateSelectorPtr_t;
typedef boost::shared_ptr < GraphComponent > GraphComponentPtr_t;
- typedef std::vector < NodePtr_t > Nodes_t;
+ typedef std::vector < StatePtr_t > States_t;
typedef std::vector < EdgePtr_t > Edges_t;
typedef ::hpp::statistics::DiscreteDistribution< EdgePtr_t >::Weight_t Weight_t;
typedef ::hpp::statistics::DiscreteDistribution< EdgePtr_t > Neighbors_t;
- typedef std::vector < NodeSelectorPtr_t > NodeSelectors_t;
+ typedef std::vector < StateSelectorPtr_t > StateSelectors_t;
typedef hpp::core::Constraint Constraint;
typedef hpp::core::ConstraintPtr_t ConstraintPtr_t;
@@ -61,10 +62,10 @@ namespace hpp {
typedef hpp::core::LockedJoints_t LockedJoints_t;
class Histogram;
- class NodeHistogram;
+ class StateHistogram;
class LeafHistogram;
typedef boost::shared_ptr <Histogram> HistogramPtr_t;
- typedef boost::shared_ptr <NodeHistogram> NodeHistogramPtr_t;
+ typedef boost::shared_ptr <StateHistogram> StateHistogramPtr_t;
typedef boost::shared_ptr <LeafHistogram> LeafHistogramPtr_t;
typedef std::list < HistogramPtr_t > Histograms_t;
} // namespace graph
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index a33e743dcafd7bd2edea28a45a36450dde83812b..0675d76329ac44916d4f398346484ec990f120ca 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -35,9 +35,9 @@ namespace hpp {
///
/// One must make sure not to create loop with shared pointers.
/// To ensure that, the classes are defined as follow:
- /// - A Graph owns (i.e. has a shared pointer to) the NodeSelector s
- /// - A NodeSelector owns the Node s related to one gripper.
- /// - A Node owns its outgoing Edge s.
+ /// - A Graph owns (i.e. has a shared pointer to) the StateSelector s
+ /// - A StateSelector owns the Node s related to one gripper.
+ /// - A State owns its outgoing Edge s.
/// - An Edge does not own anything.
class HPP_MANIPULATION_DLLAPI Graph : public GraphComponent
{
@@ -49,54 +49,96 @@ namespace hpp {
static GraphPtr_t create(const std::string& name, DevicePtr_t robot,
const ProblemPtr_t& problem);
- /// Create and insert a NodeSelector inside the graph.
- NodeSelectorPtr_t createNodeSelector (const std::string& name);
+ /// Create and insert a state selector inside the graph.
+ /// \deprecated use createStateSelector instead
+ StateSelectorPtr_t createNodeSelector (const std::string& name)
+ HPP_MANIPULATION_DEPRECATED;
- /// Set the nodeSelector
+ /// Create and insert a state selector inside the graph.
+ StateSelectorPtr_t createStateSelector (const std::string& name);
+
+ /// Set the state selector
+ /// \warning This should be done before adding nodes to the node
+ /// selector otherwise the pointer to the parent graph will NOT be
+ /// valid.
+ /// \deprecated use stateSelector instead
+ void nodeSelector (StateSelectorPtr_t ns) HPP_MANIPULATION_DEPRECATED;
+
+ /// Set the state selector
/// \warning This should be done before adding nodes to the node
/// selector otherwise the pointer to the parent graph will NOT be
/// valid.
- void nodeSelector (NodeSelectorPtr_t ns);
+ void stateSelector (StateSelectorPtr_t ns);
+
+ /// Get the state selector
+ /// \deprecated use stateSelector instead
+ StateSelectorPtr_t nodeSelector () const HPP_MANIPULATION_DEPRECATED
+ {
+ return stateSelector_;
+ }
- /// Get the nodeSelector
- NodeSelectorPtr_t nodeSelector () const
+ /// Get the state selector
+ StateSelectorPtr_t stateSelector () const
{
- return nodeSelector_;
+ return stateSelector_;
}
/// Returns the states of a configuration.
- NodePtr_t getNode (ConfigurationIn_t config) const;
+ /// \deprecated use getState instead
+ StatePtr_t getNode (ConfigurationIn_t config) const
+ HPP_MANIPULATION_DEPRECATED;
+
+ /// Returns the state of a configuration.
+ StatePtr_t getState (ConfigurationIn_t config) const;
+
+ /// Returns the state of a roadmap node
+ /// \deprecated use getState instead
+ StatePtr_t getNode(RoadmapNodePtr_t node) const;
/// Returns the state of a roadmap node
- NodePtr_t getNode(RoadmapNodePtr_t node) const;
+ StatePtr_t getState (RoadmapNodePtr_t node) const;
/// Get possible edges between two nodes.
- Edges_t getEdges (const NodePtr_t& from, const NodePtr_t& to) const;
+ Edges_t getEdges (const StatePtr_t& from, const StatePtr_t& to) const;
/// Select randomly outgoing edge of the given node.
EdgePtr_t chooseEdge(RoadmapNodePtr_t node) const;
/// Constraint to project onto the Node.
- /// \param the Node_t on which to project.
+ /// \param state the state on which to project.
/// \return The initialized projector.
- ConstraintSetPtr_t configConstraint (const NodePtr_t& node);
+ ConstraintSetPtr_t configConstraint (const StatePtr_t& state);
/// 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 EdgePtr_t& edge);
- /// Get error of a config with respect to a node constraint
+ /// Get error of a config with respect to a state constraint
///
/// \param config Configuration,
- /// \param node node containing the constraint to check config against
- /// \retval error the error of the node constraint for the
+ /// \param state state containing the constraint to check config against
+ /// \retval error the error of the state constraint for the
/// configuration
- /// \return whether the configuration belongs to the node.
- /// Call method core::ConstraintSet::isSatisfied for the node
+ /// \return whether the configuration belongs to the state.
+ /// Call method core::ConstraintSet::isSatisfied for the state
/// constraints.
+ /// \deprecated use getConfigErrorForState instead
bool getConfigErrorForNode (ConfigurationIn_t config,
- const NodePtr_t& node, vector_t& error);
+ const StatePtr_t& state, vector_t& error)
+ HPP_MANIPULATION_DEPRECATED;
+
+ /// Get error of a config with respect to a state constraint
+ ///
+ /// \param config Configuration,
+ /// \param state state containing the constraint to check config against
+ /// \retval error the error of the state constraint for the
+ /// configuration
+ /// \return whether the configuration belongs to the state.
+ /// Call method core::ConstraintSet::isSatisfied for the state
+ /// constraints.
+ bool getConfigErrorForState (ConfigurationIn_t config,
+ const StatePtr_t& state, vector_t& error);
/// Get error of a config with respect to an edge constraint
///
@@ -178,8 +220,8 @@ namespace hpp {
std::ostream& print (std::ostream& os) const;
private:
- /// This list contains a node selector for each end-effector.
- NodeSelectorPtr_t nodeSelector_;
+ /// This list contains a state selector for each end-effector.
+ StateSelectorPtr_t stateSelector_;
/// A set of constraints that will always be used, for example
/// stability constraints.
@@ -191,10 +233,10 @@ namespace hpp {
/// Weak pointer to itself.
GraphWkPtr_t wkPtr_;
- /// 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 State).
+ typedef std::map < StatePtr_t, ConstraintSetPtr_t > MapFromState;
+ typedef std::pair < StatePtr_t, ConstraintSetPtr_t > PairStateConstraints;
+ MapFromState constraintSetMapFromState_;
/// List of histograms
Histograms_t hists_;
diff --git a/include/hpp/manipulation/graph/guided-node-selector.hh b/include/hpp/manipulation/graph/guided-node-selector.hh
index 2d75139ce0411466b204b93f2f20d1e2cd9c17e7..7d424640b152fab9a2d418b1aa90a5f1b6f9d771 100644
--- a/include/hpp/manipulation/graph/guided-node-selector.hh
+++ b/include/hpp/manipulation/graph/guided-node-selector.hh
@@ -17,52 +17,14 @@
#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"
+#include "hpp/manipulation/graph/guided-state-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(RoadmapNodePtr_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
+ typedef GuidedStateSelector GuidedNodeSelector
+ HPP_MANIPULATION_DEPRECATED;
+ typedef boost::shared_ptr < GuidedNodeSelector > GuidedNodeSelectorPtr_t;
} // namespace graph
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/graph/guided-state-selector.hh b/include/hpp/manipulation/graph/guided-state-selector.hh
new file mode 100644
index 0000000000000000000000000000000000000000..4d2ddee3b9dc784dec3fdad2447daa14c94e0d89
--- /dev/null
+++ b/include/hpp/manipulation/graph/guided-state-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_STATE_SELECTOR_HH
+# define HPP_MANIPULATION_GRAPH_GUIDED_STATE_SELECTOR_HH
+
+#include "hpp/manipulation/fwd.hh"
+#include "hpp/manipulation/graph/fwd.hh"
+#include "hpp/manipulation/graph/state-selector.hh"
+
+namespace hpp {
+ namespace manipulation {
+ namespace graph {
+ class HPP_MANIPULATION_DLLAPI GuidedStateSelector : public StateSelector
+ {
+ public:
+ /// Create a new GuidedStateSelector.
+ static GuidedStateSelectorPtr_t create(const std::string& name,
+ const core::RoadmapPtr_t& roadmap);
+
+ /// Set the target
+ void setStateList (const States_t& stateList);
+
+ /// Select randomly an outgoing edge of the given node.
+ virtual EdgePtr_t chooseEdge(RoadmapNodePtr_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 GuidedStateSelectorPtr_t& weak);
+
+ /// Constructor
+ GuidedStateSelector (const std::string& name,
+ const core::RoadmapPtr_t roadmap) :
+ StateSelector (name), roadmap_ (roadmap)
+ {}
+
+ /// Print the object in a stream.
+ std::ostream& print (std::ostream& os) const;
+
+ private:
+ /// The target
+ States_t stateList_;
+
+ /// The roadmap
+ core::RoadmapPtr_t roadmap_;
+
+ /// Weak pointer to itself.
+ GuidedStateSelectorWkPtr_t wkPtr_;
+ }; // Class StateSelector
+ } // namespace graph
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_GRAPH_GUIDED_STATE_SELECTOR_HH
diff --git a/include/hpp/manipulation/graph/helper.hh b/include/hpp/manipulation/graph/helper.hh
index f891610208ee2dfe054f7c9cc8d6c953e64e6145..bf83c0011069ec133b0c160befc31e08343b7191 100644
--- a/include/hpp/manipulation/graph/helper.hh
+++ b/include/hpp/manipulation/graph/helper.hh
@@ -75,7 +75,7 @@ namespace hpp {
return both;
}
- void addToNode (NodePtr_t comp) const;
+ void addToState (StatePtr_t comp) const;
void addToEdge (EdgePtr_t comp) const;
void specifyFoliation (LevelSetEdgePtr_t lse) const;
@@ -120,7 +120,7 @@ namespace hpp {
template < int gCase >
Edges_t createEdges (
const std::string& forwName, const std::string& backName,
- const NodePtr_t& from, const NodePtr_t& to,
+ const StatePtr_t& from, const StatePtr_t& to,
const size_type& wForw, const size_type& wBack,
const FoliatedManifold& grasp, const FoliatedManifold& pregrasp,
const FoliatedManifold& place, const FoliatedManifold& preplace,
@@ -130,7 +130,7 @@ namespace hpp {
EdgePtr_t createLoopEdge (
const std::string& loopName,
- const NodePtr_t& node,
+ const StatePtr_t& state,
const size_type& w,
const bool levelSet,
const FoliatedManifold& submanifoldDef = FoliatedManifold ()
diff --git a/include/hpp/manipulation/graph/node-selector.hh b/include/hpp/manipulation/graph/node-selector.hh
index e62c0f9b68109fb82208183ebb21a4ad67a5877c..5d99087aa4d1938eba0744758d68a994ab122f9e 100644
--- a/include/hpp/manipulation/graph/node-selector.hh
+++ b/include/hpp/manipulation/graph/node-selector.hh
@@ -17,77 +17,14 @@
#ifndef HPP_MANIPULATION_GRAPH_NODE_SELECTOR_HH
# define HPP_MANIPULATION_GRAPH_NODE_SELECTOR_HH
-#include "hpp/manipulation/config.hh"
-#include "hpp/manipulation/fwd.hh"
-#include "hpp/manipulation/graph/graph.hh"
-#include "hpp/manipulation/graph/node.hh"
+#include "hpp/manipulation/graph/state-selector.hh"
namespace hpp {
namespace manipulation {
namespace graph {
- /// This class is used to get the state of a configuration. States have to
- /// be ordered because a configuration can be in several states.
- class HPP_MANIPULATION_DLLAPI NodeSelector : public GraphComponent
- {
- public:
- /// Create a new NodeSelector.
- static NodeSelectorPtr_t create(const std::string& name);
-
- /// Create an empty node
- NodePtr_t createNode (const std::string& name, bool waypoint = false,
- const int w = 0);
-
- /// Returns the state of a configuration.
- NodePtr_t getNode(ConfigurationIn_t config) const;
-
- /// Returns the state of a roadmap node
- NodePtr_t getNode(RoadmapNodePtr_t node) const;
-
- /// Returns a list of all the nodes
- Nodes_t getNodes () const;
-
- /// Select randomly an outgoing edge of the given node.
- virtual EdgePtr_t chooseEdge(RoadmapNodePtr_t from) const;
-
- /// Should never be called.
- void addNumericalConstraint (
- const core::NumericalConstraintPtr_t& /* function */,
- const SizeIntervals_t& /* passiveDofs */ = SizeIntervals_t ())
- {
- HPP_THROW_EXCEPTION (Bad_function_call, "This component does not have constraints.");
- }
-
- /// Should never be called.
- void addLockedJointConstraint
- (const core::LockedJoint& /* constraint */)
- {
- HPP_THROW_EXCEPTION (Bad_function_call, "This component does not have constraints.");
- }
-
- /// Print the object in a stream.
- virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
-
- protected:
- /// Initialization of the object.
- void init (const NodeSelectorPtr_t& weak);
-
- /// Constructor
- NodeSelector (const std::string& name) : GraphComponent (name)
- {}
-
- /// Print the object in a stream.
- virtual std::ostream& print (std::ostream& os) const;
-
- /// List of the states of one end-effector, ordered by priority.
- typedef std::pair <int, NodePtr_t> WeighedNode_t;
- typedef std::list <WeighedNode_t> WeighedNodes_t;
- WeighedNodes_t orderedStates_;
- Nodes_t waypoints_;
-
- private:
- /// Weak pointer to itself.
- NodeSelectorPtr_t wkPtr_;
- }; // Class NodeSelector
+ typedef StateSelector NodeSelector HPP_MANIPULATION_DEPRECATED;
+ typedef boost::shared_ptr < NodeSelector > NodeSelectorPtr_t;
+ typedef std::vector < NodeSelectorPtr_t > NodeSelectors_t;
} // namespace graph
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/graph/node.hh b/include/hpp/manipulation/graph/node.hh
index 9c6d2b1840514a3800100749c62ff81778fc4092..2a9421ef4693125a8bb4dbf3686b662bc81446bb 100644
--- a/include/hpp/manipulation/graph/node.hh
+++ b/include/hpp/manipulation/graph/node.hh
@@ -17,168 +17,14 @@
#ifndef HPP_MANIPULATION_GRAPH_NODE_HH
# define HPP_MANIPULATION_GRAPH_NODE_HH
-# include <boost/function.hpp>
-
-#include <hpp/core/locked-joint.hh>
-#include <hpp/core/constraint-set.hh>
-#include <hpp/core/config-projector.hh>
-
-#include "hpp/manipulation/config.hh"
-#include "hpp/manipulation/deprecated.hh"
-#include "hpp/manipulation/fwd.hh"
-#include "hpp/manipulation/graph/fwd.hh"
-#include "hpp/manipulation/graph/edge.hh"
-#include "hpp/manipulation/graph/graph-component.hh"
+# include <hpp/manipulation/graph/state.hh>
namespace hpp {
namespace manipulation {
namespace graph {
- /// \addtogroup constraint_graph
- /// \{
-
- /// State of an end-effector.
- ///
- /// Nodes of the graph of constraints. There is one
- /// graph for each end-effector.
- class HPP_MANIPULATION_DLLAPI Node : public GraphComponent
- {
- public:
- typedef boost::function < EdgePtr_t
- (const std::string&,
- const GraphWkPtr_t&,
- const NodeWkPtr_t&, const NodeWkPtr_t&) >
- EdgeFactory;
- /// Destructor
- ~Node ();
-
- /// Create a new node.
- static NodePtr_t create (const std::string& name);
-
- /// Create a link from this node to the given node.
- /// \param w if strictly negative, the edge is not included in the neighbor
- /// list. Otherwise, it is included with Weight_t w
- EdgePtr_t linkTo (const std::string& name, const NodePtr_t& to,
- const size_type& w = 1,
- EdgeFactory create = Edge::create);
-
- /// Check whether the configuration is in this state.
- /// \code
- /// return configConstraint()->isSatisfied (config);
- /// \endcode
- /// \note You should not use this method to know in which states a
- /// configuration is. This only checks if the configuration satisfies
- /// 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
- {
- return selector_;
- }
-
- /// Set the NodeSelector containing this node.
- void nodeSelector (const NodeSelectorWkPtr_t& parent)
- {
- selector_ = parent;
- };
-
- /// Get the neighbors
- const Neighbors_t& neighbors() const
- {
- return neighbors_;
- }
-
- void updateWeight (const EdgePtr_t&edge, const Weight_t& w);
-
- Weight_t getWeight (const EdgePtr_t&edge);
-
- /// Constraint to project onto this node.
- ConstraintSetPtr_t configConstraint() const;
-
- /// Add core::NumericalConstraint to the component.
- virtual void addNumericalConstraintForPath (const NumericalConstraintPtr_t& nm,
- const SizeIntervals_t& passiveDofs = SizeIntervals_t ())
- {
- numericalConstraintsForPath_.push_back (nm);
- passiveDofsForPath_.push_back (passiveDofs);
- }
-
- /// Add core::DifferentiableFunction to the component.
- virtual void addNumericalConstraintForPath (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
- HPP_MANIPULATION_DEPRECATED
- {
- numericalConstraintsForPath_.push_back (NumericalConstraint::create (function,ineq));
- }
-
- /// Insert the numerical constraints in a ConfigProjector
- /// \return true is at least one NumericalConstraintPtr_t was inserted.
- bool insertNumericalConstraintsForPath (ConfigProjectorPtr_t& proj) const
- {
- assert (numericalConstraintsForPath_.size () == passiveDofsForPath_.size ());
- IntervalsContainer_t::const_iterator itpdofs = passiveDofsForPath_.begin ();
- for (NumericalConstraints_t::const_iterator it = numericalConstraintsForPath_.begin();
- it != numericalConstraintsForPath_.end(); it++) {
- proj->add (*it, *itpdofs);
- itpdofs++;
- }
- return !numericalConstraintsForPath_.empty ();
- }
-
- /// Get a reference to the NumericalConstraints_t
- const NumericalConstraints_t& numericalConstraintsForPath () const
- {
- return numericalConstraintsForPath_;
- }
-
- /// Print the object in a stream.
- std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
-
- protected:
- /// Initialize the object.
- void init (const NodeWkPtr_t& self);
-
- /// Constructor
- Node(const std::string& name);
-
- /// Print the object in a stream.
- std::ostream& print (std::ostream& os) const;
-
- virtual void populateTooltip (dot::Tooltip& tp) const;
-
- private:
- /// List of possible motions from this state (i.e. the outgoing
- /// vertices).
- Neighbors_t neighbors_;
- std::vector <EdgePtr_t> hiddenNeighbors_;
-
- /// Set of constraints to be statisfied.
- typedef Cache < ConstraintSetPtr_t > Constraint_t;
- Constraint_t* configConstraints_;
-
- /// Stores the numerical constraints for path.
- NumericalConstraints_t numericalConstraintsForPath_;
- IntervalsContainer_t passiveDofsForPath_;
-
- /// A selector that will implement the selection of the next state.
- NodeSelectorWkPtr_t selector_;
-
- /// Weak pointer to itself.
- NodeWkPtr_t wkPtr_;
-
- bool isWaypoint_;
- }; // class Node
-
- /// \}
+ typedef State Node HPP_MANIPULATION_DEPRECATED;
+ typedef boost::shared_ptr < Node > NodePtr_t;
+ typedef std::vector < NodePtr_t > Nodes_t;
} // namespace graph
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/graph/state-selector.hh b/include/hpp/manipulation/graph/state-selector.hh
new file mode 100644
index 0000000000000000000000000000000000000000..fa560dc91e4f3de97468e6881a5f3db244dde7bd
--- /dev/null
+++ b/include/hpp/manipulation/graph/state-selector.hh
@@ -0,0 +1,95 @@
+// 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_STATE_SELECTOR_HH
+# define HPP_MANIPULATION_GRAPH_STATE_SELECTOR_HH
+
+#include "hpp/manipulation/config.hh"
+#include "hpp/manipulation/fwd.hh"
+#include "hpp/manipulation/graph/graph.hh"
+#include "hpp/manipulation/graph/state.hh"
+
+namespace hpp {
+ namespace manipulation {
+ namespace graph {
+ /// This class is used to get the state of a configuration. States have to
+ /// be ordered because a configuration can be in several states.
+ class HPP_MANIPULATION_DLLAPI StateSelector : public GraphComponent
+ {
+ public:
+ /// Create a new StateSelector.
+ static StateSelectorPtr_t create(const std::string& name);
+
+ /// Create an empty state
+ StatePtr_t createState (const std::string& name, bool waypoint = false,
+ const int w = 0);
+
+ /// Returns the state of a configuration.
+ StatePtr_t getState(ConfigurationIn_t config) const;
+
+ /// Returns the state of a roadmap state
+ StatePtr_t getState(RoadmapNodePtr_t node) const;
+
+ /// Returns a list of all the states
+ States_t getStates () const;
+
+ /// Select randomly an outgoing edge of the given state.
+ virtual EdgePtr_t chooseEdge(RoadmapNodePtr_t from) const;
+
+ /// Should never be called.
+ void addNumericalConstraint (
+ const core::NumericalConstraintPtr_t& /* function */,
+ const SizeIntervals_t& /* passiveDofs */ = SizeIntervals_t ())
+ {
+ HPP_THROW_EXCEPTION (Bad_function_call, "This component does not have constraints.");
+ }
+
+ /// Should never be called.
+ void addLockedJointConstraint
+ (const core::LockedJoint& /* constraint */)
+ {
+ HPP_THROW_EXCEPTION (Bad_function_call, "This component does not have constraints.");
+ }
+
+ /// Print the object in a stream.
+ virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
+
+ protected:
+ /// Initialization of the object.
+ void init (const StateSelectorPtr_t& weak);
+
+ /// Constructor
+ StateSelector (const std::string& name) : GraphComponent (name)
+ {}
+
+ /// Print the object in a stream.
+ virtual std::ostream& print (std::ostream& os) const;
+
+ /// List of the states of one end-effector, ordered by priority.
+ typedef std::pair <int, StatePtr_t> WeighedState_t;
+ typedef std::list <WeighedState_t> WeighedStates_t;
+ WeighedStates_t orderedStates_;
+ States_t waypoints_;
+
+ private:
+ /// Weak pointer to itself.
+ StateSelectorPtr_t wkPtr_;
+ }; // Class StateSelector
+ } // namespace graph
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_GRAPH_STATE_SELECTOR_HH
diff --git a/include/hpp/manipulation/graph/state.hh b/include/hpp/manipulation/graph/state.hh
new file mode 100644
index 0000000000000000000000000000000000000000..23dd8fb423341999c0fe523b95ea918fcb1788a9
--- /dev/null
+++ b/include/hpp/manipulation/graph/state.hh
@@ -0,0 +1,186 @@
+// 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_STATE_HH
+# define HPP_MANIPULATION_GRAPH_STATE_HH
+
+# include <boost/function.hpp>
+
+#include <hpp/core/locked-joint.hh>
+#include <hpp/core/constraint-set.hh>
+#include <hpp/core/config-projector.hh>
+
+#include "hpp/manipulation/config.hh"
+#include "hpp/manipulation/deprecated.hh"
+#include "hpp/manipulation/fwd.hh"
+#include "hpp/manipulation/graph/fwd.hh"
+#include "hpp/manipulation/graph/edge.hh"
+#include "hpp/manipulation/graph/graph-component.hh"
+
+namespace hpp {
+ namespace manipulation {
+ namespace graph {
+ /// \addtogroup constraint_graph
+ /// \{
+
+ /// State of an end-effector.
+ ///
+ /// States of the graph of constraints. There is one
+ /// graph for each end-effector.
+ class HPP_MANIPULATION_DLLAPI State : public GraphComponent
+ {
+ public:
+ typedef boost::function < EdgePtr_t
+ (const std::string&,
+ const GraphWkPtr_t&,
+ const StateWkPtr_t&, const StateWkPtr_t&) >
+ EdgeFactory;
+ /// Destructor
+ ~State ();
+
+ /// Create a new state.
+ static StatePtr_t create (const std::string& name);
+
+ /// Create a link from this state to the given state.
+ /// \param w if strictly negative, the edge is not included in the neighbor
+ /// list. Otherwise, it is included with Weight_t w
+ EdgePtr_t linkTo (const std::string& name, const StatePtr_t& to,
+ const size_type& w = 1,
+ EdgeFactory create = Edge::create);
+
+ /// Check whether the configuration is in this state.
+ /// \code
+ /// return configConstraint()->isSatisfied (config);
+ /// \endcode
+ /// \note You should not use this method to know in which states a
+ /// configuration is. This only checks if the configuration satisfies
+ /// the constraints. Instead, use the class StateSelector.
+ virtual bool contains (ConfigurationIn_t config) const;
+
+ inline bool isWaypoint () const
+ {
+ return isWaypoint_;
+ }
+
+ inline void isWaypoint (bool isWaypoint)
+ {
+ isWaypoint_ = isWaypoint;
+ }
+
+ /// Get the parent StateSelector.
+ StateSelectorWkPtr_t stateSelector () const
+ {
+ return selector_;
+ }
+
+ /// Set the StateSelector containing this state.
+ void stateSelector (const StateSelectorWkPtr_t& parent)
+ {
+ selector_ = parent;
+ };
+
+ /// Get the neighbors
+ const Neighbors_t& neighbors() const
+ {
+ return neighbors_;
+ }
+
+ void updateWeight (const EdgePtr_t&edge, const Weight_t& w);
+
+ Weight_t getWeight (const EdgePtr_t&edge);
+
+ /// Constraint to project onto this state.
+ ConstraintSetPtr_t configConstraint() const;
+
+ /// Add core::NumericalConstraint to the component.
+ virtual void addNumericalConstraintForPath (const NumericalConstraintPtr_t& nm,
+ const SizeIntervals_t& passiveDofs = SizeIntervals_t ())
+ {
+ numericalConstraintsForPath_.push_back (nm);
+ passiveDofsForPath_.push_back (passiveDofs);
+ }
+
+ /// Add core::DifferentiableFunction to the component.
+ virtual void addNumericalConstraintForPath (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
+ HPP_MANIPULATION_DEPRECATED
+ {
+ numericalConstraintsForPath_.push_back (NumericalConstraint::create (function,ineq));
+ }
+
+ /// Insert the numerical constraints in a ConfigProjector
+ /// \return true is at least one NumericalConstraintPtr_t was inserted.
+ bool insertNumericalConstraintsForPath (ConfigProjectorPtr_t& proj) const
+ {
+ assert (numericalConstraintsForPath_.size () == passiveDofsForPath_.size ());
+ IntervalsContainer_t::const_iterator itpdofs = passiveDofsForPath_.begin ();
+ for (NumericalConstraints_t::const_iterator it = numericalConstraintsForPath_.begin();
+ it != numericalConstraintsForPath_.end(); it++) {
+ proj->add (*it, *itpdofs);
+ itpdofs++;
+ }
+ return !numericalConstraintsForPath_.empty ();
+ }
+
+ /// Get a reference to the NumericalConstraints_t
+ const NumericalConstraints_t& numericalConstraintsForPath () const
+ {
+ return numericalConstraintsForPath_;
+ }
+
+ /// Print the object in a stream.
+ std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
+
+ protected:
+ /// Initialize the object.
+ void init (const StateWkPtr_t& self);
+
+ /// Constructor
+ State(const std::string& name);
+
+ /// Print the object in a stream.
+ std::ostream& print (std::ostream& os) const;
+
+ virtual void populateTooltip (dot::Tooltip& tp) const;
+
+ private:
+ /// List of possible motions from this state (i.e. the outgoing
+ /// vertices).
+ Neighbors_t neighbors_;
+ std::vector <EdgePtr_t> hiddenNeighbors_;
+
+ /// Set of constraints to be statisfied.
+ typedef Cache < ConstraintSetPtr_t > Constraint_t;
+ Constraint_t* configConstraints_;
+
+ /// Stores the numerical constraints for path.
+ NumericalConstraints_t numericalConstraintsForPath_;
+ IntervalsContainer_t passiveDofsForPath_;
+
+ /// A selector that will implement the selection of the next state.
+ StateSelectorWkPtr_t selector_;
+
+ /// Weak pointer to itself.
+ StateWkPtr_t wkPtr_;
+
+ bool isWaypoint_;
+ }; // class State
+
+ /// \}
+ } // namespace graph
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_GRAPH_STATE_HH
diff --git a/include/hpp/manipulation/graph/statistics.hh b/include/hpp/manipulation/graph/statistics.hh
index 46e19eb50f9a61b1fa4387600ce4ff0028eea7ef..b58156f18591a1a807ef92f665da85c7e23b3291 100644
--- a/include/hpp/manipulation/graph/statistics.hh
+++ b/include/hpp/manipulation/graph/statistics.hh
@@ -25,7 +25,7 @@
# include "hpp/manipulation/config.hh"
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/graph.hh"
-# include "hpp/manipulation/graph/node.hh"
+# include "hpp/manipulation/graph/state.hh"
# include <hpp/manipulation/roadmap-node.hh>
namespace hpp {
@@ -71,7 +71,7 @@ namespace hpp {
{
public :
typedef ::hpp::statistics::Bin Parent;
- NodeBin(const NodePtr_t& n);
+ NodeBin(const StatePtr_t& n);
void push_back(const RoadmapNodePtr_t& n);
@@ -79,12 +79,12 @@ namespace hpp {
bool operator==(const NodeBin& rhs) const;
- const NodePtr_t& node () const;
+ const StatePtr_t& state () const;
std::ostream& print (std::ostream& os) const;
private:
- NodePtr_t node_;
+ StatePtr_t state_;
typedef std::list <RoadmapNodePtr_t> RoadmapNodes_t;
RoadmapNodes_t roadmapNodes_;
@@ -158,7 +158,7 @@ namespace hpp {
protected:
/// Constructor
- /// \param node defines the submanifold containing the foliation.
+ /// \param state defines the submanifold containing the foliation.
/// \param constraint The constraint that create the foliation being
/// studied.
LeafHistogram (const Foliation f);
@@ -170,15 +170,15 @@ namespace hpp {
value_type threshold_;
};
- class HPP_MANIPULATION_DLLLOCAL NodeHistogram : public ::hpp::statistics::Statistics < NodeBin >
+ class HPP_MANIPULATION_DLLLOCAL StateHistogram : public ::hpp::statistics::Statistics < NodeBin >
, public Histogram
{
public:
typedef ::hpp::statistics::Statistics < NodeBin > Parent;
/// Constructor
- /// \param graph The constraint graph used to get the nodes from
+ /// \param graph The constraint graph used to get the states from
/// a configuration.
- NodeHistogram (const graph::GraphPtr_t& graph);
+ StateHistogram (const graph::GraphPtr_t& graph);
/// Insert an occurence of a value in the histogram
void add (const RoadmapNodePtr_t& n);
@@ -195,6 +195,8 @@ namespace hpp {
/// The constraint graph
graph::GraphPtr_t graph_;
};
+ typedef StateHistogram NodeHistogram HPP_MANIPULATION_DEPRECATED;
+ typedef boost::shared_ptr <StateHistogram> NodeHistogramPtr_t;
} // namespace graph
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/roadmap-node.hh b/include/hpp/manipulation/roadmap-node.hh
index 2fa8992e4b71af97c8aa60743e2b1e1b3163cb6e..50816a0aa63266d933198c4419a90004585ca2e4 100644
--- a/include/hpp/manipulation/roadmap-node.hh
+++ b/include/hpp/manipulation/roadmap-node.hh
@@ -34,14 +34,14 @@ namespace hpp {
RoadmapNode (const ConfigurationPtr_t& configuration) :
core::Node (configuration),
cacheSystem_ (defaultCachingSystem),
- node_ ()
+ state_ ()
{}
RoadmapNode (const ConfigurationPtr_t& configuration,
ConnectedComponentPtr_t cc) :
core::Node (configuration, cc),
cacheSystem_ (defaultCachingSystem),
- node_ ()
+ state_ ()
{}
/// \name Cache system
@@ -65,19 +65,35 @@ namespace hpp {
return cacheSystem_;
}
- /// Getter for the graph::Node.
- graph::NodePtr_t graphNode () const
+ /// Getter for the graph::State.
+ /// \deprecated use graphState instead.
+ graph::StatePtr_t graphNode () const HPP_MANIPULATION_DEPRECATED
{
- return node_;
+ return state_;
}
- /// Setter for the graph::Node.
- void graphNode (const graph::NodePtr_t& node)
+ /// Getter for the graph::State.
+ graph::StatePtr_t graphState () const
+ {
+ return state_;
+ }
+
+ /// Setter for the graph::State.
+ /// \deprecated use graphState instead
+ void graphNode (const graph::StatePtr_t& state)
+ HPP_MANIPULATION_DEPRECATED
{
if (cacheSystem_ != CACHE_DISABLED) cacheSystem_ = CACHE_UP_TO_DATE;
- node_ = node;
+ state_ = state;
}
+ /// Setter for the graph::State.
+ void graphState (const graph::StatePtr_t& state)
+ HPP_MANIPULATION_DEPRECATED
+ {
+ if (cacheSystem_ != CACHE_DISABLED) cacheSystem_ = CACHE_UP_TO_DATE;
+ state_ = state;
+ }
/// \}
void symbolicComponent (const SymbolicComponentPtr_t& sc)
@@ -93,7 +109,7 @@ namespace hpp {
private:
CachingSystem cacheSystem_;
- graph::NodePtr_t node_;
+ graph::StatePtr_t state_;
SymbolicComponentPtr_t symbolicCC_;
};
} // namespace manipulation
diff --git a/include/hpp/manipulation/roadmap.hh b/include/hpp/manipulation/roadmap.hh
index e236ace787192a03f30002fa02520e8dc8722fe8..bff7a43188ec798cf8fd979b0e2803b00e501bd5 100644
--- a/include/hpp/manipulation/roadmap.hh
+++ b/include/hpp/manipulation/roadmap.hh
@@ -22,6 +22,7 @@
# include "hpp/manipulation/config.hh"
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/fwd.hh"
+# include <hpp/manipulation/deprecated.hh>
namespace hpp {
namespace manipulation {
@@ -55,11 +56,15 @@ namespace hpp {
/// ConnectedComponent.
RoadmapNodePtr_t nearestNode (const ConfigurationPtr_t& configuration,
const ConnectedComponentPtr_t& connectedComponent,
- const graph::NodePtr_t& node,
+ const graph::StatePtr_t& state,
value_type& minDistance) const;
- /// Get graph node corresponding to given roadmap node
- graph::NodePtr_t getNode(RoadmapNodePtr_t node);
+ /// Get graph state corresponding to given roadmap node
+ /// \deprecated use getState instead
+ graph::StatePtr_t getNode(RoadmapNodePtr_t node) HPP_MANIPULATION_DEPRECATED;
+
+ /// Get graph state corresponding to given roadmap node
+ graph::StatePtr_t getState(RoadmapNodePtr_t node);
/// Get the symbolic components
const SymbolicComponents_t& symbolicComponents () const
diff --git a/include/hpp/manipulation/symbolic-component.hh b/include/hpp/manipulation/symbolic-component.hh
index c80add3462c73bdf5838179941d6db8eee63aa3e..7da561c7431f17f5087e627cdd35fcea5272d0d5 100644
--- a/include/hpp/manipulation/symbolic-component.hh
+++ b/include/hpp/manipulation/symbolic-component.hh
@@ -77,7 +77,7 @@ namespace hpp {
weak_ = shPtr;
}
- graph::NodePtr_t state_;
+ graph::StatePtr_t state_;
RoadmapNodes_t nodes_;
private:
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 98ea6c21ddfccfeab44ef184f608176eb32bfe93..4f241c677fd85926844636df18d32d6abbbdfc0b 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -37,12 +37,12 @@ SET(SOURCES
graph-steering-method.cc
graph-optimizer.cc
- graph/node.cc
+ graph/state.cc
graph/edge.cc
graph/graph.cc
graph/graph-component.cc
- graph/node-selector.cc
- graph/guided-node-selector.cc
+ graph/state-selector.cc
+ graph/guided-state-selector.cc
graph/statistics.cc
graph/helper.cc
diff --git a/src/astar.hh b/src/astar.hh
index 28d8285dd86e64f18c113bd273aabbd7d39b166f..07df4f38239c0cc534ee1d36fe6365b2a425cec9 100644
--- a/src/astar.hh
+++ b/src/astar.hh
@@ -26,7 +26,7 @@
# include <hpp/manipulation/fwd.hh>
# include <hpp/manipulation/roadmap-node.hh>
-# include <hpp/manipulation/graph/node-selector.hh>
+# include <hpp/manipulation/graph/state-selector.hh>
//# include <hpp/core/path-vector.hh>
namespace hpp {
@@ -44,44 +44,44 @@ namespace hpp {
{ return cost_ [n1] < val; }
}; // struc CostMapCompFunctor
- typedef std::list <graph::NodePtr_t> Nodes_t;
+ typedef std::list <graph::StatePtr_t> States_t;
typedef std::list <RoadmapNodePtr_t> RoadmapNodes_t;
typedef std::list <core::EdgePtr_t> RoadmapEdges_t;
typedef std::map <RoadmapNodePtr_t, core::EdgePtr_t> Parent_t;
Astar (const core::DistancePtr_t distance,
- const graph::NodeSelectorPtr_t& nodeSelector, RoadmapNodePtr_t from) :
- distance_ (distance), selector_ (nodeSelector),
+ const graph::StateSelectorPtr_t& stateSelector, RoadmapNodePtr_t from) :
+ distance_ (distance), selector_ (stateSelector),
from_ (from)
{
open_.push_back (from);
costFromStart_ [from] = 0;
}
- Nodes_t solution (RoadmapNodePtr_t to)
+ States_t solution (RoadmapNodePtr_t to)
{
if (parent_.find (to) != parent_.end () ||
findPath (to))
{
RoadmapNodePtr_t node = to;
- Nodes_t nodes;
+ States_t states;
- nodes.push_front (selector_->getNode (to));
+ states.push_front (selector_->getState (to));
while (node) {
Parent_t::const_iterator itNode = parent_.find (node);
if (itNode != parent_.end ()) {
node = static_cast <RoadmapNodePtr_t> (itNode->second->from ());
- nodes.push_front (selector_->getNode (node));
+ states.push_front (selector_->getState (node));
}
else node = RoadmapNodePtr_t (0);
}
// We may want to clean it a little
- // std::unique (nodes.begin(), nodes.end ());
+ // std::unique (states.begin(), states.end ());
- nodes.push_front (selector_->getNode (from_));
- return nodes;
+ states.push_front (selector_->getState (from_));
+ return states;
}
- return Nodes_t();
+ return States_t();
}
private:
@@ -157,7 +157,7 @@ namespace hpp {
std::map <RoadmapNodePtr_t, value_type> estimatedCostToGoal_;
Parent_t parent_;
core::DistancePtr_t distance_;
- graph::NodeSelectorPtr_t selector_;
+ graph::StateSelectorPtr_t selector_;
RoadmapNodePtr_t from_;
}; // class Astar
diff --git a/src/connected-component.cc b/src/connected-component.cc
index e47917ca974af9e73762fe14f7095bb6426d3f9e..f1c62e9796a3fd5c7ad59e78e2c6565f7ad869ef 100644
--- a/src/connected-component.cc
+++ b/src/connected-component.cc
@@ -25,6 +25,25 @@ namespace hpp {
namespace manipulation {
RoadmapNodes_t ConnectedComponent::empty_ = RoadmapNodes_t();
+ bool ConnectedComponent::check () const
+ {
+ std::set <core::NodePtr_t> s1;
+ for (core::NodeVector_t::const_iterator it = nodes ().begin ();
+ it != nodes ().end (); ++it) {
+ s1.insert (*it);
+ }
+ std::set <core::NodePtr_t> s2;
+ for (GraphStates_t::const_iterator it = graphStateMap_.begin();
+ it != graphStateMap_.end(); ++it ) {
+ for (RoadmapNodes_t::const_iterator itNodes = it->second.begin ();
+ itNodes != it->second.end (); ++itNodes) {
+ s2.insert (*itNodes);
+ }
+ }
+ if (s1.size () == 0) return false;
+ if (s1 == s2) return true;
+ }
+
ConnectedComponentPtr_t ConnectedComponent::create(const RoadmapWkPtr_t& roadmap)
{
ConnectedComponent* ptr = new ConnectedComponent ();
@@ -40,46 +59,47 @@ namespace hpp {
{
core::ConnectedComponent::merge(otherCC);
const ConnectedComponentPtr_t other = boost::static_pointer_cast <ConnectedComponent> (otherCC);
- /// take all graph nodes in other->graphNodeMap_ and put them in this->graphNodeMap_
- /// if they already exist in this->graphNodeMap_, append roadmap nodes from other graph node
- /// to graph node in this.
- for (GraphNodes_t::iterator otherIt = other->graphNodeMap_.begin();
- otherIt != other->graphNodeMap_.end(); otherIt++)
+ /// take all graph states in other->graphStateMap_ and put them in this->graphStateMap_
+ /// if they already exist in this->graphStateMap_, append roadmap nodes from other graph state
+ /// to graph state in this.
+ for (GraphStates_t::iterator otherIt = other->graphStateMap_.begin();
+ otherIt != other->graphStateMap_.end(); otherIt++)
{
- // find other graph node in this-graphNodeMap_ -> merge their roadmap nodes
- GraphNodes_t::iterator mapIt = this->graphNodeMap_.find(otherIt->first);
- if (mapIt != this->graphNodeMap_.end()) {
+ // find other graph state in this-graphStateMap_ -> merge their roadmap nodes
+ GraphStates_t::iterator mapIt = this->graphStateMap_.find(otherIt->first);
+ if (mapIt != this->graphStateMap_.end()) {
mapIt->second.insert(mapIt->second.end(), otherIt->second.begin(), otherIt->second.end());
} else {
- this->graphNodeMap_.insert(*otherIt);
+ this->graphStateMap_.insert(*otherIt);
}
}
- other->graphNodeMap_.clear();
+ other->graphStateMap_.clear();
+ assert (check ());
}
void ConnectedComponent::addNode(const core::NodePtr_t& node)
{
core::ConnectedComponent::addNode(node);
- // Find right graph node in map and add roadmap node to corresponding vector
+ // Find right graph state in map and add roadmap node to corresponding vector
const RoadmapNodePtr_t& n = static_cast <const RoadmapNodePtr_t> (node);
- GraphNodes_t::iterator mapIt = graphNodeMap_.find(roadmap_->getNode(n));
- if (mapIt != graphNodeMap_.end()) {
+ GraphStates_t::iterator mapIt = graphStateMap_.find(roadmap_->getState(n));
+ if (mapIt != graphStateMap_.end()) {
mapIt->second.push_back(n);
- // if graph node not found, add new map element with one roadmap node
+ // if graph state not found, add new map element with one roadmap node
} else {
RoadmapNodes_t newRoadmapNodeVector;
newRoadmapNodeVector.push_back(n);
- graphNodeMap_.insert(std::pair<graph::NodePtr_t, RoadmapNodes_t>
- (roadmap_->getNode(n), newRoadmapNodeVector));
+ graphStateMap_.insert(std::pair<graph::StatePtr_t, RoadmapNodes_t>
+ (roadmap_->getState(n), newRoadmapNodeVector));
}
-
+ assert (check ());
}
const RoadmapNodes_t& ConnectedComponent::getRoadmapNodes (
- const graph::NodePtr_t graphNode) const
+ const graph::StatePtr_t graphState) const
{
- GraphNodes_t::const_iterator mapIt = graphNodeMap_.find(graphNode);
- if (mapIt != graphNodeMap_.end())
+ GraphStates_t::const_iterator mapIt = graphStateMap_.find(graphState);
+ if (mapIt != graphStateMap_.end())
return mapIt->second;
return empty_;
}
diff --git a/src/graph-path-validation.cc b/src/graph-path-validation.cc
index baa5183669e117118558d0d8d688c4b5463ecdd7..5fdda45d1b3e83e3e615b9bcabdf5a8a90b4b7c9 100644
--- a/src/graph-path-validation.cc
+++ b/src/graph-path-validation.cc
@@ -37,8 +37,8 @@ namespace hpp {
assert (path);
bool success = impl_validate (path, reverse, validPart, report);
assert (constraintGraph_);
- assert (constraintGraph_->getNode (validPart->initial ()));
- assert (constraintGraph_->getNode (validPart->end ()));
+ assert (constraintGraph_->getState (validPart->initial ()));
+ assert (constraintGraph_->getState (validPart->end ()));
return success;
}
@@ -107,11 +107,11 @@ namespace hpp {
Configuration_t q (newPath.outputSize());
if (!newPath (q, newTR.first))
throw std::logic_error ("Initial configuration of the valid part cannot be projected.");
- const graph::NodePtr_t& origNode = constraintGraph_->getNode (q);
+ const graph::StatePtr_t& origState = constraintGraph_->getState (q);
if (!newPath (q, newTR.second))
throw std::logic_error ("End configuration of the valid part cannot be projected.");
// This may throw in the following case:
- // - node constraints: object_placement + other_function
+ // - state constraints: object_placement + other_function
// - path constraints: other_function, object_lock
// This is semantically correct but for a path going from q0 to q1,
// we ensure that
@@ -125,27 +125,27 @@ namespace hpp {
// And not:
// - other_function (q(s)) + object_placement (q(s)) < eps
// In this case, there is no good way to recover. Just return failure.
- graph::NodePtr_t destNode;
+ graph::StatePtr_t destState;
try {
- destNode = constraintGraph_->getNode (q);
+ destState = constraintGraph_->getState (q);
} catch (const std::logic_error& e) {
ConstraintSetPtr_t c = HPP_DYNAMIC_PTR_CAST(ConstraintSet, path->constraints());
hppDout (error, "Edge " << c->edge()->name()
<< " generated an error: " << e.what());
hppDout (error, "Likely, the constraints for paths are relaxed. If "
"this problem occurs often, you may want to use the same "
- "constraints for node and paths in " << c->edge()->node()->name());
+ "constraints for state and paths in " << c->edge()->state()->name());
validPart = path->extract (std::make_pair (oldTR.first,oldTR.first));
return false;
}
if (!oldPath (q, oldTR.first))
throw std::logic_error ("Initial configuration of the path to be validated cannot be projected.");
- const graph::NodePtr_t& oldOnode = constraintGraph_->getNode (q);
+ const graph::StatePtr_t& oldOstate = constraintGraph_->getState (q);
if (!oldPath (q, oldTR.second))
throw std::logic_error ("End configuration of the path to be validated cannot be projected.");
- const graph::NodePtr_t& oldDnode = constraintGraph_->getNode (q);
+ const graph::StatePtr_t& oldDstate = constraintGraph_->getState (q);
- if (origNode == oldOnode && destNode == oldDnode) {
+ if (origState == oldOstate && destState == oldDstate) {
validPart = pathNoCollision;
return false;
}
diff --git a/src/graph-steering-method.cc b/src/graph-steering-method.cc
index 1094f82e3019600113304481eeae1e9f0afce788..759195f56f60373612ed10f942bee38fc3971ba8 100644
--- a/src/graph-steering-method.cc
+++ b/src/graph-steering-method.cc
@@ -68,7 +68,8 @@ namespace hpp {
graph::Edges_t possibleEdges;
graph::Graph& graph = *problem_->constraintGraph ();
try {
- possibleEdges = graph.getEdges (graph.getNode (q1), graph.getNode (q2));
+ possibleEdges = graph.getEdges
+ (graph.getState (q1), graph.getState (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 ad579b6b2f608e15d884c3503b0bceef4c6b35c2..70cff093ee6a159b49a2f1f196e2319f0cc80497 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -49,12 +49,12 @@ namespace hpp {
if (pathValidation_ ) delete pathValidation_;
}
- NodePtr_t Edge::to () const
+ StatePtr_t Edge::to () const
{
return to_.lock();
}
- NodePtr_t Edge::from () const
+ StatePtr_t Edge::from () const
{
return from_.lock();
}
@@ -72,10 +72,10 @@ namespace hpp {
|| (src_contains_q1 && dst_contains_q0)
)) {
if (src_contains_q0) {
- assert (node ()->contains (q1));
+ assert (state ()->contains (q1));
return false;
} else if (src_contains_q1) {
- assert (node ()->contains (q0));
+ assert (state ()->contains (q0));
return true;
}
throw std::runtime_error ("This path does not seem to have been "
@@ -106,10 +106,10 @@ namespace hpp {
|| (src_contains_q1 && dst_contains_q0)
)) {
if (src_contains_q0) {
- assert (node ()->contains (q1));
+ assert (state ()->contains (q1));
return false;
} else if (src_contains_q1) {
- assert (node ()->contains (q0));
+ assert (state ()->contains (q0));
return true;
}
throw std::runtime_error ("This path does not seem to have been "
@@ -128,28 +128,28 @@ namespace hpp {
g->insertNumericalConstraints (proj);
insertNumericalConstraints (proj);
- node ()->insertNumericalConstraints (proj);
+ state ()->insertNumericalConstraints (proj);
g->insertLockedJoints (proj);
insertLockedJoints (proj);
- node ()->insertLockedJoints (proj);
+ state ()->insertLockedJoints (proj);
if (wkPtr_.lock() == other) // No intersection to be computed.
return false;
- bool nodeB_Eq_nodeA = (node() == other->node());
+ bool stateB_Eq_stateA = (state() == other->state());
other->insertNumericalConstraints (proj);
- if (!nodeB_Eq_nodeA) other->node()->insertNumericalConstraints (proj);
+ if (!stateB_Eq_stateA) other->state()->insertNumericalConstraints (proj);
other->insertLockedJoints (proj);
- if (!nodeB_Eq_nodeA) other->node()->insertLockedJoints (proj);
+ if (!stateB_Eq_stateA) other->state()->insertLockedJoints (proj);
return true;
}
EdgePtr_t Edge::create (const std::string& name,
const GraphWkPtr_t& graph,
- const NodeWkPtr_t& from, const NodeWkPtr_t& to)
+ const StateWkPtr_t& from, const StateWkPtr_t& to)
{
Edge* ptr = new Edge (name);
EdgePtr_t shPtr (ptr);
@@ -157,15 +157,15 @@ namespace hpp {
return shPtr;
}
- void Edge::init (const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
- const NodeWkPtr_t& to)
+ void Edge::init (const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
+ const StateWkPtr_t& from, const StateWkPtr_t& to)
{
GraphComponent::init (weak);
parentGraph (graph);
wkPtr_ = weak;
from_ = from;
to_ = to;
- node_ = to;
+ state_ = to;
}
std::ostream& Edge::print (std::ostream& os) const
@@ -207,16 +207,16 @@ namespace hpp {
g->insertNumericalConstraints (proj);
insertNumericalConstraints (proj);
to ()->insertNumericalConstraints (proj);
- if (node () != to ()) {
- node ()->insertNumericalConstraints (proj);
+ if (state () != to ()) {
+ state ()->insertNumericalConstraints (proj);
}
constraint->addConstraint (proj);
g->insertLockedJoints (proj);
insertLockedJoints (proj);
to ()->insertLockedJoints (proj);
- if (node () != to ()) {
- node ()->insertLockedJoints (proj);
+ if (state () != to ()) {
+ state ()->insertLockedJoints (proj);
}
constraint->edge (wkPtr_.lock ());
@@ -242,12 +242,12 @@ namespace hpp {
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
g->insertNumericalConstraints (proj);
insertNumericalConstraints (proj);
- node ()->insertNumericalConstraintsForPath (proj);
+ state ()->insertNumericalConstraintsForPath (proj);
constraint->addConstraint (proj);
g->insertLockedJoints (proj);
insertLockedJoints (proj);
- node ()->insertLockedJoints (proj);
+ state ()->insertLockedJoints (proj);
constraint->edge (wkPtr_.lock ());
@@ -336,8 +336,8 @@ namespace hpp {
}
WaypointEdgePtr_t WaypointEdge::create (const std::string& name,
- const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
- const NodeWkPtr_t& to)
+ const GraphWkPtr_t& graph, const StateWkPtr_t& from,
+ const StateWkPtr_t& to)
{
WaypointEdge* ptr = new WaypointEdge (name);
WaypointEdgePtr_t shPtr (ptr);
@@ -345,8 +345,10 @@ namespace hpp {
return shPtr;
}
- void WaypointEdge::init (const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
- const NodeWkPtr_t& to)
+ void WaypointEdge::init (const WaypointEdgeWkPtr_t& weak,
+ const GraphWkPtr_t& graph,
+ const StateWkPtr_t& from,
+ const StateWkPtr_t& to)
{
Edge::init (weak, graph, from, to);
wkPtr_ = weak;
@@ -440,13 +442,15 @@ namespace hpp {
result_ = Configuration_t (nbDof);
}
- void WaypointEdge::setWaypoint (const std::size_t index, const EdgePtr_t wEdge, const NodePtr_t wTo)
+ void WaypointEdge::setWaypoint (const std::size_t index,
+ const EdgePtr_t wEdge,
+ const StatePtr_t wTo)
{
assert (index < waypoints_.size());
waypoints_[index] = Waypoint_t (wEdge, wTo);
}
- NodePtr_t WaypointEdge::node () const
+ StatePtr_t WaypointEdge::state () const
{
if (isInNodeFrom ()) return waypoints_.back().second;
else return to ();
@@ -604,8 +608,10 @@ namespace hpp {
return false;
}
- void LevelSetEdge::init (const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from,
- const NodeWkPtr_t& to)
+ void LevelSetEdge::init (const LevelSetEdgeWkPtr_t& weak,
+ const GraphWkPtr_t& graph,
+ const StateWkPtr_t& from,
+ const StateWkPtr_t& to)
{
Edge::init (weak, graph, from, to);
wkPtr_ = weak;
@@ -613,7 +619,7 @@ namespace hpp {
LevelSetEdgePtr_t LevelSetEdge::create
(const std::string& name, const GraphWkPtr_t& graph,
- const NodeWkPtr_t& from, const NodeWkPtr_t& to)
+ const StateWkPtr_t& from, const StateWkPtr_t& to)
{
LevelSetEdge* ptr = new LevelSetEdge (name);
LevelSetEdgePtr_t shPtr (ptr);
@@ -660,7 +666,7 @@ namespace hpp {
// TODO: We assumed that this part of the code can only be reached by
// configurations that are valid.
// It would be wiser to make sure configurations are valid, for instance
- // by considering only configurations in the destination node of this
+ // by considering only configurations in the destination state of this
// edge.
ConstraintSetPtr_t cond = ConstraintSet::create (g->robot (), "Set " + n);
proj = ConfigProjector::create(g->robot(), "projCond_" + n, g->errorThreshold(), g->maxIterations());
@@ -709,8 +715,8 @@ namespace hpp {
insertNumericalConstraints (proj);
to ()->insertNumericalConstraints (proj);
- if (node () != to ()) {
- node ()->insertNumericalConstraints (proj);
+ if (state () != to ()) {
+ state ()->insertNumericalConstraints (proj);
}
constraint->addConstraint (proj);
@@ -721,8 +727,8 @@ namespace hpp {
}
insertLockedJoints (proj);
to ()->insertLockedJoints (proj);
- if (node () != to ()) {
- node ()->insertLockedJoints (proj);
+ if (state () != to ()) {
+ state ()->insertLockedJoints (proj);
}
constraint->edge (wkPtr_.lock ());
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index ae4d3e68d05a68c03c34c26630e62a00076384ff..09343aa8ccff2d55a1f58fd353ee492c06ce34a8 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -19,8 +19,8 @@
#include <hpp/util/assertion.hh>
#include <hpp/manipulation/constraint-set.hh>
-#include "hpp/manipulation/graph/node-selector.hh"
-#include "hpp/manipulation/graph/node.hh"
+#include "hpp/manipulation/graph/state-selector.hh"
+#include "hpp/manipulation/graph/state.hh"
#include "hpp/manipulation/graph/edge.hh"
#include "hpp/manipulation/graph/statistics.hh"
@@ -42,21 +42,34 @@ namespace hpp {
robot_ = robot;
wkPtr_ = weak;
insertHistogram(graph::HistogramPtr_t (
- new graph::NodeHistogram (wkPtr_.lock()))
+ new graph::StateHistogram (wkPtr_.lock()))
);
}
- NodeSelectorPtr_t Graph::createNodeSelector (const std::string& name)
+ StateSelectorPtr_t Graph::createNodeSelector (const std::string& name)
{
- nodeSelector_ = NodeSelector::create (name);
- nodeSelector_->parentGraph (wkPtr_);
- return nodeSelector_;
+ stateSelector_ = StateSelector::create (name);
+ stateSelector_->parentGraph (wkPtr_);
+ return stateSelector_;
}
- void Graph::nodeSelector (NodeSelectorPtr_t ns)
+ StateSelectorPtr_t Graph::createStateSelector (const std::string& name)
{
- nodeSelector_ = ns;
- nodeSelector_->parentGraph (wkPtr_);
+ stateSelector_ = StateSelector::create (name);
+ stateSelector_->parentGraph (wkPtr_);
+ return stateSelector_;
+ }
+
+ void Graph::nodeSelector (StateSelectorPtr_t ns)
+ {
+ stateSelector_ = ns;
+ stateSelector_->parentGraph (wkPtr_);
+ }
+
+ void Graph::stateSelector (StateSelectorPtr_t ns)
+ {
+ stateSelector_ = ns;
+ stateSelector_->parentGraph (wkPtr_);
}
void Graph::maxIterations (size_type iterations)
@@ -89,17 +102,28 @@ namespace hpp {
return problem_;
}
- NodePtr_t Graph::getNode (ConfigurationIn_t config) const
+ StatePtr_t Graph::getNode (ConfigurationIn_t config) const
{
- return nodeSelector_->getNode (config);
+ return stateSelector_->getState (config);
}
- NodePtr_t Graph::getNode(RoadmapNodePtr_t coreNode) const
+ StatePtr_t Graph::getState (ConfigurationIn_t config) const
{
- return nodeSelector_->getNode (coreNode);
+ return stateSelector_->getState (config);
}
- Edges_t Graph::getEdges (const NodePtr_t& from, const NodePtr_t& to) const
+ StatePtr_t Graph::getNode(RoadmapNodePtr_t coreNode) const
+ {
+ return stateSelector_->getState (coreNode);
+ }
+
+ StatePtr_t Graph::getState(RoadmapNodePtr_t coreNode) const
+ {
+ return stateSelector_->getState (coreNode);
+ }
+
+ Edges_t Graph::getEdges (const StatePtr_t& from, const StatePtr_t& to)
+ const
{
Edges_t edges;
for (Neighbors_t::const_iterator it = from->neighbors ().begin ();
@@ -112,18 +136,26 @@ namespace hpp {
EdgePtr_t Graph::chooseEdge (RoadmapNodePtr_t from) const
{
- return nodeSelector_->chooseEdge (from);
+ return stateSelector_->chooseEdge (from);
}
- ConstraintSetPtr_t Graph::configConstraint (const NodePtr_t& node)
+ ConstraintSetPtr_t Graph::configConstraint (const StatePtr_t& state)
{
- return node->configConstraint ();
+ return state->configConstraint ();
}
bool Graph::getConfigErrorForNode (ConfigurationIn_t config,
- const NodePtr_t& node, vector_t& error)
+ const StatePtr_t& state,
+ vector_t& error)
+ {
+ return configConstraint (state)->isSatisfied (config, error);
+ }
+
+ bool Graph::getConfigErrorForState (ConfigurationIn_t config,
+ const StatePtr_t& state,
+ vector_t& error)
{
- return configConstraint (node)->isSatisfied (config, error);
+ return configConstraint (state)->isSatisfied (config, error);
}
bool Graph::getConfigErrorForEdge (ConfigurationIn_t config,
@@ -164,14 +196,14 @@ namespace hpp {
populateTooltip (tp);
da.insertWithQuote ("tooltip", tp.toStr());
os << "digraph " << id() << " {" << da;
- nodeSelector_->dotPrint (os);
+ stateSelector_->dotPrint (os);
os << "}" << std::endl;
return os;
}
std::ostream& Graph::print (std::ostream& os) const
{
- return GraphComponent::print (os) << std::endl << *nodeSelector_;
+ return GraphComponent::print (os) << std::endl << *stateSelector_;
}
} // namespace graph
} // namespace manipulation
diff --git a/src/graph/guided-node-selector.cc b/src/graph/guided-state-selector.cc
similarity index 63%
rename from src/graph/guided-node-selector.cc
rename to src/graph/guided-state-selector.cc
index 9512e9d25e7eb92f44bbbf9c90a484d5c73fdd59..a209f1debda27bc8584123f9c2e909754d39a44a 100644
--- a/src/graph/guided-node-selector.cc
+++ b/src/graph/guided-state-selector.cc
@@ -14,7 +14,7 @@
// 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/manipulation/graph/guided-state-selector.hh"
#include <hpp/util/assertion.hh>
#include <hpp/model/configuration.hh>
@@ -29,30 +29,30 @@
namespace hpp {
namespace manipulation {
namespace graph {
- GuidedNodeSelectorPtr_t GuidedNodeSelector::create(const std::string& name,
+ GuidedStateSelectorPtr_t GuidedStateSelector::create(const std::string& name,
const core::RoadmapPtr_t& roadmap)
{
- GuidedNodeSelector* ptr = new GuidedNodeSelector (name, roadmap);
- GuidedNodeSelectorPtr_t shPtr (ptr);
+ GuidedStateSelector* ptr = new GuidedStateSelector (name, roadmap);
+ GuidedStateSelectorPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
- void GuidedNodeSelector::init (const GuidedNodeSelectorPtr_t& weak)
+ void GuidedStateSelector::init (const GuidedStateSelectorPtr_t& weak)
{
- NodeSelector::init (weak);
+ StateSelector::init (weak);
wkPtr_ = weak;
}
- void GuidedNodeSelector::setNodeList (const Nodes_t& nodeList)
+ void GuidedStateSelector::setStateList (const States_t& stateList)
{
- nodeList_ = nodeList;
+ stateList_ = stateList;
}
- EdgePtr_t GuidedNodeSelector::chooseEdge(RoadmapNodePtr_t from) const
+ EdgePtr_t GuidedStateSelector::chooseEdge(RoadmapNodePtr_t from) const
{
- if (nodeList_.empty ()) return NodeSelector::chooseEdge (from);
- Astar::Nodes_t list;
+ if (stateList_.empty ()) return StateSelector::chooseEdge (from);
+ Astar::States_t list;
bool reverse = false;
if (from->connectedComponent () == roadmap_->initNode ()->connectedComponent ()) {
Astar alg (roadmap_->distance (), wkPtr_.lock(), static_cast <RoadmapNodePtr_t> (roadmap_->initNode ()));
@@ -72,13 +72,13 @@ namespace hpp {
list = alg.solution (static_cast <RoadmapNodePtr_t> (*itg));
}
list.erase (std::unique (list.begin(), list.end ()), list.end ());
- // Check if the beginning of nodeList is list
- if (list.size() <= nodeList_.size()) {
+ // Check if the beginning of stateList is list
+ if (list.size() <= stateList_.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();
+ States_t::const_reverse_iterator it1 = stateList_.rbegin ();
+ Astar::States_t::const_reverse_iterator it2 = list.rbegin();
+ Astar::States_t::const_reverse_iterator itEnd2 = list.rend();
do {
if (*it1 != *it2) {
hppDout (error, "The target sequence of nodes does not end with "
@@ -87,23 +87,23 @@ namespace hpp {
}
++it1;
} while (++it2 != itEnd2);
- NodePtr_t node = getNode (from);
- HPP_ASSERT (node == list.front ());
- const Neighbors_t& n = node->neighbors();
- /// You stay in the same node
+ StatePtr_t state = getState (from);
+ HPP_ASSERT (state == list.front ());
+ const Neighbors_t& n = state->neighbors();
+ /// You stay in the same state
for (Neighbors_t::const_iterator it = n.begin (); it != n.end (); ++it)
- if (it->second->to () == node)
+ if (it->second->to () == state)
nn.insert (it->second, it->first);
- /// Go from node it1 to node
- // The path will be build from node. So we must find an edge from
- // node to it1, that will be reversely
+ /// Go from state it1 to state
+ // The path will be build from state. So we must find an edge from
+ // state to it1, that will be reversely
for (Neighbors_t::const_iterator it = n.begin (); it != n.end (); ++it)
if (it->second->to () == *it1)
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();
+ States_t::const_iterator it1 = stateList_.begin ();
+ Astar::States_t::const_iterator it2 = list.begin();
+ Astar::States_t::const_iterator itEnd2 = list.end();
do {
if (*it1 != *it2) {
hppDout (error, "The target sequence of nodes does not start with "
@@ -112,35 +112,35 @@ namespace hpp {
}
++it1;
} while (++it2 != itEnd2);
- NodePtr_t node = getNode (from);
- HPP_ASSERT (node == list.back ());
- const Neighbors_t& n = node->neighbors();
+ StatePtr_t state = getState (from);
+ HPP_ASSERT (state == list.back ());
+ const Neighbors_t& n = state->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)
+ /// You stay in the same state
+ /// or go from state to state it1
+ if (it->second->to () == state || it->second->to () == *it1)
nn.insert (it->second, it->first);
}
if (nn.size () > 0 && nn.totalWeight() > 0)
return nn ();
- hppDout (error, "This node has no neighbors to get to an admissible states.");
+ hppDout (error, "This state has no neighbors to get to an admissible states.");
}
return EdgePtr_t ();
}
- std::ostream& GuidedNodeSelector::dotPrint (std::ostream& os, dot::DrawingAttributes) const
+ std::ostream& GuidedStateSelector::dotPrint (std::ostream& os, dot::DrawingAttributes) const
{
- for (WeighedNodes_t::const_iterator it = orderedStates_.begin();
+ for (WeighedStates_t::const_iterator it = orderedStates_.begin();
orderedStates_.end() != it; ++it)
it->second->dotPrint (os);
return os;
}
- std::ostream& GuidedNodeSelector::print (std::ostream& os) const
+ std::ostream& GuidedStateSelector::print (std::ostream& os) const
{
os << "|-- ";
GraphComponent::print (os) << std::endl;
- for (WeighedNodes_t::const_iterator it = orderedStates_.begin();
+ for (WeighedStates_t::const_iterator it = orderedStates_.begin();
orderedStates_.end() != it; ++it)
os << it->first << " " << *it->second;
return os;
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index 62732ac968f3db5b89804a06bfd75e04db6c5567..617c5761b50d5f6605390c5d65a969611a34da78 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -31,10 +31,10 @@
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/manipulation/handle.hh>
-#include <hpp/manipulation/graph/node.hh>
+#include <hpp/manipulation/graph/state.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/state-selector.hh>
+#include <hpp/manipulation/graph/guided-state-selector.hh>
#include <hpp/manipulation/problem-solver.hh>
namespace hpp {
@@ -46,10 +46,10 @@ namespace hpp {
(GraphComponentPtr_t comp) const
{
if (nc.empty ()) return;
- NodePtr_t n;
+ StatePtr_t n;
if (forPath) {
- n = HPP_DYNAMIC_PTR_CAST (Node, comp);
- if (!n) throw std::logic_error ("Wrong type: expect a Node");
+ n = HPP_DYNAMIC_PTR_CAST (State, comp);
+ if (!n) throw std::logic_error ("Wrong type: expect a State");
}
NumericalConstraints_t::const_iterator it;
IntervalsContainer_t::const_iterator itpdof = pdof.begin ();
@@ -79,7 +79,7 @@ namespace hpp {
assert (itpdof == pdof.end ());
}
- void FoliatedManifold::addToNode (NodePtr_t comp) const
+ void FoliatedManifold::addToState (StatePtr_t comp) const
{
nc.addToComp <false> (comp);
for (LockedJoints_t::const_iterator it = lj.begin ();
@@ -121,7 +121,7 @@ namespace hpp {
// It should be
// static const bool intersec = !((gCase & NoGrasp) || (gCase & NoPlace));
// but when NoPlace | WithPreGrasp, we need a LevelSetEdge after
- // the pregrasp waypoint node. Sadly the current implementation of
+ // the pregrasp waypoint state. Sadly the current implementation of
// WaypointEdge does not allow the last edge of type other than Edge.
static const bool intersec = pregrasp || preplace || ((gCase & GraspOnly) && (gCase & PlaceOnly));
@@ -131,20 +131,20 @@ namespace hpp {
&& !((gCase & NoGrasp) && (gCase & NoPlace));
static const std::size_t nbWaypoints = (pregrasp?1:0) + (intersec?1:0) + (preplace?1:0);
- static const std::size_t Nnodes = 2 + nbWaypoints;
+ static const std::size_t Nstates = 2 + nbWaypoints;
static const std::size_t Nedges = 1 + nbWaypoints;
// static const std::size_t iNpregrasp = pregrasp?1 + 1:nbWaypoints;
// static const std::size_t iNpreplace = pregrasp?1 + 1:nbWaypoints;
- typedef boost::array <NodePtr_t, Nnodes> NodeArray;
+ typedef boost::array <StatePtr_t, Nstates> StateArray;
typedef boost::array <EdgePtr_t, Nedges> EdgeArray;
- static inline const NodePtr_t& Npregrasp (const NodeArray& n) { assert (pregrasp); return n[1]; }
- static inline const NodePtr_t& Nintersec (const NodeArray& n) { assert (intersec); return n[1 + (pregrasp?1:0)]; }
- static inline const NodePtr_t& Npreplace (const NodeArray& n) { assert (preplace); return n[1 + (pregrasp?1:0) + (intersec?1:0)]; }
+ static inline const StatePtr_t& Npregrasp (const StateArray& n) { assert (pregrasp); return n[1]; }
+ static inline const StatePtr_t& Nintersec (const StateArray& n) { assert (intersec); return n[1 + (pregrasp?1:0)]; }
+ static inline const StatePtr_t& Npreplace (const StateArray& n) { assert (preplace); return n[1 + (pregrasp?1:0) + (intersec?1:0)]; }
static inline EdgePtr_t makeWE (
const std::string& name,
- const NodePtr_t& from, const NodePtr_t& to,
+ const StatePtr_t& from, const StatePtr_t& to,
const size_type& w)
{
if (Nedges > 1) {
@@ -155,29 +155,29 @@ namespace hpp {
} else return from->linkTo (name, to, w, Edge::create);
}
- static inline NodeArray makeWaypoints (
- const NodePtr_t& from, const NodePtr_t& to,
+ static inline StateArray makeWaypoints (
+ const StatePtr_t& from, const StatePtr_t& to,
const std::string& name)
{
- NodeSelectorPtr_t ns = from->parentGraph ()->nodeSelector ();
- NodeArray nodes;
+ StateSelectorPtr_t ns = from->parentGraph ()->stateSelector ();
+ StateArray states;
std::size_t r = 0;
- nodes[r] = from; ++r;
+ states[r] = from; ++r;
if (pregrasp) {
- nodes[r] = ns->createNode (name + "_pregrasp", true); ++r;
+ states[r] = ns->createState (name + "_pregrasp", true); ++r;
}
if (intersec) {
- nodes[r] = ns->createNode (name + "_intersec", true); ++r;
+ states[r] = ns->createState (name + "_intersec", true); ++r;
}
if (preplace) {
- nodes[r] = ns->createNode (name + "_preplace", true); ++r;
+ states[r] = ns->createState (name + "_preplace", true); ++r;
}
- nodes[r] = to;
- return nodes;
+ states[r] = to;
+ return states;
}
static inline EdgePtr_t makeLSEgrasp (const std::string& name,
- const NodeArray& n, const EdgeArray& e,
+ const StateArray& n, const EdgeArray& e,
const size_type w, LevelSetEdgePtr_t& gls)
{
if (Nedges > 1) {
@@ -190,7 +190,7 @@ namespace hpp {
for (std::size_t i = 0; i < Nedges - 1; ++i)
we->setWaypoint (i, e[i], n[i]);
we->setWaypoint (T-1, gls, n[T]);
- gls->node (n.front());
+ gls->state (n.front());
gls->setShort (pregrasp);
return we;
} else {
@@ -204,7 +204,7 @@ namespace hpp {
}
static inline EdgePtr_t makeLSEplace (const std::string& name,
- const NodeArray& n, const EdgeArray& e,
+ const StateArray& n, const EdgeArray& e,
const size_type w, LevelSetEdgePtr_t& pls)
{
if (Nedges > 1) {
@@ -217,7 +217,7 @@ namespace hpp {
for (std::size_t i = Nedges - 1; i != 0; --i)
we->setWaypoint (Nedges - 1 - i, e[i], n[i]);
we->setWaypoint (Nedges - 1 - T, pls, n[T]);
- pls->node (n.back ());
+ pls->state (n.back ());
pls->setShort (preplace);
return we;
} else {
@@ -232,7 +232,7 @@ namespace hpp {
template <typename EdgeType>
static inline boost::shared_ptr<EdgeType> linkWaypoint (
- const NodeArray& nodes,
+ const StateArray& states,
const std::size_t& iF, const std::size_t& iT,
const std::string& prefix,
const std::string& suffix = "")
@@ -241,31 +241,31 @@ namespace hpp {
ss << prefix << "_" << iF << iT;
if (suffix.length () > 0) ss << "_" << suffix;
return boost::static_pointer_cast <EdgeType>
- (nodes[iF]->linkTo (ss.str(), nodes[iT], -1, EdgeType::create));
+ (states[iF]->linkTo (ss.str(), states[iT], -1, EdgeType::create));
}
template <bool forward>
static inline EdgeArray linkWaypoints (
- const NodeArray& nodes, const EdgePtr_t& edge,
+ const StateArray& states, const EdgePtr_t& edge,
const std::string& name)
{
EdgeArray e;
WaypointEdgePtr_t we = HPP_DYNAMIC_PTR_CAST(WaypointEdge, edge);
if (forward)
for (std::size_t i = 0; i < Nedges - 1; ++i) {
- e[i] = linkWaypoint <Edge> (nodes, i, i + 1, name);
- we->setWaypoint (i, e[i], nodes[i+1]);
+ e[i] = linkWaypoint <Edge> (states, i, i + 1, name);
+ we->setWaypoint (i, e[i], states[i+1]);
}
else
for (std::size_t i = Nedges - 1; i != 0; --i) {
- e[i] = linkWaypoint <Edge> (nodes, i + 1, i, name);
- we->setWaypoint (Nedges - 1 - i, e[i], nodes[i]);
+ e[i] = linkWaypoint <Edge> (states, i + 1, i, name);
+ we->setWaypoint (Nedges - 1 - i, e[i], states[i]);
}
e[(forward?Nedges - 1:0)] = we;
return e;
}
- static inline void setNodeConstraints (const NodeArray& n,
+ static inline void setStateConstraints (const StateArray& n,
const FoliatedManifold& g, const FoliatedManifold& pg,
const FoliatedManifold& p, const FoliatedManifold& pp,
const FoliatedManifold& m)
@@ -273,19 +273,19 @@ namespace hpp {
// From and to are not populated automatically
// to avoid duplicates.
if (pregrasp) {
- p .addToNode (Npregrasp (n));
- pg.addToNode (Npregrasp (n));
- m .addToNode (Npregrasp (n));
+ p .addToState (Npregrasp (n));
+ pg.addToState (Npregrasp (n));
+ m .addToState (Npregrasp (n));
}
if (intersec) {
- p .addToNode (Nintersec (n));
- g .addToNode (Nintersec (n));
- m .addToNode (Nintersec (n));
+ p .addToState (Nintersec (n));
+ g .addToState (Nintersec (n));
+ m .addToState (Nintersec (n));
}
if (preplace) {
- pp.addToNode (Npreplace (n));
- g .addToNode (Npreplace (n));
- m .addToNode (Npreplace (n));
+ pp.addToState (Npreplace (n));
+ g .addToState (Npreplace (n));
+ m .addToState (Npreplace (n));
}
}
@@ -302,7 +302,7 @@ namespace hpp {
template <bool forward>
static inline void setEdgeProp
- (const EdgeArray& e, const NodeArray& n)
+ (const EdgeArray& e, const StateArray& n)
{
/// Last is short
const std::size_t K = (forward?1:0);
@@ -314,15 +314,15 @@ namespace hpp {
B = 0;
else // There is a grasp
B = 1 + (pregrasp?1:0);
- for (std::size_t i = 0; i < B ; ++i) e[i]->node (n[0]);
- for (std::size_t i = B; i < Nedges; ++i) e[i]->node (n[Nnodes-1]);
+ for (std::size_t i = 0; i < B ; ++i) e[i]->state (n[0]);
+ for (std::size_t i = B; i < Nedges; ++i) e[i]->state (n[Nstates-1]);
}
};
}
template <int gCase> Edges_t createEdges (
const std::string& forwName, const std::string& backName,
- const NodePtr_t& from, const NodePtr_t& to,
+ const StatePtr_t& from, const StatePtr_t& to,
const size_type& wForw, const size_type& wBack,
const FoliatedManifold& grasp, const FoliatedManifold& pregrasp,
const FoliatedManifold& place, const FoliatedManifold& preplace,
@@ -331,7 +331,7 @@ namespace hpp {
{
typedef CaseTraits<gCase> T;
assert (T::valid && "Not a valid case.");
- typedef typename T::NodeArray NodeArray;
+ typedef typename T::StateArray StateArray;
typedef typename T::EdgeArray EdgeArray;
// Create the edges
@@ -340,15 +340,15 @@ namespace hpp {
weForwLs, weBackLs;
std::string name = forwName;
- NodeArray n = T::makeWaypoints (from, to, name);
+ StateArray n = T::makeWaypoints (from, to, name);
EdgeArray eF = T::template linkWaypoints <true> (n, weForw, name);
- // Set the nodes constraints
- // Note that submanifold is not taken into account for nodes
+ // Set the states constraints
+ // Note that submanifold is not taken into account for states
// because the edges constraints will enforce configuration to stay
// in a leaf, and so in the manifold itself.
- T::setNodeConstraints (n, grasp, pregrasp, place, preplace,
+ T::setStateConstraints (n, grasp, pregrasp, place, preplace,
submanifoldDef);
// Set the edges properties
@@ -409,7 +409,7 @@ namespace hpp {
EdgePtr_t createLoopEdge (
const std::string& loopName,
- const NodePtr_t& node,
+ const StatePtr_t& state,
const size_type& w,
const bool levelSet,
const FoliatedManifold& submanifoldDef)
@@ -417,10 +417,10 @@ namespace hpp {
// Create the edges
EdgePtr_t loop;
if (levelSet)
- loop = node->linkTo (loopName, node, w, LevelSetEdge::create);
- else loop = node->linkTo (loopName, node, w, Edge::create);
+ loop = state->linkTo (loopName, state, w, LevelSetEdge::create);
+ else loop = state->linkTo (loopName, state, w, Edge::create);
- loop->node (node);
+ loop->state (state);
submanifoldDef.addToEdge (loop);
if (levelSet) {
@@ -513,9 +513,9 @@ namespace hpp {
typedef std::vector <index_t> IndexV_t;
typedef std::list <index_t> IndexL_t;
typedef std::pair <index_t, index_t> Grasp_t;
- typedef boost::tuple <NodePtr_t,
+ typedef boost::tuple <StatePtr_t,
FoliatedManifold>
- NodeAndManifold_t;
+ StateAndManifold_t;
//typedef std::vector <index_t, index_t> GraspV_t;
/// GraspV_t corresponds to a unique ID of a permutation.
/// - its size is the number of grippers,
@@ -545,9 +545,9 @@ namespace hpp {
struct Result {
GraphPtr_t graph;
- typedef unsigned long nodeid_type;
- std::tr1::unordered_map<nodeid_type, NodeAndManifold_t> nodes;
- typedef std::pair<nodeid_type, nodeid_type> edgeid_type;
+ typedef unsigned long stateid_type;
+ std::tr1::unordered_map<stateid_type, StateAndManifold_t> states;
+ typedef std::pair<stateid_type, stateid_type> edgeid_type;
struct edgeid_hash {
std::tr1::hash<edgeid_type::first_type> first;
std::tr1::hash<edgeid_type::second_type> second;
@@ -606,14 +606,14 @@ namespace hpp {
return true;
}
- inline nodeid_type nodeid (const GraspV_t& iG)
+ inline stateid_type stateid (const GraspV_t& iG)
{
- nodeid_type iGOH = iG[0];
- nodeid_type res;
+ stateid_type iGOH = iG[0];
+ stateid_type res;
for (index_t i = 1; i < nG; ++i) {
res = iGOH + dims[i] * (iG[i]);
if (res < iGOH) {
- hppDout (info, "Node ID overflowed. There are too many states...");
+ hppDout (info, "State ID overflowed. There are too many states...");
}
iGOH = res;
// iGOH += dims[i] * (iG[i]);
@@ -621,24 +621,24 @@ namespace hpp {
return iGOH;
}
- bool hasNode (const GraspV_t& iG)
+ bool hasState (const GraspV_t& iG)
{
- return nodes.count(nodeid(iG)) > 0;
+ return states.count(stateid(iG)) > 0;
}
- NodeAndManifold_t& operator() (const GraspV_t& iG)
+ StateAndManifold_t& operator() (const GraspV_t& iG)
{
- return nodes [nodeid(iG)];
+ return states [stateid(iG)];
}
bool hasEdge (const GraspV_t& g1, const GraspV_t& g2)
{
- return edges.count(edgeid_type(nodeid(g1), nodeid(g2))) > 0;
+ return edges.count(edgeid_type(stateid(g1), stateid(g2))) > 0;
}
void addEdge (const GraspV_t& g1, const GraspV_t& g2)
{
- edges.insert(edgeid_type(nodeid(g1), nodeid(g2)));
+ edges.insert(edgeid_type(stateid(g1), stateid(g2)));
}
inline boost::array<NumericalConstraintPtr_t,3>& graspConstraint (
@@ -695,7 +695,7 @@ namespace hpp {
return false;
}
- /// Get a node name from a set of grasps
+ /// Get a state name from a set of grasps
std::string name (const GraspV_t& idxOH, bool abbrev = false) const {
assert (idxOH.size () == nG);
std::stringstream ss;
@@ -758,13 +758,13 @@ namespace hpp {
}
};
- const NodeAndManifold_t& makeNode (Result& r, const GraspV_t& g,
+ const StateAndManifold_t& makeState (Result& r, const GraspV_t& g,
const int priority)
{
- NodeAndManifold_t& nam = r (g);
+ StateAndManifold_t& nam = r (g);
if (!nam.get<0>()) {
- hppDout (info, "Creating node " << r.name (g));
- nam.get<0>() = r.graph->nodeSelector ()->createNode
+ hppDout (info, "Creating state " << r.name (g));
+ nam.get<0>() = r.graph->stateSelector ()->createState
(r.name (g), false, priority);
// Loop over the grippers and create grasping constraints if required
FoliatedManifold unused;
@@ -791,7 +791,7 @@ namespace hpp {
o.get<0>().get<2>(),
nam.get<1>(), unused);
}
- nam.get<1>().addToNode (nam.get<0>());
+ nam.get<1>().addToState (nam.get<0>());
createLoopEdge (r.nameLoopEdge (g),
nam.get<0>(), 0,
@@ -814,8 +814,8 @@ namespace hpp {
<< r.name (gFrom) << "\nto " << r.name (gTo));
return;
}
- const NodeAndManifold_t& from = makeNode (r, gFrom, priority),
- to = makeNode (r, gTo, priority+1);
+ const StateAndManifold_t& from = makeState (r, gFrom, priority),
+ to = makeState (r, gTo, priority+1);
const Object_t& o = r.object (gTo[iG]);
// Detect when grasping an object already grasped.
@@ -935,7 +935,7 @@ namespace hpp {
IndexV_t nIdx_g (idx_g.size() - 1);
IndexV_t nIdx_oh (idx_oh.size() - 1);
bool curGraspIsAllowed = r.graspIsAllowed(grasps);
- if (curGraspIsAllowed) makeNode (r, grasps, depth);
+ if (curGraspIsAllowed) makeState (r, grasps, depth);
for (IndexV_t::const_iterator itx_g = idx_g.begin ();
itx_g != idx_g.end (); ++itx_g) {
@@ -950,7 +950,7 @@ namespace hpp {
nGrasps [*itx_g] = *itx_oh;
bool nextGraspIsAllowed = r.graspIsAllowed(nGrasps);
- if (nextGraspIsAllowed) makeNode (r, nGrasps, depth + 1);
+ if (nextGraspIsAllowed) makeState (r, nGrasps, depth + 1);
if (curGraspIsAllowed && nextGraspIsAllowed)
makeEdge (r, grasps, nGrasps, *itx_g, depth);
@@ -973,8 +973,8 @@ namespace hpp {
const Rules_t& rules)
{
if (!graph) throw std::logic_error ("The graph must be initialized");
- NodeSelectorPtr_t ns = graph->nodeSelector ();
- if (!ns) throw std::logic_error ("The graph does not have a NodeSelector");
+ StateSelectorPtr_t ns = graph->stateSelector ();
+ if (!ns) throw std::logic_error ("The graph does not have a StateSelector");
Result r (grippers, objects, graph);
r.setRules (rules);
@@ -987,7 +987,7 @@ namespace hpp {
recurseGrippers (r, availG, availOH, iG, 0);
- hppDout (info, "Created a graph with " << r.nodes.size() << " states "
+ hppDout (info, "Created a graph with " << r.states.size() << " states "
"and " << r.edges.size() << " edges.");
}
@@ -1048,8 +1048,8 @@ namespace hpp {
}
GraphPtr_t graph = Graph::create (graphName,
ps->robot(), ps->problem());
- graph->nodeSelector (
- GuidedNodeSelector::create ("nodeSelector",
+ graph->stateSelector (
+ GuidedStateSelector::create ("stateSelector",
ps->roadmap ()));
graph->maxIterations (ps->maxIterations ());
graph->errorThreshold (ps->errorThreshold ());
diff --git a/src/graph/node-selector.cc b/src/graph/state-selector.cc
similarity index 58%
rename from src/graph/node-selector.cc
rename to src/graph/state-selector.cc
index e5f77ca908742d8147076d549cfcd4f7ed827d4f..28dcefe3c07e2898bf6e24c1e67121f971b89426 100644
--- a/src/graph/node-selector.cc
+++ b/src/graph/state-selector.cc
@@ -18,115 +18,115 @@
#include <hpp/model/configuration.hh>
#include "hpp/manipulation/roadmap-node.hh"
-#include "hpp/manipulation/graph/node-selector.hh"
+#include "hpp/manipulation/graph/state-selector.hh"
#include <cstdlib>
namespace hpp {
namespace manipulation {
namespace graph {
- NodeSelectorPtr_t NodeSelector::create(const std::string& name)
+ StateSelectorPtr_t StateSelector::create(const std::string& name)
{
- NodeSelector* ptr = new NodeSelector (name);
- NodeSelectorPtr_t shPtr (ptr);
+ StateSelector* ptr = new StateSelector (name);
+ StateSelectorPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
- void NodeSelector::init (const NodeSelectorPtr_t& weak)
+ void StateSelector::init (const StateSelectorPtr_t& weak)
{
GraphComponent::init (weak);
wkPtr_ = weak;
}
- NodePtr_t NodeSelector::createNode (const std::string& name,
+ StatePtr_t StateSelector::createState (const std::string& name,
bool waypoint, const int w)
{
- NodePtr_t newNode = Node::create (name);
- newNode->nodeSelector(wkPtr_);
- newNode->parentGraph(graph_);
- newNode->isWaypoint (waypoint);
- if (waypoint) waypoints_.push_back(newNode);
+ StatePtr_t newState = State::create (name);
+ newState->stateSelector(wkPtr_);
+ newState->parentGraph(graph_);
+ newState->isWaypoint (waypoint);
+ if (waypoint) waypoints_.push_back(newState);
else {
bool found = false;
- for (WeighedNodes_t::iterator it = orderedStates_.begin();
+ for (WeighedStates_t::iterator it = orderedStates_.begin();
it != orderedStates_.end (); ++it) {
if (it->first < w) {
- orderedStates_.insert (it, WeighedNode_t(w,newNode));
+ orderedStates_.insert (it, WeighedState_t(w,newState));
found = true;
break;
}
}
if (!found)
- orderedStates_.push_back (WeighedNode_t(w,newNode));
+ orderedStates_.push_back (WeighedState_t(w,newState));
}
- return newNode;
+ return newState;
}
- Nodes_t NodeSelector::getNodes () const
+ States_t StateSelector::getStates () const
{
- Nodes_t ret;
- for (WeighedNodes_t::const_iterator it = orderedStates_.begin();
+ States_t ret;
+ for (WeighedStates_t::const_iterator it = orderedStates_.begin();
it != orderedStates_.end (); ++it)
ret.push_back (it->second);
return ret;
}
- NodePtr_t NodeSelector::getNode(ConfigurationIn_t config) const
+ StatePtr_t StateSelector::getState(ConfigurationIn_t config) const
{
- for (WeighedNodes_t::const_iterator it = orderedStates_.begin();
+ for (WeighedStates_t::const_iterator it = orderedStates_.begin();
orderedStates_.end() != it; ++it) {
if (it->second->contains(config))
return it->second;
}
std::stringstream oss;
- oss << "A configuration has no node:" << model::displayConfig (config);
+ oss << "A configuration has no state:" << model::displayConfig (config);
throw std::logic_error (oss.str ());
}
- NodePtr_t NodeSelector::getNode(RoadmapNodePtr_t node) const
+ StatePtr_t StateSelector::getState(RoadmapNodePtr_t node) const
{
- NodePtr_t n;
+ StatePtr_t n;
switch (node->cachingSystem ()) {
case RoadmapNode::CACHE_UP_TO_DATE:
- n = node->graphNode ();
+ n = node->graphState ();
break;
case RoadmapNode::CACHE_DISABLED:
case RoadmapNode::CACHE_NEED_UPDATE:
- n = getNode (*(node->configuration ()));
- node->graphNode (n);
+ n = getState (*(node->configuration ()));
+ node->graphState (n);
break;
default:
- n = getNode (*(node->configuration ()));
+ n = getState (*(node->configuration ()));
hppDout (error, "Unimplemented caching system.");
break;
}
return n;
}
- EdgePtr_t NodeSelector::chooseEdge(RoadmapNodePtr_t from) const
+ EdgePtr_t StateSelector::chooseEdge(RoadmapNodePtr_t from) const
{
- NodePtr_t node = getNode (from);
- const Neighbors_t neighborPicker = node->neighbors();
+ StatePtr_t state = getState (from);
+ const Neighbors_t neighborPicker = state->neighbors();
if (neighborPicker.totalWeight () == 0) {
return EdgePtr_t ();
}
return neighborPicker ();
}
- std::ostream& NodeSelector::dotPrint (std::ostream& os, dot::DrawingAttributes) const
+ std::ostream& StateSelector::dotPrint (std::ostream& os, dot::DrawingAttributes) const
{
- for (WeighedNodes_t::const_iterator it = orderedStates_.begin();
+ for (WeighedStates_t::const_iterator it = orderedStates_.begin();
orderedStates_.end() != it; ++it)
it->second->dotPrint (os);
return os;
}
- std::ostream& NodeSelector::print (std::ostream& os) const
+ std::ostream& StateSelector::print (std::ostream& os) const
{
os << "|-- ";
GraphComponent::print (os) << std::endl;
- for (WeighedNodes_t::const_iterator it = orderedStates_.begin();
+ for (WeighedStates_t::const_iterator it = orderedStates_.begin();
orderedStates_.end() != it; ++it)
os << it->first << " " << *it->second;
return os;
diff --git a/src/graph/node.cc b/src/graph/state.cc
similarity index 83%
rename from src/graph/node.cc
rename to src/graph/state.cc
index b8fbb2d0fe2c0940882effb5dd41e8fe5fd58860..1819c2f681ba14ddb05e46b3250cc11d35bfbf96 100644
--- a/src/graph/node.cc
+++ b/src/graph/state.cc
@@ -14,7 +14,7 @@
// 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/node.hh"
+#include "hpp/manipulation/graph/state.hh"
#include <hpp/constraints/differentiable-function.hh>
@@ -26,31 +26,31 @@
namespace hpp {
namespace manipulation {
namespace graph {
- Node::Node (const std::string& name) :
+ State::State (const std::string& name) :
GraphComponent (name), configConstraints_ (new Constraint_t()),
isWaypoint_ (false)
{}
- Node::~Node ()
+ State::~State ()
{
if (configConstraints_) delete configConstraints_;
}
- NodePtr_t Node::create (const std::string& name)
+ StatePtr_t State::create (const std::string& name)
{
- Node* node = new Node (name);
- NodePtr_t shPtr(node);
+ State* state = new State (name);
+ StatePtr_t shPtr(state);
shPtr->init(shPtr);
return shPtr;
}
- void Node::init (const NodeWkPtr_t& weak)
+ void State::init (const StateWkPtr_t& weak)
{
GraphComponent::init (weak);
wkPtr_ = weak;
}
- EdgePtr_t Node::linkTo(const std::string& name, const NodePtr_t& to,
+ EdgePtr_t State::linkTo(const std::string& name, const StatePtr_t& to,
const size_type& w, EdgeFactory create)
{
EdgePtr_t newEdge = create(name, graph_, wkPtr_, to);
@@ -59,16 +59,16 @@ namespace hpp {
return newEdge;
}
- bool Node::contains (ConfigurationIn_t config) const
+ bool State::contains (ConfigurationIn_t config) const
{
return configConstraint()->isSatisfied (config);
}
- std::ostream& Node::dotPrint (std::ostream& os, dot::DrawingAttributes da) const
+ std::ostream& State::dotPrint (std::ostream& os, dot::DrawingAttributes da) const
{
da.insertWithQuote ("label", name ());
da.insert ("style","filled");
- dot::Tooltip tp; tp.addLine ("Node contains:");
+ dot::Tooltip tp; tp.addLine ("State contains:");
populateTooltip (tp);
da.insertWithQuote ("tooltip", tp.toStr());
os << id () << " " << da << ";" << std::endl;
@@ -86,7 +86,7 @@ namespace hpp {
return os;
}
- void Node::populateTooltip (dot::Tooltip& tp) const
+ void State::populateTooltip (dot::Tooltip& tp) const
{
GraphComponent::populateTooltip (tp);
tp.addLine ("");
@@ -97,7 +97,7 @@ namespace hpp {
}
}
- std::ostream& Node::print (std::ostream& os) const
+ std::ostream& State::print (std::ostream& os) const
{
os << "| |-- ";
GraphComponent::print (os) << std::endl;
@@ -107,7 +107,7 @@ namespace hpp {
return os;
}
- ConstraintSetPtr_t Node::configConstraint() const
+ ConstraintSetPtr_t State::configConstraint() const
{
if (!*configConstraints_) {
std::string n = "(" + name () + ")";
@@ -126,7 +126,7 @@ namespace hpp {
return configConstraints_->get ();
}
- void Node::updateWeight (const EdgePtr_t& e, const Weight_t& w)
+ void State::updateWeight (const EdgePtr_t& e, const Weight_t& w)
{
for (Neighbors_t::const_iterator it = neighbors_.begin();
it != neighbors_.end(); ++it) {
@@ -138,7 +138,7 @@ namespace hpp {
hppDout (error, "Edge not found");
}
- Weight_t Node::getWeight (const EdgePtr_t& e)
+ Weight_t State::getWeight (const EdgePtr_t& e)
{
for (Neighbors_t::const_iterator it = neighbors_.begin();
it != neighbors_.end(); ++it)
diff --git a/src/graph/statistics.cc b/src/graph/statistics.cc
index 4823ca9be2f721351d78f910287dbd2d90f62346..d1b862f22f79b856cb659c5014a7d099e23897d5 100644
--- a/src/graph/statistics.cc
+++ b/src/graph/statistics.cc
@@ -98,8 +98,8 @@ namespace hpp {
return os;
}
- NodeBin::NodeBin(const NodePtr_t& n):
- node_(n), roadmapNodes_()
+ NodeBin::NodeBin(const StatePtr_t& n):
+ state_(n), roadmapNodes_()
{}
void NodeBin::push_back(const RoadmapNodePtr_t& n)
@@ -109,17 +109,17 @@ namespace hpp {
bool NodeBin::operator<(const NodeBin& rhs) const
{
- return node_->id () < rhs.node ()->id ();
+ return state_->id () < rhs.state ()->id ();
}
bool NodeBin::operator==(const NodeBin& rhs) const
{
- return node_ == rhs.node ();
+ return state_ == rhs.state ();
}
- const NodePtr_t& NodeBin::node () const
+ const StatePtr_t& NodeBin::state () const
{
- return node_;
+ return state_;
}
std::ostream& NodeBin::print (std::ostream& os) const
@@ -152,7 +152,7 @@ namespace hpp {
std::ostream& NodeBin::printValue (std::ostream& os) const
{
- return os << "NodeBin (" << node_->name () << ")";
+ return os << "NodeBin (" << state_->name () << ")";
}
LeafHistogramPtr_t LeafHistogram::create (const Foliation f)
@@ -193,32 +193,32 @@ namespace hpp {
return HistogramPtr_t (new LeafHistogram (f_));
}
- NodeHistogram::NodeHistogram (const graph::GraphPtr_t& graph) :
+ StateHistogram::StateHistogram (const graph::GraphPtr_t& graph) :
graph_ (graph) {}
- void NodeHistogram::add (const RoadmapNodePtr_t& n)
+ void StateHistogram::add (const RoadmapNodePtr_t& n)
{
- iterator it = insert (NodeBin (graph_->getNode (n)));
+ iterator it = insert (NodeBin (graph_->getState (n)));
it->push_back (n);
if (numberOfObservations()%10 == 0) {
hppDout (info, *this);
}
}
- std::ostream& NodeHistogram::print (std::ostream& os) const
+ std::ostream& StateHistogram::print (std::ostream& os) const
{
- os << "Graph Node Histogram contains: " << std::endl;
+ os << "Graph State Histogram contains: " << std::endl;
return Parent::print (os);
}
- const graph::GraphPtr_t& NodeHistogram::constraintGraph () const
+ const graph::GraphPtr_t& StateHistogram::constraintGraph () const
{
return graph_;
}
- HistogramPtr_t NodeHistogram::clone () const
+ HistogramPtr_t StateHistogram::clone () const
{
- return HistogramPtr_t (new NodeHistogram (graph_));
+ return HistogramPtr_t (new StateHistogram (graph_));
}
unsigned int LeafBin::numberOfObsOutOfConnectedComponent (const core::ConnectedComponentPtr_t& cc) const
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 0c98d574b6cb6741a379544972671df8cfa60aeb..c6406a51818b999f318c2b7c0d4c5fac0b71684b 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -35,7 +35,7 @@
#include "hpp/manipulation/roadmap.hh"
#include "hpp/manipulation/roadmap-node.hh"
#include "hpp/manipulation/graph/edge.hh"
-#include "hpp/manipulation/graph/node-selector.hh"
+#include "hpp/manipulation/graph/state-selector.hh"
namespace hpp {
namespace manipulation {
@@ -128,9 +128,9 @@ namespace hpp {
DevicePtr_t robot = HPP_DYNAMIC_PTR_CAST(Device, problem ().robot ());
HPP_ASSERT(robot);
- const graph::Nodes_t& graphNodes = problem_.constraintGraph ()
- ->nodeSelector ()->getNodes ();
- graph::Nodes_t::const_iterator itNode;
+ const graph::States_t& graphStates = problem_.constraintGraph ()
+ ->stateSelector ()->getStates ();
+ graph::States_t::const_iterator itState;
core::Nodes_t newNodes;
core::PathPtr_t path;
@@ -148,9 +148,9 @@ namespace hpp {
itcc != roadmap ()->connectedComponents ().end (); ++itcc) {
// Find the nearest neighbor.
core::value_type distance;
- for (itNode = graphNodes.begin (); itNode != graphNodes.end (); ++itNode) {
+ for (itState = graphStates.begin (); itState != graphStates.end (); ++itState) {
HPP_START_TIMECOUNTER(nearestNeighbor);
- RoadmapNodePtr_t near = roadmap_->nearestNode (q_rand, HPP_STATIC_PTR_CAST(ConnectedComponent,*itcc), *itNode, distance);
+ RoadmapNodePtr_t near = roadmap_->nearestNode (q_rand, HPP_STATIC_PTR_CAST(ConnectedComponent,*itcc), *itState, distance);
HPP_STOP_TIMECOUNTER(nearestNeighbor);
HPP_DISPLAY_LAST_TIMECOUNTER(nearestNeighbor);
if (!near) continue;
diff --git a/src/path-optimization/config-optimization.cc b/src/path-optimization/config-optimization.cc
index ffa166338f9798c34c9ec8db5060efc20a2e417d..cb2dc1a42f3abce3440b97530f1a76fa4419aa35 100644
--- a/src/path-optimization/config-optimization.cc
+++ b/src/path-optimization/config-optimization.cc
@@ -21,7 +21,7 @@
#include <hpp/core/path.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/manipulation/device.hh>
-#include <hpp/manipulation/graph/node.hh>
+#include <hpp/manipulation/graph/state.hh>
#include <hpp/manipulation/graph/edge.hh>
#include <hpp/manipulation/constraint-set.hh>
diff --git a/src/roadmap.cc b/src/roadmap.cc
index 48b08ea50f9af15766640ba4dd6d2d4dcb8ff240..94190a6c1162a0ef6bb641c994351c3284990503 100644
--- a/src/roadmap.cc
+++ b/src/roadmap.cc
@@ -24,7 +24,7 @@
#include <hpp/manipulation/roadmap.hh>
#include <hpp/manipulation/roadmap-node.hh>
#include <hpp/manipulation/symbolic-component.hh>
-#include <hpp/manipulation/graph/node.hh>
+#include <hpp/manipulation/graph/state.hh>
#include <hpp/manipulation/graph/statistics.hh>
namespace hpp {
@@ -92,12 +92,15 @@ namespace hpp {
RoadmapNodePtr_t Roadmap::nearestNode (const ConfigurationPtr_t& configuration,
const ConnectedComponentPtr_t& connectedComponent,
- const graph::NodePtr_t& node,
+ const graph::StatePtr_t& state,
value_type& minDistance) const
{
core::NodePtr_t result = NULL;
minDistance = std::numeric_limits <value_type>::infinity ();
- const RoadmapNodes_t& roadmapNodes = connectedComponent->getRoadmapNodes (node);
+ const RoadmapNodes_t& roadmapNodes = connectedComponent->getRoadmapNodes (state);
+ // std::cout << "State: " << state->name () << std::endl;
+ // std::cout << "roadmapNodes.size () = " << roadmapNodes.size ()
+ // << std::endl;
for (RoadmapNodes_t::const_iterator itNode = roadmapNodes.begin ();
itNode != roadmapNodes.end (); ++itNode) {
value_type d = (*distance()) (*(*itNode)->configuration (),
@@ -120,9 +123,9 @@ namespace hpp {
return node;
}
- graph::NodePtr_t Roadmap::getNode(RoadmapNodePtr_t node)
+ graph::StatePtr_t Roadmap::getState(RoadmapNodePtr_t node)
{
- return graph_->getNode(node);
+ return graph_->getState(node);
}
void Roadmap::addEdge (const core::EdgePtr_t& edge)
diff --git a/src/symbolic-component.cc b/src/symbolic-component.cc
index f3a52f01c25e035b42fdb2338f4449f00905a026..fe591e8f26c44a6edd8623ef71e0152c4a4c76fc 100644
--- a/src/symbolic-component.cc
+++ b/src/symbolic-component.cc
@@ -17,7 +17,7 @@
#include <hpp/manipulation/symbolic-component.hh>
#include <hpp/manipulation/roadmap.hh>
-#include <hpp/manipulation/graph/node.hh>
+#include <hpp/manipulation/graph/state.hh>
namespace hpp {
namespace manipulation {
@@ -32,7 +32,7 @@ namespace hpp {
void SymbolicComponent::addNode (const RoadmapNodePtr_t& node)
{
assert(state_);
- graph::NodePtr_t state = roadmap_->getNode(node);
+ graph::StatePtr_t state = roadmap_->getState(node);
// Sanity check
if (state_ == state) // Oops
@@ -44,7 +44,7 @@ namespace hpp {
void SymbolicComponent::setFirstNode (const RoadmapNodePtr_t& node)
{
assert(!state_);
- state_ = roadmap_->getNode(node);
+ state_ = roadmap_->getState(node);
nodes_.push_back(node);
}
diff --git a/src/symbolic-planner.cc b/src/symbolic-planner.cc
index c4aa63f35b9f8cd979f516f839320413f2e46ef9..b775db67726ac0feb45d1edcecb532957e91412b 100644
--- a/src/symbolic-planner.cc
+++ b/src/symbolic-planner.cc
@@ -45,7 +45,7 @@
#include "hpp/manipulation/symbolic-component.hh"
#include "hpp/manipulation/graph/edge.hh"
#include "hpp/manipulation/graph/graph.hh"
-#include "hpp/manipulation/graph/node-selector.hh"
+#include "hpp/manipulation/graph/state-selector.hh"
#define CastToWSC_ptr(var, scPtr) \
WeighedSymbolicComponentPtr_t var = \
diff --git a/tests/test-constraintgraph.cc b/tests/test-constraintgraph.cc
index afa8b7ba1519a10854c841fee536ea9f76505500..53084deb97d369ab5d10b44db8bc8e9adb139f80 100644
--- a/tests/test-constraintgraph.cc
+++ b/tests/test-constraintgraph.cc
@@ -22,8 +22,8 @@
#include <hpp/constraints/position.hh>
#include <hpp/constraints/relative-com.hh>
-#include "hpp/manipulation/graph/node.hh"
-#include "hpp/manipulation/graph/node-selector.hh"
+#include "hpp/manipulation/graph/state.hh"
+#include "hpp/manipulation/graph/state-selector.hh"
#include "hpp/manipulation/graph/graph.hh"
#include "hpp/manipulation/graph/edge.hh"
#include "hpp/manipulation/device.hh"