diff --git a/include/hpp/manipulation/graph/node.hh b/include/hpp/manipulation/graph/node.hh index 1ff71f246d17909006449c6578b91e11fe5c8d1c..7e21f243a1b56125f43d90174459ba7b20f6a721 100644 --- a/include/hpp/manipulation/graph/node.hh +++ b/include/hpp/manipulation/graph/node.hh @@ -71,6 +71,9 @@ namespace hpp { /// Print the object in a stream. std::ostream& print (std::ostream& os) const; + /// Constraint to project onto this node. + ConstraintPtr_t configConstraint(); + protected: /// Initialize the object. void init (const NodeWkPtr_t& self); diff --git a/src/graph/edge.cc b/src/graph/edge.cc index 98ffd75613590526087aed46a97498388f66f263..363846405c240647d1e69285e33a83154230b6ac 100644 --- a/src/graph/edge.cc +++ b/src/graph/edge.cc @@ -43,6 +43,43 @@ namespace hpp { return os; } + ConstraintPtr_t Edge::configConstraint(ConfigurationIn_t config) + { + if (!configConstraints_) { + NodePtr_t to = to_.lock(); + if (!to) + HPP_THROW_EXCEPTION (Bad_function_call, "Edge does not have a destination."); + ConstraintSetPtr_t configConst = buildConstraintSet (graph_, name () + "-cfg"); + insertListIn <LockedDofs_t> (lockedDofConstraints_, configConst); + insertListIn <LockedDofs_t> (to->lockedDofConstraints(), configConst); + DifferentiableFunctions_t toNumConst = to->numericalConstraints(); + if (numericalConstraints_.size() > 0 || toNumConst.size() > 0) { + ConfigProjectorPtr_t cp = buildConfigProjector (graph_, name () + "cfgproj"); + insertListIn <DifferentiableFunctions_t> (numericalConstraints_, cp); + insertListIn <DifferentiableFunctions_t> (toNumConst, cp); + configConst->addConstraint (HPP_DYNAMIC_PTR_CAST(Constraint, cp)); + } + configConstraints_ = configConst; + } + configConstraints_->setLeafParameterFromConfig (config); + return configConstraints_; + } + + ConstraintPtr_t Edge::pathConstraint(ConfigurationIn_t config) + { + if (!pathConstraints_) { + ConstraintSetPtr_t pathConst = buildConstraintSet (graph_, name () + "-pathconstraint"); + insertListIn <LockedDofs_t> (lockedDofConstraints_, pathConst); + if (numericalConstraints_.size () > 0) { + ConfigProjectorPtr_t cp = buildConfigProjector (graph_, name () + "pathproj"); + insertListIn <DifferentiableFunctions_t> (numericalConstraints_, cp); + pathConst->addConstraint (HPP_DYNAMIC_PTR_CAST(Constraint, cp)); + } + pathConstraints_ = pathConst; + } + pathConstraints_->setLeafParameterFromConfig (config); + return pathConstraints_; + } } // namespace graph } // namespace manipulation } // namespace hpp diff --git a/src/graph/node.cc b/src/graph/node.cc index 732585ec93db2e782c34b6e45fbec6819ea0c05f..70a205d53e6aa0adbb12492d2aa5409bd8bd81e5 100644 --- a/src/graph/node.cc +++ b/src/graph/node.cc @@ -58,6 +58,21 @@ namespace hpp { os << *(*it); return os; } + + ConstraintPtr_t Node::configConstraint() + { + if (!configConstraints_) { + ConstraintSetPtr_t configConst = buildConstraintSet (graph_, name () + "-cfgconstraint"); + insertListIn <LockedDofs_t> (lockedDofConstraints_, configConst); + if (numericalConstraints_.size () > 0) { + ConfigProjectorPtr_t cp = buildConfigProjector (graph_, name () + "cfgproj"); + insertListIn <DifferentiableFunctions_t> (numericalConstraints_, cp); + configConst->addConstraint (HPP_DYNAMIC_PTR_CAST(Constraint, cp)); + } + configConstraints_ = configConst; + } + return configConstraints_; + } } // namespace graph } // namespace manipulation } // namespace hpp