From de513440329e42f4e360c6139b249e6f7516d9bf Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 5 Apr 2016 10:47:44 +0200
Subject: [PATCH] Add a PathValidation in Edge and simplify the choice of path
validation type in ProblemSolver
---
include/hpp/manipulation/graph/edge.hh | 10 ++++++++++
include/hpp/manipulation/problem-solver.hh | 3 +++
include/hpp/manipulation/problem.hh | 20 ++++++++++++++++++++
src/graph-path-validation.cc | 7 ++++++-
src/graph/edge.cc | 15 +++++++++++++--
src/problem-solver.cc | 16 +++++++++-------
src/problem.cc | 18 ++++++++++++++++++
7 files changed, 79 insertions(+), 10 deletions(-)
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index d4ed17e3..38925a2e 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -114,6 +114,12 @@ namespace hpp {
return steeringMethod_->get();
}
+ /// Get path validation associated to the edge.
+ const core::PathValidationPtr_t& pathValidation () const
+ {
+ return pathValidation_->get();
+ }
+
/// Get direction of the path compare to the edge
/// \return true is reverse
virtual bool direction (const core::PathPtr_t& path) const;
@@ -158,6 +164,7 @@ namespace hpp {
private:
typedef Cache < ConstraintSetPtr_t > Constraint_t;
typedef Cache < core::SteeringMethodPtr_t > SteeringMethod_t;
+ typedef Cache < core::PathValidationPtr_t > PathValidation_t;
/// See pathConstraint member function.
Constraint_t* pathConstraints_;
@@ -175,6 +182,9 @@ namespace hpp {
/// Steering method used to create paths associated to the edge
SteeringMethod_t* steeringMethod_;
+ /// Path validation associated to the edge
+ PathValidation_t* pathValidation_;
+
/// Weak pointer to itself.
EdgeWkPtr_t wkPtr_;
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index ee1fda93..2256157d 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -132,6 +132,9 @@ namespace hpp {
virtual void addFunctionToConfigProjector
(const std::string& constraintName, const std::string& functionName);
+ virtual void pathValidationType (const std::string& type,
+ const value_type& tolerance);
+
/// Create a new problem.
virtual void resetProblem ();
diff --git a/include/hpp/manipulation/problem.hh b/include/hpp/manipulation/problem.hh
index f7384192..0563b31a 100644
--- a/include/hpp/manipulation/problem.hh
+++ b/include/hpp/manipulation/problem.hh
@@ -18,6 +18,7 @@
# define HPP_MANIPULATION_PROBLEM_HH
# include <hpp/core/problem.hh>
+# include <hpp/core/problem-solver.hh>
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/device.hh"
@@ -61,12 +62,31 @@ namespace hpp {
/// Get the path validation as a GraphPathValidation
GraphPathValidationPtr_t pathValidation () const;
+ void pathValidation (const PathValidationPtr_t& pathValidation);
+
/// Get the steering method as a GraphSteeringMethod
GraphSteeringMethodPtr_t steeringMethod () const;
+ /// Build a new path validation
+ /// \note Current obstacles are added to the created object.
+ /// \todo Keep a pointer to this value to update it when a new obstacle
+ /// is added.
+ PathValidationPtr_t pathValidationFactory () const;
+
+ void setPathValidationFactory (
+ const core::PathValidationBuilder_t& factory,
+ const value_type& tol)
+ {
+ pvFactory_ = factory;
+ pvTol_ = tol;
+ }
+
private:
/// The graph of constraints
graph::GraphPtr_t graph_;
+
+ core::PathValidationBuilder_t pvFactory_;
+ value_type pvTol_;
}; // class Problem
/// \}
} // namespace manipulation
diff --git a/src/graph-path-validation.cc b/src/graph-path-validation.cc
index ba56e6f5..ffd704c5 100644
--- a/src/graph-path-validation.cc
+++ b/src/graph-path-validation.cc
@@ -90,7 +90,12 @@ namespace hpp {
return impl_validate (pathVector, reverse, validPart, report);
PathPtr_t pathNoCollision;
- if (pathValidation_->validate (path, reverse, pathNoCollision, report)) {
+ ConstraintSetPtr_t c = HPP_DYNAMIC_PTR_CAST(ConstraintSet, path->constraints());
+ PathValidationPtr_t validation (c
+ ? c->edge()->pathValidation()
+ : pathValidation_);
+
+ if (validation->validate (path, reverse, pathNoCollision, report)) {
validPart = path;
return true;
}
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index d447571f..43c142e0 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -37,7 +37,8 @@ namespace hpp {
GraphComponent (name), isShort_ (false),
pathConstraints_ (new Constraint_t()),
configConstraints_ (new Constraint_t()),
- steeringMethod_ (new SteeringMethod_t())
+ steeringMethod_ (new SteeringMethod_t()),
+ pathValidation_ (new PathValidation_t())
{}
Edge::~Edge ()
@@ -45,6 +46,7 @@ namespace hpp {
if (pathConstraints_ ) delete pathConstraints_;
if (configConstraints_) delete configConstraints_;
if (steeringMethod_ ) delete steeringMethod_;
+ if (pathValidation_ ) delete pathValidation_;
}
NodePtr_t Edge::to () const
@@ -244,9 +246,18 @@ namespace hpp {
constraint->edge (wkPtr_.lock ());
// Build steering method
- steeringMethod_->set(g->problem()->steeringMethod()
+ const ProblemPtr_t& problem (g->problem());
+ steeringMethod_->set(problem->steeringMethod()
->innerSteeringMethod()->copy());
steeringMethod_->get()->constraints (constraint);
+ // Build path validation and relative motion matrix
+ // TODO this path validation will not contain obstacles added after
+ // its creation.
+ pathValidation_->set(problem->pathValidationFactory ());
+ using core::RelativeMotion;
+ RelativeMotion::matrix_type matrix (RelativeMotion::matrix (g->robot()));
+ RelativeMotion::fromConstraint (matrix, g->robot(), constraint);
+ pathValidation_->get()->filterCollisionPairs (matrix);
return constraint;
}
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index fbc1a9e4..8c3b4347 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -82,12 +82,6 @@ namespace hpp {
{
parent_t::add<core::PathPlannerBuilder_t>
("M-RRT", ManipulationPlanner::create);
- using core::PathValidationBuilder_t;
- parent_t::add <PathValidationBuilder_t> ("Graph-Discretized",
- GraphPathValidation::create <core::DiscretizedCollisionChecking>);
- parent_t::add <PathValidationBuilder_t> ("Graph-Progressive",
- GraphPathValidation::create <
- core::continuousCollisionChecking::Progressive >);
using core::PathOptimizerBuilder_t;
parent_t::add <PathOptimizerBuilder_t> ("Graph-RandomShortcut",
GraphOptimizer::create <core::RandomShortcut>);
@@ -115,7 +109,6 @@ namespace hpp {
#endif
pathPlannerType ("M-RRT");
- pathValidationType ("Graph-Discretized", 0.05);
steeringMethodType ("Graph-SteeringMethodStraight");
}
@@ -301,6 +294,15 @@ namespace hpp {
}
}
+ void ProblemSolver::pathValidationType (const std::string& type,
+ const value_type& tolerance)
+ {
+ parent_t::pathValidationType(type, tolerance);
+ problem_->setPathValidationFactory (
+ parent_t::get<core::PathValidationBuilder_t>(type),
+ tolerance);
+ }
+
void ProblemSolver::resetRoadmap ()
{
if (!problem ())
diff --git a/src/problem.cc b/src/problem.cc
index 7758f09c..155bb89a 100644
--- a/src/problem.cc
+++ b/src/problem.cc
@@ -43,6 +43,24 @@ namespace hpp {
Parent::pathValidation());
}
+ void Problem::pathValidation (const PathValidationPtr_t& pathValidation)
+ {
+ GraphPathValidationPtr_t pv (GraphPathValidation::create (pathValidation));
+ pv->constraintGraph (graph_);
+ Parent::pathValidation (pv);
+ }
+
+ PathValidationPtr_t Problem::pathValidationFactory () const
+ {
+ PathValidationPtr_t pv (pvFactory_ (robot(), pvTol_));
+ const core::ObjectVector_t& obstacles (collisionObstacles ());
+ // Insert obstacles in path validation object
+ for (core::ObjectVector_t::const_iterator _obs = obstacles.begin ();
+ _obs != obstacles.end (); ++_obs)
+ pv->addObstacle (*_obs);
+ return pv;
+ }
+
GraphSteeringMethodPtr_t Problem::steeringMethod () const
{
return HPP_DYNAMIC_PTR_CAST (GraphSteeringMethod,
--
GitLab