[graph::State] Check that constraints are not parameterizable

  - when inserting them. Throw if they are.
  - Add a test.

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

@@ -50,7 +50,7 @@ namespace hpp {
             return id_;
           }
 
-          /// Add Implicit to the component.
+          /// Add constraint to the component.
           virtual void addNumericalConstraint (
               const ImplicitPtr_t& numConstraint);
 

include/hpp/manipulation/graph/state.hh

@@ -126,7 +126,14 @@ namespace hpp {
             return configConstraints_;
           }
 
-          /// Add constraints::Implicit to the component.
+          /// Add constraint to the state
+	  /// Call the parent implementation.
+	  /// \throw std::logic_error if the constraint is parameterizable
+	  /// (contains at least one Equality comparison type).
+          virtual void addNumericalConstraint (
+              const ImplicitPtr_t& numConstraint);
+
+          /// Add a constraint for paths that lie in this state.
           virtual void addNumericalConstraintForPath (const ImplicitPtr_t& nm)
           {
             invalidate();

src/graph/state.cc

@@ -142,6 +142,23 @@ namespace hpp {
         hppDout (error, "Edge not found");
         return 0;
       }
+
+      void State::addNumericalConstraint(const ImplicitPtr_t& numConstraint)
+      {
+        ComparisonTypes_t comp(numConstraint->comparisonType());
+        for(constraints::ComparisonType c : comp)
+        {
+          if (c == constraints::Equality)
+          {
+            throw std::logic_error("Failed to insert constraint \""
+	     + numConstraint->function().name()
+	     + "\" as a state constraint since it contains a comparison "
+             + "of type Equality");
+          }
+        }
+        GraphComponent::addNumericalConstraint(numConstraint);
+      }
+
     } // namespace graph
   } // namespace manipulation
 } // namespace hpp

tests/test-constraintgraph.cc

@@ -141,14 +141,24 @@ BOOST_AUTO_TEST_CASE (Initialization)
   using hpp_test::graph_;
   using hpp::pinocchio::LiegroupElement;
   using hpp::pinocchio::LiegroupSpace;
+  using hpp::constraints::ComparisonTypes_t;
+  using hpp::constraints::ImplicitPtr_t;
+  using hpp::constraints::EqualToZero;
+  using hpp::constraints::Equality;
   using hpp::constraints::LockedJoint;
   using hpp::manipulation::graph::Edge;
   using hpp::manipulation::graph::EdgePtr_t;
 
   initialize(true);
   hpp::manipulation::DevicePtr_t robot(graph_->robot());
-  graph_->addNumericalConstraint(LockedJoint::create
-    (robot->jointAt(1), LiegroupElement(LiegroupSpace::R1(true))));
+  ImplicitPtr_t constraint(LockedJoint::create
+			   (robot->jointAt(1), LiegroupElement
+			    (LiegroupSpace::R1(true))));
+  graph_->addNumericalConstraint(constraint);
+  // Check that states refuse parameterizable constraints
+  constraint->comparisonType(ComparisonTypes_t(1,Equality));
+  BOOST_CHECK_THROW(n1->addNumericalConstraint(constraint), std::logic_error);
+
   for (std::size_t i=0; i < components.size(); ++i)
   {
     EdgePtr_t edge(HPP_DYNAMIC_PTR_CAST(Edge, components[i]));