diff --git a/include/hpp/manipulation/constraint-set.hh b/include/hpp/manipulation/constraint-set.hh index 96da5079fcbfaff960ff62f5da0c045cf1ce8610..4a080cb56e0fa52aa6ee12759667f6c8bfd19cab 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 cde31f153348029a841c0dd8ace02d2bf96847fd..f203ccd72db9a2d27d962bd5f747e0b6a6bf32e5 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 4b0e22077fda7f371044f38653abb932499f5a75..54bc6399df05907be44e9b3dbed8f0178e395e2c 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 4848798ccad30c229a4d7f7469fd05f4069077b4..87094b0b04c430912a6eae042466bb740593369b 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 cd63fe1f47f26d67a32673b58655ae29ef81f1b9..64f1bd2e4faf29b474189a6a8550b8ed41ad477a 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 49e99c38ec57d3accd0502dbf76d2f6854bfa10a..75564fbc81d146874dd29d72ca50949b2e1fafb8 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