diff --git a/CMakeLists.txt b/CMakeLists.txt
index a90f6af8ea8dc0dedc72993fa4ac470e0436d299..cea373bcdff689623b3d950763b029e95a1acb13 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -55,6 +55,7 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/problem.hh
include/hpp/manipulation/problem-solver.hh
include/hpp/manipulation/robot.hh
+ include/hpp/manipulation/roadmap.hh
include/hpp/manipulation/manipulation-planner.hh
include/hpp/manipulation/graph-path-validation.hh
include/hpp/manipulation/graph-steering-method.hh
@@ -62,6 +63,7 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/graph/edge.hh
include/hpp/manipulation/graph/node-selector.hh
include/hpp/manipulation/graph/graph.hh
+ include/hpp/manipulation/graph/statistics.hh
include/hpp/manipulation/graph/graph-component.hh
include/hpp/manipulation/graph/fwd.hh
)
diff --git a/include/hpp/manipulation/graph/statistics.hh b/include/hpp/manipulation/graph/statistics.hh
new file mode 100644
index 0000000000000000000000000000000000000000..653cea81c5a0cac2101687d262c6507e824eed0a
--- /dev/null
+++ b/include/hpp/manipulation/graph/statistics.hh
@@ -0,0 +1,153 @@
+// 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_STATISTICS_HH
+# define HPP_MANIPULATION_GRAPH_STATISTICS_HH
+
+# include <hpp/core/node.hh>
+# include <hpp/core/constraint-set.hh>
+# include <hpp/statistics/bin.hh>
+
+# include "hpp/manipulation/config.hh"
+# include "hpp/manipulation/fwd.hh"
+
+namespace hpp {
+ namespace manipulation {
+ namespace graph {
+ /// This class is used to do statistics on the roadmap.
+ /// It keeps a track of which leaf of a foliation have been visited.
+ class HPP_MANIPULATION_DLLLOCAL LeafBin : public ::hpp::statistics::Bin
+ {
+ public :
+ LeafBin(const vector_t& v): value_(v), configs_() {}
+
+ void push_back(const Configuration_t& cfg) {
+ configs_.push_back(cfg);
+ }
+
+ bool operator<(const LeafBin& rhs) const {
+ const vector_t& v = rhs.value ();
+ assert (value_.size() == v.size());
+ for (int p = 0; p < value_.size(); p++) {
+ if (value_[p] != v[p])
+ return value_[p] < v[p];
+ }
+ return false;
+ }
+
+ bool operator==(const LeafBin& rhs) const {
+ return value_ == rhs.value();
+ }
+
+ const vector_t& value () const {
+ return value_;
+ }
+
+ private:
+ vector_t value_;
+
+ std::list <Configuration_t> configs_;
+
+ std::ostream& printValue (std::ostream& os) const
+ {
+ os << "LeafBin (";
+ size_t s = value_.size();
+ if (s != 0) {
+ for (size_t i = 0; i < s - 1; i++)
+ os << value_[i] << "; ";
+ os << value_[s-1];
+ }
+ os << ")";
+ return os;
+ }
+ };
+
+ /// This class is used to do statistics on the roadmap.
+ /// It keeps a track of which leaf of a foliation have been visited.
+ class HPP_MANIPULATION_DLLLOCAL NodeBin : public ::hpp::statistics::Bin
+ {
+ public :
+ NodeBin(const ConstraintSetPtr_t& c): constraint_(c), configs_() {}
+
+ void push_back(const Configuration_t& cfg) {
+ configs_.push_back(cfg);
+ }
+
+ bool operator<(const NodeBin& rhs) const {
+ return constraint_.get() < rhs.constraint ().get();
+ }
+
+ bool operator==(const NodeBin& rhs) const {
+ return constraint_ == rhs.constraint ();
+ }
+
+ const ConstraintSetPtr_t& constraint () const {
+ return constraint_;
+ }
+
+ private:
+ ConstraintSetPtr_t constraint_;
+
+ std::list <Configuration_t> configs_;
+
+ std::ostream& printValue (std::ostream& os) const
+ {
+ return os << "NodeBin (" << constraint_->name () << ")";
+ }
+ };
+
+ class HPP_MANIPULATION_DLLLOCAL LeafHistogram : public ::hpp::statistics::Statistics < LeafBin >
+ {
+ public:
+ typedef ::hpp::statistics::Statistics < LeafBin > Parent;
+ /// Constructor
+ /// \param constraint The constraint that create the foliation being
+ /// studied.
+ LeafHistogram (const ConstraintSetPtr_t& constraint) :
+ constraint_ (constraint) {}
+
+ /// Insert an occurence of a value in the histogram
+ void add (const Configuration_t& config)
+ {
+ LeafBin b(constraint_->offsetFromConfig (config));
+ increment (b);
+ b.push_back (config);
+ }
+
+ std::ostream& print (std::ostream& os) const
+ {
+ os << "Histogram constains ConstraintSet: ";
+ os << *constraint_ << std::endl;
+ return Parent::print (os);
+ }
+
+ const ConstraintSetPtr_t& constraint () const
+ {
+ return constraint_;
+ }
+
+ private:
+ /// The constraint that creates the foliation.
+ ConstraintSetPtr_t constraint_;
+ };
+ } // namespace graph
+ } // namespace manipulation
+
+ std::ostream& operator<< (std::ostream& os, const manipulation::graph::LeafHistogram& h);
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_GRAPH_STATISTICS_HH
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 07608832919b6e9da1b5de3eef236d3d2a3d07fc..423b64a688e85aca7ba4b704bf2ee2285b01f241 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -149,9 +149,12 @@ namespace hpp {
void buildCompositeRobot (const std::string& robotName,
const Names_t& robotNames);
- /// Create new problem.
+ /// Create a new problem.
virtual void resetProblem ();
+ /// Create a new Roadmap
+ virtual void resetRoadmap ();
+
/// Get pointer to problem
ProblemPtr_t problem () const
{
diff --git a/include/hpp/manipulation/roadmap.hh b/include/hpp/manipulation/roadmap.hh
new file mode 100644
index 0000000000000000000000000000000000000000..2fa6db7ea24719c645bb7c9a7b0857eeb5319431
--- /dev/null
+++ b/include/hpp/manipulation/roadmap.hh
@@ -0,0 +1,62 @@
+// 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_ROADMAP_HH
+# define HPP_MANIPULATION_ROADMAP_HH
+
+# include <hpp/core/roadmap.hh>
+# include <hpp/core/constraint-set.hh>
+
+# include "hpp/manipulation/fwd.hh"
+# include "hpp/manipulation/graph/statistics.hh"
+
+namespace hpp {
+ namespace manipulation {
+ /// Extension of hpp::core::Roadmap. It adds the ability of doing
+ /// statistics on the graph
+ class HPP_MANIPULATION_DLLAPI Roadmap : public core::Roadmap
+ {
+ public:
+ typedef core::Roadmap Parent;
+
+ /// Return a shared pointer to a new instance
+ static RoadmapPtr_t create (const core::DistancePtr_t& distance, const core::DevicePtr_t& robot);
+
+ /// Add a ConstraintSet that creates a foliation.
+ void statAddFoliation (ConstraintSetPtr_t constraint);
+
+ /// Clear the histograms and call parent implementation.
+ void clear ();
+
+ /// Catch event 'New node added'
+ void push_node (const core::NodePtr_t& n);
+
+ protected:
+ /// Register a new configuration.
+ void statInsert (ConfigurationIn_t config);
+
+ /// Constructor
+ Roadmap (const core::DistancePtr_t& distance, const core::DevicePtr_t& robot);
+
+ private:
+ /// Keep track of the leaf that are explored.
+ /// There should be one histogram per foliation.
+ std::vector < graph::LeafHistogram > histograms_;
+ };
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_ROADMAP_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index d7a2044eff32102a14a34177348b67a7ca916557..962b747bdf3e7fd02bb9326e35597288ff6521ad 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -22,9 +22,10 @@ SET(LIBRARY_NAME ${PROJECT_NAME})
ADD_LIBRARY(${LIBRARY_NAME} SHARED
axial-handle.cc
handle.cc
+ manipulation-planner.cc
problem-solver.cc
robot.cc
- manipulation-planner.cc
+ roadmap.cc
graph-path-validation.cc
graph-steering-method.cc
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 7ad6ff534eefc99d5a4c6557cf39ba65c4bb47dc..4f593ede72d510db0399a42852bfb25456e521f1 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -25,6 +25,7 @@
#include "hpp/manipulation/graph/graph.hh"
#include "hpp/manipulation/manipulation-planner.hh"
#include "hpp/manipulation/problem.hh"
+#include "hpp/manipulation/roadmap.hh"
#include "hpp/manipulation/problem-solver.hh"
@@ -165,5 +166,12 @@ namespace hpp {
}
}
}
+
+ void ProblemSolver::resetRoadmap ()
+ {
+ if (!problem ())
+ throw std::runtime_error ("The problem is not defined.");
+ roadmap (Roadmap::create (problem ()->distance (), problem ()->robot ()));
+ }
} // namespace manipulation
} // namespace hpp
diff --git a/src/roadmap.cc b/src/roadmap.cc
new file mode 100644
index 0000000000000000000000000000000000000000..10657083e3038e02bd1763cc20cdc604650d5250
--- /dev/null
+++ b/src/roadmap.cc
@@ -0,0 +1,67 @@
+// 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/roadmap.hh"
+
+namespace hpp {
+ std::ostream& operator<< (std::ostream& os, const manipulation::graph::LeafHistogram& h)
+ {
+ return h.print (os);
+ }
+
+ namespace manipulation {
+ Roadmap::Roadmap (const core::DistancePtr_t& distance, const core::DevicePtr_t& robot) :
+ core::Roadmap (distance, robot) {}
+
+ RoadmapPtr_t Roadmap::create (const core::DistancePtr_t& distance, const core::DevicePtr_t& robot)
+ {
+ return RoadmapPtr_t (new Roadmap (distance, robot));
+ }
+
+ void Roadmap::clear ()
+ {
+ Parent::clear ();
+ std::vector < graph::LeafHistogram > newHistograms;
+ std::vector < graph::LeafHistogram >::iterator it;
+ for (it = histograms_.begin(); it != histograms_.end(); it++) {
+ newHistograms.push_back (graph::LeafHistogram (it->constraint ()));
+ }
+ histograms_ = newHistograms;
+ }
+
+ void Roadmap::push_node (const core::NodePtr_t& n)
+ {
+ Parent::push_node (n);
+ statInsert (*(n->configuration ()));
+ }
+
+ void Roadmap::statInsert (ConfigurationIn_t config)
+ {
+ std::vector < graph::LeafHistogram >::iterator it;
+ for (it = histograms_.begin(); it != histograms_.end(); it++) {
+ it->add (config);
+ if (it->numberOfObservations()%10 == 0) {
+ hppDout(info, *it);
+ }
+ }
+ }
+
+ void Roadmap::statAddFoliation (ConstraintSetPtr_t constraint)
+ {
+ histograms_.push_back (graph::LeafHistogram (constraint));
+ }
+ } // namespace manipulation
+} // namespace hpp