Commit 0ff6fabe authored by Eiichi Yoshida's avatar Eiichi Yoshida
Browse files

Validate the directly connected path between init and goal with correct

penetration
parent 68a10403
......@@ -432,6 +432,13 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId)
if (!goalConfig)
return KD_ERROR ;
if(!hppProblem.roadmapBuilder()){
cerr << "ChppPlanner::solveOneProblem: problem Id="
<< problemId << ": Define a roadmap builder with penetration"
<< endl;
return KD_ERROR ;
}
/*
Try first a direct path.
*/
......@@ -439,13 +446,20 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId)
if ( initConfig && goalConfig && steeringMethod) {
CkwsDirectPathShPtr directPath = steeringMethod->makeDirectPath(*initConfig, *goalConfig);
hppDevice->directPathValidators()->validate(*directPath);
double penetration = hppProblem.roadmapBuilder()->penetration();
CkwsValidatorDPCollisionShPtr dpValidator = CkwsValidatorDPCollision::create(hppDevice, penetration);
dpValidator->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);
cout<<"Problem solved with direct connection. "<<endl;
return KD_OK;
}
} else {
......
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