From f6d3575b27e51640238c74624933130f2455e20f Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 1 Oct 2018 19:00:01 +0200
Subject: [PATCH] ProblemSolver stores ConstraintsAndComplements_t
---
include/hpp/manipulation/constraint-set.hh | 14 ++++++++++++++
include/hpp/manipulation/fwd.hh | 1 +
include/hpp/manipulation/graph/edge.hh | 1 -
include/hpp/manipulation/graph/graph.hh | 14 +-------------
include/hpp/manipulation/problem-solver.hh | 3 +++
src/problem-solver.cc | 14 ++++++++++----
6 files changed, 29 insertions(+), 18 deletions(-)
diff --git a/include/hpp/manipulation/constraint-set.hh b/include/hpp/manipulation/constraint-set.hh
index 96da5079..4a080cb5 100644
--- a/include/hpp/manipulation/constraint-set.hh
+++ b/include/hpp/manipulation/constraint-set.hh
@@ -63,6 +63,20 @@ namespace hpp {
graph::EdgePtr_t edge_;
ConstraintSetWkPtr_t weak_;
}; // class ConstraintSet
+
+ struct ConstraintAndComplement_t {
+ ImplicitPtr_t constraint;
+ ImplicitPtr_t complement;
+ ImplicitPtr_t both;
+ ConstraintAndComplement_t (const ImplicitPtr_t& constr,
+ const ImplicitPtr_t& comp,
+ const ImplicitPtr_t& b) :
+ constraint (constr), complement (comp), both (b)
+ {
+ }
+ };
+ typedef std::vector <ConstraintAndComplement_t>
+ ConstraintsAndComplements_t;
/// \}
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index cde31f15..f203ccd7 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -102,6 +102,7 @@ namespace hpp {
typedef std::vector <ObjectPtr_t> Objects_t;
typedef core::Constraint Constraint;
typedef core::ConstraintPtr_t ConstraintPtr_t;
+ typedef constraints::ImplicitPtr_t ImplicitPtr_t;
typedef constraints::LockedJoint LockedJoint;
typedef constraints::LockedJointPtr_t LockedJointPtr_t;
typedef hpp::core::ComparisonTypes_t ComparisonTypes_t;
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 4b0e2207..54bc6399 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -28,7 +28,6 @@
namespace hpp {
namespace manipulation {
- typedef constraints::ImplicitPtr_t ImplicitPtr_t;
namespace graph {
/// \addtogroup constraint_graph
/// \{
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index 4848798c..87094b0b 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -19,6 +19,7 @@
# include <boost/tuple/tuple.hpp>
# include "hpp/manipulation/config.hh"
+# include "hpp/manipulation/constraint-set.hh"
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/fwd.hh"
# include "hpp/manipulation/graph/graph-component.hh"
@@ -316,19 +317,6 @@ namespace hpp {
value_type errorThreshold_;
size_type maxIterations_;
- struct ConstraintAndComplement_t {
- ImplicitPtr_t constraint;
- ImplicitPtr_t complement;
- ImplicitPtr_t both;
- ConstraintAndComplement_t (const ImplicitPtr_t& constr,
- const ImplicitPtr_t& comp,
- const ImplicitPtr_t& b) :
- constraint (constr), complement (comp), both (b)
- {
- }
- };
- typedef std::vector <ConstraintAndComplement_t>
- ConstraintsAndComplements_t;
ConstraintsAndComplements_t constraintsAndComplements_;
friend class GraphComponent;
}; // Class Graph
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index cd63fe1f..64f1bd2e 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -22,6 +22,7 @@
# include <hpp/pinocchio/device.hh>
# include <hpp/core/problem-solver.hh>
# include <hpp/core/container.hh>
+# include "hpp/manipulation/constraint-set.hh"
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/deprecated.hh"
# include "hpp/manipulation/device.hh"
@@ -136,6 +137,8 @@ namespace hpp {
core::Container <graph::GraphPtr_t> graphs;
+ ConstraintsAndComplements_t constraintsAndComplements;
+
protected:
virtual void initializeProblem (ProblemPtr_t problem);
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 49e99c38..75564fbc 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -230,6 +230,11 @@ namespace hpp {
throw std::runtime_error ("The graph is not defined.");
initSteeringMethod();
constraintGraph_->initialize();
+
+ for (std::size_t i = 0; i < constraintsAndComplements.size(); ++i) {
+ const ConstraintAndComplement_t& c = constraintsAndComplements[i];
+ constraintGraph ()->registerConstraints (c.constraint, c.complement, c.both);
+ }
}
void ProblemSolver::createPlacementConstraint
@@ -336,9 +341,6 @@ namespace hpp {
(const std::string& name, const std::string& gripper,
const std::string& handle)
{
- if (!constraintGraph ()) {
- throw std::runtime_error ("The graph is not defined.");
- }
GripperPtr_t g = robot_->grippers.get (gripper, GripperPtr_t());
if (!g) throw std::runtime_error ("No gripper with name " + gripper + ".");
HandlePtr_t h = robot_->handles.get (handle, HandlePtr_t());
@@ -351,7 +353,11 @@ namespace hpp {
addNumericalConstraint ( name, constraint);
addNumericalConstraint (cname, complement);
addNumericalConstraint (bname, both);
- constraintGraph ()->registerConstraints (constraint, complement, both);
+
+ constraintsAndComplements.push_back (
+ ConstraintAndComplement_t (constraint, complement, both));
+ if (constraintGraph ())
+ constraintGraph ()->registerConstraints (constraint, complement, both);
}
void ProblemSolver::createPreGraspConstraint
--
GitLab