diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 5abb4d777283c012129227f40c6e2fff6d79e833..a3af38f5fb992ff7a2f545f47b0e15b11ea0c2ae 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -19,6 +19,7 @@
#include <hpp/core/constraint-set.hh>
+#include "hpp/manipulation/config.hh"
#include "hpp/manipulation/fwd.hh"
#include "hpp/manipulation/graph/node.hh"
@@ -33,13 +34,7 @@ namespace hpp {
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;
- }
+ const ConstraintPtr_t& constraints);
/// Projector to project onto the same leaf as config.
/// \return The initialized projector.
@@ -54,13 +49,7 @@ namespace hpp {
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;
- }
+ const NodeWkPtr_t& to, const ConstraintPtr_t& constraints);
/// Constructor
Edge()
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index 26644ba11c6295073b7172a1e0a4122018046b47..cbdce39df11f4c6b5173733657279a1a127298db 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -20,6 +20,7 @@
# include "hpp/manipulation/robot.hh"
# include "hpp/manipulation/graph/node-selector.hh"
+#include "hpp/manipulation/config.hh"
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/fwd.hh"
@@ -40,21 +41,10 @@ namespace hpp {
{
public:
/// Create a new Graph.
- static GraphPtr_t create(RobotPtr_t robot)
- {
- Graph* ptr = new Graph;
- GraphPtr_t shPtr (ptr);
- ptr->init (shPtr, robot);
- return shPtr;
- }
+ static GraphPtr_t create(RobotPtr_t robot);
/// Create and insert a NodeSelector inside the graph.
- NodeSelectorPtr_t createNodeSelector()
- {
- NodeSelectorPtr_t newNodeSelector = NodeSelector::create();
- nodeSelectors_.push_back(newNodeSelector);
- return newNodeSelector;
- }
+ NodeSelectorPtr_t createNodeSelector();
/// Returns the states of a configuration.
virtual Nodes_t getNode(const Configuration_t config) const;
@@ -64,11 +54,7 @@ namespace hpp {
protected:
/// Initialization of the object.
- void init (const GraphWkPtr_t& weak, RobotPtr_t robot)
- {
- robot_ = robot;
- wkPtr_ = weak;
- }
+ void init (const GraphWkPtr_t& weak, RobotPtr_t robot);
/// Constructor
Graph ()
diff --git a/include/hpp/manipulation/graph/node-selector.hh b/include/hpp/manipulation/graph/node-selector.hh
index b9fc75a9ea880eae4422fa1cf975bc01ada0e26b..ff69ec22cbd17595586ef56051d2f2f12c096661 100644
--- a/include/hpp/manipulation/graph/node-selector.hh
+++ b/include/hpp/manipulation/graph/node-selector.hh
@@ -17,6 +17,7 @@
#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/node.hh"
@@ -29,21 +30,10 @@ namespace hpp {
{
public:
/// Create a new NodeSelector.
- static NodeSelectorPtr_t create()
- {
- NodeSelector* ptr = new NodeSelector();
- NodeSelectorPtr_t shPtr (ptr);
- ptr->init (shPtr);
- return shPtr;
- }
+ static NodeSelectorPtr_t create();
/// 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;
- }
+ NodePtr_t createNode (const ConstraintPtr_t& constraints);
/// Returns the state of a configuration.
virtual NodePtr_t getNode(const Configuration_t config) const;
@@ -53,10 +43,7 @@ namespace hpp {
protected:
/// Initialization of the object.
- void init (const NodeSelectorPtr_t& weak)
- {
- wkPtr_ = weak;
- }
+ void init (const NodeSelectorPtr_t& weak);
/// Constructor
NodeSelector ()
diff --git a/include/hpp/manipulation/graph/node.hh b/include/hpp/manipulation/graph/node.hh
index 27132b48b9bc94179e875724e01dec20ac67ae5c..0dd4a52ba2883a7496bc2cd7360dd3b7ed76a555 100644
--- a/include/hpp/manipulation/graph/node.hh
+++ b/include/hpp/manipulation/graph/node.hh
@@ -34,21 +34,10 @@ namespace hpp {
{
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;
- }
+ static NodePtr_t create (const ConstraintPtr_t& constraints);
/// Create a link from this node to the given node.
- EdgePtr_t linkTo(const NodePtr_t& to, const ConstraintPtr_t& constraints)
- {
- EdgePtr_t newEdge = Edge::create(wkPtr_, to, constraints);
- neighbors_.push_back(newEdge);
- return newEdge;
- }
+ EdgePtr_t linkTo(const NodePtr_t& to, const ConstraintPtr_t& constraints);
/// Check whether the configuration is in this state.
/// \return True if this state contains this configuration
@@ -56,14 +45,7 @@ namespace hpp {
/// \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
- {
- // TODO: This is not the most efficient way. We should
- // compute the value of the constraint instead of apllying
- // the constraint.
- Configuration_t cfg = config;
- return constraints_->apply(cfg) && ( cfg == config );
- }
+ virtual bool contains (const Configuration_t config) const;
/// Get the constraint set associated to the node.
const ConstraintPtr_t constraints () const
@@ -79,11 +61,7 @@ namespace hpp {
protected:
/// Initialize the object.
- void init (const NodeWkPtr_t& self, const ConstraintPtr_t& constraints)
- {
- wkPtr_ = self;
- constraints_ = constraints;
- }
+ void init (const NodeWkPtr_t& self, const ConstraintPtr_t& constraints);
Node()
{}
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index d06cc63b3d0876a20d0fd0b6e39090bc9e319a71..286b86d7b3ac7f07ab3332f67f737aeab9654388 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -25,6 +25,11 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED
problem-solver.cc
robot.cc
manipulation-planner.cc
+
+ graph/node.cc
+ graph/edge.cc
+ graph/graph.cc
+ graph/node-selector.cc
)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-core)
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
new file mode 100644
index 0000000000000000000000000000000000000000..eb31d71d3cbc744c5b51f9482f46fb8667cfb258
--- /dev/null
+++ b/src/graph/edge.cc
@@ -0,0 +1,41 @@
+// Copyright (c) 2014, LAAS-CNRS
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+#include "hpp/manipulation/graph/edge.hh"
+
+namespace hpp {
+ namespace manipulation {
+ namespace graph {
+ EdgePtr_t Edge::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;
+ }
+
+ void Edge::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;
+ }
+ } // namespace graph
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
new file mode 100644
index 0000000000000000000000000000000000000000..43e1456649c2da7004cc8dc10303fc3a06d1d221
--- /dev/null
+++ b/src/graph/graph.cc
@@ -0,0 +1,44 @@
+// Copyright (c) 2014, LAAS-CNRS
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+#include "hpp/manipulation/graph/graph.hh"
+
+namespace hpp {
+ namespace manipulation {
+ namespace graph {
+ GraphPtr_t Graph::create(RobotPtr_t robot)
+ {
+ Graph* ptr = new Graph;
+ GraphPtr_t shPtr (ptr);
+ ptr->init (shPtr, robot);
+ return shPtr;
+ }
+
+ void Graph::init (const GraphWkPtr_t& weak, RobotPtr_t robot)
+ {
+ robot_ = robot;
+ wkPtr_ = weak;
+ }
+
+ NodeSelectorPtr_t Graph::createNodeSelector()
+ {
+ NodeSelectorPtr_t newNodeSelector = NodeSelector::create();
+ nodeSelectors_.push_back(newNodeSelector);
+ return newNodeSelector;
+ }
+ } // namespace graph
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/graph/node-selector.cc b/src/graph/node-selector.cc
new file mode 100644
index 0000000000000000000000000000000000000000..d50609654456e78aec4606c773757176c1273375
--- /dev/null
+++ b/src/graph/node-selector.cc
@@ -0,0 +1,43 @@
+// Copyright (c) 2014, LAAS-CNRS
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+#include "hpp/manipulation/graph/node-selector.hh"
+
+namespace hpp {
+ namespace manipulation {
+ namespace graph {
+ NodeSelectorPtr_t NodeSelector::create()
+ {
+ NodeSelector* ptr = new NodeSelector();
+ NodeSelectorPtr_t shPtr (ptr);
+ ptr->init (shPtr);
+ return shPtr;
+ }
+
+ void NodeSelector::init (const NodeSelectorPtr_t& weak)
+ {
+ wkPtr_ = weak;
+ }
+
+ NodePtr_t NodeSelector::createNode (const ConstraintPtr_t& constraints)
+ {
+ NodePtr_t newNode = Node::create(constraints);
+ orderedStates_.push_back(newNode);
+ return newNode;
+ }
+ } // namespace graph
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/graph/node.cc b/src/graph/node.cc
new file mode 100644
index 0000000000000000000000000000000000000000..ec2e33fb58913267f0a8ce74c8eb74895813081f
--- /dev/null
+++ b/src/graph/node.cc
@@ -0,0 +1,53 @@
+// Copyright (c) 2014, LAAS-CNRS
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+#include "hpp/manipulation/graph/node.hh"
+
+namespace hpp {
+ namespace manipulation {
+ namespace graph {
+ NodePtr_t Node::create (const ConstraintPtr_t& constraints)
+ {
+ Node* node = new Node;
+ NodePtr_t shPtr(node);
+ shPtr->init(shPtr, constraints);
+ return shPtr;
+ }
+
+ void Node::init (const NodeWkPtr_t& self, const ConstraintPtr_t& constraints)
+ {
+ wkPtr_ = self;
+ constraints_ = constraints;
+ }
+
+ EdgePtr_t Node::linkTo(const NodePtr_t& to, const ConstraintPtr_t& constraints)
+ {
+ EdgePtr_t newEdge = Edge::create(wkPtr_, to, constraints);
+ neighbors_.push_back(newEdge);
+ return newEdge;
+ }
+
+ bool Node::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.
+ Configuration_t cfg = config;
+ return constraints_->apply(cfg) && ( cfg == config );
+ }
+ } // namespace graph
+ } // namespace manipulation
+} // namespace hpp