From 0ed2895f5729cfd6db84589e112ae127fa3cbb82 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 28 Nov 2014 10:27:23 +0100
Subject: [PATCH] Add class pathProjector::Progressive
---
CMakeLists.txt | 1 +
.../path-projector/progressive.hh | 40 ++++++
src/CMakeLists.txt | 1 +
src/path-projector/progressive.cc | 119 ++++++++++++++++++
4 files changed, 161 insertions(+)
create mode 100644 include/hpp/manipulation/path-projector/progressive.hh
create mode 100644 src/path-projector/progressive.cc
diff --git a/CMakeLists.txt b/CMakeLists.txt
index fd5c5080..134d7a57 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 00000000..2598443d
--- /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 23ca6b1c..8e101e59 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 00000000..8d34fb9a
--- /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
--
GitLab