From 3d34550d7d6080ee5d74b4165f2fd3ae04eb8db5 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 11 Jul 2014 16:10:41 +0200
Subject: [PATCH] Implement method to create manipulation graph
---
include/hpp/manipulation/graph/edge.hh | 30 ++++++++++-
include/hpp/manipulation/graph/graph.hh | 7 +++
.../hpp/manipulation/graph/node-selector.hh | 10 +++-
include/hpp/manipulation/graph/node.hh | 52 ++++++++++++++++++-
4 files changed, 96 insertions(+), 3 deletions(-)
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index b33094c1..5a730555 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -31,6 +31,16 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI Edge
{
public:
+ /// Create a new Edge.
+ static EdgePtr_t create (const NodeWkPtr_t& from, const NodeWkPtr_t& to.
+ const ConstraintPtr_t& constraints)
+ {
+ Edge* ptr = new Edge;
+ EdgePtr_t shPtr (ptr);
+ ptr->init(shPtr, from, to, constraints);
+ return shPtr;
+ }
+
/// Projector to project onto the same leaf as config.
/// \return The initialized projector.
/// \param config Configuration that will initialize the projector.
@@ -41,9 +51,24 @@ namespace hpp {
/// \param config Configuration that will initialize the projector.
ConfigProjectorPtr_t pathProjector(const Configuration_t config);
+ protected:
+ /// Initialization of the object.
+ void init (const EdgeWkPtr_t& weak, const NodeWkPtr_t& from,
+ const NodeWkPtr_t& to, const ConstraintPtr_t& constraints)
+ {
+ wkPtr_ = weak;
+ from_ = from;
+ to_ = to;
+ constraints_ = constraints;
+ }
+
+ /// Constructor
+ Edge()
+ {}
+
private:
/// The two ends of the transition.
- NodeWkPtr_t startState, endState;
+ NodeWkPtr_t from_, to_;
/// Set of constraints to be statisfied.
ConstraintPtr_t constraints_;
@@ -55,6 +80,9 @@ namespace hpp {
/// Projectors ensuring that a path between two configurations in
/// the same leaf lies in the leaf.
ConfigProjectorPtr_t pathProjector_;
+
+ /// Weak pointer to itself.
+ EdgeWkPtr_t wkPtr_;
}; // class Edge
} // namespace graph
} // namespace manipulation
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index 0989cb7a..47cf7c7c 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -26,6 +26,13 @@ namespace hpp {
/// Description of the constraint graph
/// This class contains a graph representing a robot with several
/// end-effectors.
+ ///
+ /// 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.
+ /// - An Edge does not own anything.
class HPP_MANIPULATION_DLLAPI Graph
{
public:
diff --git a/include/hpp/manipulation/graph/node-selector.hh b/include/hpp/manipulation/graph/node-selector.hh
index e8a27570..581b7a45 100644
--- a/include/hpp/manipulation/graph/node-selector.hh
+++ b/include/hpp/manipulation/graph/node-selector.hh
@@ -28,6 +28,14 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI NodeSelector
{
public:
+ /// Create a nodes with the constraints
+ NodePtr_t createNode (const ConstraintPtr_t& constraints)
+ {
+ NodePtr_t newNode = Node::create(constraints);
+ orderedStates_.push_back(newNode);
+ return newNode;
+ }
+
/// Returns the state of a configuration.
virtual NodePtr_t getNode(const Configuration_t config) const;
@@ -36,7 +44,7 @@ namespace hpp {
private:
/// List of the states of one end-effector, ordered by priority.
- std::vector< NodeWkPtr_t > orderedStates_;
+ std::vector< NodePtr_t > orderedStates_;
}; // Class NodeSelector
} // namespace graph
} // namespace manipulation
diff --git a/include/hpp/manipulation/graph/node.hh b/include/hpp/manipulation/graph/node.hh
index 1353b8b7..d586c82e 100644
--- a/include/hpp/manipulation/graph/node.hh
+++ b/include/hpp/manipulation/graph/node.hh
@@ -33,13 +33,60 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI Node
{
public:
+ /// Create a new node.
+ static NodePtr_t create (const ConstraintPtr_t& constraints)
+ {
+ Node* node = new Node;
+ NodePtr_t shPtr(node);
+ shPtr->init(shPtr, constraints);
+ return shPtr;
+ }
+
+ /// Create a link from this node to the given node.
+ EdgePtr_t linkTo(const EdgePtr_t& to, const ConstraintPtr_t& constraints)
+ {
+ EdgePtr_t newEdge = Edge::create(wkPtr_, to, constraints);
+ neighbors_.push_back(newEdge);
+ newEdge->
+ }
+
/// Check whether the configuration is in this state.
/// \return True if this state contains this configuration
/// \param config The configuration to be tested.
/// \note You should note use that 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(const Configuration_t config) const;
+ virtual bool contains (const Configuration_t config) const
+ {
+ // TODO: This is not the most efficient way. We should
+ // compute the value of the constraint instead of apllying
+ // the constraint.
+ ConfigurationOut_t cfg = config;
+ return constraints_->apply(cfg) && ( cfg == config );
+ }
+
+ /// Get the constraint set associated to the node.
+ const ConstraintPtr_t constraints () const
+ {
+ return constraints_;
+ }
+
+ /// Set the constraint set associated to the node.
+ void constraints (const ConstraintPtr_t& constraints)
+ {
+ constraints_ = constraints;
+ }
+
+ protected:
+ /// Initialize the object.
+ void init (const NodeWkPtr_t& self, const ConstraintPtr_t& constraints)
+ {
+ wkPtr_ = self;
+ constraints_ = constraints;
+ }
+
+ Node()
+ {}
private:
/// List of possible motions from this state (i.e. the outgoing
@@ -51,6 +98,9 @@ namespace hpp {
/// A selector that will implement the selection of the next state.
NodeSelectorPtr_t selector_;
+
+ /// Weak pointer to itself.
+ NodeWkPtr_t wkPtr_;
}; // class Node
} // namespace graph
} // namespace manipulation
--
GitLab