Unverified Commit 5ebc43f6 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #159 from florent-lamiraux/carlike

[SimpleShortcut] Implement new path optimizer.
parents 37c341a4 9e49605f
......@@ -96,6 +96,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/core/path-optimization/partial-shortcut.hh
include/hpp/core/path-optimization/quadratic-program.hh
include/hpp/core/path-optimization/random-shortcut.hh
include/hpp/core/path-optimization/simple-shortcut.hh
include/hpp/core/path-optimization/simple-time-parameterization.hh
include/hpp/core/path-optimization/spline-gradient-based.hh
include/hpp/core/path-optimization/spline-gradient-based-abstract.hh
......
......@@ -261,6 +261,8 @@ namespace hpp {
namespace pathOptimization {
HPP_PREDEF_CLASS (RandomShortcut);
typedef boost::shared_ptr <RandomShortcut> RandomShortcutPtr_t;
HPP_PREDEF_CLASS (SimpleShortcut);
typedef boost::shared_ptr <SimpleShortcut> SimpleShortcutPtr_t;
HPP_PREDEF_CLASS (Cost);
typedef boost::shared_ptr <Cost> CostPtr_t;
HPP_PREDEF_CLASS (GradientBased);
......
//
// Copyright (c) 2019 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core 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-core 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-core If not, see
// <http://www.gnu.org/licenses/>.
#ifndef HPP_CORE_PATH_OPTIMIZATION_SIMPLE_SHORTCUT_HH
# define HPP_CORE_PATH_OPTIMIZATION_SIMPLE_SHORTCUT_HH
# include <hpp/core/path-optimizer.hh>
namespace hpp {
namespace core {
namespace pathOptimization {
/// \addtogroup path_optimization
/// \{
/// Simple shortcut
///
/// Find shortest path composed of direct paths between all pairs of
/// waypoints of input path.
///
/// To do so, the optimizer builds a roadmap the nodes of which are the
/// input path waypoints and the edges of which are the collision-free
/// output of the steering method between all pairs of nodes.
class HPP_CORE_DLLAPI SimpleShortcut : public PathOptimizer
{
public:
/// Return shared pointer to new object.
static SimpleShortcutPtr_t create (const Problem& problem);
/// Optimize path
virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
protected:
SimpleShortcut (const Problem& problem);
}; // class SimpleShortcut
/// \}
} // namespace pathOptimization
} // namespace core
} // namespace hpp
#endif // HPP_CORE_PATH_OPTIMIZATION_SIMPLE_SHORTCUT_HH
......@@ -61,6 +61,7 @@ path-validation/discretized-joint-bound.cc
path-optimization/spline-gradient-based-abstract.cc #
path-optimization/partial-shortcut.cc #
path-optimization/random-shortcut.cc
path-optimization/simple-shortcut.cc
path-optimization/simple-time-parameterization.cc#
path-planner.cc #
path-planner/k-prm-star.cc
......
//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux, Joseph Mirabel
//
// This file is part of hpp-core
// hpp-core 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-core 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-core If not, see
// <http://www.gnu.org/licenses/>.
#include <hpp/core/path-optimization/simple-shortcut.hh>
#include <hpp/util/assertion.hh>
#include <hpp/util/debug.hh>
#include <hpp/core/path-validation.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/problem-target.hh>
#include <hpp/core/path-projector.hh>
#include <hpp/core/roadmap.hh>
namespace hpp {
namespace core {
namespace pathOptimization {
SimpleShortcutPtr_t
SimpleShortcut::create (const Problem& problem)
{
SimpleShortcut* ptr = new SimpleShortcut (problem);
return SimpleShortcutPtr_t (ptr);
}
SimpleShortcut::SimpleShortcut (const Problem& problem) :
PathOptimizer (problem)
{
}
PathVectorPtr_t SimpleShortcut::optimize (const PathVectorPtr_t& path)
{
RoadmapPtr_t roadmap (Roadmap::create (problem ().distance (),
problem ().robot ()));
std::vector <NodePtr_t> nodes;
ConfigurationPtr_t qPtr (new Configuration_t (path->initial ()));
roadmap->initNode (qPtr);
NodePtr_t node (roadmap->initNode ());
nodes.push_back (node);
for (std::size_t i=0; i<path->numberPaths (); ++i) {
PathPtr_t p (path->pathAtRank (i));
qPtr = ConfigurationPtr_t (new Configuration_t (p->end ()));
node = roadmap->addNodeAndEdge (node, qPtr, p);
nodes.push_back (node);
}
roadmap->addGoalNode (node->configuration ());
const SteeringMethod& sm (*(problem ().steeringMethod ()));
PathValidationPtr_t pv (problem ().pathValidation ());
PathProjectorPtr_t proj (problem ().pathProjector ());
for (std::size_t i=0; i < nodes.size () - 1; ++i) {
for (std::size_t j=i+2; j < nodes.size (); ++j) {
PathPtr_t path (steer (*(nodes [i]->configuration ()),
*(nodes [j]->configuration ())));
PathValidationReportPtr_t report;
PathPtr_t unused;
if ((path) &&
(pv->validate (path, false, unused, report))) {
roadmap->addEdge (nodes [i], nodes [j], path);
}
}
}
PathVectorPtr_t result (problem ().target ()->computePath (roadmap));
assert (result);
return result;
}
} // namespace pathOptimization
} // namespace core
} // namespace hpp
......@@ -56,6 +56,7 @@
#include <hpp/core/path-projector/recursive-hermite.hh>
#include <hpp/core/path-optimization/partial-shortcut.hh>
#include <hpp/core/path-optimization/random-shortcut.hh>
#include <hpp/core/path-optimization/simple-shortcut.hh>
#include <hpp/core/path-optimization/simple-time-parameterization.hh>
#include <hpp/core/path-validation/discretized-collision-checking.hh>
#include <hpp/core/path-validation/discretized-joint-bound.hh>
......@@ -232,6 +233,7 @@ namespace hpp {
// Store path optimization methods in map.
pathOptimizers.add ("RandomShortcut", pathOptimization::RandomShortcut::create);
pathOptimizers.add ("SimpleShortcut", pathOptimization::SimpleShortcut::create);
pathOptimizers.add ("PartialShortcut", pathOptimization::PartialShortcut::create);
pathOptimizers.add ("SimpleTimeParameterization", pathOptimization::SimpleTimeParameterization::create);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment