From 212324fc75b6b6094fba334bea6fe85115ddc527 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 8 Aug 2014 16:05:33 +0200
Subject: [PATCH] Add class GraphPathValidation
---
CMakeLists.txt | 1 +
include/hpp/manipulation/fwd.hh | 2 +
.../hpp/manipulation/graph-path-validation.hh | 75 +++++++++++++
src/CMakeLists.txt | 1 +
src/graph-path-validation.cc | 106 ++++++++++++++++++
5 files changed, 185 insertions(+)
create mode 100644 include/hpp/manipulation/graph-path-validation.hh
create mode 100644 src/graph-path-validation.cc
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4627f4a..d6cdf72 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -49,6 +49,7 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/problem-solver.hh
include/hpp/manipulation/robot.hh
include/hpp/manipulation/manipulation-planner.hh
+ include/hpp/manipulation/graph-path-validation.hh
include/hpp/manipulation/graph/node.hh
include/hpp/manipulation/graph/edge.hh
include/hpp/manipulation/graph/node-selector.hh
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 72e3d4c..1bee02b 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -66,6 +66,8 @@ namespace hpp {
HPP_PREDEF_CLASS (ManipulationPlanner);
typedef boost::shared_ptr < ManipulationPlanner > ManipulationPlannerPtr_t;
typedef core::ConnectedComponentPtr_t ConnectedComponentPtr_t;
+ HPP_PREDEF_CLASS (GraphPathValidation);
+ typedef boost::shared_ptr < GraphPathValidation > GraphPathValidationPtr_t;
typedef std::vector <DevicePtr_t> Devices_t;
typedef std::vector <ObjectPtr_t> Objects_t;
diff --git a/include/hpp/manipulation/graph-path-validation.hh b/include/hpp/manipulation/graph-path-validation.hh
new file mode 100644
index 0000000..7b8805b
--- /dev/null
+++ b/include/hpp/manipulation/graph-path-validation.hh
@@ -0,0 +1,75 @@
+// Copyright (c) 2014, LAAS-CNRS
+// 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/>.
+
+
+#ifndef HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
+# define HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
+
+# include <hpp/core/path.hh>
+# include <hpp/core/path-vector.hh>
+# include <hpp/core/path-validation.hh>
+
+# include "hpp/manipulation/fwd.hh"
+# include "hpp/manipulation/graph/graph.hh"
+# include "hpp/manipulation/graph/node.hh"
+
+namespace hpp {
+ namespace manipulation {
+ using hpp::core::PathValidation;
+ using hpp::core::PathValidationPtr_t;
+ using hpp::core::Path;
+ using hpp::core::PathPtr_t;
+ using hpp::core::PathVector;
+ using hpp::core::PathVectorPtr_t;
+ using graph::GraphPtr_t;
+
+ /// Path validation for a constraint graph
+ ///
+ /// This class encapsulates another path validation class.
+ /// The encapsulated path validation is responsible for collision
+ /// checking, whereas this class checks if a path is valid regarding
+ /// the constraint graph.
+ class HPP_MANIPULATION_DLLAPI GraphPathValidation : public PathValidation
+ {
+ public:
+ /// Check that the path is valid regarding the constraint graph and call
+ /// the encapsulated PathValidation::validate.
+ bool validate (const PathPtr_t& path, bool reverse, PathPtr_t& validPart);
+
+ /// Create a new instance of this class.
+ /// \param pathValidation a PathValidation that is responsible for collision
+ // checking.
+ // \param graph A pointer to the constraint graph.
+ static GraphPathValidationPtr_t create (const PathValidationPtr_t& pathValidation, const GraphPtr_t& graph);
+
+ protected:
+ /// Constructor
+ GraphPathValidation (const PathValidationPtr_t& pathValidation, const GraphPtr_t& graph);
+
+ private:
+ /// Do validation regarding the constraint graph for PathVector
+ bool impl_validate (const PathVectorPtr_t& path, bool reverse, PathPtr_t& validPart);
+ /// Do validation regarding the constraint graph for Path
+ bool impl_validate (const PathPtr_t& path, bool reverse, PathPtr_t& validPart);
+ /// The encapsulated PathValidation.
+ PathValidationPtr_t pathValidation_;
+ /// Pointer to the constraint graph.
+ GraphPtr_t constraintGraph_;
+ };
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 8849796..6982ee3 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -25,6 +25,7 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED
problem-solver.cc
robot.cc
manipulation-planner.cc
+ graph-path-validation.cc
graph/node.cc
graph/edge.cc
diff --git a/src/graph-path-validation.cc b/src/graph-path-validation.cc
new file mode 100644
index 0000000..4879bc1
--- /dev/null
+++ b/src/graph-path-validation.cc
@@ -0,0 +1,106 @@
+// Copyright (c) 2014, LAAS-CNRS
+// 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/graph-path-validation.hh"
+
+namespace hpp {
+ namespace manipulation {
+ GraphPathValidationPtr_t GraphPathValidation::create (
+ const PathValidationPtr_t& pathValidation, const GraphPtr_t& graph)
+ {
+ GraphPathValidation* p = new GraphPathValidation (pathValidation, graph);
+ return GraphPathValidationPtr_t (p);
+ }
+
+ GraphPathValidation::GraphPathValidation (
+ const PathValidationPtr_t& pathValidation, const GraphPtr_t& graph) :
+ pathValidation_ (pathValidation), constraintGraph_ (graph)
+ {}
+
+ bool GraphPathValidation::validate (
+ const PathPtr_t& path, bool reverse, PathPtr_t& validPart)
+ {
+ PathPtr_t pathGraphValid;
+ bool graphValid = impl_validate (path, reverse, pathGraphValid);
+ bool collisionValid = pathValidation_->validate (pathGraphValid, reverse, validPart);
+ return graphValid && collisionValid;
+ }
+
+ bool GraphPathValidation::impl_validate (
+ const PathVectorPtr_t& path, bool reverse, PathPtr_t& validPart)
+ {
+ size_t start = 0,
+ end = path->numberPaths ();
+ int inc = 1;
+ if (reverse) {
+ std::swap (start, end);
+ inc = -1;
+ }
+ PathPtr_t validSubPart;
+ value_type timeOffset = path->timeRange().first;
+ for (size_t index = start; index != end; index += inc) {
+ // We should stop at the first non valid subpath.
+ if (!impl_validate (path->pathAtRank (index), reverse, validSubPart)) {
+ if (reverse)
+ validPart = path->extract (
+ std::make_pair (timeOffset + validSubPart->timeRange().first,
+ path->timeRange().second));
+ else
+ validPart = path->extract (
+ std::make_pair (path->timeRange().first,
+ timeOffset + validSubPart->timeRange().second));
+ return false;
+ }
+ timeOffset += path->pathAtRank (index)->length();
+ }
+ // Here, every subpath is valid.
+ validPart = path;
+ return true;
+ }
+
+ bool GraphPathValidation::impl_validate (
+ const PathPtr_t& path, bool reverse, PathPtr_t& validPart)
+ {
+ PathVectorPtr_t pathVector (path->as <PathVector> ());
+ if (pathVector)
+ return impl_validate (pathVector, reverse, validPart);
+
+ value_type tmin = path->timeRange ().first;
+ value_type tmax = path->timeRange ().second;
+ if (reverse)
+ std::swap (tmin, tmax);
+ const Path& configAt (*path);
+ graph::Nodes_t origNodes = constraintGraph_->getNode (configAt (tmin));
+ graph::Nodes_t destNodes = constraintGraph_->getNode (configAt (tmax));
+ std::vector <graph::Edges_t> possibleEdges (constraintGraph_->getEdge (origNodes, destNodes));
+ // We check for all of them if both nodes are on the same leaf.
+ ConstraintSetPtr_t constraints;
+ while (!possibleEdges.empty ()) {
+ constraints = constraintGraph_->pathConstraint (possibleEdges.back(), configAt (tmin));
+ // TODO: We need a quick way of checking that a configuration
+ // statisfies a constraint.
+ Configuration_t cfg = configAt (tmax);
+ if (constraints->apply(cfg) && ( cfg == configAt(tmax) )) {
+ validPart = path;
+ return true;
+ }
+ possibleEdges.pop_back();
+ }
+ validPart = path->extract (std::make_pair (tmin, tmin));
+ return false;
+ }
+ } // namespace manipulation
+} // namespace hpp
--
GitLab