From 18ecc541f9a3373d93d3586570d83d22bc1b0100 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Fri, 30 Apr 2021 09:39:43 +0000
Subject: [PATCH] [graph::Graph] Fix registration of constraints and
complements
Do not clear constraintsAndComplements_ after graph initialization,
Add method clearConstraintsAndComplement(),
call this method in ProblemSolver::initConstraintGraph().
---
include/hpp/manipulation/graph/graph.hh | 4 ++++
src/graph/graph.cc | 6 +++++-
src/problem-solver.cc | 1 +
3 files changed, 10 insertions(+), 1 deletion(-)
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index 8bd7099..730b654 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -81,6 +81,10 @@ namespace hpp {
/// Select randomly outgoing edge of the given node.
EdgePtr_t chooseEdge(RoadmapNodePtr_t node) const;
+ /// Clear the vector of constraints and complements
+ /// \sa registerConstraints
+ void clearConstraintsAndComplement();
+
/// Register a triple of constraints to be inserted in nodes and edges
/// \param constraint a constraint (grasp of placement)
/// \param complement the complement constraint
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index e98c0a6..8500e9a 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -56,7 +56,6 @@ namespace hpp {
assert(components_.size() >= 1 && components_[0].lock() == wkPtr_.lock());
for (std::size_t i = 1; i < components_.size(); ++i)
components_[i].lock()->initialize();
- constraintsAndComplements_.clear ();
isInit_ = true;
}
@@ -153,6 +152,11 @@ namespace hpp {
return stateSelector_->chooseEdge (from);
}
+ void Graph::clearConstraintsAndComplement()
+ {
+ constraintsAndComplements_.clear();
+ }
+
void Graph::registerConstraints
(const ImplicitPtr_t& constraint,
const ImplicitPtr_t& complement,
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 5007cad..3f0ebbe 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -221,6 +221,7 @@ namespace hpp {
if (!constraintGraph_)
throw std::runtime_error ("The graph is not defined.");
initSteeringMethod();
+ constraintGraph_->clearConstraintsAndComplement();
for (std::size_t i = 0; i < constraintsAndComplements.size(); ++i) {
const ConstraintAndComplement_t& c = constraintsAndComplements[i];
constraintGraph ()->registerConstraints (c.constraint, c.complement, c.both);
--
GitLab