Add Graph::initialize and fix const-correctness of constructor of constraints

include/hpp/manipulation/graph/edge.hh

@@ -29,32 +29,6 @@
 namespace hpp {
   namespace manipulation {
     namespace graph {
-      /// \cond
-      /// Cache mechanism that enable const-correctness of member functions.
-      template <typename C>
-        class HPP_MANIPULATION_LOCAL Cache
-      {
-        public:
-          void set (const C& c)
-          {
-            c_ = c;
-          }
-
-          operator bool() const
-          {
-            return (bool)c_;
-          }
-
-          const C& get () const
-          {
-            return c_;
-          }
-
-        private:
-          C c_;
-      };
-      /// \endcond
-
       /// \addtogroup constraint_graph
       /// \{
 
@@ -141,13 +115,13 @@ namespace hpp {
 	  /// Get steering method associated to the edge.
 	  const core::SteeringMethodPtr_t& steeringMethod () const
 	  {
-	    return steeringMethod_->get();
+	    return steeringMethod_;
 	  }
 
 	  /// Get path validation associated to the edge.
 	  const core::PathValidationPtr_t& pathValidation () const
 	  {
-	    return pathValidation_->get();
+	    return pathValidation_;
 	  }
 
           const RelativeMotion::matrix_type& relativeMotion () const
@@ -190,9 +164,11 @@ namespace hpp {
           /// \return The initialized constraint.
           ConstraintSetPtr_t pathConstraint() const;
 
-          virtual ConstraintSetPtr_t buildConfigConstraint() const;
+          virtual ConstraintSetPtr_t buildConfigConstraint();
+
+          virtual ConstraintSetPtr_t buildPathConstraint();
 
-          virtual ConstraintSetPtr_t buildPathConstraint() const;
+          virtual void initialize ();
 
           /// Print the object in a stream.
           virtual std::ostream& print (std::ostream& os) const;
@@ -200,16 +176,12 @@ namespace hpp {
           bool isShort_;
 
         private:
-          typedef Cache < ConstraintSetPtr_t > Constraint_t;
-          typedef Cache < core::SteeringMethodPtr_t > SteeringMethod_t;
-          typedef Cache < core::PathValidationPtr_t > PathValidation_t;
-
           /// See pathConstraint member function.
-          Constraint_t* pathConstraints_;
+          ConstraintSetPtr_t pathConstraints_;
 
           /// Constraint ensuring that a q_proj will be in to_ and in the
           /// same leaf of to_ as the configuration used for initialization.
-          Constraint_t* configConstraints_;
+          ConstraintSetPtr_t configConstraints_;
 
           /// The two ends of the transition.
           StateWkPtr_t from_, to_;
@@ -218,11 +190,11 @@ namespace hpp {
           StateWkPtr_t state_;
 
 	  /// Steering method used to create paths associated to the edge
-	  SteeringMethod_t* steeringMethod_;
+          core::SteeringMethodPtr_t steeringMethod_;
 
 	  /// Path validation associated to the edge
           mutable RelativeMotion::matrix_type relMotion_;
-	  PathValidation_t* pathValidation_;
+          core::PathValidationPtr_t pathValidation_;
 
           /// Weak pointer to itself.
           EdgeWkPtr_t wkPtr_;
@@ -334,7 +306,7 @@ namespace hpp {
 
           virtual bool applyConstraints (core::NodePtr_t n_offset, ConfigurationOut_t q) const;
 
-          virtual ConstraintSetPtr_t buildConfigConstraint() const;
+          virtual ConstraintSetPtr_t buildConfigConstraint();
 
           void buildHistogram ();
 
@@ -383,6 +355,8 @@ namespace hpp {
           /// Populate DrawingAttributes tooltip
           virtual void populateTooltip (dot::Tooltip& tp) const;
 
+          virtual void initialize ();
+
         private:
           bool applyConstraintsWithOffset (ConfigurationIn_t qoffset,
               ConfigurationIn_t qlevelset, ConfigurationOut_t q) const;

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

@@ -121,6 +121,8 @@ namespace hpp {
           /// Populate DrawingAttributes tooltip
           virtual void populateTooltip (dot::Tooltip& tp) const;
 
+          virtual void initialize () = 0;
+
         private:
           /// Name of the component.
           std::string name_;
@@ -128,6 +130,8 @@ namespace hpp {
           GraphComponentWkPtr_t wkPtr_;
           /// ID of the component (index in components vector).
 	  std::size_t id_;
+
+          friend class Graph;
       };
 
       std::ostream& operator<< (std::ostream& os, const GraphComponent& graphComp);

include/hpp/manipulation/graph/graph.hh

@@ -214,6 +214,8 @@ namespace hpp {
           /// Print the component in DOT language.
           virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
 
+          virtual void initialize ();
+
         protected:
           /// Initialization of the object.
           void init (const GraphWkPtr_t& weak, DevicePtr_t robot);

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

@@ -84,6 +84,8 @@ namespace hpp {
           WeighedStates_t orderedStates_;
           States_t waypoints_;
 
+          virtual void initialize () {};
+
         private:
           /// Weak pointer to itself.
           StateSelectorPtr_t wkPtr_;

include/hpp/manipulation/graph/state.hh

@@ -104,7 +104,10 @@ namespace hpp {
           Weight_t getWeight (const EdgePtr_t&edge);
 
           /// Constraint to project onto this state.
-          ConstraintSetPtr_t configConstraint() const;
+          ConstraintSetPtr_t configConstraint() const
+          {
+            return configConstraints_;
+          }
 
           /// Add core::NumericalConstraint to the component.
           virtual void addNumericalConstraintForPath (const NumericalConstraintPtr_t& nm,
@@ -156,6 +159,8 @@ namespace hpp {
 
           virtual void populateTooltip (dot::Tooltip& tp) const;
 
+          virtual void initialize ();
+
         private:
           /// List of possible motions from this state (i.e. the outgoing
           /// vertices).
@@ -163,8 +168,7 @@ namespace hpp {
           std::vector <EdgePtr_t> hiddenNeighbors_;
 
           /// Set of constraints to be statisfied.
-          typedef Cache < ConstraintSetPtr_t > Constraint_t;
-          Constraint_t* configConstraints_;
+          ConstraintSetPtr_t configConstraints_;
 
           /// Stores the numerical constraints for path.
           NumericalConstraints_t numericalConstraintsForPath_;

include/hpp/manipulation/problem-solver.hh

@@ -71,6 +71,9 @@ namespace hpp {
 
         /// Get the constraint graph
         graph::GraphPtr_t constraintGraph () const;
+
+        /// Should be called before any call on the graph is made.
+        void initConstraintGraph ();
         /// \}
 
 	/// Create placement constraint

src/graph/edge.cc

@@ -38,19 +38,14 @@ namespace hpp {
     namespace graph {
       Edge::Edge (const std::string& name) :
 	GraphComponent (name), isShort_ (false),
-        pathConstraints_ (new Constraint_t()),
-	configConstraints_ (new Constraint_t()),
-        steeringMethod_ (new SteeringMethod_t()),
-        pathValidation_ (new PathValidation_t())
+        pathConstraints_ (),
+	configConstraints_ (),
+        steeringMethod_ (),
+        pathValidation_ ()
       {}
 
       Edge::~Edge ()
-      {
-        if (pathConstraints_  ) delete pathConstraints_;
-        if (configConstraints_) delete configConstraints_;
-        if (steeringMethod_   ) delete steeringMethod_;
-        if (pathValidation_   ) delete pathValidation_;
-      }
+      {}
 
       StatePtr_t Edge::to () const
       {
@@ -159,6 +154,12 @@ namespace hpp {
         state_ = to;
       }
 
+      void Edge::initialize ()
+      {
+        configConstraints_ = buildConfigConstraint ();
+        pathConstraints_ = buildPathConstraint ();
+      }
+
       std::ostream& Edge::print (std::ostream& os) const
       {
         os << "|   |   |-- ";
@@ -181,13 +182,10 @@ namespace hpp {
 
       ConstraintSetPtr_t Edge::configConstraint() const
       {
-        if (!*configConstraints_) {
-          configConstraints_->set (buildConfigConstraint ());
-        }
-        return configConstraints_->get ();
+        return configConstraints_;
       }
 
-      ConstraintSetPtr_t Edge::buildConfigConstraint() const
+      ConstraintSetPtr_t Edge::buildConfigConstraint()
       {
         std::string n = "(" + name () + ")";
         GraphPtr_t g = graph_.lock ();
@@ -216,14 +214,10 @@ namespace hpp {
 
       ConstraintSetPtr_t Edge::pathConstraint() const
       {
-        if (!*pathConstraints_) {
-	  ConstraintSetPtr_t pathConstraints (buildPathConstraint ());
-          pathConstraints_->set (pathConstraints);
-        }
-        return pathConstraints_->get ();
+        return pathConstraints_;
       }
 
-      ConstraintSetPtr_t Edge::buildPathConstraint() const
+      ConstraintSetPtr_t Edge::buildPathConstraint()
       {
         std::string n = "(" + name () + ")";
         GraphPtr_t g = graph_.lock ();
@@ -244,16 +238,15 @@ namespace hpp {
 
         // Build steering method
         const ProblemPtr_t& problem (g->problem());
-        steeringMethod_->set(problem->steeringMethod()
-          ->innerSteeringMethod()->copy());
-        steeringMethod_->get()->constraints (constraint);
+        steeringMethod_ = problem->steeringMethod()->innerSteeringMethod()->copy();
+        steeringMethod_->constraints (constraint);
         // Build path validation and relative motion matrix
         // TODO this path validation will not contain obstacles added after
         // its creation.
-        pathValidation_->set(problem->pathValidationFactory ());
+        pathValidation_ = problem->pathValidationFactory ();
         relMotion_ = RelativeMotion::matrix (g->robot());
         RelativeMotion::fromConstraint (relMotion_, g->robot(), constraint);
-        pathValidation_->get()->filterCollisionPairs (relMotion_);
+        pathValidation_->filterCollisionPairs (relMotion_);
         return constraint;
       }
 
@@ -273,12 +266,7 @@ namespace hpp {
 	const
       {
         using pinocchio::displayConfig;
-	core::SteeringMethodPtr_t sm (steeringMethod_->get());
-	if (!sm) {
-	  buildPathConstraint ();
-	}
-	sm = (steeringMethod_->get());
-	if (!sm) {
+	if (!steeringMethod_) {
 	  std::ostringstream oss;
 	  oss << "No steering method set in edge " << name () << ".";
 	  throw std::runtime_error (oss.str ().c_str ());
@@ -287,7 +275,7 @@ namespace hpp {
         constraints->configProjector ()->rightHandSideFromConfig(q1);
         if (constraints->isSatisfied (q1)) {
           if (constraints->isSatisfied (q2)) {
-            path = (*sm) (q1, q2);
+            path = (*steeringMethod_) (q1, q2);
             return (bool)path;
           } else {
 	    hppDout(info, "q2 = " << displayConfig (q2)
@@ -686,7 +674,13 @@ namespace hpp {
         g->insertHistogram (hist_);
       }
 
-      ConstraintSetPtr_t LevelSetEdge::buildConfigConstraint() const
+      void LevelSetEdge::initialize ()
+      {
+        Edge::initialize();
+        buildHistogram ();
+      }
+
+      ConstraintSetPtr_t LevelSetEdge::buildConfigConstraint()
       {
         std::string n = "(" + name () + ")";
         GraphPtr_t g = graph_.lock ();

src/graph/graph.cc

@@ -47,6 +47,14 @@ namespace hpp {
             );
       }
 
+      void Graph::initialize ()
+      {
+        hists_.clear ();
+        assert(components_.size() >= 1 && components_[0].lock() == wkPtr_.lock());
+        for (std::size_t i = 1; i < components_.size(); ++i)
+          components_[i].lock()->initialize();
+      }
+
       StateSelectorPtr_t Graph::createNodeSelector (const std::string& name)
       {
         stateSelector_ = StateSelector::create (name);

src/graph/state.cc

@@ -27,14 +27,12 @@ namespace hpp {
   namespace manipulation {
     namespace graph {
       State::State (const std::string& name) :
-	GraphComponent (name), configConstraints_ (new Constraint_t()),
+	GraphComponent (name), configConstraints_ (),
         isWaypoint_ (false)
       {}
 
       State::~State ()
-      {
-        if (configConstraints_) delete configConstraints_;
-      }
+      {}
 
       StatePtr_t State::create (const std::string& name)
       {
@@ -107,23 +105,19 @@ namespace hpp {
         return os;
       }
 
-      ConstraintSetPtr_t State::configConstraint() const
+      void State::initialize()
       {
-        if (!*configConstraints_) {
-          std::string n = "(" + name () + ")";
-          GraphPtr_t g = graph_.lock ();
-          ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
-
-          ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj " + n, g->errorThreshold(), g->maxIterations());
-          g->insertNumericalConstraints (proj);
-          insertNumericalConstraints (proj);
-          constraint->addConstraint (proj);
-
-          g->insertLockedJoints (proj);
-          insertLockedJoints (proj);
-          configConstraints_->set (constraint);
-        }
-        return configConstraints_->get ();
+        std::string n = "(" + name () + ")";
+        GraphPtr_t g = graph_.lock ();
+        configConstraints_ = ConstraintSet::create (g->robot (), "Set " + n);
+
+        ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj " + n, g->errorThreshold(), g->maxIterations());
+        g->insertNumericalConstraints (proj);
+        insertNumericalConstraints (proj);
+        configConstraints_->addConstraint (proj);
+
+        g->insertLockedJoints (proj);
+        insertLockedJoints (proj);
       }
 
       void State::updateWeight (const EdgePtr_t& e, const Weight_t& w)

src/problem-solver.cc

@@ -184,6 +184,14 @@ namespace hpp {
       return constraintGraph_;
     }
 
+    void ProblemSolver::initConstraintGraph ()
+    {
+      if (!constraintGraph_)
+        throw std::runtime_error ("The graph is not defined.");
+      initSteeringMethod();
+      constraintGraph_->initialize();
+    }
+
     void ProblemSolver::createPlacementConstraint
     (const std::string& name, const StringList_t& surface1,
      const StringList_t& surface2, const value_type& margin)