Commit 67c12097 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Fix SEGV when solving a problem with qgoal == qinit

parent 16d2b2bd
......@@ -48,7 +48,7 @@ namespace hpp {
{
}
PathVectorPtr_t solution ()
void solution (PathVectorPtr_t sol)
{
NodePtr_t node = findPath ();
Edges_t edges;
......@@ -62,16 +62,11 @@ namespace hpp {
}
else node = NodePtr_t (0x0);
}
PathVectorPtr_t pathVector;
for (Edges_t::const_iterator itEdge = edges.begin ();
itEdge != edges.end (); ++itEdge) {
const PathPtr_t& path ((*itEdge)->path ());
if (!pathVector)
pathVector = PathVector::create (path->outputSize (),
path->outputDerivativeSize ());
pathVector->appendPath (path);
}
return pathVector;
for (Edges_t::const_iterator itEdge = edges.begin ();
itEdge != edges.end (); ++itEdge) {
const PathPtr_t& path ((*itEdge)->path ());
sol->appendPath (path);
}
}
private:
......
......@@ -58,7 +58,17 @@ namespace hpp {
PathVectorPtr_t GoalConfigurations::computePath(const RoadmapPtr_t& roadmap) const
{
Astar astar (roadmap, problem_->distance ());
return astar.solution ();
PathVectorPtr_t sol = PathVector::create (
problem_->robot()->configSize(), problem_->robot()->numberDof());
astar.solution (sol);
// This happens when q_init == q_goal
if (sol->numberPaths() == 0) {
ConfigurationPtr_t q (roadmap->initNode()->configuration ());
sol->appendPath(
(*problem_->steeringMethod()) (*q, *q)
);
}
return sol;
}
} // namespace problemTarget
} // namespace core
......
......@@ -55,7 +55,17 @@ namespace hpp {
PathVectorPtr_t TaskTarget::computePath(const RoadmapPtr_t& roadmap) const
{
Astar astar (roadmap, problem_->distance ());
return astar.solution ();
PathVectorPtr_t sol = PathVector::create (
problem_->robot()->configSize(), problem_->robot()->numberDof());
astar.solution (sol);
// This happens when q_init already satisfies the task.
if (sol->numberPaths() == 0) {
ConfigurationPtr_t q (roadmap->initNode()->configuration ());
sol->appendPath(
(*problem_->steeringMethod()) (*q, *q)
);
}
return sol;
}
} // namespace problemTarget
} // namespace core
......
Supports Markdown
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