diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 3eed3da21452d2662ab0a7c0cf64e3fe8293c679..31b9f037ec8b0792eee7e6e24ade8a4de14eea1a 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -73,6 +73,10 @@ namespace hpp {
typedef boost::shared_ptr < GraphPathValidation > GraphPathValidationPtr_t;
HPP_PREDEF_CLASS (GraphSteeringMethod);
typedef boost::shared_ptr < GraphSteeringMethod > GraphSteeringMethodPtr_t;
+ typedef core::PathOptimizer PathOptimizer;
+ typedef core::PathOptimizerPtr_t PathOptimizerPtr_t;
+ HPP_PREDEF_CLASS (GraphOptimizer);
+ typedef boost::shared_ptr < GraphOptimizer > GraphOptimizerPtr_t;
typedef core::PathProjectorPtr_t PathProjectorPtr_t;
typedef std::vector <model::DevicePtr_t> Devices_t;
diff --git a/include/hpp/manipulation/graph-optimizer.hh b/include/hpp/manipulation/graph-optimizer.hh
index b6bcfabcab3b1900969360517573ab3e174a73ed..89f9d02cb9fb27110cf7a34ae1c905138a5d8524 100644
--- a/include/hpp/manipulation/graph-optimizer.hh
+++ b/include/hpp/manipulation/graph-optimizer.hh
@@ -30,7 +30,6 @@
namespace hpp {
namespace manipulation {
- using hpp::core::PathOptimizer;
using hpp::core::Path;
using hpp::core::PathPtr_t;
using hpp::core::PathVector;
@@ -44,35 +43,29 @@ namespace hpp {
/// 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);
+ template <typename InnerPathOptimizer_t>
+ static GraphOptimizerPtr_t create (const core::Problem& problem);
virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
/// Get the encapsulated optimizer
- const InnerPtr_t& innerOptimizer ()
+ const PathOptimizerPtr_t& innerOptimizer ()
{
return pathOptimizer_;
}
protected:
/// Constructor
- GraphOptimizer (const Problem& problem) :
- PathOptimizer (problem),
- pathOptimizer_ (InnerPathOptimizer_t::create (problem))
+ GraphOptimizer (const core::Problem& problem, PathOptimizerPtr_t inner) :
+ PathOptimizer (problem), pathOptimizer_ (inner)
{}
private:
/// The encapsulated PathOptimizer
- InnerPtr_t pathOptimizer_;
+ PathOptimizerPtr_t pathOptimizer_;
/// Append all paths of in to out
/// \param in a path vector, possibly containing other path vector
@@ -83,71 +76,14 @@ namespace hpp {
/// Member function definition
template <typename InnerPathOptimizer_t>
- typename GraphOptimizer<InnerPathOptimizer_t>::Ptr_t
- GraphOptimizer<InnerPathOptimizer_t>::create (const core::Problem& problem)
+ GraphOptimizerPtr_t GraphOptimizer::create
+ (const core::Problem& problem)
{
- return GraphOptimizer<InnerPathOptimizer_t>::Ptr_t (
- new GraphOptimizer<InnerPathOptimizer_t> (dynamic_cast <const Problem&> (problem))
+ return GraphOptimizerPtr_t (
+ new GraphOptimizer (problem, InnerPathOptimizer_t::create (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
diff --git a/src/graph-optimizer.cc b/src/graph-optimizer.cc
index 8d24f7a32255d677b74d59658cde4474e77d8309..cae5ddb641f3283a09c3dfcfc4f021161e66e8c9 100644
--- a/src/graph-optimizer.cc
+++ b/src/graph-optimizer.cc
@@ -18,5 +18,49 @@
namespace hpp {
namespace manipulation {
+ PathVectorPtr_t GraphOptimizer::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;
+ }
+
+ void GraphOptimizer::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
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index a054add3703ded53d119487718598c2d30972eef..f775460e4d2969a3de32ff34e95100cbfb4f99bb 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -49,7 +49,7 @@ namespace hpp {
addPathValidationType ("Graph-discretized",
GraphPathValidation::create <core::DiscretizedCollisionChecking>);
addPathOptimizerType ("Graph-RandomShortcut",
- GraphOptimizer <core::RandomShortcut>::create);
+ GraphOptimizer::create <core::RandomShortcut>);
pathPlannerType ("M-RRT");
pathValidationType ("Graph-discretized", 0.05);
}