diff --git a/CMakeLists.txt b/CMakeLists.txt
index 134d7a5767981f34fabc72833a450dea4fae5211..dc4b724f519fe0ab86b6862de0be077852292fd9 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 846babf04c1fc3f6f1ec242688721c1c369cee6d..35e728983024919ee18535fa18278557c55bbdbd 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/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;
diff --git a/include/hpp/manipulation/manipulation-planner.hh b/include/hpp/manipulation/manipulation-planner.hh
index ca3dafa8b1184a34ecfe4f9f7e4533d0e979ae3e..fcd3a054e3c1b6e03067cf6fcd6457efa4393d6d 100644
--- a/include/hpp/manipulation/manipulation-planner.hh
+++ b/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_;
};
/// \}
diff --git a/include/hpp/manipulation/path-projector.hh b/include/hpp/manipulation/path-projector.hh
deleted file mode 100644
index f1ba4fc26511f1311bdc4b213eddbb09edc6e580..0000000000000000000000000000000000000000
--- a/include/hpp/manipulation/path-projector.hh
+++ /dev/null
@@ -1,63 +0,0 @@
-// 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
deleted file mode 100644
index 6e9871b3e188feaa9d635193064ecbbd2689feef..0000000000000000000000000000000000000000
--- a/include/hpp/manipulation/path-projector/dichotomy.hh
+++ /dev/null
@@ -1,38 +0,0 @@
-// 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/include/hpp/manipulation/path-projector/progressive.hh b/include/hpp/manipulation/path-projector/progressive.hh
deleted file mode 100644
index 2598443df7a18fcbea5708ccc38e89b558d993a5..0000000000000000000000000000000000000000
--- a/include/hpp/manipulation/path-projector/progressive.hh
+++ /dev/null
@@ -1,40 +0,0 @@
-// 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 8e101e59b28190f03bc424c9f465487c8a765bc9..3073519ccdb3e474d0606410f935a304c20b8ec5 100644
--- a/src/CMakeLists.txt
+++ b/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
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index d047805a690fed516ff331d677c917f67d434149..68394bcb034c9c641e7dc0b124c6515f874167a3 100644
--- a/src/manipulation-planner.cc
+++ b/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)
diff --git a/src/path-projector.cc b/src/path-projector.cc
deleted file mode 100644
index dcd5a6faec6d4fcc3c424257dacced730238f410..0000000000000000000000000000000000000000
--- a/src/path-projector.cc
+++ /dev/null
@@ -1,70 +0,0 @@
-// 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
diff --git a/src/path-projector/dichotomy.cc b/src/path-projector/dichotomy.cc
deleted file mode 100644
index 4cb27194e49f05fa017656cfc0d80a44a750b3aa..0000000000000000000000000000000000000000
--- a/src/path-projector/dichotomy.cc
+++ /dev/null
@@ -1,105 +0,0 @@
-// 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
diff --git a/src/path-projector/progressive.cc b/src/path-projector/progressive.cc
deleted file mode 100644
index d046e3927864702ef0eb7cbeab70ea337f2cc1f5..0000000000000000000000000000000000000000
--- a/src/path-projector/progressive.cc
+++ /dev/null
@@ -1,120 +0,0 @@
-// 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
diff --git a/tests/path-projection.cc b/tests/path-projection.cc
index 53c88f8c026f57f989389aa15033f267d0d55824..1f0b00486f92012b402d0ef7d8e0869132235912 100644
--- a/tests/path-projection.cc
+++ b/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;