Improve initialization of the graph and fix more const-correctness errors.

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

@@ -102,7 +102,9 @@ namespace hpp {
           /// Initialize the component
           void init (const GraphComponentWkPtr_t& weak);
 
-          GraphComponent(const std::string& name) : name_ (name), id_(-1)
+          GraphComponent(const std::string& name) : isInit_(false)
+                                                    , name_ (name)
+                                                    , id_(-1)
           {}
 
           /// Stores the numerical constraints.
@@ -114,6 +116,13 @@ namespace hpp {
           /// A weak pointer to the parent graph.
           GraphWkPtr_t graph_;
 
+          bool isInit_;
+
+          void throwIfNotInitialized () const
+          {
+            if (!isInit_) throw std::logic_error ("The graph should have been initialized first.");
+          }
+
           /// Print the object in a stream.
           virtual std::ostream& print (std::ostream& os) const;
           friend std::ostream& operator<< (std::ostream&, const GraphComponent&);

include/hpp/manipulation/graph/graph.hh

@@ -107,12 +107,12 @@ namespace hpp {
           /// Constraint to project onto the Node.
           /// \param state the state on which to project.
           /// \return The initialized projector.
-          ConstraintSetPtr_t configConstraint (const StatePtr_t& state);
+          ConstraintSetPtr_t configConstraint (const StatePtr_t& state) const;
 
           /// Constraint to project onto the same leaf as config.
           /// \param edges a list of edges defining the foliation.
           /// \return The constraint.
-          ConstraintSetPtr_t configConstraint (const EdgePtr_t& edge);
+          ConstraintSetPtr_t configConstraint (const EdgePtr_t& edge) const;
 
 	  /// Get error of a config with respect to a state constraint
 	  ///
@@ -125,7 +125,7 @@ namespace hpp {
 	  /// constraints.
 	  /// \deprecated use getConfigErrorForState instead
 	  bool getConfigErrorForNode (ConfigurationIn_t config,
-				      const StatePtr_t& state, vector_t& error)
+				      const StatePtr_t& state, vector_t& error) const
 	    HPP_MANIPULATION_DEPRECATED;
 
 	  /// Get error of a config with respect to a state constraint
@@ -138,7 +138,7 @@ namespace hpp {
 	  /// Call method core::ConstraintSet::isSatisfied for the state
 	  /// constraints.
 	  bool getConfigErrorForState (ConfigurationIn_t config,
-				      const StatePtr_t& state, vector_t& error);
+				      const StatePtr_t& state, vector_t& error) const;
 
 	  /// Get error of a config with respect to an edge constraint
 	  ///
@@ -152,7 +152,7 @@ namespace hpp {
 	  /// input configuration and method core::ConstraintSet::isSatisfied
 	  /// for the edge constraint.
 	  bool getConfigErrorForEdge (ConfigurationIn_t config,
-				      const EdgePtr_t& edge, vector_t& error);
+				      const EdgePtr_t& edge, vector_t& error) const;
 
 	  /// Get error of a config with respect to an edge foliation leaf
 	  ///
@@ -166,12 +166,12 @@ namespace hpp {
 	  /// on the edge constraints.
 	  bool getConfigErrorForEdgeLeaf
 	    (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
-	     const EdgePtr_t& edge, vector_t& error);
+	     const EdgePtr_t& edge, vector_t& error) const;
 
           /// Constraint to project a path.
           /// \param edge a list of edges defining the foliation.
           /// \return The constraint.
-          ConstraintSetPtr_t pathConstraint (const EdgePtr_t& edge);
+          ConstraintSetPtr_t pathConstraint (const EdgePtr_t& edge) const;
 
           /// Set maximal number of iterations
           void maxIterations (size_type iterations);

include/hpp/manipulation/graph/state-selector.hh

@@ -84,7 +84,7 @@ namespace hpp {
           WeighedStates_t orderedStates_;
           States_t waypoints_;
 
-          virtual void initialize () {};
+          virtual void initialize () { isInit_ = true; };
 
         private:
           /// Weak pointer to itself.

include/hpp/manipulation/graph/state.hh

@@ -106,6 +106,7 @@ namespace hpp {
           /// Constraint to project onto this state.
           ConstraintSetPtr_t configConstraint() const
           {
+            throwIfNotInitialized ();
             return configConstraints_;
           }
 
@@ -113,6 +114,7 @@ namespace hpp {
           virtual void addNumericalConstraintForPath (const NumericalConstraintPtr_t& nm,
               const SizeIntervals_t& passiveDofs = SizeIntervals_t ())
           {
+            isInit_ = false;
             numericalConstraintsForPath_.push_back (nm);
             passiveDofsForPath_.push_back (passiveDofs);
           }
@@ -121,6 +123,7 @@ namespace hpp {
           virtual void addNumericalConstraintForPath (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
             HPP_MANIPULATION_DEPRECATED
           {
+            isInit_ = false;
             numericalConstraintsForPath_.push_back (NumericalConstraint::create (function,ineq));
           }
 

src/graph/edge.cc

@@ -158,6 +158,7 @@ namespace hpp {
       {
         configConstraints_ = buildConfigConstraint ();
         pathConstraints_ = buildPathConstraint ();
+        isInit_ = true;
       }
 
       std::ostream& Edge::print (std::ostream& os) const
@@ -182,6 +183,7 @@ namespace hpp {
 
       ConstraintSetPtr_t Edge::configConstraint() const
       {
+        throwIfNotInitialized ();
         return configConstraints_;
       }
 
@@ -214,6 +216,7 @@ namespace hpp {
 
       ConstraintSetPtr_t Edge::pathConstraint() const
       {
+        throwIfNotInitialized ();
         return pathConstraints_;
       }
 
@@ -722,29 +725,34 @@ namespace hpp {
       void LevelSetEdge::insertParamConstraint (const NumericalConstraintPtr_t& nm,
               const SizeIntervals_t& passiveDofs)
       {
+        isInit_ = false;
         paramNumericalConstraints_.push_back (nm);
         paramPassiveDofs_.push_back (passiveDofs);
       }
 
       void LevelSetEdge::insertParamConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq)
       {
+        isInit_ = false;
         insertParamConstraint (NumericalConstraint::create (function, ineq));
       }
 
       void LevelSetEdge::insertParamConstraint (const LockedJointPtr_t lockedJoint)
       {
+        isInit_ = false;
         paramLockedJoints_.push_back (lockedJoint);
       }
 
       void LevelSetEdge::insertConditionConstraint (const NumericalConstraintPtr_t& nm,
               const SizeIntervals_t& passiveDofs)
       {
+        isInit_ = false;
         condNumericalConstraints_.push_back (nm);
         condPassiveDofs_.push_back (passiveDofs);
       }
 
       void LevelSetEdge::insertConditionConstraint (const LockedJointPtr_t lockedJoint)
       {
+        isInit_ = false;
         condLockedJoints_.push_back (lockedJoint);
       }
 

src/graph/graph-component.cc

@@ -48,6 +48,7 @@ namespace hpp {
       void GraphComponent::addNumericalConstraint (const NumericalConstraintPtr_t& nm,
           const SizeIntervals_t& passiveDofs)
       {
+        isInit_ = false;
         numericalConstraints_.push_back(nm);
         passiveDofs_.push_back (passiveDofs);
       }
@@ -59,6 +60,7 @@ namespace hpp {
 
       void GraphComponent::resetNumericalConstraints ()
       {
+        isInit_ = false;
 	numericalConstraints_.clear();
         passiveDofs_.clear();
       }
@@ -66,11 +68,13 @@ namespace hpp {
       void GraphComponent::addLockedJointConstraint
       (const LockedJointPtr_t& constraint)
       {
+        isInit_ = false;
         lockedJoints_.push_back (constraint);
       }
 
       void GraphComponent::resetLockedJoints ()
       {
+        isInit_ = false;
 	lockedJoints_.clear();
       }
 

src/graph/graph.cc

@@ -53,17 +53,17 @@ namespace hpp {
         assert(components_.size() >= 1 && components_[0].lock() == wkPtr_.lock());
         for (std::size_t i = 1; i < components_.size(); ++i)
           components_[i].lock()->initialize();
+        isInit_ = true;
       }
 
       StateSelectorPtr_t Graph::createNodeSelector (const std::string& name)
       {
-        stateSelector_ = StateSelector::create (name);
-        stateSelector_->parentGraph (wkPtr_);
-        return stateSelector_;
+        return createStateSelector (name);
       }
 
       StateSelectorPtr_t Graph::createStateSelector (const std::string& name)
       {
+        isInit_ = false;
         stateSelector_ = StateSelector::create (name);
         stateSelector_->parentGraph (wkPtr_);
         return stateSelector_;
@@ -71,18 +71,19 @@ namespace hpp {
 
       void Graph::nodeSelector (StateSelectorPtr_t ns)
       {
-        stateSelector_ = ns;
-        stateSelector_->parentGraph (wkPtr_);
+        stateSelector (ns);
       }
 
       void Graph::stateSelector (StateSelectorPtr_t ns)
       {
+        isInit_ = false;
         stateSelector_ = ns;
         stateSelector_->parentGraph (wkPtr_);
       }
 
       void Graph::maxIterations (size_type iterations)
       {
+        isInit_ = false;
         maxIterations_ = iterations;
       }
 
@@ -93,6 +94,7 @@ namespace hpp {
 
       void Graph::errorThreshold (const value_type& threshold)
       {
+        isInit_ = false;
         errorThreshold_ = threshold;
       }
 
@@ -148,27 +150,27 @@ namespace hpp {
         return stateSelector_->chooseEdge (from);
       }
 
-      ConstraintSetPtr_t Graph::configConstraint (const StatePtr_t& state)
+      ConstraintSetPtr_t Graph::configConstraint (const StatePtr_t& state) const
       {
         return state->configConstraint ();
       }
 
       bool Graph::getConfigErrorForNode (ConfigurationIn_t config,
 					 const StatePtr_t& state,
-					 vector_t& error)
+					 vector_t& error) const
       {
-	return configConstraint (state)->isSatisfied (config, error);
+	return getConfigErrorForState (config, state, error);
       }
 
       bool Graph::getConfigErrorForState (ConfigurationIn_t config,
 					  const StatePtr_t& state,
-					  vector_t& error)
+					  vector_t& error) const
       {
 	return configConstraint (state)->isSatisfied (config, error);
       }
 
       bool Graph::getConfigErrorForEdge (ConfigurationIn_t config,
-					 const EdgePtr_t& edge, vector_t& error)
+					 const EdgePtr_t& edge, vector_t& error) const
       {
 	ConstraintSetPtr_t cs (pathConstraint (edge));
 	ConfigProjectorPtr_t cp (cs->configProjector ());
@@ -178,7 +180,7 @@ namespace hpp {
 
       bool Graph::getConfigErrorForEdgeLeaf
       (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
-       const EdgePtr_t& edge, vector_t& error)
+       const EdgePtr_t& edge, vector_t& error) const
       {
 	ConstraintSetPtr_t cs (pathConstraint (edge));
 	ConfigProjectorPtr_t cp (cs->configProjector ());
@@ -186,12 +188,12 @@ namespace hpp {
 	return cs->isSatisfied (config, error);
       }
 
-      ConstraintSetPtr_t Graph::configConstraint (const EdgePtr_t& edge)
+      ConstraintSetPtr_t Graph::configConstraint (const EdgePtr_t& edge) const
       {
         return edge->configConstraint ();
       }
 
-      ConstraintSetPtr_t Graph::pathConstraint (const EdgePtr_t& edge)
+      ConstraintSetPtr_t Graph::pathConstraint (const EdgePtr_t& edge) const
       {
         return edge->pathConstraint ();
       }

src/graph/state.cc

@@ -107,6 +107,8 @@ namespace hpp {
 
       void State::initialize()
       {
+        isInit_ = true;
+
         std::string n = "(" + name () + ")";
         GraphPtr_t g = graph_.lock ();
         configConstraints_ = ConstraintSet::create (g->robot (), "Set " + n);