Add cost in GraphComponent.

include/hpp/manipulation/graph/graph-component.hh

@@ -57,6 +57,9 @@ namespace hpp {
               const ImplicitPtr_t& numConstraint,
               const segments_t& passiveDofs = segments_t ());
 
+          /// Add a cost function Implicit to the component.
+          virtual void addNumericalCost (const ImplicitPtr_t& numCost);
+
 	  /// Reset the numerical constraints stored in the component.
 	  virtual void resetNumericalConstraints ();
 
@@ -85,6 +88,9 @@ namespace hpp {
           /// Get a reference to the NumericalConstraints_t
           const NumericalConstraints_t& numericalConstraints() const;
 
+          /// Get a reference to the NumericalConstraints_t
+          const NumericalConstraints_t& numericalCosts() const;
+
           /// Get a reference to the NumericalConstraints_t
           const IntervalsContainer_t& passiveDofs() const;
 
@@ -119,6 +125,8 @@ namespace hpp {
           NumericalConstraints_t numericalConstraints_;
           /// Stores the passive dofs for each numerical constraints.
           IntervalsContainer_t passiveDofs_;
+          /// Stores the numerical costs.
+          NumericalConstraints_t numericalCosts_;
           /// List of LockedJoint constraints: \todo to be removed
           const LockedJoints_t lockedJoints_;
           /// A weak pointer to the parent graph.

src/graph/graph-component.cc

@@ -59,11 +59,18 @@ namespace hpp {
         passiveDofs_.push_back (passiveDofs);
       }
 
+      void GraphComponent::addNumericalCost (const ImplicitPtr_t& cost)
+      {
+        isInit_ = false;
+        numericalCosts_.push_back(cost);
+      }
+
       void GraphComponent::resetNumericalConstraints ()
       {
         isInit_ = false;
 	numericalConstraints_.clear();
         passiveDofs_.clear();
+        numericalCosts_.clear();
       }
 
       void GraphComponent::addLockedJointConstraint
@@ -85,6 +92,10 @@ namespace hpp {
           ++itpdof;
         }
         assert (itpdof == passiveDofs_.end ());
+        for (NumericalConstraints_t::const_iterator it = numericalCosts_.begin();
+            it != numericalCosts_.end(); ++it) {
+          proj->add (*it, 1);
+        }
         return !numericalConstraints_.empty ();
       }
 
@@ -93,6 +104,11 @@ namespace hpp {
         return numericalConstraints_;
       }
 
+      const NumericalConstraints_t& GraphComponent::numericalCosts() const
+      {
+        return numericalCosts_;
+      }
+
       const std::vector <segments_t>& GraphComponent::passiveDofs() const
       {
         return passiveDofs_;