Merge pull request #159 from florent-lamiraux/carlike

[SimpleShortcut] Implement new path optimizer.

CMakeLists.txt

@@ -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

include/hpp/core/fwd.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);

include/hpp/core/path-optimization/simple-shortcut.hh

+//
+// 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

src/CMakeLists.txt

@@ -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

src/path-optimization/simple-shortcut.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

src/problem-solver.cc

@@ -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);