diff --git a/CMakeLists.txt b/CMakeLists.txt
index fd5c5080ef87d472d14eb7af1a955b29dbb5e08b..134d7a5767981f34fabc72833a450dea4fae5211 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -59,6 +59,7 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/manipulation-planner.hh
include/hpp/manipulation/path-projector.hh
include/hpp/manipulation/path-projector/dichotomy.hh
+ include/hpp/manipulation/path-projector/progressive.hh
include/hpp/manipulation/graph-path-validation.hh
include/hpp/manipulation/graph-steering-method.hh
include/hpp/manipulation/graph/node.hh
diff --git a/include/hpp/manipulation/path-projector/progressive.hh b/include/hpp/manipulation/path-projector/progressive.hh
new file mode 100644
index 0000000000000000000000000000000000000000..2598443df7a18fcbea5708ccc38e89b558d993a5
--- /dev/null
+++ b/include/hpp/manipulation/path-projector/progressive.hh
@@ -0,0 +1,40 @@
+// Copyright (c) 2014, 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_PATHPROJECTOR_PROGRESSIVE_HH
+# define HPP_MANIPULATION_PATHPROJECTOR_PROGRESSIVE_HH
+
+# include "hpp/manipulation/path-projector.hh"
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathProjector {
+ class HPP_MANIPULATION_DLLAPI Progressive : public PathProjector
+ {
+ public:
+ Progressive (const core::DistancePtr_t distance, value_type step);
+
+ protected:
+ bool impl_apply (const StraightPathPtr_t path, PathPtr_t& projection) const;
+
+ private:
+ value_type step_;
+ };
+ } // namespace pathProjector
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_PATHPROJECTOR_PROGRESSIVE_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 23ca6b1c02b78f87b84f6e337da14145c4a6d667..8e101e59b28190f03bc424c9f465487c8a765bc9 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -31,6 +31,7 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED
path-projector.cc
path-projector/dichotomy.cc
+ path-projector/progressive.cc
graph/node.cc
graph/edge.cc
diff --git a/src/path-projector/progressive.cc b/src/path-projector/progressive.cc
new file mode 100644
index 0000000000000000000000000000000000000000..8d34fb9a04aa63b16fec6ac75cd6af52f2252be7
--- /dev/null
+++ b/src/path-projector/progressive.cc
@@ -0,0 +1,119 @@
+// Copyright (c) 2014, 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/>.
+
+#include "hpp/manipulation/path-projector/progressive.hh"
+
+#include <hpp/core/path-vector.hh>
+
+#include <limits>
+#include <queue>
+#include <stack>
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathProjector {
+ Progressive::Progressive (const core::DistancePtr_t d, value_type step) :
+ PathProjector (d), step_ (step)
+ {}
+
+ bool Progressive::impl_apply (const StraightPathPtr_t path, PathPtr_t& projection) const
+ {
+ ConstraintSetPtr_t constraints = path->constraints ();
+ const StraightPath& sp = *path;
+ core::interval_t timeRange = sp.timeRange ();
+ const Configuration_t& q1 = sp(timeRange.first);
+ const Configuration_t& q2 = sp(timeRange.second);
+ constraints->offsetFromConfig(q1);
+ if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
+ return false;
+ }
+ if (!constraints->configProjector ()) {
+ projection = path;
+ return true;
+ }
+
+ bool pathIsFullyProjected = false;
+ std::queue <core::StraightPathPtr_t> paths;
+ StraightPathPtr_t toSplit =
+ core::StraightPath::create (sp.device (), q1, q2, d (q1, q2));
+ Configuration_t qi (q1.size());
+ value_type curStep, curLength;
+ while (true) {
+ const StraightPath& toSplitRef = *toSplit;
+ if (toSplitRef.length () < step_) {
+ paths.push (toSplit);
+ pathIsFullyProjected = true;
+ break;
+ }
+ timeRange = toSplitRef.timeRange ();
+ const Configuration_t& qb = toSplitRef (timeRange.first);
+ curStep = step_;
+ bool stop = false;
+ /// Find the good length.
+ /// Here, it would be good to have an upper bound of the Hessian
+ /// of the constraint.
+ do {
+ toSplitRef (qi, curStep);
+ if (!constraints->apply (qi)) {
+ stop = true;
+ break;
+ }
+ curLength = d (qb, qi);
+ if (curStep < 0.02) {
+ stop = true;
+ break;
+ }
+ curStep /= 2;
+ } while (curLength > step_);
+ if (stop) break;
+ StraightPathPtr_t part =
+ core::StraightPath::create (sp.device (), qb, qi, curLength);
+ paths.push (part);
+ toSplit =
+ core::StraightPath::create (sp.device (), qi, q2, d (qi, q2));
+ }
+ switch (paths.size ()) {
+ case 0:
+ timeRange = sp.timeRange();
+ projection = sp.extract (std::make_pair (timeRange.first, timeRange.first));
+ return false;
+ break;
+ case 1:
+ projection = paths.front ();
+ projection->constraints (constraints);
+ break;
+ default:
+ core::PathVectorPtr_t pv = core::PathVector::create (sp.device ()->configSize ());
+ qi = q1;
+ while (!paths.empty ()) {
+ assert ((qi - (*paths.front ())(paths.front ()->timeRange().first)).isZero ());
+ assert (constraints->isSatisfied (qi));
+ qi = (*paths.front ())(paths.front ()->timeRange().second);
+ assert (constraints->isSatisfied (qi));
+ paths.front ()->constraints (constraints);
+ pv->appendPath (paths.front ());
+ paths.pop ();
+ }
+ projection = pv;
+ break;
+ }
+ assert (((*projection)(projection->timeRange ().first) - (*path)(path->timeRange ().first)).isZero());
+ assert (!pathIsFullyProjected || ((*projection)(projection->timeRange ().second) - (*path)(path->timeRange ().second)).isZero());
+ return pathIsFullyProjected;
+ }
+ } // namespace pathProjector
+ } // namespace manipulation
+} // namespace hpp