From 3b69832fb4fa82c1657a63dbfe03f90df0b62242 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 14 Dec 2015 17:14:58 +0100
Subject: [PATCH] Add class WeighedDistance
---
CMakeLists.txt | 1 +
include/hpp/manipulation/fwd.hh | 2 +
include/hpp/manipulation/weighed-distance.hh | 75 ++++++++++++++
src/CMakeLists.txt | 1 +
src/weighed-distance.cc | 101 +++++++++++++++++++
5 files changed, 180 insertions(+)
create mode 100644 include/hpp/manipulation/weighed-distance.hh
create mode 100644 src/weighed-distance.cc
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 31ad631..a054a8d 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 c9d1de0..9251a26 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 0000000..472a6d5
--- /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 1e1d904..f5920d8 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 0000000..4709eb4
--- /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
--
GitLab