From 00c0da12e2b28f2bf79b0c120f9b00e7a7595873 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 17 Sep 2015 18:41:21 +0200
Subject: [PATCH] Add template class GraphOptimizer
---
CMakeLists.txt | 1 +
include/hpp/manipulation/graph-optimizer.hh | 154 ++++++++++++++++++++
src/CMakeLists.txt | 1 +
src/graph-optimizer.cc | 22 +++
src/problem-solver.cc | 5 +-
5 files changed, 182 insertions(+), 1 deletion(-)
create mode 100644 include/hpp/manipulation/graph-optimizer.hh
create mode 100644 src/graph-optimizer.cc
diff --git a/CMakeLists.txt b/CMakeLists.txt
index dc97934..a40f409 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -71,6 +71,7 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/manipulation-planner.hh
include/hpp/manipulation/graph-path-validation.hh
include/hpp/manipulation/graph-steering-method.hh
+ include/hpp/manipulation/graph-optimizer.hh
include/hpp/manipulation/graph/node.hh
include/hpp/manipulation/graph/edge.hh
include/hpp/manipulation/graph/node-selector.hh
diff --git a/include/hpp/manipulation/graph-optimizer.hh b/include/hpp/manipulation/graph-optimizer.hh
new file mode 100644
index 0000000..b6bcfab
--- /dev/null
+++ b/include/hpp/manipulation/graph-optimizer.hh
@@ -0,0 +1,154 @@
+// Copyright (c) 2015, 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_GRAPHOPTIMIZER_HH
+# define HPP_MANIPULATION_GRAPHOPTIMIZER_HH
+
+# include <hpp/core/path.hh>
+# include <hpp/core/path-vector.hh>
+# include <hpp/core/path-optimizer.hh>
+# include <hpp/core/problem.hh>
+
+# include <hpp/manipulation/fwd.hh>
+# include <hpp/manipulation/graph/fwd.hh>
+# include <hpp/manipulation/config.hh>
+# include <hpp/manipulation/constraint-set.hh>
+
+namespace hpp {
+ namespace manipulation {
+ using hpp::core::PathOptimizer;
+ using hpp::core::Path;
+ using hpp::core::PathPtr_t;
+ using hpp::core::PathVector;
+ using hpp::core::PathVectorPtr_t;
+
+ /// \addtogroup path_optimization
+ /// \{
+
+ /// Path optimizer for paths created with the constraint graph
+ ///
+ /// This class encapsulates another path optimizer class. This optimizer
+ /// calls the inner optimizer on every subpaths with the same set of
+ /// constraints.
+ template <typename InnerPathOptimizer_t>
+ class HPP_MANIPULATION_DLLAPI GraphOptimizer : public PathOptimizer
+ {
+ public:
+ typedef boost::shared_ptr <InnerPathOptimizer_t> InnerPtr_t;
+ typedef boost::shared_ptr <GraphOptimizer> Ptr_t;
+
+ static Ptr_t create (const core::Problem& problem);
+
+ // static Ptr_t create (const Problem& problem);
+
+ virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
+
+ /// Get the encapsulated optimizer
+ const InnerPtr_t& innerOptimizer ()
+ {
+ return pathOptimizer_;
+ }
+
+ protected:
+ /// Constructor
+ GraphOptimizer (const Problem& problem) :
+ PathOptimizer (problem),
+ pathOptimizer_ (InnerPathOptimizer_t::create (problem))
+ {}
+
+ private:
+ /// The encapsulated PathOptimizer
+ InnerPtr_t pathOptimizer_;
+
+ /// Append all paths of in to out
+ /// \param in a path vector, possibly containing other path vector
+ /// \param out a flat PathVector (do not contain PathVector)
+ static void unpack (PathVectorPtr_t in, PathVectorPtr_t out);
+ };
+ /// \}
+
+ /// Member function definition
+ template <typename InnerPathOptimizer_t>
+ typename GraphOptimizer<InnerPathOptimizer_t>::Ptr_t
+ GraphOptimizer<InnerPathOptimizer_t>::create (const core::Problem& problem)
+ {
+ return GraphOptimizer<InnerPathOptimizer_t>::Ptr_t (
+ new GraphOptimizer<InnerPathOptimizer_t> (dynamic_cast <const Problem&> (problem))
+ );
+ // return GraphOptimizer<InnerPathOptimizer_t>::create
+ // (dynamic_cast <const Problem&> (problem));
+ }
+
+ // template <typename InnerPathOptimizer_t>
+ // typename GraphOptimizer<InnerPathOptimizer_t>::Ptr_t
+ // GraphOptimizer<InnerPathOptimizer_t>::create (const Problem& problem)
+ // {
+ // return GraphOptimizer<InnerPathOptimizer_t>::Ptr_t (
+ // new GraphOptimizer<InnerPathOptimizer_t> (problem));
+ // }
+
+ template <typename InnerPathOptimizer_t> PathVectorPtr_t
+ GraphOptimizer<InnerPathOptimizer_t>::optimize (const PathVectorPtr_t& path)
+ {
+ PathVectorPtr_t opted = PathVector::create
+ (path->outputSize(), path->outputDerivativeSize()),
+ expanded = PathVector::create
+ (path->outputSize(), path->outputDerivativeSize()),
+ toConcat;
+ unpack (path, expanded);
+ ConstraintSetPtr_t c;
+ for (std::size_t i_s = 0; i_s < expanded->numberPaths ();) {
+ PathVectorPtr_t toOpt = PathVector::create (
+ path->outputSize(), path->outputDerivativeSize());
+ PathPtr_t current = expanded->pathAtRank (i_s);
+ toOpt->appendPath (current);
+ graph::EdgePtr_t edge;
+ c = HPP_DYNAMIC_PTR_CAST (ConstraintSet, current->constraints ());
+ if (c) edge = c->edge ();
+ std::size_t i_e = i_s + 1;
+ for (; i_e < expanded->numberPaths (); ++i_e) {
+ current = expanded->pathAtRank (i_e);
+ c = HPP_DYNAMIC_PTR_CAST (ConstraintSet, current->constraints ());
+ if (!c && edge) break;
+ if (c && edge != c->edge ()) break;
+ toOpt->appendPath (current);
+ }
+ toConcat = pathOptimizer_->optimize (toOpt);
+ i_s = i_e;
+ opted->concatenate (*toConcat);
+ }
+ return opted;
+ }
+
+ template <typename InnerPathOptimizer_t>
+ void GraphOptimizer<InnerPathOptimizer_t>::unpack
+ (PathVectorPtr_t in, PathVectorPtr_t out)
+ {
+ for (size_t i = 0; i != in->numberPaths (); i++) {
+ PathPtr_t current = in->pathAtRank (i);
+ PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST (PathVector, current);
+ if (pv) {
+ unpack (pv, out);
+ } else {
+ out->appendPath (current);
+ }
+ }
+ }
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_GRAPHOPTIMIZER_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 679752a..26021e7 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -30,6 +30,7 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED
device.cc
graph-path-validation.cc
graph-steering-method.cc
+ graph-optimizer.cc
graph/node.cc
graph/edge.cc
diff --git a/src/graph-optimizer.cc b/src/graph-optimizer.cc
new file mode 100644
index 0000000..8d24f7a
--- /dev/null
+++ b/src/graph-optimizer.cc
@@ -0,0 +1,22 @@
+// Copyright (c) 2015, Joseph Mirabel
+// 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-optimizer.hh>
+
+namespace hpp {
+ namespace manipulation {
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 56b9842..a054add 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -22,7 +22,7 @@
#include <hpp/model/gripper.hh>
-#include <hpp/core/roadmap.hh>
+#include <hpp/core/random-shortcut.hh>
#include <hpp/core/discretized-collision-checking.hh>
#include "hpp/manipulation/device.hh"
@@ -32,6 +32,7 @@
#include "hpp/manipulation/problem.hh"
#include "hpp/manipulation/roadmap.hh"
#include "hpp/manipulation/constraint-set.hh"
+#include "hpp/manipulation/graph-optimizer.hh"
#include "hpp/manipulation/graph-path-validation.hh"
namespace hpp {
@@ -47,6 +48,8 @@ namespace hpp {
addPathPlannerType ("M-RRT", ManipulationPlanner::create);
addPathValidationType ("Graph-discretized",
GraphPathValidation::create <core::DiscretizedCollisionChecking>);
+ addPathOptimizerType ("Graph-RandomShortcut",
+ GraphOptimizer <core::RandomShortcut>::create);
pathPlannerType ("M-RRT");
pathValidationType ("Graph-discretized", 0.05);
}
--
GitLab