Add member function pathConstraint and configConstraint

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);

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

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