From 3a5d0876a196d66a52bdc1974ac97835c5016f5b Mon Sep 17 00:00:00 2001
From: Le Quang Anh <43576719+Toefinder@users.noreply.github.com>
Date: Thu, 3 Mar 2022 15:29:04 +0100
Subject: [PATCH] Initialize the propagated constraints with q init
For the parametrizable constraints, we should initialize them before using
the solver to check if the initial configuration satisfies all the
propagated constraints.
---
src/path-planner/states-path-finder.cc | 5 +++--
1 file changed, 3 insertions(+), 2 deletions(-)
diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc
index b7fcba45..d31d14ca 100644
--- a/src/path-planner/states-path-finder.cc
+++ b/src/path-planner/states-path-finder.cc
@@ -1059,10 +1059,11 @@ namespace hpp {
<< " and " << it->first.second << " should be satisfied at q init");
for (NumericalConstraints_t::iterator ctrIt (constraintList.begin());
ctrIt != constraintList.end(); ++ctrIt) {
- analyseSolver.add(*ctrIt);
+ analyseSolver.add((*ctrIt)->copy());
}
}
-
+ // initialize the right hand side with the initial config
+ analyseSolver.rightHandSideFromConfig(*q1_);
if (analyseSolver.isSatisfied(*q1_)) {
return true;
}
--
GitLab