From 1ad7f333bd7fde526df93f9977a26fef0f0bcd99 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 22 Aug 2014 19:12:00 +0200
Subject: [PATCH] Integrate GraphSteeringMethod
---
include/hpp/manipulation/problem.hh | 7 ++++++-
src/graph-path-validation.cc | 11 +++++++----
src/manipulation-planner.cc | 5 ++---
3 files changed, 15 insertions(+), 8 deletions(-)
diff --git a/include/hpp/manipulation/problem.hh b/include/hpp/manipulation/problem.hh
index 615b429..1d74c27 100644
--- a/include/hpp/manipulation/problem.hh
+++ b/include/hpp/manipulation/problem.hh
@@ -19,6 +19,7 @@
#include "hpp/manipulation/robot.hh"
#include "hpp/manipulation/graph/graph.hh"
#include "hpp/manipulation/graph-path-validation.hh"
+#include "hpp/manipulation/graph-steering-method.hh"
#include "hpp/manipulation/fwd.hh"
namespace hpp {
@@ -30,9 +31,10 @@ namespace hpp {
/// Constructor
Problem (RobotPtr_t robot) : core::Problem (robot),
- graph_()
+ graph_(), steeringMethod_ (new GraphSteeringMethod (robot))
{
Parent::pathValidation (GraphPathValidation::create (Parent::pathValidation(), graph_));
+ steeringMethod (steeringMethod_);
}
/// Set the graph of constraints
@@ -42,6 +44,7 @@ namespace hpp {
GraphPathValidationPtr_t graphValidation = pathValidation();
if (graphValidation)
graphValidation->constraintGraph(graph);
+ steeringMethod_->constraintGraph (graph);
}
/// Get the graph of constraints
@@ -66,6 +69,8 @@ namespace hpp {
private:
/// The graph of constraints
graph::GraphPtr_t graph_;
+ /// Steering method
+ GraphSteeringMethodPtr_t steeringMethod_;
};
} // namespace manipulation
} // namespace hpp
diff --git a/src/graph-path-validation.cc b/src/graph-path-validation.cc
index f27f669..66089c1 100644
--- a/src/graph-path-validation.cc
+++ b/src/graph-path-validation.cc
@@ -33,9 +33,10 @@ namespace hpp {
bool GraphPathValidation::validate (
const PathPtr_t& path, bool reverse, PathPtr_t& validPart)
{
+ assert (path);
PathPtr_t pathGraphValid;
bool graphValid = impl_validate (path, reverse, pathGraphValid);
- bool collisionValid = pathValidation_->validate (pathGraphValid, reverse, validPart);
+ bool collisionValid = pathValidation_->validate (pathGraphValid, reverse, validPart);
return graphValid && collisionValid;
}
@@ -45,18 +46,20 @@ namespace hpp {
size_t start = 0,
end = path->numberPaths ();
int inc = 1;
+ value_type timeOffset = path->timeRange().first;
if (reverse) {
std::swap (start, end);
+ start--;end--;
inc = -1;
+ timeOffset = path->timeRange ().second;
}
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,
+ std::make_pair (timeOffset - validSubPart->timeRange().second,
path->timeRange().second));
else
validPart = path->extract (
@@ -64,7 +67,7 @@ namespace hpp {
timeOffset + validSubPart->timeRange().second));
return false;
}
- timeOffset += path->pathAtRank (index)->length();
+ timeOffset += inc * path->pathAtRank (index)->length();
}
// Here, every subpath is valid.
validPart = path;
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 78b51e6..1bfddb0 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -107,9 +107,9 @@ namespace hpp {
return false;
core::SteeringMethodPtr_t sm (problem().steeringMethod());
core::PathPtr_t path = (*sm) (*q_near, qProj_);
- path->constraints (graph->pathConstraint (edges, *q_near));
if (!path)
return false;
+ path->constraints (graph->pathConstraint (edges, *q_near));
core::PathValidationPtr_t pathValidation (problem ().pathValidation ());
pathValidation->validate (path, false, validPath);
return true;
@@ -138,8 +138,7 @@ namespace hpp {
if (possibleEdges.empty())
continue;
path = (*sm) (*q1, *q2);
- path->constraints (graph->pathConstraint (possibleEdges[0], *q1));
- if (pathValidation->validate (path, false, validPath)) {
+ if (path && pathValidation->validate (path, false, validPath)) {
roadmap ()->addEdge (*itn1, *itn2, path);
core::interval_t timeRange = path->timeRange ();
roadmap ()->addEdge (*itn2, *itn1, path->extract
--
GitLab