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
34. Added remove functions for hppProblemVector
......
/** \mainpage
\section sec_intro Introduction
\section hppCore_sec_intro Introduction
This package implements basic classes used as interfaces with KineoWorks.
The main classes are:
......
......@@ -432,8 +432,30 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId)
if (!goalConfig)
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
if ( initConfig && goalConfig && hppProblem.roadmapBuilder() ) {
if (hppProblem.roadmapBuilder()) {
if(KD_OK == hppProblem.roadmapBuilder()->solveProblem( *initConfig , *goalConfig , kwsPath)) {
cout << "--- Problem solved.----" << endl;
......@@ -458,7 +480,7 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId)
// Add the path to vector of paths of the problem.
hppProblem.addPath(kwsPath);
} else {
cerr << " ChppPlanner::solveOneProblem: problem ill-defined:initConfig, GoalConfig or RoadmapBuilder not initialised" << endl ;
cerr << " ChppPlanner::solveOneProblem: no roadmap builder" << endl ;
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