From 45c41f47e631430ee292b88cdcbb7886b59db624 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Tue, 21 Sep 2021 14:39:14 +0000
Subject: [PATCH] [StatesPathFinder] Refactor
- Use parent implementation for method solve,
- reimplement tryToConnectInitAndGoals,
- remove InStatePath class and call core::DiffusingPlanner instead.
- optimize paths on sub-manifolds with random shortcut and insert path
in roadmap.
---
CMakeLists.txt | 2 -
.../path-planner/in-state-path.hh | 135 ---------
.../path-planner/states-path-finder.hh | 46 +--
src/path-planner/in-state-path.cc | 232 --------------
src/path-planner/states-path-finder.cc | 283 +++++++-----------
src/problem-solver.cc | 2 -
6 files changed, 128 insertions(+), 572 deletions(-)
delete mode 100644 include/hpp/manipulation/path-planner/in-state-path.hh
delete mode 100644 src/path-planner/in-state-path.cc
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0272208..3519603 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -82,7 +82,6 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/manipulation/path-planner/end-effector-trajectory.hh
include/hpp/manipulation/path-planner/states-path-finder.hh
- include/hpp/manipulation/path-planner/in-state-path.hh
include/hpp/manipulation/problem-target/state.hh
@@ -126,7 +125,6 @@ SET(${PROJECT_NAME}_SOURCES
src/path-planner/end-effector-trajectory.cc
src/path-planner/states-path-finder.cc
- src/path-planner/in-state-path.cc
src/problem-target/state.cc
diff --git a/include/hpp/manipulation/path-planner/in-state-path.hh b/include/hpp/manipulation/path-planner/in-state-path.hh
deleted file mode 100644
index 4395ed6..0000000
--- a/include/hpp/manipulation/path-planner/in-state-path.hh
+++ /dev/null
@@ -1,135 +0,0 @@
-// Copyright (c) 2021, Joseph Mirabel
-// Authors: Joseph Mirabel (joseph.mirabel@laas.fr),
-// Florent Lamiraux (florent.lamiraux@laas.fr)
-// Alexandre Thiault (athiault@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_PATH_PLANNER_IN_STATE_PATH_HH
-# define HPP_MANIPULATION_PATH_PLANNER_IN_STATE_PATH_HH
-
-# include <hpp/core/path-planner.hh>
-# include <hpp/core/config-projector.hh>
-# include <hpp/core/collision-validation.hh>
-# include <hpp/core/joint-bound-validation.hh>
-
-# include <hpp/manipulation/config.hh>
-# include <hpp/manipulation/fwd.hh>
-# include <hpp/manipulation/problem.hh>
-# include <hpp/manipulation/steering-method/graph.hh>
-
-namespace hpp {
- namespace manipulation {
- namespace pathPlanner {
- /// \addtogroup path_planner
- /// \{
-
- ///
- /// Path planner designed to build a path between two configurations
- /// on the same leaf of a given state graph edge
- class HPP_MANIPULATION_DLLAPI InStatePath : public core::PathPlanner
- {
- public:
- virtual ~InStatePath()
- {}
-
- static InStatePathPtr_t create (
- const core::ProblemConstPtr_t& problem);
-
- static InStatePathPtr_t createWithRoadmap (
- const core::ProblemConstPtr_t& problem,
- const core::RoadmapPtr_t& roadmap);
-
- InStatePathPtr_t copy () const;
-
- void maxIterPlanner(const unsigned long& maxiter);
- void timeOutPlanner(const double& timeout);
- void resetRoadmap(const bool& resetroadmap);
- void plannerType(const std::string& plannertype);
- void addOptimizerType(const std::string& opttype);
- void resetOptimizerTypes();
-
- /// Set the edge along which q_init and q_goal will be linked.
- /// Use setEdge before setInit and setGoal.
- void setEdge (const graph::EdgePtr_t& edge);
- void setInit (const ConfigurationPtr_t& q);
- void setGoal (const ConfigurationPtr_t& q);
-
- virtual void oneStep()
- {}
- virtual core::PathVectorPtr_t solve();
-
- const core::RoadmapPtr_t& leafRoadmap() const;
- void mergeLeafRoadmapWith(const core::RoadmapPtr_t& roadmap) const;
-
- protected:
- InStatePath (const core::ProblemConstPtr_t& problem,
- const core::RoadmapPtr_t& roadmap) :
- PathPlanner(problem),
- problem_ (HPP_STATIC_PTR_CAST(const manipulation::Problem, problem)),
- leafRoadmap_(roadmap),
- constraints_(), weak_()
- {
- const core::DevicePtr_t& robot = problem_->robot();
- leafProblem_ = core::Problem::create(robot);
- leafProblem_->setParameter
- ("kPRM*/numberOfNodes", core::Parameter((size_type) 2000));
- leafProblem_->clearConfigValidations();
- leafProblem_->addConfigValidation(core::CollisionValidation::create(robot));
- leafProblem_->addConfigValidation(core::JointBoundValidation::create(robot));
- for (const core::CollisionObjectPtr_t & obs: problem_->collisionObstacles()) {
- leafProblem_->addObstacle(obs);
- }
- leafProblem_->pathProjector(problem->pathProjector());
- }
-
- InStatePath (const InStatePath& other) :
- PathPlanner(other.problem_),
- problem_(other.problem_), leafProblem_(other.leafProblem_),
- leafRoadmap_ (other.leafRoadmap_),
- constraints_(other.constraints_),
- weak_ ()
- {}
-
- void init (InStatePathWkPtr_t weak)
- {
- weak_ = weak;
- }
-
- private:
-
- // a pointer to the problem used to create the InStatePath instance
- ProblemConstPtr_t problem_;
- // a new problem created for this InStatePath instance
- core::ProblemPtr_t leafProblem_;
- core::RoadmapPtr_t leafRoadmap_;
- ConstraintSetPtr_t constraints_;
-
- double timeOutPathPlanning_ = 0;
- unsigned long maxIterPathPlanning_ = 0;
- bool resetRoadmap_ = true;
- std::string plannerType_ = "BiRRT*";
- std::vector<std::string> optimizerTypes_;
-
- /// Weak pointer to itself
- InStatePathWkPtr_t weak_;
-
- }; // class InStatePath
- /// \}
-
- } // namespace pathPlanner
- } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_STEERING_METHOD_CROSS_STATE_OPTIMIZATION_HH
diff --git a/include/hpp/manipulation/path-planner/states-path-finder.hh b/include/hpp/manipulation/path-planner/states-path-finder.hh
index 288414d..9fd0db9 100644
--- a/include/hpp/manipulation/path-planner/states-path-finder.hh
+++ b/include/hpp/manipulation/path-planner/states-path-finder.hh
@@ -114,11 +114,6 @@ namespace hpp {
StatesPathFinderPtr_t copy () const;
- core::ProblemConstPtr_t problem() const
- {
- return problem_;
- }
-
/// create a vector of configurations between two configurations
/// \return a Configurations_t from q1 to q2 if found. An empty
/// vector if a path could not be built.
@@ -139,34 +134,20 @@ namespace hpp {
int solveStep(std::size_t wp);
Configuration_t configSolved (std::size_t wp) const;
- /// Step 7 of the algorithm
- core::PathVectorPtr_t pathFromConfigList (std::size_t i) const;
-
/// deletes from memory the latest working states list, which is used to
/// resume finding solutions from that list in case of failure at a
/// later step.
void reset();
- core::PathVectorPtr_t buildPath (ConfigurationIn_t q1, ConfigurationIn_t q2);
virtual void startSolve();
virtual void oneStep();
- virtual core::PathVectorPtr_t solve ();
+ /// Do nothing
+ virtual void tryConnectInitAndGoals ();
protected:
StatesPathFinder (const core::ProblemConstPtr_t& problem,
- const core::RoadmapPtr_t&) :
- PathPlanner(problem),
- problem_ (HPP_STATIC_PTR_CAST(const manipulation::Problem, problem)),
- sameRightHandSide_ (), weak_ ()
- {
- gatherGraphConstraints ();
- }
-
- StatesPathFinder (const StatesPathFinder& other) :
- PathPlanner(other.problem_),
- problem_ (other.problem_), constraints_ (), index_ (other.index_),
- sameRightHandSide_ (other.sameRightHandSide_), weak_ ()
- {}
+ const core::RoadmapPtr_t&);
+ StatesPathFinder (const StatesPathFinder& other);
void init (StatesPathFinderWkPtr_t weak)
{
@@ -201,7 +182,7 @@ namespace hpp {
bool solveOptimizationProblem ();
/// Step 6 of the algorithm
- core::Configurations_t buildConfigList () const;
+ core::Configurations_t getConfigList () const;
/// Functions used in assert statements
bool checkWaypointRightHandSide (std::size_t ictr, std::size_t jslv) const;
@@ -214,30 +195,31 @@ namespace hpp {
/// A pointer to the manipulation problem
ProblemConstPtr_t problem_;
+ /// Path planning problem in each leaf.
+ core::ProblemPtr_t inStateProblem_;
/// Vector of parameterizable edge numerical constraints
NumericalConstraints_t constraints_;
- /// Map of indexes in constraints_
+ /// Map of indices in constraints_
std::map < std::string, std::size_t > index_;
/// associative map that stores pairs of constraints of the form
/// (constraint, constraint/hold)
std::map <ImplicitPtr_t, ImplicitPtr_t> sameRightHandSide_;
- mutable OptimizationData* optData_ = nullptr;
+ mutable OptimizationData* optData_;
+ /// Index of the sequence of states
std::size_t idxSol_ = 0;
graph::Edges_t lastBuiltTransitions_;
- bool skipColAnalysis_ = false;
+ bool skipColAnalysis_;
// Variables used across several calls to oneStep
ConfigurationPtr_t q1_, q2_;
core::Configurations_t configList_;
- std::size_t idxConfigList_ = 0;
- size_type nTryConfigList_ = 0;
- InStatePathPtr_t planner_;
- core::PathVectorPtr_t solution_;
- bool solved_ = false, interrupt_ = false;
+ std::size_t idxConfigList_;
+ size_type nTryConfigList_;
+ bool solved_, interrupt_;
/// Weak pointer to itself
StatesPathFinderWkPtr_t weak_;
diff --git a/src/path-planner/in-state-path.cc b/src/path-planner/in-state-path.cc
deleted file mode 100644
index 69c065c..0000000
--- a/src/path-planner/in-state-path.cc
+++ /dev/null
@@ -1,232 +0,0 @@
-// Copyright (c) 2021, Joseph Mirabel
-// Authors: Joseph Mirabel (joseph.mirabel@laas.fr),
-// Florent Lamiraux (florent.lamiraux@laas.fr)
-// Alexandre Thiault (athiault@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-planner/in-state-path.hh>
-
-#include <hpp/util/exception-factory.hh>
-
-#include <hpp/pinocchio/configuration.hh>
-
-#include <hpp/core/path-vector.hh>
-#include <hpp/core/roadmap.hh>
-#include <hpp/core/edge.hh>
-
-#include <hpp/core/bi-rrt-planner.hh>
-#include <hpp/core/path-planner/k-prm-star.hh>
-#include <hpp/core/diffusing-planner.hh>
-
-#include <hpp/core/path-optimizer.hh>
-#include <hpp/core/path-optimization/random-shortcut.hh>
-#include <hpp/core/path-optimization/simple-shortcut.hh>
-#include <hpp/core/path-optimization/partial-shortcut.hh>
-#include <hpp/core/path-optimization/simple-time-parameterization.hh>
-#include <hpp/manipulation/path-optimization/random-shortcut.hh>
-
-#include <hpp/manipulation/graph/edge.hh>
-#include <hpp/manipulation/roadmap.hh>
-
-
-namespace hpp {
- namespace manipulation {
- namespace pathPlanner {
-
- InStatePathPtr_t InStatePath::create (
- const core::ProblemConstPtr_t& problem)
- {
- InStatePath* ptr;
- RoadmapPtr_t r = Roadmap::create(problem->distance(), problem->robot());
- try {
- ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
- ptr = new InStatePath (p, r);
- } catch (std::exception&) {
- throw std::invalid_argument
- ("The problem must be of type hpp::manipulation::Problem.");
- }
- InStatePathPtr_t shPtr (ptr);
- ptr->init (shPtr);
- return shPtr;
- }
-
- InStatePathPtr_t InStatePath::createWithRoadmap (
- const core::ProblemConstPtr_t& problem,
- const core::RoadmapPtr_t& roadmap)
- {
- InStatePath* ptr;
- core::RoadmapPtr_t r2 = roadmap;
- RoadmapPtr_t r;
- try {
- ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
- r = HPP_DYNAMIC_PTR_CAST (Roadmap, r2);
- ptr = new InStatePath (p, r);
- } catch (std::exception&) {
- if (!r)
- throw std::invalid_argument
- ("The roadmap must be of type hpp::manipulation::Roadmap.");
- else
- throw std::invalid_argument
- ("The problem must be of type hpp::manipulation::Problem.");
- }
- InStatePathPtr_t shPtr (ptr);
- ptr->init (shPtr);
- return shPtr;
- }
-
- InStatePathPtr_t InStatePath::copy () const
- {
- InStatePath *ptr = new InStatePath(*this);
- InStatePathPtr_t shPtr(ptr);
- ptr->init(shPtr);
- return shPtr;
- }
-
- void InStatePath::maxIterPlanner(const unsigned long& maxiter)
- {
- maxIterPathPlanning_ = maxiter;
- }
-
- void InStatePath::timeOutPlanner(const double& timeout)
- {
- timeOutPathPlanning_ = timeout;
- }
-
- void InStatePath::resetRoadmap(const bool& resetroadmap)
- {
- resetRoadmap_ = resetroadmap;
- }
-
- void InStatePath::plannerType(const std::string& plannertype)
- {
- plannerType_ = plannertype;
- }
-
- void InStatePath::addOptimizerType(const std::string& opttype)
- {
- optimizerTypes_.push_back(opttype);
- }
-
- void InStatePath::resetOptimizerTypes()
- {
- optimizerTypes_.clear();
- }
-
- void InStatePath::setEdge (const graph::EdgePtr_t& edge)
- {
- constraints_ = edge->pathConstraint();
- leafProblem_->pathValidation(edge->pathValidation());
- leafProblem_->constraints(constraints_);
- leafProblem_->steeringMethod(edge->steeringMethod());
- }
-
- void InStatePath::setInit (const ConfigurationPtr_t& qinit)
- {
- if (!constraints_)
- throw std::logic_error("Use setEdge before setInit and setGoal");
- constraints_->configProjector()->rightHandSideFromConfig(*qinit);
- leafProblem_->initConfig(qinit);
- leafProblem_->resetGoalConfigs();
- }
-
- void InStatePath::setGoal (const ConfigurationPtr_t& qgoal)
- {
- if (!constraints_)
- throw std::logic_error("Use setEdge before setInit and setGoal");
- ConfigurationPtr_t qgoalc (new Configuration_t (*qgoal));
- constraints_->apply(*qgoalc);
- assert((*qgoal-*qgoalc).isZero());
- leafProblem_->resetGoalConfigs();
- leafProblem_->addGoalConfig(qgoal);
- }
-
- core::PathVectorPtr_t InStatePath::solve()
- {
- if (!constraints_)
- throw std::logic_error("Use setEdge, setInit and setGoal before solve");
- if (resetRoadmap_ || !leafRoadmap_)
- leafRoadmap_ = core::Roadmap::create (leafProblem_->distance(), problem_->robot());
-
- core::PathPlannerPtr_t planner;
- // TODO: BiRRT* does not work properly:
- // - discontinuities due to an algorithmic mistake involving qProj_
- // - not using path projectors, it should
- if (plannerType_ == "kPRM*")
- planner = core::pathPlanner::kPrmStar::createWithRoadmap(leafProblem_, leafRoadmap_);
- else if (plannerType_ == "DiffusingPlanner")
- planner = core::DiffusingPlanner::createWithRoadmap(leafProblem_, leafRoadmap_);
- else if (plannerType_ == "BiRRT*")
- planner = core::BiRRTPlanner::createWithRoadmap(leafProblem_, leafRoadmap_);
- else {
- hppDout(warning, "Unknown planner type specified. Setting to default DiffusingPlanner");
- planner = core::DiffusingPlanner::createWithRoadmap(leafProblem_, leafRoadmap_);
- }
- if (maxIterPathPlanning_)
- planner->maxIterations(maxIterPathPlanning_);
- if (timeOutPathPlanning_)
- planner->timeOut(timeOutPathPlanning_);
- if (!maxIterPathPlanning_ && !timeOutPathPlanning_)
- planner->stopWhenProblemIsSolved(true);
- core::PathVectorPtr_t path = planner->solve();
-
- for (const std::string& optType: optimizerTypes_) {
- namespace manipOpt = pathOptimization;
- namespace coreOpt = core::pathOptimization;
- PathOptimizerPtr_t optimizer;
- if (optType == "RandomShortcut")
- optimizer = coreOpt::RandomShortcut::create(problem_);
- else if (optType == "SimpleShortcut")
- optimizer = coreOpt::SimpleShortcut::create(problem_);
- else if (optType == "PartialShortcut")
- optimizer = coreOpt::PartialShortcut::create(problem_);
- else if (optType == "SimpleTimeParameterization")
- optimizer = coreOpt::SimpleTimeParameterization::create(problem_);
- else if (optType == "Graph-RandomShortcut")
- optimizer = manipOpt::RandomShortcut::create(problem_);
- else
- continue;
- try {
- path = optimizer->optimize(path);
- } catch (const hpp::Exception& e) {
- hppDout(info, "could not optimize " << e.what());
- }
- }
- return path;
- }
-
- const core::RoadmapPtr_t& InStatePath::leafRoadmap() const
- {
- return leafRoadmap_;
- }
-
- void InStatePath::mergeLeafRoadmapWith(const core::RoadmapPtr_t& roadmap) const {
- std::map<core::NodePtr_t, core::NodePtr_t> cNode;
- for (const core::NodePtr_t& node: leafRoadmap_->nodes()) {
- cNode[node] = roadmap->addNode(node->configuration());
- }
- for (const core::EdgePtr_t& edge: leafRoadmap_->edges()) {
- if (edge->path()->length() == 0)
- assert (edge->from() == edge->to());
- else
- roadmap->addEdges(
- cNode[edge->from()], cNode[edge->to()], edge->path());
- }
- // TODO this is inefficient because the roadmap recomputes the connected
- // component at every step. A merge function should be added in roadmap.cc
- }
-
- } // namespace pathPlanner
- } // namespace manipulation
-} // namespace hpp
diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc
index 92cde65..1cb04b6 100644
--- a/src/path-planner/states-path-finder.cc
+++ b/src/path-planner/states-path-finder.cc
@@ -35,11 +35,17 @@
#include <hpp/constraints/solver/by-substitution.hh>
#include <hpp/constraints/explicit.hh>
+#include <hpp/core/diffusing-planner.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/core/path-planning-failed.hh>
+#include <hpp/core/path-projector/progressive.hh>
#include <hpp/core/configuration-shooter.hh>
#include <hpp/core/collision-validation.hh>
#include <hpp/core/collision-validation-report.hh>
+#include <hpp/core/problem-target.hh>
+#include <hpp/core/path-optimization/random-shortcut.hh>
+
+#include <hpp/manipulation/constraint-set.hh>
#include <hpp/manipulation/graph/edge.hh>
#include <hpp/manipulation/graph/state.hh>
@@ -63,10 +69,45 @@ namespace hpp {
using graph::LockedJoints_t;
using graph::segments_t;
+ static void displayRoadmap(const core::RoadmapPtr_t& roadmap)
+ {
+ unsigned i=0;
+ for (auto cc : roadmap->connectedComponents()){
+ hppDout(info, " CC " << i); ++i;
+ for (auto n : cc->nodes()) {
+ hppDout(info, pinocchio::displayConfig(*(n->configuration())));
+ }
+ }
+ }
+
+ StatesPathFinder::StatesPathFinder(const core::ProblemConstPtr_t& problem,
+ const core::RoadmapPtr_t& roadmap) :
+ PathPlanner(problem, roadmap),
+ problem_ (HPP_STATIC_PTR_CAST(const manipulation::Problem, problem)),
+ constraints_(), index_(), sameRightHandSide_(), optData_(0x0),
+ idxSol_(0), lastBuiltTransitions_(), skipColAnalysis_(false),
+ q1_(0x0), q2_(0x0), configList_(), idxConfigList_(0),
+ nTryConfigList_(0), solved_(false), interrupt_(false),
+ weak_()
+ {
+ gatherGraphConstraints ();
+ inStateProblem_ = core::Problem::create(problem_->robot());
+ core::PathProjectorPtr_t pathProjector
+ (core::pathProjector::Progressive::create
+ (inStateProblem_, 1e-2));
+ inStateProblem_->pathProjector(pathProjector);
+ }
+
+ StatesPathFinder::StatesPathFinder (const StatesPathFinder& other) :
+ PathPlanner(other.problem_),
+ problem_ (other.problem_), constraints_ (), index_ (other.index_),
+ sameRightHandSide_ (other.sameRightHandSide_), weak_ ()
+ {}
StatesPathFinderPtr_t StatesPathFinder::create (
const core::ProblemConstPtr_t& problem)
{
+ hppDout(info, "");
StatesPathFinder* ptr;
RoadmapPtr_t r = Roadmap::create(problem->distance(), problem->robot());
try {
@@ -86,20 +127,16 @@ namespace hpp {
const core::RoadmapPtr_t& roadmap)
{
StatesPathFinder* ptr;
- core::RoadmapPtr_t r2 = roadmap; // unused
- RoadmapPtr_t r;
- try {
- ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
- r = HPP_DYNAMIC_PTR_CAST (Roadmap, r2);
- ptr = new StatesPathFinder (p, r);
- } catch (std::exception&) {
- if (!r)
- throw std::invalid_argument
- ("The roadmap must be of type hpp::manipulation::Roadmap.");
- else
- throw std::invalid_argument
- ("The problem must be of type hpp::manipulation::Problem.");
- }
+ ProblemConstPtr_t p = HPP_DYNAMIC_PTR_CAST (const Problem, problem);
+ RoadmapPtr_t r = HPP_DYNAMIC_PTR_CAST (Roadmap, roadmap);
+ if (!r)
+ throw std::invalid_argument
+ ("The roadmap must be of type hpp::manipulation::Roadmap.");
+ if (!p)
+ throw std::invalid_argument
+ ("The problem must be of type hpp::manipulation::Problem.");
+
+ ptr = new StatesPathFinder (p, r);
StatesPathFinderPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
@@ -325,7 +362,7 @@ namespace hpp {
std::vector <bool> isTargetWaypoint;
// Waypoints lying in each intermediate state
matrix_t waypoint;
- Configuration_t q1, q2;
+ const Configuration_t q1, q2;
core::DevicePtr_t robot;
// Matrix specifying for each constraint and each waypoint how
// the right hand side is initialized in the solver.
@@ -466,7 +503,7 @@ namespace hpp {
std::ostringstream oss1; oss1.precision (2);
std::ostringstream oss2; oss2.precision (2);
oss1 << constraints_ [i]->function ().name () << " & ";
-
+
oss1 << "$\\left(\\begin{array}{c} ";
for (std::size_t k=0; k<mask.size (); ++k) {
oss1 << mask[k] << "\\\\ ";
@@ -622,7 +659,7 @@ namespace hpp {
}
++index;
} // for (NumericalConstraints_t::const_iterator it
- displayStatusMatrix (transitions);
+ // displayStatusMatrix (transitions);
// Fill solvers with target constraints of transition
for (std::size_t j = 0; j < d.N; ++j) {
d.solvers [j] = transitions [j]->
@@ -631,7 +668,7 @@ namespace hpp {
const Solver_t& otherSolver = transitions [j+1]->
pathConstraint ()->configProjector ()->solver ();
for (std::size_t i = 0; i < constraints_.size (); i++) {
- if (d.M_status(i, j-1) == OptimizationData::ABSENT &&
+ if (d.M_status(i, j-1) == OptimizationData::ABSENT &&
d.M_status(i, j) == OptimizationData::EQUAL_TO_GOAL &&
!contains(d.solvers[j], constraints_[i]) &&
otherSolver.contains(constraints_[i])) {
@@ -796,7 +833,7 @@ namespace hpp {
if (!ok) {
hppDout(info, "Analysis found a collision at WP " << wp);
return false;
- }
+ }
hppDout(info, "Analysis at WP " << wp << " passed after " << tries << " solve tries");
}
return true;
@@ -884,14 +921,14 @@ namespace hpp {
return 3;
}
return 0;
- }
+ }
return 1;
}
std::string StatesPathFinder::displayConfigsSolved() const {
const OptimizationData& d = *optData_;
std::ostringstream oss;
- oss << "configs = [" << std::endl;
+ oss << "configs = [" << std::endl;
oss << " " << pinocchio::displayConfig(d.q1) << ", # 0" << std::endl;
for (size_type j = 0; j < d.waypoint.cols(); j++)
oss << " " << pinocchio::displayConfig(d.waypoint.col(j)) << ", # " << j+1 << std::endl;
@@ -938,7 +975,7 @@ namespace hpp {
wp--; // backtrack: try a new solution for the latest waypoint
} while (wp>1 && d.isTargetWaypoint[wp-1]);
}
-
+
// Completely reset a solution when too many tries have failed
if (wp > 1 && nFails >= nFailsMax) {
for (std::size_t k = 2; k <= d.solvers.size(); k++)
@@ -982,12 +1019,13 @@ namespace hpp {
}
displayConfigsSolved();
- displayRhsMatrix ();
+ // displayRhsMatrix ();
return true;
}
- core::Configurations_t StatesPathFinder::buildConfigList () const
+ // Get list of configurations from solution of optimization problem
+ core::Configurations_t StatesPathFinder::getConfigList () const
{
OptimizationData& d = *optData_;
core::Configurations_t pv;
@@ -1002,6 +1040,9 @@ namespace hpp {
return pv;
}
+ // Loop over all the possible paths in the constraint graph between
+ // the states of the initial configuration and of the final configurations
+ // and compute waypoint configurations in each state.
core::Configurations_t StatesPathFinder::computeConfigList (
ConfigurationIn_t q1, ConfigurationIn_t q2)
{
@@ -1038,8 +1079,8 @@ namespace hpp {
if (buildOptimizationProblem (transitions)) {
lastBuiltTransitions_ = transitions;
if (skipColAnalysis_ || analyseOptimizationProblem (transitions)) {
- if (solveOptimizationProblem ()) {
- core::Configurations_t path = buildConfigList ();
+ if (solveOptimizationProblem ()) {
+ core::Configurations_t path = getConfigList ();
hppDout (warning, " Solution " << idxSol << ": solved configurations list");
return path;
} else {
@@ -1062,39 +1103,6 @@ namespace hpp {
return empty_path;
}
- core::PathVectorPtr_t StatesPathFinder::pathFromConfigList (std::size_t i) const
- {
- hppDout(info, "calling StatesPathFinder::pathFromConfigList for transition " << i);
- try {
- const Edges_t& transitions = lastBuiltTransitions_;
- InStatePathPtr_t planner = InStatePath::create(problem_);
- planner->addOptimizerType("Graph-RandomShortcut");
- planner->plannerType("DiffusingPlanner");
- planner->maxIterPlanner(problem_->getParameter
- ("StatesPathFinder/innerPlannerMaxIterations").intValue());
- planner->timeOutPlanner(problem_->getParameter
- ("StatesPathFinder/innerPlannerTimeOut").floatValue());
- planner->resetRoadmap(true);
- ConfigurationPtr_t q1 (new Configuration_t (configSolved(i)));
- ConfigurationPtr_t q2 (new Configuration_t (configSolved(i+1)));
- hppDout(info, 3);
- planner->setEdge(transitions[i]);
- hppDout(info, 4);
- planner->setInit(q1);
- planner->setGoal(q2);
- hppDout(info, "calling InStatePath::computePath for transition " << i);
- return planner->solve();
- } catch(const core::path_planning_failed&(e)) {
- std::ostringstream oss;
- oss << "Error " << e.what() << "\n";
- oss << "Solution \"latest\":\nFailed to build path at edge " << i << ": ";
- oss << lastBuiltTransitions_[i]->pathConstraint()->name();
- hppDout(warning, oss.str());
- core::PathVectorPtr_t empty;
- return empty;
- }
- }
-
void StatesPathFinder::reset() {
idxSol_ = 0;
if (optData_) {
@@ -1106,121 +1114,77 @@ namespace hpp {
nTryConfigList_ = 0;
}
- core::PathVectorPtr_t StatesPathFinder::buildPath (ConfigurationIn_t q1, ConfigurationIn_t q2)
- {
- reset();
- size_type nTry = 0;
- size_type nTryMax = problem_->getParameter
- ("StatesPathFinder/maxTriesBuildPath").intValue();
-
- InStatePathPtr_t planner = InStatePath::create(problem_);
- planner->addOptimizerType("Graph-RandomShortcut");
- planner->plannerType("DiffusingPlanner");
- planner->maxIterPlanner(problem_->getParameter
- ("StatesPathFinder/innerPlannerMaxIterations").intValue());
- planner->timeOutPlanner(problem_->getParameter
- ("StatesPathFinder/innerPlannerTimeOut").floatValue());
- planner->resetRoadmap(true);
-
- while (true) {
- skipColAnalysis_ = (nTryConfigList_ >= 1); // already passed, don't redo it
- core::Configurations_t configList = computeConfigList(q1, q2);
- if (configList.size() <= 1) {// max depth reached
- reset();
- continue;
- }
- const Edges_t& transitions = lastBuiltTransitions_;
- std::size_t i = 0;
- try {
- core::PathVectorPtr_t ans = core::PathVector::create (
- problem()->robot()->configSize(), problem()->robot()->numberDof());
- for (i = 0; i < configList.size()-1; i++) {
- ConfigurationPtr_t q1 (new Configuration_t (configSolved(i)));
- ConfigurationPtr_t q2 (new Configuration_t (configSolved(i+1)));
- planner->setEdge(transitions[i]);
- planner->setInit(q1);
- planner->setGoal(q2);
- hppDout(info, "calling InStatePath::solve for transition " << i);
- ans->concatenate(planner->solve());
- }
- hppDout(warning, "Solution " << idxSol_ << ": Success"
- << "\n-----------------------------------------------");
- return ans;
- } catch(const core::path_planning_failed&(e)) {
- std::ostringstream oss;
- oss << "Error " << e.what() << "\n";
- oss << "Solution " << idxSol_ << ": Failed to build path at edge " << i << ": ";
- oss << lastBuiltTransitions_[i]->name();
- hppDout(warning, oss.str());
-
- // Retry nTryMax times to build another solution for the same states list
- if (++nTry >= nTryMax) {
- nTry = 0;
- idxSol_++;
- }
- }
- }
- core::PathVectorPtr_t empty;
- return empty;
- }
-
void StatesPathFinder::startSolve ()
{
+ PathPlanner::startSolve();
assert(problem_);
q1_ = problem_->initConfig();
assert(q1_);
core::Configurations_t q2s = problem_->goalConfigs();
- assert(q2s.size());
+ if (q2s.size() != 1) {
+ std::ostringstream os;
+ os << "StatesPathFinder accept one and only one goal configuration, ";
+ os << q2s.size() << " provided.";
+ throw std::logic_error(os.str().c_str());
+ }
q2_ = q2s[0];
reset();
-
- planner_ = InStatePath::create(problem_);
- planner_->addOptimizerType("Graph-RandomShortcut");
- planner_->plannerType("DiffusingPlanner");
- planner_->maxIterPlanner(problem_->getParameter
- ("StatesPathFinder/innerPlannerMaxIterations").intValue());
- planner_->timeOutPlanner(problem_->getParameter
- ("StatesPathFinder/innerPlannerTimeOut").floatValue());
- planner_->resetRoadmap(true);
}
void StatesPathFinder::oneStep ()
{
if (idxConfigList_ == 0) {
- solution_ = core::PathVector::create (
- problem()->robot()->configSize(), problem()->robot()->numberDof());
skipColAnalysis_ = (nTryConfigList_ >= 1); // already passed, don't redo it
configList_ = computeConfigList(*q1_, *q2_);
- roadmap()->clear();
if (configList_.size() <= 1) { // max depth reached
reset();
- return;
+ throw core::path_planning_failed("Maximal depth reached.");
}
}
+ ConfigurationPtr_t q1, q2;
try {
const Edges_t& transitions = lastBuiltTransitions_;
- ConfigurationPtr_t q1 (new Configuration_t (configSolved(idxConfigList_)));
- ConfigurationPtr_t q2 (new Configuration_t (configSolved(idxConfigList_+1)));
- planner_->setEdge(transitions[idxConfigList_]);
- planner_->setInit(q1);
- planner_->setGoal(q2);
-
- hppDout(info, "calling InStatePath::solve for transition " << idxConfigList_);
-
- core::PathVectorPtr_t aux = planner_->solve();
- for (std::size_t r = 0; r < aux->numberPaths()-1; r++)
- assert(aux->pathAtRank(r)->end() == aux->pathAtRank(r+1)->initial());
- solution_->concatenate(aux);
- planner_->mergeLeafRoadmapWith(roadmap());
-
+ q1 = ConfigurationPtr_t(new Configuration_t(configSolved
+ (idxConfigList_)));
+ q2 = ConfigurationPtr_t(new Configuration_t(configSolved
+ (idxConfigList_+1)));
+ const graph::EdgePtr_t& edge(transitions[idxConfigList_]);
+ // Copy edge constraints
+ core::ConstraintSetPtr_t constraints(HPP_DYNAMIC_PTR_CAST(
+ core::ConstraintSet, edge->pathConstraint()->copy()));
+ // Initialize right hand side
+ constraints->configProjector()->rightHandSideFromConfig(*q1);
+ assert(constraints->isSatisfied(*q2));
+ inStateProblem_->constraints(constraints);
+ inStateProblem_->pathValidation(edge->pathValidation());
+ inStateProblem_->initConfig(q1);
+ inStateProblem_->resetGoalConfigs();
+ inStateProblem_->addGoalConfig(q2);
+
+ core::PathPlannerPtr_t inStatePlanner
+ (core::DiffusingPlanner::create(inStateProblem_));
+ core::PathOptimizerPtr_t inStateOptimizer
+ (core::pathOptimization::RandomShortcut::create(inStateProblem_));
+ inStatePlanner->maxIterations(problem_->getParameter
+ ("StatesPathFinder/innerPlannerMaxIterations").intValue());
+ inStatePlanner->timeOut(problem_->getParameter
+ ("StatesPathFinder/innerPlannerTimeOut").floatValue());
+ hppDout(info, "calling InStatePlanner_.solve for transition "
+ << idxConfigList_);
+
+ core::PathVectorPtr_t path = inStatePlanner->solve();
+ for (std::size_t r = 0; r < path->numberPaths()-1; r++)
+ assert(path->pathAtRank(r)->end() ==
+ path->pathAtRank(r+1)->initial());
+ roadmap()->merge(inStatePlanner->roadmap());
+ // core::PathVectorPtr_t opt = inStateOptimizer->optimize(path);
+ // roadmap()->insertPathVector(opt, true);
idxConfigList_++;
if (idxConfigList_ == configList_.size()-1) {
hppDout(warning, "Solution " << idxSol_ << ": Success"
<< "\n-----------------------------------------------");
- //solution_ = aux;
- solved_ = true;
}
} catch(const core::path_planning_failed&(e)) {
@@ -1241,28 +1205,9 @@ namespace hpp {
}
}
- core::PathVectorPtr_t StatesPathFinder::solve ()
- {
- namespace bpt = boost::posix_time;
-
- interrupt_ = false;
- solved_ = false;
- unsigned long int nIter (0);
- startSolve ();
- if (interrupt_) throw core::path_planning_failed("Interruption");
- while (!solved_) {
- // Execute one step
- hppStartBenchmark(ONE_STEP);
- oneStep ();
- hppStopBenchmark(ONE_STEP);
- hppDisplayBenchmark(ONE_STEP);
-
- ++nIter;
- //solved = problem()->target()->reached (roadmap());
- if (interrupt_) throw core::path_planning_failed("Interruption");
- }
- return solution_;
- }
+ void StatesPathFinder::tryConnectInitAndGoals()
+ {
+ }
std::vector<std::string> StatesPathFinder::constraintNamesFromSolverAtWaypoint
(std::size_t wp)
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index f923dc2..d2c1b4c 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -122,8 +122,6 @@ namespace hpp {
pathPlanners.add ("M-RRT", ManipulationPlanner::create);
pathPlanners.add ("EndEffectorTrajectory", pathPlanner::EndEffectorTrajectory::createWithRoadmap);
pathPlanners.add ("StatesPathFinder", pathPlanner::StatesPathFinder::createWithRoadmap);
- pathPlanners.add ("InStatePath", pathPlanner::InStatePath::createWithRoadmap);
-
pathValidations.add ("Graph-Discretized" , createDiscretizedCollisionGraphPathValidation);
pathValidations.add ("Graph-DiscretizedCollision" , createDiscretizedCollisionGraphPathValidation);
pathValidations.add ("Graph-DiscretizedJointBound" , createDiscretizedJointBoundGraphPathValidation);
--
GitLab