Skip to content
Snippets Groups Projects
Commit 1ad7f333 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Integrate GraphSteeringMethod

parent d25d4a9e
No related branches found
No related tags found
No related merge requests found
...@@ -19,6 +19,7 @@ ...@@ -19,6 +19,7 @@
#include "hpp/manipulation/robot.hh" #include "hpp/manipulation/robot.hh"
#include "hpp/manipulation/graph/graph.hh" #include "hpp/manipulation/graph/graph.hh"
#include "hpp/manipulation/graph-path-validation.hh" #include "hpp/manipulation/graph-path-validation.hh"
#include "hpp/manipulation/graph-steering-method.hh"
#include "hpp/manipulation/fwd.hh" #include "hpp/manipulation/fwd.hh"
namespace hpp { namespace hpp {
...@@ -30,9 +31,10 @@ namespace hpp { ...@@ -30,9 +31,10 @@ namespace hpp {
/// Constructor /// Constructor
Problem (RobotPtr_t robot) : core::Problem (robot), Problem (RobotPtr_t robot) : core::Problem (robot),
graph_() graph_(), steeringMethod_ (new GraphSteeringMethod (robot))
{ {
Parent::pathValidation (GraphPathValidation::create (Parent::pathValidation(), graph_)); Parent::pathValidation (GraphPathValidation::create (Parent::pathValidation(), graph_));
steeringMethod (steeringMethod_);
} }
/// Set the graph of constraints /// Set the graph of constraints
...@@ -42,6 +44,7 @@ namespace hpp { ...@@ -42,6 +44,7 @@ namespace hpp {
GraphPathValidationPtr_t graphValidation = pathValidation(); GraphPathValidationPtr_t graphValidation = pathValidation();
if (graphValidation) if (graphValidation)
graphValidation->constraintGraph(graph); graphValidation->constraintGraph(graph);
steeringMethod_->constraintGraph (graph);
} }
/// Get the graph of constraints /// Get the graph of constraints
...@@ -66,6 +69,8 @@ namespace hpp { ...@@ -66,6 +69,8 @@ namespace hpp {
private: private:
/// The graph of constraints /// The graph of constraints
graph::GraphPtr_t graph_; graph::GraphPtr_t graph_;
/// Steering method
GraphSteeringMethodPtr_t steeringMethod_;
}; };
} // namespace manipulation } // namespace manipulation
} // namespace hpp } // namespace hpp
...@@ -33,9 +33,10 @@ namespace hpp { ...@@ -33,9 +33,10 @@ namespace hpp {
bool GraphPathValidation::validate ( bool GraphPathValidation::validate (
const PathPtr_t& path, bool reverse, PathPtr_t& validPart) const PathPtr_t& path, bool reverse, PathPtr_t& validPart)
{ {
assert (path);
PathPtr_t pathGraphValid; PathPtr_t pathGraphValid;
bool graphValid = impl_validate (path, reverse, 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; return graphValid && collisionValid;
} }
...@@ -45,18 +46,20 @@ namespace hpp { ...@@ -45,18 +46,20 @@ namespace hpp {
size_t start = 0, size_t start = 0,
end = path->numberPaths (); end = path->numberPaths ();
int inc = 1; int inc = 1;
value_type timeOffset = path->timeRange().first;
if (reverse) { if (reverse) {
std::swap (start, end); std::swap (start, end);
start--;end--;
inc = -1; inc = -1;
timeOffset = path->timeRange ().second;
} }
PathPtr_t validSubPart; PathPtr_t validSubPart;
value_type timeOffset = path->timeRange().first;
for (size_t index = start; index != end; index += inc) { for (size_t index = start; index != end; index += inc) {
// We should stop at the first non valid subpath. // We should stop at the first non valid subpath.
if (!impl_validate (path->pathAtRank (index), reverse, validSubPart)) { if (!impl_validate (path->pathAtRank (index), reverse, validSubPart)) {
if (reverse) if (reverse)
validPart = path->extract ( validPart = path->extract (
std::make_pair (timeOffset + validSubPart->timeRange().first, std::make_pair (timeOffset - validSubPart->timeRange().second,
path->timeRange().second)); path->timeRange().second));
else else
validPart = path->extract ( validPart = path->extract (
...@@ -64,7 +67,7 @@ namespace hpp { ...@@ -64,7 +67,7 @@ namespace hpp {
timeOffset + validSubPart->timeRange().second)); timeOffset + validSubPart->timeRange().second));
return false; return false;
} }
timeOffset += path->pathAtRank (index)->length(); timeOffset += inc * path->pathAtRank (index)->length();
} }
// Here, every subpath is valid. // Here, every subpath is valid.
validPart = path; validPart = path;
......
...@@ -107,9 +107,9 @@ namespace hpp { ...@@ -107,9 +107,9 @@ namespace hpp {
return false; return false;
core::SteeringMethodPtr_t sm (problem().steeringMethod()); core::SteeringMethodPtr_t sm (problem().steeringMethod());
core::PathPtr_t path = (*sm) (*q_near, qProj_); core::PathPtr_t path = (*sm) (*q_near, qProj_);
path->constraints (graph->pathConstraint (edges, *q_near));
if (!path) if (!path)
return false; return false;
path->constraints (graph->pathConstraint (edges, *q_near));
core::PathValidationPtr_t pathValidation (problem ().pathValidation ()); core::PathValidationPtr_t pathValidation (problem ().pathValidation ());
pathValidation->validate (path, false, validPath); pathValidation->validate (path, false, validPath);
return true; return true;
...@@ -138,8 +138,7 @@ namespace hpp { ...@@ -138,8 +138,7 @@ namespace hpp {
if (possibleEdges.empty()) if (possibleEdges.empty())
continue; continue;
path = (*sm) (*q1, *q2); path = (*sm) (*q1, *q2);
path->constraints (graph->pathConstraint (possibleEdges[0], *q1)); if (path && pathValidation->validate (path, false, validPath)) {
if (pathValidation->validate (path, false, validPath)) {
roadmap ()->addEdge (*itn1, *itn2, path); roadmap ()->addEdge (*itn1, *itn2, path);
core::interval_t timeRange = path->timeRange (); core::interval_t timeRange = path->timeRange ();
roadmap ()->addEdge (*itn2, *itn1, path->extract roadmap ()->addEdge (*itn2, *itn1, path->extract
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment