Commit 68a10403 authored by florent's avatar florent
Browse files

In ChppPlanner::solveOneProblem, try direct path before roadmap builder.

parent a0804289
2008/03/05
35. In ChppPlanner::solveOneProblem, try direct path before roadmap builder.
hppCore 1.3 released February 18, 2008 hppCore 1.3 released February 18, 2008
34. Added remove functions for hppProblemVector 34. Added remove functions for hppProblemVector
......
/** \mainpage /** \mainpage
\section sec_intro Introduction \section hppCore_sec_intro Introduction
This package implements basic classes used as interfaces with KineoWorks. This package implements basic classes used as interfaces with KineoWorks.
The main classes are: The main classes are:
......
...@@ -432,8 +432,30 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId) ...@@ -432,8 +432,30 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId)
if (!goalConfig) if (!goalConfig)
return KD_ERROR ; return KD_ERROR ;
/*
Try first a direct path.
*/
CkwsSteeringMethodShPtr steeringMethod = hppDevice->steeringMethod();
if ( initConfig && goalConfig && steeringMethod) {
CkwsDirectPathShPtr directPath = steeringMethod->makeDirectPath(*initConfig, *goalConfig);
hppDevice->directPathValidators()->validate(*directPath);
if (directPath->isValid()) {
kwsPath = CkwsPath::create(hppDevice);
kwsPath->appendDirectPath(directPath);
// Add the path to vector of paths of the problem.
hppProblem.addPath(kwsPath);
return KD_OK;
}
} else {
cerr << " ChppPlanner::solveOneProblem: problem ill-defined:initConfig, GoalConfig or steering method not defined" << endl ;
return KD_ERROR ;
}
// solve the problem with the roadmapBuilder // solve the problem with the roadmapBuilder
if ( initConfig && goalConfig && hppProblem.roadmapBuilder() ) { if (hppProblem.roadmapBuilder()) {
if(KD_OK == hppProblem.roadmapBuilder()->solveProblem( *initConfig , *goalConfig , kwsPath)) { if(KD_OK == hppProblem.roadmapBuilder()->solveProblem( *initConfig , *goalConfig , kwsPath)) {
cout << "--- Problem solved.----" << endl; cout << "--- Problem solved.----" << endl;
...@@ -458,7 +480,7 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId) ...@@ -458,7 +480,7 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId)
// Add the path to vector of paths of the problem. // Add the path to vector of paths of the problem.
hppProblem.addPath(kwsPath); hppProblem.addPath(kwsPath);
} else { } else {
cerr << " ChppPlanner::solveOneProblem: problem ill-defined:initConfig, GoalConfig or RoadmapBuilder not initialised" << endl ; cerr << " ChppPlanner::solveOneProblem: no roadmap builder" << endl ;
return KD_ERROR ; return KD_ERROR ;
} }
......
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