Merge pull request #73 from jmirabel/devel

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

src/graph/validation.cc

@@ -41,9 +41,9 @@ namespace hpp {
       {
         const NumericalConstraints_t& Ancs = A->numericalConstraints();
         const NumericalConstraints_t& Bncs = B->numericalConstraints();
-        for (NumericalConstraints_t::const_iterator _nc = Ancs.begin();
-            _nc != Ancs.end(); ++_nc)
-          if (std::find (Bncs.begin(), Bncs.end(), *_nc) == Bncs.end())
+        for (NumericalConstraints_t::const_iterator _nc = Bncs.begin();
+            _nc != Bncs.end(); ++_nc)
+          if (std::find (Ancs.begin(), Ancs.end(), *_nc) == Ancs.end())
             return false;
         return true;
       }