From 16ae96b12e80d64e4971691ca6b45d88f0990262 Mon Sep 17 00:00:00 2001
From: Le Quang Anh <43576719+Toefinder@users.noreply.github.com>
Date: Wed, 16 Mar 2022 17:32:46 +0100
Subject: [PATCH] Check if solver already has stricter constraints
Before adding another constraint to a solver, check if it already has
the stricter version of the constraint. `constraint/hold` is stricter
than `constraint` and stricter than `constraint/complement`.
---
.../path-planner/states-path-finder.hh | 16 +++++++++++++-
src/path-planner/states-path-finder.cc | 21 +++++++++++++++++--
2 files changed, 34 insertions(+), 3 deletions(-)
diff --git a/include/hpp/manipulation/path-planner/states-path-finder.hh b/include/hpp/manipulation/path-planner/states-path-finder.hh
index df3e4bea..1073a277 100644
--- a/include/hpp/manipulation/path-planner/states-path-finder.hh
+++ b/include/hpp/manipulation/path-planner/states-path-finder.hh
@@ -176,7 +176,16 @@ namespace hpp {
graph::Edges_t getTransitionList2 (const GraphSearchData& data) const;
/// Step 3 of the algorithm
+
+ // check that the solver either contains exactly same constraint
+ // or a constraint with similar parametrizable form
+ // constraint/both and constraint/complement
bool contains (const Solver_t& solver, const ImplicitPtr_t& c) const;
+
+ // check that the solver either contains exactly same constraint
+ // or a stricter version of the constraint
+ // constraint/both stricter than constraint and stricter than constraint/complement
+ bool containsStricter (const Solver_t& solver, const ImplicitPtr_t& c) const;
bool checkConstantRightHandSide (size_type index);
bool buildOptimizationProblem (const graph::Edges_t& transitions);
bool buildOptimizationProblem2 (const graph::Edges_t& transitions);
@@ -214,9 +223,14 @@ namespace hpp {
std::map < std::string, std::size_t > index_;
/// associative map that stores pairs of constraints of the form
- /// (constraint, constraint/hold)
+ /// (constraint/complement, constraint/hold)
std::map <ImplicitPtr_t, ImplicitPtr_t> sameRightHandSide_;
+ /// associative map that stores pairs of constraints of either form
+ /// (constraint, constraint/hold)
+ /// or (constraint/complement, constraint/hold)
+ std::map <ImplicitPtr_t, ImplicitPtr_t> stricterConstraints_;
+
mutable OptimizationData* optData_;
/// Index of the sequence of states
std::size_t idxSol_ = 0;
diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc
index 39fb070d..9a90d241 100644
--- a/src/path-planner/states-path-finder.cc
+++ b/src/path-planner/states-path-finder.cc
@@ -104,7 +104,7 @@ namespace hpp {
goalDefinedByConstraints_(false),
q1_(0x0), q2_(0x0), configList_(), idxConfigList_(0),
nTryConfigList_(0), solved_(false), interrupt_(false),
- weak_()
+ weak_(), stricterConstraints_()
{
gatherGraphConstraints ();
inStateProblem_ = core::Problem::create(problem_->robot());
@@ -298,6 +298,12 @@ namespace hpp {
}
}
}
+ // both is the intersection of both constraint and constraint/complement
+ for (ConstraintsAndComplements_t::const_iterator it(cac.begin ());
+ it != cac.end (); ++it) {
+ stricterConstraints_ [it->constraint] = it->both;
+ stricterConstraints_ [it->complement] = it->both;
+ }
}
bool StatesPathFinder::findTransitions (GraphSearchData& d) const
@@ -676,6 +682,17 @@ namespace hpp {
return false;
}
+ bool StatesPathFinder::containsStricter
+ (const Solver_t& solver, const ImplicitPtr_t& c) const
+ {
+ if (solver.contains (c)) return true;
+ std::map <ImplicitPtr_t, ImplicitPtr_t>::const_iterator it
+ (stricterConstraints_.find (c));
+ if (it != stricterConstraints_.end() && solver.contains (it->second))
+ return true;
+ return false;
+ }
+
bool StatesPathFinder::buildOptimizationProblem
(const graph::Edges_t& transitions)
{
@@ -803,7 +820,7 @@ namespace hpp {
}
// Add in the constraints for the goal
for (auto goalConstraint: goalConstraints_) {
- if (!contains(d.solvers [d.N-1], goalConstraint)) {
+ if (!containsStricter(d.solvers [d.N-1], goalConstraint)) {
d.solvers [d.N-1].add(goalConstraint);
hppDout(info, "Adding goal constraint " << goalConstraint->function().name()
<< " to solver for waypoint" << d.N);
--
GitLab