diff --git a/CMakeLists.txt b/CMakeLists.txt
index dc4b724f519fe0ab86b6862de0be077852292fd9..fd5c5080ef87d472d14eb7af1a955b29dbb5e08b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -57,6 +57,8 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/robot.hh
include/hpp/manipulation/roadmap.hh
include/hpp/manipulation/manipulation-planner.hh
+ include/hpp/manipulation/path-projector.hh
+ include/hpp/manipulation/path-projector/dichotomy.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/fwd.hh b/include/hpp/manipulation/fwd.hh
index 503966d203e49398acaacc427c68cee79d735d97..bcc0762b5419b8be9133ba9383f88f0780f8c62f 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -76,6 +76,11 @@ namespace hpp {
typedef boost::shared_ptr < GraphPathValidation > GraphPathValidationPtr_t;
HPP_PREDEF_CLASS (GraphSteeringMethod);
typedef boost::shared_ptr < GraphSteeringMethod > GraphSteeringMethodPtr_t;
+ HPP_PREDEF_CLASS (PathProjector);
+ typedef PathProjector* PathProjectorPtr_t;
+ namespace pathProjector {
+ HPP_PREDEF_CLASS (Dichotomy);
+ }
typedef std::vector <DevicePtr_t> Devices_t;
typedef std::vector <ObjectPtr_t> Objects_t;
diff --git a/include/hpp/manipulation/manipulation-planner.hh b/include/hpp/manipulation/manipulation-planner.hh
index 3045a16e7ce34d5c6159bfd8f8c46013c3a97ba0..b47ea3f562b9622b8fc0a6caba98fa65c750a50c 100644
--- a/include/hpp/manipulation/manipulation-planner.hh
+++ b/include/hpp/manipulation/manipulation-planner.hh
@@ -85,14 +85,15 @@ namespace hpp {
enum TypeOfFailure {
PROJECTION,
STEERING_METHOD,
- PATH_VALIDATION
+ PATH_VALIDATION,
+ PATH_PROJECTION
};
struct Reasons {
typedef ::hpp::statistics::SuccessBin::Reason Reason;
- Reason projFailed, smFailed, pvFailed;
+ Reason projFailed, smFailed, pvFailed, pprojFailed;
- Reasons (const Reason& proj, const Reason& sm, const Reason& pv) :
- projFailed (proj), smFailed (sm), pvFailed (pv) {}
+ Reasons (const Reason& proj, const Reason& sm, const Reason& pv, const Reason& pp) :
+ projFailed (proj), smFailed (sm), pvFailed (pv), pprojFailed (pp) {}
const Reason& get (TypeOfFailure t)
{
switch (t) {
@@ -102,6 +103,8 @@ namespace hpp {
return smFailed;
case PATH_VALIDATION:
return pvFailed;
+ case PATH_PROJECTION:
+ return pprojFailed;
}
return ::hpp::statistics::SuccessBin::REASON_UNKNOWN;
}
@@ -112,6 +115,8 @@ namespace hpp {
void addFailure (TypeOfFailure t, const graph::EdgePtr_t& edge);
+ PathProjectorPtr_t pathProjector_;
+
mutable Configuration_t qProj_;
};
} // namespace manipulation
diff --git a/include/hpp/manipulation/path-projector.hh b/include/hpp/manipulation/path-projector.hh
new file mode 100644
index 0000000000000000000000000000000000000000..f1ba4fc26511f1311bdc4b213eddbb09edc6e580
--- /dev/null
+++ b/include/hpp/manipulation/path-projector.hh
@@ -0,0 +1,63 @@
+// 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_HH
+# define HPP_MANIPULATION_PATHPROJECTOR_HH
+
+# include "hpp/manipulation/config.hh"
+# include "hpp/manipulation/fwd.hh"
+
+# include <hpp/core/path.hh>
+# include <hpp/core/straight-path.hh>
+
+namespace hpp {
+ namespace manipulation {
+ /// This class projects a path using constraints.
+ class HPP_MANIPULATION_DLLAPI PathProjector
+ {
+ public:
+ typedef hpp::core::Path Path;
+ typedef hpp::core::PathPtr_t PathPtr_t;
+ typedef hpp::core::StraightPath StraightPath;
+ typedef hpp::core::StraightPathPtr_t StraightPathPtr_t;
+ typedef hpp::core::PathVector PathVector;
+ typedef hpp::core::PathVectorPtr_t PathVectorPtr_t;
+
+ /// Destructor
+ virtual ~PathProjector ();
+
+ /// Apply the constraints to the path.
+ /// \param[in] the input path,
+ /// \param[out] the output path.
+ /// \return True if projection succeded
+ bool apply (const PathPtr_t path, PathPtr_t& projection) const;
+
+ protected:
+ /// Constructor
+ PathProjector (const core::DistancePtr_t distance);
+
+ /// Method to be reimplemented by inherited class.
+ virtual bool impl_apply (const StraightPathPtr_t path, PathPtr_t& projection) const = 0;
+
+ value_type d (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
+
+ private:
+ core::DistancePtr_t distance_;
+ };
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_PATHPROJECTOR_HH
diff --git a/include/hpp/manipulation/path-projector/dichotomy.hh b/include/hpp/manipulation/path-projector/dichotomy.hh
new file mode 100644
index 0000000000000000000000000000000000000000..6e9871b3e188feaa9d635193064ecbbd2689feef
--- /dev/null
+++ b/include/hpp/manipulation/path-projector/dichotomy.hh
@@ -0,0 +1,38 @@
+// 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_DICHOTOMY_HH
+# define HPP_MANIPULATION_PATHPROJECTOR_DICHOTOMY_HH
+
+# include "hpp/manipulation/path-projector.hh"
+namespace hpp {
+ namespace manipulation {
+ namespace pathProjector {
+ class HPP_MANIPULATION_DLLAPI Dichotomy : public PathProjector
+ {
+ public:
+ Dichotomy (const core::DistancePtr_t distance, value_type maxPathLength);
+
+ protected:
+ bool impl_apply (const StraightPathPtr_t path, PathPtr_t& projection) const;
+
+ private:
+ value_type maxPathLength_;
+ };
+ } // namespace pathProjector
+ } // namespace manipulation
+} // namespace hpp
+#endif // HPP_MANIPULATION_PATHPROJECTOR_DICHOTOMY_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 3073519ccdb3e474d0606410f935a304c20b8ec5..23ca6b1c02b78f87b84f6e337da14145c4a6d667 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -29,6 +29,9 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED
graph-path-validation.cc
graph-steering-method.cc
+ path-projector.cc
+ path-projector/dichotomy.cc
+
graph/node.cc
graph/edge.cc
graph/graph.cc
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 3fe36ae9914fd7e0f91ebfa16f0af81984488802..9f73089f1270f2a610b5516e936e0932b8b733a7 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -21,6 +21,8 @@
#include "hpp/manipulation/graph/statistics.hh"
#include "hpp/manipulation/robot.hh"
#include "hpp/manipulation/problem.hh"
+#include "hpp/manipulation/path-projector.hh"
+#include "hpp/manipulation/path-projector/dichotomy.hh"
#include "hpp/manipulation/manipulation-planner.hh"
#include "hpp/manipulation/graph/edge.hh"
@@ -116,6 +118,11 @@ namespace hpp {
addFailure (STEERING_METHOD, edge);
return false;
}
+ core::PathPtr_t projPath;
+ if (!pathProjector_->apply (path, projPath)) {
+ addFailure (PATH_PROJECTION, edge);
+ if (!projPath || projPath->length () == 0) return false;
+ }
GraphPathValidationPtr_t pathValidation (problem_.pathValidation ());
pathValidation->validate (path, false, validPath);
if (validPath->length () == 0)
@@ -136,7 +143,8 @@ namespace hpp {
std::string edgeStr = "(" + edge->name () + ")";
Reasons r (SuccessBin::createReason ("Projection for " + edgeStr),
SuccessBin::createReason ("SteeringMethod for " + edgeStr),
- SuccessBin::createReason ("PathValidation returned length 0 for " + edgeStr));
+ SuccessBin::createReason ("PathValidation returned length 0 for " + edgeStr),
+ SuccessBin::createReason ("Path could not be fully projected for " + edgeStr));
failureReasons_.insert (EdgeReasonPair (edge, r));
extendStatistics_.addFailure (r.get (t));
return;
@@ -187,7 +195,7 @@ namespace hpp {
const core::RoadmapPtr_t& roadmap) :
core::PathPlanner (problem, roadmap),
shooter_ (new core::BasicConfigurationShooter (problem.robot ())),
- problem_ (problem),
+ problem_ (problem), pathProjector_ (new pathProjector::Dichotomy (problem_.distance (), 0.1)),
qProj_ (problem.robot ()->configSize ())
{}
diff --git a/src/path-projector.cc b/src/path-projector.cc
new file mode 100644
index 0000000000000000000000000000000000000000..c10f9301c04f37f1f8b15118684f9e1423d4a660
--- /dev/null
+++ b/src/path-projector.cc
@@ -0,0 +1,61 @@
+// 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/util/pointer.hh>
+#include <hpp/core/path-vector.hh>
+#include <hpp/core/distance.hh>
+
+#include "hpp/manipulation/path-projector.hh"
+
+namespace hpp {
+ namespace manipulation {
+ PathProjector::PathProjector (const core::DistancePtr_t distance) :
+ distance_ (distance)
+ {}
+
+ PathProjector::~PathProjector ()
+ {}
+
+ value_type PathProjector::d (ConfigurationIn_t q1, ConfigurationIn_t q2) const
+ {
+ assert (distance_ != NULL);
+ return (*distance_) (q1, q2);
+ }
+
+ bool PathProjector::apply (const PathPtr_t path, PathPtr_t& proj) const
+ {
+ assert (path);
+ PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST (PathVector, path);
+ if (!pv) {
+ StraightPathPtr_t sp = HPP_DYNAMIC_PTR_CAST (StraightPath, path);
+ if (!sp) throw std::invalid_argument ("Unknow inherited class of Path");
+ return impl_apply (sp, proj);
+ }
+ else {
+ PathVectorPtr_t res = PathVector::create (pv->outputSize());
+ PathPtr_t part;
+ for (size_t i = 0; i < pv->numberPaths (); i++) {
+ if (!apply (pv->pathAtRank (i), part)) {
+ if (part && part->length () > 0) res->appendPath (part);
+ return false;
+ }
+ res->appendPath (part);
+ }
+ }
+ return true;
+ }
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/path-projector/dichotomy.cc b/src/path-projector/dichotomy.cc
new file mode 100644
index 0000000000000000000000000000000000000000..5643d6c07c3d48b3578c907347cb5e43d13b3691
--- /dev/null
+++ b/src/path-projector/dichotomy.cc
@@ -0,0 +1,103 @@
+// 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/dichotomy.hh"
+
+#include <hpp/core/path-vector.hh>
+
+#include <queue>
+#include <stack>
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathProjector {
+ Dichotomy::Dichotomy (const core::DistancePtr_t d, value_type maxPathLength) :
+ PathProjector (d), maxPathLength_ (maxPathLength)
+ {}
+
+ bool Dichotomy::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 = true;
+ std::queue <core::StraightPathPtr_t> paths;
+ std::stack <core::StraightPathPtr_t> pathToSplit;
+ Configuration_t qi (q1.size());
+ core::StraightPathPtr_t sPath = core::StraightPath::create (sp.device (), q1, q2, d (q1, q2));
+ pathToSplit.push (sPath);
+ while (!pathToSplit.empty ()) {
+ sPath = pathToSplit.top ();
+ const StraightPath& sPathRef = *sPath;
+ pathToSplit.pop ();
+ double l = sPathRef.length ();
+ if (l < maxPathLength_) {
+ paths.push (sPath);
+ continue;
+ }
+ timeRange = sPathRef.timeRange ();
+ const Configuration_t& qb = sPathRef (timeRange.first);
+ sPathRef (qi, timeRange.first + l / 2);
+ const Configuration_t& qe = sPathRef (timeRange.second);
+ if (!constraints->apply (qi)) {
+ pathIsFullyProjected = false;
+ break;
+ }
+ StraightPathPtr_t firstPart =
+ core::StraightPath::create (sp.device (), qb, qi, d (qb, qi));
+ StraightPathPtr_t secondPart =
+ core::StraightPath::create (sp.device (), qi, qe, d (qi, qe));
+ if (secondPart->length () == 0 || firstPart->length () == 0) {
+ pathIsFullyProjected = false;
+ break;
+ }
+ pathToSplit.push (secondPart);
+ pathToSplit.push (firstPart);
+ }
+ switch (paths.size ()) {
+ case 0:
+ return false;
+ break;
+ case 1:
+ projection = paths.front ();
+ projection->constraints (constraints);
+ break;
+ default:
+ core::PathVectorPtr_t pv = core::PathVector::create (sp.device ()->configSize ());
+ while (!paths.empty ()) {
+ paths.front ()->constraints (constraints);
+ pv->appendPath (paths.front ());
+ paths.pop ();
+ }
+ projection = pv;
+ break;
+ }
+ return pathIsFullyProjected;
+ }
+ } // namespace pathProjector
+ } // namespace manipulation
+} // namespace hpp