Problem uses manipulation::WeighedDistance

include/hpp/manipulation/problem.hh

@@ -38,12 +38,7 @@ namespace hpp {
         Problem (DevicePtr_t robot);
 
         /// Set the graph of constraints
-        void constraintGraph (const graph::GraphPtr_t& graph)
-        {
-          graph_ = graph;
-          if (pathValidation ())
-            pathValidation ()->constraintGraph (graph);
-        }
+        void constraintGraph (const graph::GraphPtr_t& graph);
 
         /// Get the graph of constraints
         graph::GraphPtr_t constraintGraph () const

src/problem.cc

@@ -15,6 +15,7 @@
 // hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
 
 #include <hpp/manipulation/problem.hh>
+#include <hpp/manipulation/weighed-distance.hh>
 #include <hpp/manipulation/graph-steering-method.hh>
 
 namespace hpp {
@@ -23,6 +24,17 @@ namespace hpp {
       : Parent (robot), graph_()
     {
       Parent::steeringMethod (GraphSteeringMethod::create (this));
+      distance (WeighedDistance::create (robot, graph_));
+    }
+
+    void Problem::constraintGraph (const graph::GraphPtr_t& graph)
+    {
+      graph_ = graph;
+      if (pathValidation ())
+        pathValidation ()->constraintGraph (graph);
+      WeighedDistancePtr_t d = HPP_DYNAMIC_PTR_CAST (WeighedDistance,
+          distance ());
+      if (d) d->constraintGraph (graph);
     }
 
     GraphPathValidationPtr_t Problem::pathValidation () const