From 13c5ef9d80f14e03ebe1e7f094adbd65891d50f9 Mon Sep 17 00:00:00 2001
From: Le Quang Anh <43576719+Toefinder@users.noreply.github.com>
Date: Fri, 4 Feb 2022 17:55:34 +0100
Subject: [PATCH] [WIP] [StatesPathFinder] Solve when goal is a set of
constraints
---
.../path-planner/states-path-finder.hh | 9 +-
src/path-planner/states-path-finder.cc | 109 ++++++++++++++++--
2 files changed, 106 insertions(+), 12 deletions(-)
diff --git a/include/hpp/manipulation/path-planner/states-path-finder.hh b/include/hpp/manipulation/path-planner/states-path-finder.hh
index e0fad8a9..9cde92f2 100644
--- a/include/hpp/manipulation/path-planner/states-path-finder.hh
+++ b/include/hpp/manipulation/path-planner/states-path-finder.hh
@@ -78,7 +78,7 @@ namespace hpp {
/// - method analyseOptimizationProblem loops over the waypoint solvers,
/// tests what happens when solving each waypoint after initializing
/// only the right hand sides that are equal to the initial or goal
- /// configuraion, and detects if a collision is certain to block any attemps
+ /// configuration, and detects if a collision is certain to block any attemps
/// to solve the problem in the solveOptimizationProblem step.
/// - method solveOptimizationProblem tries to solve for each waypoint after
/// initializing the right hand sides with the proper values, backtracking
@@ -119,6 +119,12 @@ namespace hpp {
/// vector if a path could not be built.
core::Configurations_t computeConfigList (ConfigurationIn_t q1,
ConfigurationIn_t q2);
+ /// create a vector of configurations from initial configuration
+ /// to a configuration satisfying the goal constraints, when goal
+ /// is defined as a set of constraints
+ /// \return a Configurations_t from q1 to q2 if found. An empty
+ /// vector if a path could not be built.
+ core::Configurations_t computeConfigList (ConfigurationIn_t q1);
// access functions for Python interface
std::vector<std::string> constraintNamesFromSolverAtWaypoint
@@ -216,6 +222,7 @@ namespace hpp {
// Constraints defining the goal
NumericalConstraints_t goalConstraints_;
+ bool goalDefinedByConstraints_;
// Variables used across several calls to oneStep
ConfigurationPtr_t q1_, q2_;
core::Configurations_t configList_;
diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc
index 62819f76..398e2a20 100644
--- a/src/path-planner/states-path-finder.cc
+++ b/src/path-planner/states-path-finder.cc
@@ -16,12 +16,14 @@
// received a copy of the GNU Lesser General Public License along with
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+#define HPP_DEBUG
#include <hpp/manipulation/path-planner/states-path-finder.hh>
#include <map>
#include <queue>
#include <vector>
+#include <hpp/util/debug.hh>
#include <hpp/util/exception-factory.hh>
#include <hpp/util/timer.hh>
@@ -679,7 +681,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]->
@@ -1066,6 +1068,7 @@ namespace hpp {
core::Configurations_t StatesPathFinder::computeConfigList (
ConfigurationIn_t q1, ConfigurationIn_t q2)
{
+ assert (!goalDefinedByConstraints_);
const graph::GraphPtr_t& graph(problem_->constraintGraph ());
GraphSearchData d;
d.s1 = graph->getState (q1);
@@ -1123,6 +1126,70 @@ namespace hpp {
return empty_path;
}
+ // Loop over all the possible paths in the constraint graph starting from
+ // the states of the initial configuration and with increasing lengths,
+ // apply goal constraints on the end node and try to project configurations
+ core::Configurations_t StatesPathFinder::computeConfigList (
+ ConfigurationIn_t q1)
+ {
+ assert (goalDefinedByConstraints_);
+ const graph::GraphPtr_t& graph(problem_->constraintGraph ());
+ GraphSearchData d;
+ d.s1 = graph->getState (q1);
+ d.s2 = graph->getState (q2); // TODO: fix this
+ d.maxDepth = problem_->getParameter
+ ("StatesPathFinder/maxDepth").intValue();
+
+ // Find
+ d.queue1.push (d.addInitState());
+ std::size_t idxSol = (d.s1 == d.s2 ? 1 : 0);
+ if (idxSol_ < idxSol) idxSol_ = idxSol;
+
+ bool maxDepthReached;
+ while (!(maxDepthReached = findTransitions (d))) { // mut
+ Edges_t transitions = getTransitionList (d, idxSol); // const, const
+ while (! transitions.empty()) {
+ if (idxSol >= idxSol_) {
+#ifdef HPP_DEBUG
+ std::ostringstream ss;
+ ss << " Trying solution " << idxSol << ": \n\t";
+ for (std::size_t j = 0; j < transitions.size(); ++j)
+ ss << transitions[j]->name() << ", \n\t";
+ hppDout (info, ss.str());
+#endif // HPP_DEBUG
+ if (optData_) {
+ delete optData_;
+ optData_ = nullptr;
+ }
+ optData_ = new OptimizationData (problem(), q1, q2, transitions);
+
+ if (buildOptimizationProblem (transitions)) {
+ lastBuiltTransitions_ = transitions;
+ if (skipColAnalysis_ || analyseOptimizationProblem (transitions)) {
+ if (solveOptimizationProblem ()) {
+ core::Configurations_t path = getConfigList ();
+ hppDout (warning, " Solution " << idxSol << ": solved configurations list");
+ return path;
+ } else {
+ hppDout (info, " Failed solution " << idxSol << " at step 5 (solve opt pb)");
+ }
+ } else {
+ hppDout (info, " Failed solution " << idxSol << " at step 4 (analyse opt pb)");
+ }
+ } else {
+ hppDout (info, " Failed solution " << idxSol << " at step 3 (build opt pb)");
+ }
+ } // if (idxSol >= idxSol_)
+ transitions = getTransitionList(d, ++idxSol);
+ if (idxSol_ < idxSol) idxSol_ = idxSol;
+ }
+ }
+ core::Configurations_t empty_path;
+ ConfigurationPtr_t q (new Configuration_t (q1));
+ empty_path.push_back(q);
+ return empty_path;
+ }
+
void StatesPathFinder::reset() {
idxSol_ = 0;
if (optData_) {
@@ -1146,6 +1213,7 @@ namespace hpp {
GoalConfigurationsPtr_t goalConfigs
(HPP_DYNAMIC_PTR_CAST(GoalConfigurations, target));
if (goalConfigs){
+ goalDefinedByConstraints_ = false;
core::Configurations_t q2s = goalConfigs->configurations();
if (q2s.size() != 1) {
std::ostringstream os;
@@ -1155,10 +1223,17 @@ namespace hpp {
throw std::logic_error(os.str().c_str());
}
q2_ = q2s[0];
- }
- TaskTargetPtr_t taskTarget(HPP_DYNAMIC_PTR_CAST(TaskTarget,target));
- if(taskTarget){
+ } else {
+ TaskTargetPtr_t taskTarget(HPP_DYNAMIC_PTR_CAST(TaskTarget,target));
+ if(!taskTarget){
+ std::ostringstream os;
+ os << "StatesPathFinder only accept goal defined as "
+ "either a configuration or a set of constraints.";
+ throw std::logic_error(os.str().c_str());
+ }
+ goalDefinedByConstraints_ = true;
goalConstraints_ = taskTarget->constraints();
+ hppDout(info, "goal defined as a set of constraints");
}
reset();
}
@@ -1167,7 +1242,14 @@ namespace hpp {
{
if (idxConfigList_ == 0) {
skipColAnalysis_ = (nTryConfigList_ >= 1); // already passed, don't redo it
- configList_ = computeConfigList(*q1_, *q2_);
+ // TODO: accommodate when goal is a set of constraints
+ assert(q1_);
+ if (!goalDefinedByConstraints_) {
+ assert(q2_);
+ configList_ = computeConfigList(*q1_, *q2_);
+ } else {
+ configList_ = computeConfigList(*q1_);
+ }
if (configList_.size() <= 1) { // max depth reached
reset();
throw core::path_planning_failed("Maximal depth reached.");
@@ -1187,12 +1269,17 @@ namespace hpp {
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);
+ if (!goalDefinedByConstraints_) {
+ assert(constraints->isSatisfied(*q2));
+ inStateProblem_->constraints(constraints);
+ inStateProblem_->pathValidation(edge->pathValidation());
+ inStateProblem_->initConfig(q1);
+ inStateProblem_->resetGoalConfigs();
+ inStateProblem_->addGoalConfig(q2);
+ } else {
+ // TODO: algorithm when the goal is a set of constraints
+ hppDout(warning, "Not implemented for goal as a set of constraints");
+ }
core::PathPlannerPtr_t inStatePlanner
(core::DiffusingPlanner::create(inStateProblem_));
--
GitLab