Commit 2fae7321 authored by florent's avatar florent Committed by florent
Browse files

Add private method ChppProblem::planPath.

	* include/hppCore/hppProblem.h,
	* src/hppProblem.cpp:
	This method tries to solve a problem first by using
	the steering method and then by calling the roadmap builder.
parent 4eae9c4d
......@@ -318,6 +318,15 @@ class ChppProblem
ktStatus validateInitConfig(CkwsConfigShPtr& inOutInitConfig,
CkwsPathShPtr& inOutPath) const;
/**
\brief Plan a path between initial and goal configurations
First, tries a direct path and then calls the roadmap builder.
*/
ktStatus planPath(const CkwsConfigConstShPtr inInitConfig,
const CkwsConfigConstShPtr inGoalConfig,
const CkwsPathShPtr& inOutPath);
/**
\brief Set penetration of collision direct path validator of the robot
*/
......
......@@ -310,158 +310,170 @@ ktStatus ChppProblem::validateInitConfig(CkwsConfigShPtr& inOutInitConfig,
// ==========================================================================
ktStatus ChppProblem::solve()
ktStatus ChppProblem::planPath(const CkwsConfigConstShPtr inInitConfig,
const CkwsConfigConstShPtr inGoalConfig,
const CkwsPathShPtr& inOutPath)
{
if (checkProblem() != KD_OK) {
ODEBUG1(":solve: problem formulation is not correct.");
return KD_ERROR;
}
CkwsConfigShPtr initConf = initConfig();
CkwsConfigShPtr goalConf = goalConfig();
/*
Test that initial configuration is valid
*/
CkwsPathShPtr solutionPath = CkwsPath::create(getRobot());
if (validateInitConfig(initConf, solutionPath) != KD_OK) {
return KD_ERROR;
}
double penetration = roadmapBuilder()->penetration();
/*
Try first a direct path.
*/
CkwsSteeringMethodShPtr steeringMethod = getRobot()->steeringMethod();
if ( initConf && goalConf && steeringMethod) {
CkwsDirectPathShPtr directPath = steeringMethod->makeDirectPath(*initConf, *goalConf);
CkwsDirectPathShPtr directPath =
steeringMethod->makeDirectPath(*inInitConfig, *inGoalConfig);
if (directPath) {
/*
Retrieve validators of device
*/
CkwsDirectPathValidatorSetConstShPtr dpValidators =
getRobot()->directPathValidators();
dpValidators->validate(*directPath);
if (directPath->isValid()) {
ODEBUG2(":solve: Problem solved with direct connection. ");
if (directPath) {
/*
Retrieve validators of device
*/
CkwsDirectPathValidatorSetConstShPtr dpValidators =
getRobot()->directPathValidators();
/* Add direct path to roadmap if not already included */
if (roadmapBuilder()) {
CkwsRoadmapShPtr roadmap = roadmapBuilder()->roadmap();
ODEBUG2(":solve: number of edges in roadmap before inserting nodes = " << roadmap->countEdges());
dpValidators->validate(*directPath);
if (directPath->isValid()) {
CkwsNodeShPtr startNode = roadmap->nodeWithConfig(*initConf);
CkwsNodeShPtr goalNode = roadmap->nodeWithConfig(*goalConf);
ODEBUG2(":planPath: Problem solved with direct connection. ");
ODEBUG2(":solve: number of edges in roadmap after creating nodes = " << roadmap->countEdges());
/* Add direct path to roadmap if not already included */
/* If start and goal node are not in roadmap, add them. */
if (!startNode) {
startNode = CkwsNode::create(*initConf);
if (roadmap->addNode(startNode) != KD_OK) {
ODEBUG1(":solve: failed to add start node in roadmap.");
startNode.reset();
}
}
if (!goalNode) {
goalNode = CkwsNode::create(*goalConf);
if (roadmap->addNode(goalNode) != KD_OK) {
ODEBUG1(":solve: failed to add goal node in roadmap.");
goalNode.reset();
}
}
CkwsRoadmapShPtr roadmap = roadmapBuilder()->roadmap();
ODEBUG2
(":planPath: number of edges in roadmap before inserting nodes = "
<< roadmap->countEdges());
ODEBUG2(":solve: number of edges in roadmap after adding nodes = " << roadmap->countEdges());
CkwsNodeShPtr startNode = roadmap->nodeWithConfig(*inInitConfig);
CkwsNodeShPtr goalNode = roadmap->nodeWithConfig(*inGoalConfig);
if (startNode && goalNode) {
/* Add edge only if goal node is not accessible from initial node */
if (!startNode->hasTransitiveOutNode(goalNode)) {
CkwsEdgeShPtr edge=CkwsEdge::create (directPath);
ODEBUG2(":planPath: number of edges in roadmap after creating nodes = "
<< roadmap->countEdges());
if (roadmap->addEdge(startNode, goalNode, edge) == KD_ERROR) {
ODEBUG2(":solve: Failed to add direct path in roadmap.");
}
}
ODEBUG2(":solve: number of edges in roadmap after attempting at adding edge= " << roadmap->countEdges());
/* If start and goal node are not in roadmap, add them. */
if (!startNode) {
startNode = CkwsNode::create(*inInitConfig);
if (roadmap->addNode(startNode) != KD_OK) {
ODEBUG1(":planPath: failed to add start node in roadmap.");
startNode.reset();
}
}
if (!goalNode) {
goalNode = CkwsNode::create(*inGoalConfig);
if (roadmap->addNode(goalNode) != KD_OK) {
ODEBUG1(":planPath: failed to add goal node in roadmap.");
goalNode.reset();
}
}
ODEBUG2(":planPath: number of edges in roadmap after adding nodes = "
<< roadmap->countEdges());
if (startNode && goalNode) {
/* Add edge only if goal node is not accessible from initial node */
if (!startNode->hasTransitiveOutNode(goalNode)) {
CkwsEdgeShPtr edge=CkwsEdge::create (directPath);
if (roadmap->addEdge(startNode, goalNode, edge) == KD_ERROR) {
ODEBUG2(":planPath: Failed to add direct path in roadmap.");
}
}
solutionPath->appendDirectPath(directPath);
// Add the path to vector of paths of the problem.
addPath(KIT_DYNAMIC_PTR_CAST(CkwsPath, solutionPath->clone()));
return KD_OK;
ODEBUG2
(":planPath: number edges in roadmap after attempt at adding edge= "
<< roadmap->countEdges());
}
inOutPath->appendDirectPath(directPath);
// Add the path to vector of paths of the problem.
addPath(KIT_DYNAMIC_PTR_CAST(CkwsPath, inOutPath->clone()));
return KD_OK;
}
} /* if (directPath) */
/*
solve the problem with the roadmapBuilder
*/
CkwsPathShPtr kwsPath = CkwsPath::create(getRobot());
if(KD_OK == roadmapBuilder()->solveProblem(*inInitConfig ,
*inGoalConfig , kwsPath)) {
ODEBUG2(":planPath: --- Problem solved.----");
} else {
ODEBUG1(":solve: problem ill-defined:initConf, GoalConfig or steering method not defined");
return KD_ERROR ;
ODEBUG1(":planPath: ---- Problem NOT solved.----");
return KD_ERROR;
}
if (!kwsPath) {
ODEBUG1(":planPath: no path after successfully solving the problem");
ODEBUG1(":planPath: this should not happen.");
return KD_ERROR;
}
if (kwsPath->length()== 0) {
ODEBUG1
(":planPath: Path length is 0 after successfully solving the problem");
ODEBUG1(":planPath: this should not happen.");
return KD_ERROR;
}
/*
solve the problem with the roadmapBuilder
Store path before optimization
*/
if (roadmapBuilder()) {
if (inOutPath->appendPath(kwsPath) != KD_OK) {
ODEBUG1
(":planPath: failed at appending solution path to extraction path.");
return KD_ERROR;
}
addPath(CkwsPath::createCopy(inOutPath));
return KD_OK;
}
CkwsPathShPtr kwsPath = CkwsPath::create(getRobot());
// ==========================================================================
if(KD_OK == roadmapBuilder()->solveProblem( *initConf , *goalConf , kwsPath)) {
ODEBUG2(":solve: --- Problem solved.----");
} else {
ODEBUG1(":solve: ---- Problem NOT solved.----");
return KD_ERROR;
}
if (!kwsPath) {
ODEBUG1(":solve: no path after successfully solving the problem");
ODEBUG1(":solve: this should not happen.");
return KD_ERROR;
}
ktStatus ChppProblem::solve()
{
if (checkProblem() != KD_OK) {
ODEBUG1(":solve: problem formulation is not correct.");
return KD_ERROR;
}
if (kwsPath->length()== 0) {
ODEBUG1(":solve: Path length is 0 after successfully solving the problem");
ODEBUG1(":solve: this should not happen.");
return KD_ERROR;
}
CkwsConfigShPtr initConf = initConfig();
CkwsConfigShPtr goalConf = goalConfig();
/*
Test that initial configuration is valid
*/
CkwsPathShPtr solutionPath = CkwsPath::create(getRobot());
/*
Store path before optimization
*/
if (solutionPath->appendPath(kwsPath) != KD_OK) {
ODEBUG1(":solve: failed at appending solution path to extraction path.");
return KD_ERROR;
}
addPath(KIT_DYNAMIC_PTR_CAST(CkwsPath, solutionPath->clone()));
if (validateInitConfig(initConf, solutionPath) != KD_OK) {
return KD_ERROR;
}
// optimizer for the path
if (pathOptimizer()) {
if (pathOptimizer()->optimizePath(solutionPath, penetration)
== KD_OK) {
if (planPath(initConf, goalConf, solutionPath) != KD_OK) {
ODEBUG1(":solve: failed to plan a path between init and goal config.");
return KD_ERROR;
}
ODEBUG2(":solve: path optimized with penetration "
<< penetration);
}
else {
ODEBUG1(":solve: path optimization failed.");
}
double penetration = roadmapBuilder()->penetration();
} else {
ODEBUG1(":solve: no Optimizer Defined ");
}
// optimizer for the path
if (pathOptimizer()) {
if (pathOptimizer()->optimizePath(solutionPath, penetration)
== KD_OK) {
if (solutionPath) {
ODEBUG2(":solve: number of direct path: "<<kwsPath->countDirectPaths());
// Add the path to vector of paths of the problem.
addPath(KIT_DYNAMIC_PTR_CAST(CkwsPath, solutionPath->clone()));
ODEBUG2(":solve: path optimized with penetration "
<< penetration);
}
else {
ODEBUG1(":solve: path optimization failed.");
}
} else {
ODEBUG1(":solve: no roadmap builder");
return KD_ERROR ;
ODEBUG1(":solve: no Optimizer Defined ");
}
if (solutionPath) {
ODEBUG2(":solve: number of direct path: "
<< solutionPath->countDirectPaths());
// Add the path to vector of paths of the problem.
addPath(KIT_DYNAMIC_PTR_CAST(CkwsPath, solutionPath->clone()));
}
return KD_OK ;
}
......
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