Commit 7ed0d440 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[Refactoring] Update path planners to API change.

parent b5c2124a
......@@ -49,7 +49,7 @@ namespace hpp {
/// Store weak pointer to itself
void init (const BiRRTPlannerWkPtr_t& weak);
PathPtr_t extendInternal (const SteeringMethodPtr_t& sm, Configuration_t& qProj_, const NodePtr_t& near,
const ConfigurationPtr_t& target, bool reverse=false);
const Configuration_t& target, bool reverse=false);
ConfigurationShooterPtr_t configurationShooter_;
ConnectedComponentPtr_t startComponent_;
......
......@@ -50,7 +50,7 @@ namespace hpp {
/// \param near node in the roadmap,
/// \param target target configuration
virtual PathPtr_t extend (const NodePtr_t& near,
const ConfigurationPtr_t& target);
const Configuration_t& target);
private:
ConfigurationShooterPtr_t configurationShooter_;
mutable Configuration_t qProj_;
......
......@@ -58,13 +58,16 @@ namespace hpp {
/// Return true if the configuration is visible from the given
/// connected component.
bool visibleFromCC (const ConfigurationPtr_t q,
bool visibleFromCC (const Configuration_t q,
const ConnectedComponentPtr_t cc);
/// Apply the problem constraints on a given configuration qTo by
/// projecting it on the tangent space of qFrom.
ConfigurationPtr_t applyConstraints (const ConfigurationPtr_t qFrom,
const ConfigurationPtr_t qTo);
void applyConstraints (
const Configuration_t& qFrom,
const Configuration_t& qTo,
Configuration_t& qOut);
bool constrApply_; // True if applyConstraints has successed
};
/// \}
......
......@@ -69,7 +69,7 @@ namespace hpp {
}
PathPtr_t BiRRTPlanner::extendInternal (const SteeringMethodPtr_t& sm, Configuration_t& qProj_, const NodePtr_t& near,
const ConfigurationPtr_t& target, bool reverse)
const Configuration_t& target, bool reverse)
{
const ConstraintSetPtr_t& constraints (sm->constraints ());
if (constraints)
......@@ -77,12 +77,12 @@ namespace hpp {
ConfigProjectorPtr_t configProjector (constraints->configProjector ());
if (configProjector)
{
configProjector->projectOnKernel (*(near->configuration ()), *target,
configProjector->projectOnKernel (*(near->configuration ()), target,
qProj_);
}
else
{
qProj_ = *target;
qProj_ = target;
}
if (constraints->apply (qProj_))
{
......@@ -93,7 +93,7 @@ namespace hpp {
return PathPtr_t ();
}
}
return reverse ? (*sm) (*target, *(near->configuration ())) : (*sm) (*(near->configuration ()), *target);
return reverse ? (*sm) (target, *(near->configuration ())) : (*sm) (*(near->configuration ()), target);
}
......@@ -118,7 +118,8 @@ namespace hpp {
bool startComponentConnected(false), pathValidFromStart(false);
ConfigurationPtr_t q_new;
// first try to connect to start component
ConfigurationPtr_t q_rand = configurationShooter_->shoot ();
Configuration_t q_rand;
configurationShooter_->shoot (q_rand);
near = roadmap()->nearestNode (q_rand, startComponent_, distance);
path = extendInternal (problem().steeringMethod(), qProj_, near, q_rand);
if (path)
......
......@@ -93,26 +93,26 @@ namespace hpp {
}
PathPtr_t DiffusingPlanner::extend (const NodePtr_t& near,
const ConfigurationPtr_t& target)
const Configuration_t& target)
{
const SteeringMethodPtr_t& sm (problem ().steeringMethod ());
const ConstraintSetPtr_t& constraints (sm->constraints ());
if (constraints) {
ConfigProjectorPtr_t configProjector (constraints->configProjector ());
if (configProjector) {
configProjector->projectOnKernel (*(near->configuration ()), *target,
configProjector->projectOnKernel (*(near->configuration ()), target,
qProj_);
} else {
qProj_ = *target;
qProj_ = target;
}
if (!constraints->apply (qProj_)) {
return PathPtr_t ();
}
} else {
qProj_ = *target;
qProj_ = target;
}
// Here, qProj_ is a configuration that satisfies the constraints
// or *target if there are no constraints.
// or target if there are no constraints.
PathPtr_t path = (*sm) (*(near->configuration ()), qProj_);
PathProjectorPtr_t pp = problem ().pathProjector();
if (pp) {
......@@ -161,7 +161,8 @@ namespace hpp {
Nodes_t newNodes, nearestNeighbors;
PathPtr_t validPath, path;
// Pick a random node
ConfigurationPtr_t q_rand = configurationShooter_->shoot ();
Configuration_t q_rand;
configurationShooter_->shoot (q_rand);
//
// First extend each connected component toward q_rand
//
......
......@@ -196,7 +196,7 @@ namespace hpp {
itCC != roadmap ()->connectedComponents ().end (); ++itCC) {
if (*itCC != initCC) {
value_type d;
NodePtr_t near (nn->search (initNode->configuration (),
NodePtr_t near (nn->search (*initNode->configuration (),
*itCC, d, true));
assert (near);
ConfigurationPtr_t q1 (initNode->configuration ());
......@@ -226,7 +226,7 @@ namespace hpp {
itCC != roadmap ()->connectedComponents ().end (); ++itCC) {
if (*itCC != goalCC) {
value_type d;
NodePtr_t near (nn->search ((*itn)->configuration (), *itCC, d,
NodePtr_t near (nn->search (*(*itn)->configuration (), *itCC, d,
false));
assert (near);
ConfigurationPtr_t q1 (near->configuration ());
......
......@@ -94,7 +94,7 @@ namespace hpp {
void kPrmStar::generateRandomConfig ()
{
// shoot a valid random configuration
ConfigurationPtr_t qrand;
Configuration_t qrand;
// Report of configuration validation: unused here
ValidationReportPtr_t validationReport;
// Configuration validation methods associated to the problem
......@@ -111,10 +111,10 @@ namespace hpp {
bool valid (false);
// After 10000 trials throw if no valid configuration has been found.
do {
qrand = shooter->shoot ();
valid = (!constraints || constraints->apply (*qrand));
shooter->shoot (qrand);
valid = (!constraints || constraints->apply (qrand));
if (valid)
valid = configValidations->validate (*qrand, validationReport);
valid = configValidations->validate (qrand, validationReport);
nbTry++;
} while (!valid && nbTry < 10000);
if (!valid) {
......@@ -126,7 +126,7 @@ namespace hpp {
state_ = LINK_NODES;
linkingNodeIt_ = r->nodes ().begin ();
neighbors_ =roadmap ()->nearestNodes
((*linkingNodeIt_)->configuration (), numberNeighbors_);
(*(*linkingNodeIt_)->configuration (), numberNeighbors_);
itNeighbor_ = neighbors_.begin ();
}
}
......
......@@ -66,7 +66,7 @@ namespace hpp {
weakPtr_ = weak;
}
bool VisibilityPrmPlanner::visibleFromCC (const ConfigurationPtr_t q,
bool VisibilityPrmPlanner::visibleFromCC (const Configuration_t q,
const ConnectedComponentPtr_t cc){
PathPtr_t validPart;
bool found = false;
......@@ -80,7 +80,7 @@ namespace hpp {
n_it != cc->nodes ().end (); ++n_it){
if(nodeStatus_ [*n_it]){// only iterate on guard nodes
ConfigurationPtr_t qCC = (*n_it)->configuration ();
PathPtr_t path = (*sm) (*q, *qCC);
PathPtr_t path = (*sm) (q, *qCC);
PathValidationReportPtr_t report;
if (path && pathValidation->validate (path, false, validPart,
report)){
......@@ -88,7 +88,9 @@ namespace hpp {
if (path->length () < length) {
length = path->length ();
// Save shortest edge
delayedEdge = DelayedEdge_t (*n_it, q, path->reverse ());
delayedEdge = DelayedEdge_t (*n_it,
boost::make_shared<Configuration_t>(q),
path->reverse ());
}
found = true;
}
......@@ -102,22 +104,22 @@ namespace hpp {
else return false;
}
ConfigurationPtr_t VisibilityPrmPlanner::applyConstraints
(const ConfigurationPtr_t qFrom, const ConfigurationPtr_t qTo){
ConfigurationPtr_t qProj (new Configuration_t (*qTo));
void VisibilityPrmPlanner::applyConstraints (const Configuration_t& qFrom,
const Configuration_t& qTo, Configuration_t& qout)
{
ConstraintSetPtr_t constraints (problem ().constraints ());
if (constraints) {
ConfigProjectorPtr_t configProjector (constraints->configProjector ());
if (configProjector) {
constrApply_ = false; // while apply has not successed
configProjector->projectOnKernel (*qFrom, *qTo, *qProj);
if (constraints->apply (*qProj)) {
constrApply_ = false; // while apply has not successed
configProjector->projectOnKernel (qFrom, qTo, qout);
if (constraints->apply (qout)) {
constrApply_ = true;
return qProj;
return;
}
}
}
return qTo;
qout = qTo;
}
void VisibilityPrmPlanner::oneStep ()
......@@ -126,11 +128,11 @@ namespace hpp {
ConfigurationShooterPtr_t configurationShooter (problem().configurationShooter());
ConfigValidationsPtr_t configValidations (problem ().configValidations());
RoadmapPtr_t r (roadmap ());
ConfigurationPtr_t q_rand;
value_type count; // number of times q has been seen
constrApply_ = true; // stay true if no constraint in Problem
ConfigurationPtr_t q_init
(new Configuration_t (*(r->initNode ()->configuration ())));
Configuration_t q_init (*(r->initNode ()->configuration ())),
q_proj (robot->configSize()),
q_rand (robot->configSize());
/* Initialization of guard status */
nodeStatus_ [r->initNode ()] = true; // init node is guard
......@@ -142,11 +144,11 @@ namespace hpp {
// Shoot random config as long as not collision-free
ValidationReportPtr_t report;
do {
q_rand = configurationShooter->shoot ();
q_rand = applyConstraints(q_init, q_rand);
robot->currentConfiguration (*q_rand);
configurationShooter->shoot (q_rand);
applyConstraints(q_init, q_rand, q_proj);
robot->currentConfiguration (q_proj);
robot->computeForwardKinematics ();
} while (!configValidations->validate (*q_rand, report) ||
} while (!configValidations->validate (q_proj, report) ||
!constrApply_);
count = 0;
......@@ -154,16 +156,16 @@ namespace hpp {
r->connectedComponents ().begin ();
itcc != r->connectedComponents ().end (); ++itcc) {
ConnectedComponentPtr_t cc = *itcc;
if (visibleFromCC (q_rand, cc)) {
if (visibleFromCC (q_proj, cc)) {
// delayedEdges_ will completed if visible
count++; // count how many times q has been seen
}
}
if (count == 0){ // q not visible from anywhere
NodePtr_t newNode = r->addNode (q_rand); // add q as a guard node
NodePtr_t newNode = r->addNode (q_proj); // add q as a guard node
nodeStatus_ [newNode] = true;
hppDout(info, "q is a guard node: " << displayConfig (*q_rand));
hppDout(info, "q is a guard node: " << displayConfig (q_proj));
}
if (count > 1){ // q visible several times
// Insert delayed edges from list and add q as a connection node
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment