Skip to content
Snippets Groups Projects
Commit 45c41f47 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

[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.
parent 36d3e355
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
+ 0
− 2
View file @ 45c41f47
......@@ -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
......
include/hpp/manipulation/path-planner/in-state-path.hh deleted 100644 → 0
+ 0
− 135
View file @ 36d3e355
// 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
include/hpp/manipulation/path-planner/states-path-finder.hh
+ 14
− 32
View file @ 45c41f47
......@@ -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_;
......
src/path-planner/in-state-path.cc deleted 100644 → 0
+ 0
− 232
View file @ 36d3e355
// 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
src/path-planner/states-path-finder.cc
+ 114
− 169
View file @ 45c41f47
......@@ -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)
......
src/problem-solver.cc
+ 0
− 2
View file @ 45c41f47
......@@ -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);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment