Move PathProjector to hpp-core

CMakeLists.txt

@@ -57,9 +57,6 @@ 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/path-projector/progressive.hh
   include/hpp/manipulation/graph-path-validation.hh
   include/hpp/manipulation/graph-steering-method.hh
   include/hpp/manipulation/graph/node.hh

include/hpp/manipulation/fwd.hh

@@ -76,11 +76,7 @@ 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 core::PathProjectorPtr_t PathProjectorPtr_t;
 
     typedef std::vector <DevicePtr_t> Devices_t;
     typedef std::vector <ObjectPtr_t> Objects_t;

include/hpp/manipulation/manipulation-planner.hh

@@ -122,8 +122,6 @@ namespace hpp {
 
         void addFailure (TypeOfFailure t, const graph::EdgePtr_t& edge);
 
-        PathProjectorPtr_t pathProjector_;
-
         mutable Configuration_t qProj_;
     };
     /// \}

include/hpp/manipulation/path-projector.hh

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

include/hpp/manipulation/path-projector/dichotomy.hh

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

include/hpp/manipulation/path-projector/progressive.hh

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

src/CMakeLists.txt

@@ -29,10 +29,6 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED
   graph-path-validation.cc
   graph-steering-method.cc
 
-  path-projector.cc
-  path-projector/dichotomy.cc
-  path-projector/progressive.cc
-
   graph/node.cc
   graph/edge.cc
   graph/graph.cc

src/manipulation-planner.cc

@@ -20,13 +20,11 @@
 
 #include <hpp/core/path-validation.hh>
 #include <hpp/core/connected-component.hh>
+#include "hpp/core/path-projector.hh"
 
 #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/path-projector/progressive.hh"
 #include "hpp/manipulation/graph/edge.hh"
 
 namespace hpp {
@@ -105,6 +103,7 @@ namespace hpp {
         core::PathPtr_t& validPath)
     {
       graph::GraphPtr_t graph = problem_.constraintGraph ();
+      PathProjectorPtr_t pathProjector = problem_.pathProjector ();
       // Select next node in the constraint graph.
       const ConfigurationPtr_t q_near = n_near->configuration ();
       graph::NodePtr_t node = graph->getNode (*q_near);
@@ -123,15 +122,16 @@ namespace hpp {
         addFailure (STEERING_METHOD, edge);
         return false;
       }
-      //core::PathPtr_t projPath = path;
       core::PathPtr_t projPath;
-      if (!pathProjector_->apply (path, projPath)) {
-        if (!projPath || projPath->length () == 0) {
-          addFailure (PATH_PROJECTION_ZERO, edge);
-          return false;
+      if (pathProjector) {
+        if (!pathProjector->apply (path, projPath)) {
+          if (!projPath || projPath->length () == 0) {
+            addFailure (PATH_PROJECTION_ZERO, edge);
+            return false;
+          }
+          addFailure (PATH_PROJECTION_SHORTER, edge);
         }
-        addFailure (PATH_PROJECTION_SHORTER, edge);
-      }
+      } else projPath = path;
       GraphPathValidationPtr_t pathValidation (problem_.pathValidation ());
       pathValidation->validate (projPath, false, validPath);
       if (validPath->length () == 0)
@@ -168,6 +168,7 @@ namespace hpp {
     {
       const core::SteeringMethodPtr_t& sm (problem ().steeringMethod ());
       core::PathValidationPtr_t pathValidation (problem ().pathValidation ());
+      PathProjectorPtr_t pathProjector (problem().pathProjector ());
       core::PathPtr_t path, projPath, validPath;
       graph::GraphPtr_t graph = problem_.constraintGraph ();
       bool connectSucceed = false;
@@ -186,7 +187,9 @@ namespace hpp {
             assert (*q1 != *q2);
             path = (*sm) (*q1, *q2);
             if (!path) continue;
-            if (!pathProjector_->apply (path, projPath)) continue;
+            if (pathProjector) {
+              if (!pathProjector->apply (path, projPath)) continue;
+            } else projPath = path;
             if (pathValidation->validate (projPath, false, validPath)) {
               roadmap ()->addEdge (*itn1, *itn2, projPath);
               core::interval_t timeRange = projPath->timeRange ();
@@ -206,8 +209,7 @@ namespace hpp {
         const core::RoadmapPtr_t& roadmap) :
       core::PathPlanner (problem, roadmap),
       shooter_ (new core::BasicConfigurationShooter (problem.robot ())),
-      problem_ (problem), pathProjector_ (new pathProjector::Progressive (problem_.distance (), 0.2)),
-      qProj_ (problem.robot ()->configSize ())
+      problem_ (problem), qProj_ (problem.robot ()->configSize ())
     {}
 
     void ManipulationPlanner::init (const ManipulationPlannerWkPtr_t& weak)

src/path-projector.cc

-// 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.hh"
-
-#include <hpp/util/pointer.hh>
-#include <hpp/core/path-vector.hh>
-#include <hpp/core/distance.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);
-      bool success = false;
-      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");
-        success = impl_apply (sp, proj);
-      } else {
-        PathVectorPtr_t res = PathVector::create (pv->outputSize (), pv->outputDerivativeSize ());
-        PathPtr_t part;
-        success = true;
-        for (size_t i = 0; i < pv->numberPaths (); i++) {
-          if (!apply (pv->pathAtRank (i), part)) {
-            // We add the path only if part is not NULL and:
-            // - either its length is not zero,
-            // - or it's not the first one.
-            if (part && (part->length () > 0 || i == 0)) res->appendPath (part);
-            success = false;
-            break;
-          }
-          res->appendPath (part);
-        }
-        proj = res;
-      }
-      assert (proj);
-      assert (((*proj)(proj->timeRange ().first) - (*path)(path->timeRange ().first)).isZero());
-      assert (!success || ((*proj)(proj->timeRange ().second) - (*path)(path->timeRange ().second)).isZero());
-      return success;
-    }
-  } // namespace manipulation
-} // namespace hpp

src/path-projector/dichotomy.cc

-// 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 <hpp/core/config-projector.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 ConfigProjectorPtr_t& cp = constraints->configProjector ();
-        const StraightPath& sp = *path;
-        core::interval_t timeRange = sp.timeRange ();
-        const Configuration_t& q1 = sp(timeRange.first);
-        const Configuration_t& q2 = sp(timeRange.second);
-        if (cp) cp->rightHandSideFromConfig(q1);
-        if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
-          return false;
-        }
-        if (!cp) {
-          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.outputSize (), sp.outputDerivativeSize ());
-            while (!paths.empty ()) {
-              paths.front ()->constraints (constraints);
-              pv->appendPath (paths.front ());
-              paths.pop ();
-            }
-            projection = pv;
-            break;
-        }
-        return pathIsFullyProjected;
-      }
-    } // namespace pathProjector
-  } // namespace manipulation
-} // namespace hpp

src/path-projector/progressive.cc

-// 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 <hpp/core/config-projector.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 ConfigProjectorPtr_t& cp = constraints->configProjector ();
-        const StraightPath& sp = *path;
-        core::interval_t timeRange = sp.timeRange ();
-        const Configuration_t& q1 = sp(timeRange.first);
-        const Configuration_t& q2 = sp(timeRange.second);
-        if (cp) cp->rightHandSideFromConfig(q1);
-        if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
-          return false;
-        }
-        if (!cp) {
-          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_;
-          curLength = std::numeric_limits <value_type>::max();
-          bool stop = false;
-          /// Find the good length.
-          /// Here, it would be good to have an upper bound of the Hessian
-          /// of the constraint.
-          do {
-            if (curStep < 0.02) {
-              stop = true;
-              break;
-            }
-            toSplitRef (qi, curStep);
-            if (constraints->apply (qi)) {
-              curLength = d (qb, qi);
-            }
-            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.outputSize (), sp.outputDerivativeSize ());
-            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

tests/path-projection.cc

@@ -14,7 +14,7 @@
 // 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-projector/progressive.hh>
 
 #define ARM_LENGTH 1
 #define FOREARM_LENGTH 1
@@ -67,7 +67,8 @@ using hpp::core::ConfigProjectorPtr_t;
 
 using boost::assign::list_of;
 
-using hpp::manipulation::pathProjector::Progressive;
+using hpp::core::pathProjector::ProgressivePtr_t;
+using hpp::core::pathProjector::Progressive;
 
 static matrix3_t identity () { matrix3_t R; R.setIdentity (); return R;}
 
@@ -173,7 +174,8 @@ int main (int , char**) {
   cs->addConstraint (proj);
   WeighedDistancePtr_t dist = WeighedDistance::create (r, list_of (1)(1));
   const WeighedDistance& d = *dist;
-  Progressive pp (dist, 0.1);
+  ProgressivePtr_t pp_ptr = Progressive::create (dist, 0.1);
+  Progressive pp = *pp_ptr;
 
   Configuration_t qinit (2), qgoal (2);
   qinit[0] =  M_PI / 3; qinit[1] = -M_PI / 3;