diff --git a/CMakeLists.txt b/CMakeLists.txt
index 31ad63123cd7ec377642c3d401bcd3416a269f22..a054a8d2c8dcf440ddb74c30623317f052f11f5d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -64,6 +64,7 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/problem.hh
include/hpp/manipulation/problem-solver.hh
include/hpp/manipulation/device.hh
+ include/hpp/manipulation/weighed-distance.hh
include/hpp/manipulation/constraint-set.hh
include/hpp/manipulation/roadmap.hh
include/hpp/manipulation/roadmap-node.hh
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index c9d1de0a240bf9c06f134b9966e83f22aaa2ee9c..9251a26b2c5b35b63d63f66b4ef80c02fbb1ce19 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -55,6 +55,8 @@ namespace hpp {
typedef boost::shared_ptr <Roadmap> RoadmapPtr_t;
HPP_PREDEF_CLASS (RoadmapNode);
typedef RoadmapNode* RoadmapNodePtr_t;
+ HPP_PREDEF_CLASS (WeighedDistance);
+ typedef boost::shared_ptr<WeighedDistance> WeighedDistancePtr_t;
typedef constraints::RelativeOrientation RelativeOrientation;
typedef constraints::RelativePosition RelativePosition;
typedef constraints::RelativeOrientationPtr_t RelativeOrientationPtr_t;
diff --git a/include/hpp/manipulation/weighed-distance.hh b/include/hpp/manipulation/weighed-distance.hh
new file mode 100644
index 0000000000000000000000000000000000000000..472a6d57540c81538f1af640acd3d22e53541871
--- /dev/null
+++ b/include/hpp/manipulation/weighed-distance.hh
@@ -0,0 +1,75 @@
+// Copyright (c) 2015 CNRS
+// Authors: Joseph Mirabel
+//
+// This file is part of hpp-manipulation
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef HPP_MANIPULATION_DISTANCE_HH
+# define HPP_MANIPULATION_DISTANCE_HH
+
+# include <hpp/core/weighed-distance.hh>
+# include <hpp/manipulation/fwd.hh>
+# include <hpp/manipulation/graph/fwd.hh>
+# include <hpp/manipulation/config.hh>
+
+namespace hpp {
+ namespace manipulation {
+ /// \addtogroup steering_method
+ /// \{
+
+ /// Class for distance between configurations
+ class HPP_MANIPULATION_DLLAPI WeighedDistance : public core::WeighedDistance
+ {
+ public:
+ static WeighedDistancePtr_t create (const DevicePtr_t& robot,
+ const graph::GraphPtr_t& graph);
+
+ static WeighedDistancePtr_t createCopy
+ (const WeighedDistancePtr_t& distance);
+
+ virtual core::DistancePtr_t clone () const;
+
+ /// Set the graph of constraints
+ void constraintGraph (const graph::GraphPtr_t& graph)
+ {
+ graph_ = graph;
+ }
+
+ /// Get the graph of constraints
+ graph::GraphPtr_t constraintGraph () const
+ {
+ return graph_;
+ }
+
+ protected:
+ WeighedDistance (const DevicePtr_t& robot, const graph::GraphPtr_t graph);
+
+ WeighedDistance (const WeighedDistance& distance);
+
+ /// Derived class should implement this function
+ virtual value_type impl_distance (
+ ConfigurationIn_t q1, ConfigurationIn_t q2) const;
+ virtual value_type impl_distance (
+ core::NodePtr_t n1, core::NodePtr_t n2) const;
+
+ void init (WeighedDistanceWkPtr_t self);
+
+ private:
+ graph::GraphPtr_t graph_;
+ WeighedDistanceWkPtr_t weak_;
+ }; // class Distance
+ /// \}
+ } // namespace manipulation
+} // namespace hpp
+#endif // HPP_MANIPULATION_DISTANCE_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 1e1d9043935929fe67cd3c123ed252d2db3e506f..f5920d83a9f4aeecdc332ef17b73f18e7ec52cb3 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -28,6 +28,7 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED
constraint-set.cc
roadmap-node.cc
device.cc
+ weighed-distance.cc
problem.cc
graph-path-validation.cc
graph-steering-method.cc
diff --git a/src/weighed-distance.cc b/src/weighed-distance.cc
new file mode 100644
index 0000000000000000000000000000000000000000..4709eb4a1368a591596c061a30ef77236dc302a4
--- /dev/null
+++ b/src/weighed-distance.cc
@@ -0,0 +1,101 @@
+// Copyright (c) 2015, Joseph Mirabel
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+#include <hpp/manipulation/weighed-distance.hh>
+
+#include <hpp/util/debug.hh>
+
+#include <hpp/manipulation/roadmap-node.hh>
+#include <hpp/manipulation/device.hh>
+#include <hpp/manipulation/graph/graph.hh>
+#include <hpp/manipulation/graph/edge.hh>
+
+namespace hpp {
+ namespace manipulation {
+ WeighedDistancePtr_t WeighedDistance::create
+ (const DevicePtr_t& robot, const graph::GraphPtr_t& graph)
+ {
+ WeighedDistance* ptr = new WeighedDistance (robot, graph);
+ WeighedDistancePtr_t shPtr (ptr);
+ ptr->init (shPtr);
+ return shPtr;
+ }
+
+ WeighedDistancePtr_t WeighedDistance::createCopy
+ (const WeighedDistancePtr_t& distance)
+ {
+ WeighedDistance* ptr = new WeighedDistance (*distance);
+ WeighedDistancePtr_t shPtr (ptr);
+ ptr->init (shPtr);
+ return shPtr;
+ }
+
+ core::DistancePtr_t WeighedDistance::clone () const
+ {
+ return createCopy (weak_.lock ());
+ }
+
+ WeighedDistance::WeighedDistance (const DevicePtr_t& robot,
+ const graph::GraphPtr_t graph) :
+ core::WeighedDistance (robot), graph_ (graph)
+ {
+ }
+
+ WeighedDistance::WeighedDistance (const WeighedDistance& distance) :
+ core::WeighedDistance (distance), graph_ (distance.graph_)
+ {
+ }
+
+ void WeighedDistance::init (WeighedDistanceWkPtr_t self)
+ {
+ weak_ = self;
+ }
+
+ value_type WeighedDistance::impl_distance (ConfigurationIn_t q1,
+ ConfigurationIn_t q2) const
+ {
+ value_type d = core::WeighedDistance::impl_distance (q1, q2);
+ return d;
+
+ // graph::Edges_t pes = graph_->getEdges
+ // (graph_->getNode (q1), graph_->getNode (q2));
+ // while (!pes.empty ()) {
+ // if (pes.back ()->canConnect (q1, q2))
+ // return d;
+ // pes.pop_back ();
+ // }
+ // return d + 100;
+ }
+
+ value_type WeighedDistance::impl_distance (core::NodePtr_t n1,
+ core::NodePtr_t n2) const
+ {
+ Configuration_t& q1 = *n1->configuration(),
+ q2 = *n2->configuration();
+ value_type d = core::WeighedDistance::impl_distance (q1, q2);
+
+ graph::Edges_t pes = graph_->getEdges (
+ graph_->getNode (static_cast <RoadmapNodePtr_t>(n1)),
+ graph_->getNode (static_cast <RoadmapNodePtr_t>(n2)));
+ while (!pes.empty ()) {
+ if (pes.back ()->canConnect (q1, q2))
+ return d;
+ pes.pop_back ();
+ }
+ return d + 100;
+ }
+ } // namespace manipulation
+} // namespace hpp