diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh index dc9a9dba26d75abc128c0971a2b803e3a2c7ea38..163492f08f402080f9545856de258fd8672706cf 100644 --- a/include/hpp/manipulation/graph/edge.hh +++ b/include/hpp/manipulation/graph/edge.hh @@ -222,7 +222,8 @@ namespace hpp { LeafHistogramPtr_t histogram () const; - void insertConfigConstraint (const NumericalConstraintPtr_t& nm); + void insertConfigConstraint (const NumericalConstraintPtr_t& nm, + const SizeIntervals_t& passiveDofs = SizeIntervals_t ()); void insertConfigConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq) __attribute__ ((deprecated)); @@ -250,6 +251,7 @@ namespace hpp { /// Extra NumericalConstraints_t NumericalConstraints_t extraNumericalConstraints_; + IntervalsContainer_t extraPassiveDofs_; /// Extra LockedJoints_t LockedJoints_t extraLockedJoints_; diff --git a/include/hpp/manipulation/graph/fwd.hh b/include/hpp/manipulation/graph/fwd.hh index b6997fb14ff08bbfd71984e3885580046606db86..ff2f0529ebd3dd02f93d044ffefd6355ea54b7d9 100644 --- a/include/hpp/manipulation/graph/fwd.hh +++ b/include/hpp/manipulation/graph/fwd.hh @@ -55,6 +55,8 @@ namespace hpp { typedef hpp::core::Equality Equality; typedef hpp::core::ComparisonTypePtr_t ComparisonTypePtr_t; typedef hpp::core::DifferentiableFunctionPtr_t DifferentiableFunctionPtr_t; + typedef hpp::core::SizeIntervals_t SizeIntervals_t; + typedef std::vector <SizeIntervals_t> IntervalsContainer_t; typedef std::list < NumericalConstraintPtr_t > NumericalConstraints_t; typedef std::list < LockedJointPtr_t > LockedJoints_t; diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh index be8ceb6bdb86e9fc10f00c4a5893996958a30d01..f06ed8dc36b8b080399f78a43c6164ee523c035f 100644 --- a/include/hpp/manipulation/graph/graph-component.hh +++ b/include/hpp/manipulation/graph/graph-component.hh @@ -50,7 +50,11 @@ namespace hpp { int id () const; /// Add core::NumericalConstraint to the component. - virtual void addNumericalConstraint (const NumericalConstraintPtr_t& numConstraint); + /// \param passiveDofs see ConfigProjector::addNumericalConstraint + // for more information. + virtual void addNumericalConstraint ( + const NumericalConstraintPtr_t& numConstraint, + const SizeIntervals_t& passiveDofs = SizeIntervals_t ()); /// Add core::DifferentiableFunction to the component. virtual void addNumericalConstraint @@ -90,6 +94,8 @@ namespace hpp { /// Stores the numerical constraints. NumericalConstraints_t numericalConstraints_; + /// Stores the passive dofs for each numerical constraints. + std::vector <SizeIntervals_t> passiveDofs_; /// List of LockedJoint constraints LockedJoints_t lockedJoints_; /// A weak pointer to the parent graph. diff --git a/include/hpp/manipulation/graph/node-selector.hh b/include/hpp/manipulation/graph/node-selector.hh index fe0bac4fde33c6484f00e955ca6cc204bb4ed562..bcb02d8cbebe4a968579f37f8f0447bc2c59c252 100644 --- a/include/hpp/manipulation/graph/node-selector.hh +++ b/include/hpp/manipulation/graph/node-selector.hh @@ -43,7 +43,9 @@ namespace hpp { virtual EdgePtr_t chooseEdge(const NodePtr_t& node) const; /// Should never be called. - void addNumericalConstraint (const core::NumericalConstraintPtr_t& /* function */) + void addNumericalConstraint ( + const core::NumericalConstraintPtr_t& /* function */, + const SizeIntervals_t& /* passiveDofs */ = SizeIntervals_t ()) { HPP_THROW_EXCEPTION (Bad_function_call, "This component does not have constraints."); } diff --git a/include/hpp/manipulation/graph/node.hh b/include/hpp/manipulation/graph/node.hh index 8973f3029cae321a9c221e8fa92fd13dce3ea601..00036883cabd1e20725ecb37dfdac3fd311e3525 100644 --- a/include/hpp/manipulation/graph/node.hh +++ b/include/hpp/manipulation/graph/node.hh @@ -85,9 +85,11 @@ namespace hpp { ConstraintSetPtr_t configConstraint() const; /// Add core::NumericalConstraint to the component. - virtual void addNumericalConstraintForPath (const NumericalConstraintPtr_t& nm) + virtual void addNumericalConstraintForPath (const NumericalConstraintPtr_t& nm, + const SizeIntervals_t& passiveDofs = SizeIntervals_t ()) { numericalConstraintsForPath_.push_back (nm); + passiveDofsForPath_.push_back (passiveDofs); } /// Add core::DifferentiableFunction to the component. @@ -139,6 +141,7 @@ namespace hpp { /// Stores the numerical constraints for path. NumericalConstraints_t numericalConstraintsForPath_; + IntervalsContainer_t passiveDofsForPath_; /// A selector that will implement the selection of the next state. NodeSelectorWkPtr_t selector_; diff --git a/src/graph/edge.cc b/src/graph/edge.cc index 2b17af68bf532e4549b4a6967fbff0fc01e7ba32..b5c1976e26a3544eaf35563e87c0d5beb5fd7b6a 100644 --- a/src/graph/edge.cc +++ b/src/graph/edge.cc @@ -429,10 +429,13 @@ namespace hpp { ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations()); g->insertNumericalConstraints (proj); + IntervalsContainer_t::const_iterator itpdof = extraPassiveDofs_.begin (); for (NumericalConstraints_t::const_iterator it = extraNumericalConstraints_.begin (); it != extraNumericalConstraints_.end (); ++it) { - proj->add (*it); + proj->add (*it, *itpdof); + ++itpdof; } + assert (itpdof == extraPassiveDofs_.end ()); !extraNumericalConstraints_.empty (); insertNumericalConstraints (proj); to ()->insertNumericalConstraints (proj); @@ -450,9 +453,11 @@ namespace hpp { return extraConstraints_->get (); } - void LevelSetEdge::insertConfigConstraint (const NumericalConstraintPtr_t& nm) + void LevelSetEdge::insertConfigConstraint (const NumericalConstraintPtr_t& nm, + const SizeIntervals_t& passiveDofs) { extraNumericalConstraints_.push_back (nm); + extraPassiveDofs_.push_back (passiveDofs); } void LevelSetEdge::insertConfigConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq) diff --git a/src/graph/graph-component.cc b/src/graph/graph-component.cc index 9406aa54110207f713cdf0e60272638bac2350da..227337265f42dc8de76028caf50dbb8418a00f36 100644 --- a/src/graph/graph-component.cc +++ b/src/graph/graph-component.cc @@ -63,9 +63,11 @@ namespace hpp { return os; } - void GraphComponent::addNumericalConstraint (const NumericalConstraintPtr_t& nm) + void GraphComponent::addNumericalConstraint (const NumericalConstraintPtr_t& nm, + const SizeIntervals_t& passiveDofs) { numericalConstraints_.push_back(nm); + passiveDofs_.push_back (passiveDofs); } void GraphComponent::addNumericalConstraint (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq) @@ -81,9 +83,13 @@ namespace hpp { bool GraphComponent::insertNumericalConstraints (ConfigProjectorPtr_t& proj) const { + IntervalsContainer_t::const_iterator itpdof = passiveDofs_.begin (); for (NumericalConstraints_t::const_iterator it = numericalConstraints_.begin(); - it != numericalConstraints_.end(); ++it) - proj->add (*it); + it != numericalConstraints_.end(); ++it) { + proj->add (*it, *itpdof); + ++itpdof; + } + assert (itpdof == passiveDofs_.end ()); return !numericalConstraints_.empty (); }