diff --git a/CMakeLists.txt b/CMakeLists.txt
index 08e3371215c95b1f356445fbf850430b7e6084e9..8bfd99b700da45ff4bb9226b21bb9381863b31a3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -101,6 +101,7 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/path-optimization/small-steps.hh
include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh
+ include/hpp/manipulation/path-optimization/random-shortcut.hh
include/hpp/manipulation/path-optimization/spline-gradient-based.hh
include/hpp/manipulation/problem-target/state.hh
diff --git a/include/hpp/manipulation/path-optimization/random-shortcut.hh b/include/hpp/manipulation/path-optimization/random-shortcut.hh
new file mode 100644
index 0000000000000000000000000000000000000000..7b77789474fe99b403b5607657ba35cf1915823e
--- /dev/null
+++ b/include/hpp/manipulation/path-optimization/random-shortcut.hh
@@ -0,0 +1,61 @@
+// Copyright (c) 2018, 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/>.
+
+#ifndef HPP_MANIPULATION_PATH_OPTIMIZATION_RANDOM_SHORTCUT_HH
+# define HPP_MANIPULATION_PATH_OPTIMIZATION_RANDOM_SHORTCUT_HH
+
+#include <hpp/core/path-optimization/random-shortcut.hh>
+
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/config.hh>
+
+namespace hpp {
+ namespace manipulation {
+ /// \addtogroup path_optimization
+ /// \{
+ namespace pathOptimization {
+ HPP_PREDEF_CLASS (RandomShortcut);
+ typedef boost::shared_ptr<RandomShortcut> RandomShortcutPtr_t;
+
+ class HPP_MANIPULATION_DLLAPI RandomShortcut :
+ public core::pathOptimization::RandomShortcut
+ {
+ public:
+ /// Return shared pointer to new object.
+ static RandomShortcutPtr_t create (const core::Problem& problem)
+ {
+ return RandomShortcutPtr_t (new RandomShortcut (problem));
+ }
+
+ protected:
+ RandomShortcut (const core::Problem& problem)
+ : core::pathOptimization::RandomShortcut (problem)
+ {}
+
+ /// Sample times along currentOpt.
+ /// t1 and t2 will not be on a path whose transition was short.
+ virtual bool shootTimes (const core::PathVectorPtr_t& currentOpt,
+ const value_type& t0,
+ value_type& t1,
+ value_type& t2,
+ const value_type& t3);
+ }; // class RandomShortcut
+ /// \}
+ } // namespace pathOptimization
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_PATH_OPTIMIZATION_RANDOM_SHORTCUT_HH
diff --git a/include/hpp/manipulation/path-optimization/spline-gradient-based.hh b/include/hpp/manipulation/path-optimization/spline-gradient-based.hh
index 320076d9a30814b7ceeb64ca17669e17b54f70e4..dea847e5546d66f0dcb06dbd8f3b17885dd0cae1 100644
--- a/include/hpp/manipulation/path-optimization/spline-gradient-based.hh
+++ b/include/hpp/manipulation/path-optimization/spline-gradient-based.hh
@@ -82,6 +82,7 @@ namespace hpp {
const SplinePtr_t& spline, LinearConstraint& lc) const;
}; // SplineGradientBased
} // namespace pathOptimization
+ /// \}
} // namespace manipulation
} // namespace hpp
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e2d703b5bd025c0dea880114522cf496803360d1..aca6483826da17dbf7743f6cb1cbe5d143730c6a 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -46,6 +46,7 @@ SET(SOURCES
graph/dot.cc
+ path-optimization/random-shortcut.cc
path-optimization/spline-gradient-based.cc
path-optimization/enforce-transition-semantic.cc
diff --git a/src/path-optimization/random-shortcut.cc b/src/path-optimization/random-shortcut.cc
new file mode 100644
index 0000000000000000000000000000000000000000..6d11ff377b783467734f6da31e1c9f9afd78dde4
--- /dev/null
+++ b/src/path-optimization/random-shortcut.cc
@@ -0,0 +1,70 @@
+// Copyright (c) 2018, 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/path-optimization/random-shortcut.hh>
+
+#include <hpp/core/path.hh>
+#include <hpp/core/path-vector.hh>
+#include <hpp/manipulation/constraint-set.hh>
+#include <hpp/manipulation/graph/edge.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathOptimization {
+ using core::PathPtr_t;
+ using core::PathVector;
+ using core::PathVectorPtr_t;
+
+ bool isShort (const PathVectorPtr_t& path, const value_type& t)
+ {
+ value_type localT;
+ std::size_t i = path->rankAtParam (t, localT);
+ PathPtr_t p = path->pathAtRank (i);
+ PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST (PathVector, p);
+ if (pv) return isShort (pv, localT);
+ ConstraintSetPtr_t c = HPP_DYNAMIC_PTR_CAST(ConstraintSet, path->constraints());
+ if (c) return c->edge()->isShort();
+ return false;
+ }
+
+ bool RandomShortcut::shootTimes (const PathVectorPtr_t& currentOpt,
+ const value_type& t0,
+ value_type& t1,
+ value_type& t2,
+ const value_type& t3)
+ {
+ bool ok = false;
+ for (int i = 0; i < 5; ++i)
+ {
+ value_type u2 = (t3-t0) * rand ()/RAND_MAX;
+ value_type u1 = (t3-t0) * rand ()/RAND_MAX;
+ if (isShort (currentOpt, u1) || isShort (currentOpt, u2))
+ continue;
+ if (u1 < u2) {
+ t1 = t0 + u1;
+ t2 = t0 + u2;
+ } else {
+ t1 = t0 + u2;
+ t2 = t0 + u1;
+ }
+ ok = true;
+ break;
+ }
+ return ok;
+ }
+ } // namespace pathOptimization
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 171ac2b6aff4dbfbaf2393e836011d9f907a9f49..8f8d0955c67e7e70123936b7359b544023dee2fa 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -26,7 +26,7 @@
#include <hpp/constraints/convex-shape-contact.hh>
-#include <hpp/core/random-shortcut.hh>
+#include <hpp/core/path-optimization/random-shortcut.hh>
#include <hpp/core/path-optimization/partial-shortcut.hh>
#include <hpp/core/path-projector/progressive.hh>
#include <hpp/core/path-projector/dichotomy.hh>
@@ -53,6 +53,7 @@
#include "hpp/manipulation/graph-path-validation.hh"
#include "hpp/manipulation/graph-node-optimizer.hh"
#include "hpp/manipulation/path-optimization/spline-gradient-based.hh"
+#include "hpp/manipulation/path-optimization/random-shortcut.hh"
#include "hpp/manipulation/path-optimization/enforce-transition-semantic.hh"
#include "hpp/manipulation/problem-target/state.hh"
#include "hpp/manipulation/steering-method/cross-state-optimization.hh"
@@ -109,8 +110,10 @@ namespace hpp {
pathPlanners.add ("M-RRT", ManipulationPlanner::create);
pathPlanners.add ("SymbolicPlanner", SymbolicPlanner::create);
+ pathOptimizers.add ("RandomShortcut",
+ pathOptimization::RandomShortcut::create);
pathOptimizers.add ("Graph-RandomShortcut",
- GraphOptimizer::create <core::RandomShortcut>);
+ GraphOptimizer::create <core::pathOptimization::RandomShortcut>);
pathOptimizers.add ("PartialShortcut", core::pathOptimization::
PartialShortcut::createWithTraits <PartialShortcutTraits>);
pathOptimizers.add ("Graph-PartialShortcut",