Commit 34765532 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

Enable users to define several goal configurations

    In some motion planning problems like manipulation planning where the goal is defined
    as a sub-manifold, it is sometimes useful to be able to define several goal
    configurations.

    See porting notes for information
parent 9592ff02
1. Enable users to define several goal configurations.
2. Some methods that used to return ktStatus now assert and return void.
See porting notes in the documentation.
2012/02/03 Release 2.3
1. Fix parser to handle devices that are not of type hpp::model::Device.
......
INPUT = @CMAKE_SOURCE_DIR@/include \
@CMAKE_SOURCE_DIR@/doc \
@CMAKE_SOURCE_DIR@/doc/additionalDoc
/*!
\page porting Porting code from version 2.3 to 2.4
This page explains how to port client code from version 2.3 to 2.4.
\section hpp_core_porting_sec_goal_conf Goal configurations
From version 2.4 on, several goal configurations can be defined by users.
In some motion planning problems, this feature is usefull. For instance,
in manipulation planning problems, the goal is defined as a submanifold
of the configuration space. In this case, it is efficient to sample the
goal sub-manifold and to add goal configurations to the roadmap builder.
The following methods have changed or are new in 2.4
\subsection hpp_core_porting_subsec_planner hpp::core::Planner class
\li hpp::core::Planner::initConfIthProblem(unsigned int rank, const CkwsConfigShPtr& config) asserts that preconditions are satisfied and returns void.
\li hpp::core::Planner::goalConfIthProblem(unsigned int rank) const now return a vector
of shared pointers to goal configurations instead of a single shared
pointer.
\li hpp::core::Planner::goalConfIthProblem(unsigned int rank, const CkwsConfigShPtr config) has been replaced by hpp::core::Planner::addGoalConfIthProblem(unsigned int rank, const CkwsConfigShPtr& config). This new method returns void.
\li hpp::core::Planner::void resetGoalConfIthProblem (unsigned int rank) has been
added.
\subsection hpp_core_porting_subsec_problem hpp::core::Problem class
\li hpp::core::Problem::initConfig (const CkwsConfigShPtr& inConfig) asserts that
preconditions are satisfied and returns void.
\li hpp::core::Problem::goalConfig (const CkwsConfigShPtr& inConfig) has been replaced
by hpp::core::Problem::addGoalConfig (const CkwsConfigShPtr& inConfig). This latter
method returns void.
\li hpp::core::Problem::resetGoalConfig () has been added.
*/
......@@ -152,23 +152,24 @@ namespace hpp {
/// \brief Set initial configuration of i-th robot.
/// \param rank Id of robot in vector.
/// \param config the configuration
/// \return KD_OK or KD_ERROR
ktStatus initConfIthProblem(unsigned int rank,
const CkwsConfigShPtr config);
void initConfIthProblem(unsigned int rank,
const CkwsConfigShPtr& config);
/// \brief Get goal configuration of i-th robot.
/// \brief Get vector of goal configurations of i-th robot.
/// \param rank : Id of problem in vector.
/// \return the goal configuration of robot.
/// \return KD_OK or KD_ERROR
CkwsConfigShPtr goalConfIthProblem(unsigned int rank) const;
/// \return vector of goal configurations.
const std::vector<CkwsConfigShPtr>&
goalConfIthProblem(unsigned int rank) const;
/// \brief Set goal configuration of i-th robot.
/// \brief Add a goal configuration of i-th robot.
/// \param rank Id of robot in vector.
/// \param config the configuration
/// \return KD_OK or KD_ERROR
ktStatus goalConfIthProblem(unsigned int rank,
const CkwsConfigShPtr config);
void addGoalConfIthProblem(unsigned int rank,
const CkwsConfigShPtr& config);
/// Reset the set of goal configurations
void resetGoalConfIthProblem (unsigned int rank);
/// \brief Set roadmap builder of i-th problem.
/// \param rank Rank of problem in Planner::problemVector_.
......
......@@ -47,6 +47,9 @@ namespace hpp {
class Problem
{
public:
typedef std::vector <CkwsConfigShPtr>::iterator goalConfigIterator_t;
typedef std::vector <CkwsConfigShPtr>::const_iterator
goalConfigConstIterator_t;
/// \brief Create a path planning problem.
/// \param robot robot associated to the path planning problem.
/// \param penetration dynamic penetration for validating direct paths.
......@@ -73,19 +76,19 @@ namespace hpp {
return robot_;
};
/// \brief Get shared pointer to initial configuration.
/// Get shared pointer to initial configuration.
const CkwsConfigShPtr& initConfig() const {
return initConf_;
};
/// \brief Set initial configuration.
ktStatus initConfig ( const CkwsConfigShPtr& inConfig );
/// \brief Get shared pointer to goal configuration.
const CkwsConfigShPtr& goalConfig() const {
return goalConf_;
};
/// \brief Set goal configuration.
ktStatus goalConfig ( const CkwsConfigShPtr& inConfig );
/// \brief Check that problem is well formulated
/// Set initial configuration.
void initConfig ( const CkwsConfigShPtr& inConfig );
/// Get shared pointer to goal configuration.
const std::vector <CkwsConfigShPtr>& goalConfigurations () const;
/// Add goal configuration.
void addGoalConfig ( const CkwsConfigShPtr& inConfig );
/// Reset the set of goal configurations
void resetGoalConfig ();
/// Check that problem is well formulated
ktStatus checkProblem() const;
/// @}
......@@ -212,7 +215,6 @@ namespace hpp {
/// 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
......@@ -224,7 +226,7 @@ namespace hpp {
/// \brief Shared pointer to initial configuration.
CkwsConfigShPtr initConf_;
/// \brief Shared pointer to goal configuration.
CkwsConfigShPtr goalConf_;
std::vector <CkwsConfigShPtr> goalConfigurations_;
/// \brief Shared pointer to a roadmap associate to the robot
CkwsRoadmapShPtr roadmap_;
/// \brief Shared pointer to a roadmapBuilder
......
......@@ -286,52 +286,33 @@ namespace hpp {
// ======================================================================
ktStatus Planner::initConfIthProblem(unsigned int rank,
const CkwsConfigShPtr config)
void Planner::initConfIthProblem(unsigned int rank,
const CkwsConfigShPtr& config)
{
ktStatus status = KD_ERROR;
if (rank < getNbHppProblems()) {
status = problemVector_[rank].initConfig(config);
}
else {
hppDout(error, "Wrong problem id");
}
return status;
problemVector_[rank].initConfig(config);
}
// ======================================================================
CkwsConfigShPtr Planner::goalConfIthProblem(unsigned int rank) const
const std::vector<CkwsConfigShPtr>&
Planner::goalConfIthProblem(unsigned int rank) const
{
CkwsConfigShPtr config;
if (rank < getNbHppProblems()) {
config = problemVector_[rank].goalConfig();
}
else {
hppDout(error, "Wrong problem id");
}
return config;
return problemVector_ [rank].goalConfigurations ();
}
// ======================================================================
ktStatus Planner::goalConfIthProblem(unsigned int rank,
const CkwsConfigShPtr config)
void Planner::addGoalConfIthProblem(unsigned int rank,
const CkwsConfigShPtr& config)
{
ktStatus status = KD_ERROR;
problemVector_ [rank].addGoalConfig (config);
}
if (rank < getNbHppProblems()) {
status = problemVector_[rank].goalConfig(config);
}
else {
hppDout(error, "Wrong problem id");
}
// ======================================================================
return status;
void Planner::resetGoalConfIthProblem (unsigned int rank)
{
problemVector_ [rank].resetGoalConfig ();
}
// ======================================================================
......@@ -582,8 +563,6 @@ namespace hpp {
hppDout(error,"Roadmap builder should be set to define penetration.");
return KD_ERROR;
}
double penetration = hppProblem.penetration();
if (pathId >= hppProblem.getNbPaths()) {
hppDout(error, "Problem Id="
<< pathId << " is bigger than number of paths="
......
......@@ -80,7 +80,7 @@ namespace hpp {
notificator_(inProblem.notificator_),
robot_(inProblem.robot_),
initConf_(inProblem.initConf_),
goalConf_(inProblem.goalConf_),
goalConfigurations_(inProblem.goalConfigurations_),
roadmap_(inProblem.roadmap_),
roadmapBuilder_(inProblem.roadmapBuilder_),
pathOptimizer_(inProblem.pathOptimizer_),
......@@ -95,26 +95,32 @@ namespace hpp {
// ======================================================================
ktStatus Problem::initConfig ( const CkwsConfigShPtr& inConfig )
void Problem::initConfig ( const CkwsConfigShPtr& inConfig )
{
if (inConfig->device() != robot_) {
hppDout(error, "Configuration device does not match problem device.");
return KD_ERROR;
}
assert (inConfig->device() == robot_);
initConf_ = inConfig;
return KD_OK;
}
// ======================================================================
ktStatus Problem::goalConfig ( const CkwsConfigShPtr& inConfig )
void Problem::addGoalConfig (const CkwsConfigShPtr& inConfig)
{
if (inConfig->device() != robot_) {
hppDout(error, "Configuration device does not match problem device.");
return KD_ERROR;
}
goalConf_ = inConfig;
return KD_OK;
assert (inConfig->device () == robot_);
goalConfigurations_.push_back (inConfig);
}
// ======================================================================
const std::vector<CkwsConfigShPtr>& Problem::goalConfigurations () const
{
return goalConfigurations_;
};
// ======================================================================
void Problem::resetGoalConfig ()
{
goalConfigurations_.clear ();
}
// ======================================================================
......@@ -223,7 +229,7 @@ namespace hpp {
hppDout(error,"No init config in problem.");
return KD_ERROR;
}
if (!goalConfig()) {
if (!goalConfigurations ().size ()) {
hppDout(error,"No goal config in problem.");
return KD_ERROR;
}
......@@ -237,10 +243,14 @@ namespace hpp {
hppDout(error,"Define a steering method.");
return KD_ERROR;
}
// Test that goal configuration is valid
if (validateConfig(getRobot(), goalConfig()) != KD_OK) {
hppDout(error,"Goal configuration not valid.");
return KD_ERROR;
// Test that goal configurations are valid
for (goalConfigConstIterator_t it = goalConfigurations_.begin ();
it != goalConfigurations_.end (); it++) {
CkwsConfigShPtr goalConf (*it);
if (validateConfig(getRobot(), goalConf) != KD_OK) {
hppDout(error,"Found one goal configuration not valid.");
return KD_ERROR;
}
}
return KD_OK;
}
......@@ -300,7 +310,6 @@ namespace hpp {
// ======================================================================
ktStatus Problem::planPath(const CkwsConfigConstShPtr inInitConfig,
const CkwsConfigConstShPtr inGoalConfig,
const CkwsPathShPtr& inOutPath)
{
/*
......@@ -309,80 +318,91 @@ namespace hpp {
CkppSteeringMethodComponentShPtr steeringMethod =
getRobot()->steeringMethodComponent ();
CkwsDirectPathShPtr directPath =
steeringMethod->kwsSteeringMethod ()->makeDirectPath
(*inInitConfig, *inGoalConfig);
for (goalConfigIterator_t it = goalConfigurations_.begin ();
it != goalConfigurations_.end (); it++) {
CkwsConfigShPtr goalConfig = *it;
CkwsDirectPathShPtr directPath =
steeringMethod->kwsSteeringMethod ()->makeDirectPath
(*inInitConfig, *goalConfig);
if (directPath) {
/*
Retrieve validators of device
*/
CkwsValidatorSetConstShPtr dpValidators =
getRobot()->directPathValidators();
if (directPath) {
// Retrieve validators of device
CkwsValidatorSetConstShPtr dpValidators =
getRobot()->directPathValidators();
dpValidators->validate(*directPath);
if (directPath->isValid()) {
dpValidators->validate(*directPath);
if (directPath->isValid()) {
hppDout(info,"Problem solved with direct connection. ");
hppDout(info,"Problem solved with direct connection. ");
/* Add direct path to roadmap if not already included */
// Add direct path to roadmap if not already included
CkwsRoadmapShPtr roadmap = roadmapBuilder()->roadmap();
hppDout(info, "Number of edges in roadmap before inserting nodes = "
<< roadmap->countEdges());
CkwsRoadmapShPtr roadmap = roadmapBuilder()->roadmap();
hppDout(info, "Number of edges in roadmap before inserting nodes = "
<< roadmap->countEdges());
CkwsNodeShPtr startNode = roadmap->nodeWithConfig(*inInitConfig);
CkwsNodeShPtr goalNode = roadmap->nodeWithConfig(*inGoalConfig);
CkwsNodeShPtr startNode = roadmap->nodeWithConfig(*inInitConfig);
CkwsNodeShPtr goalNode = roadmap->nodeWithConfig(*goalConfig);
hppDout(info,"Number of edges in roadmap after creating nodes = "
<< roadmap->countEdges());
hppDout(info,"Number of edges in roadmap after creating nodes = "
<< 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) {
hppDout(error,"Failed to add start node in roadmap.");
startNode.reset();
// If start and goal node are not in roadmap, add them.
if (!startNode) {
startNode = CkwsNode::create(*inInitConfig);
if (roadmap->addNode(startNode) != KD_OK) {
hppDout(error,"Failed to add start node in roadmap.");
startNode.reset();
}
}
}
if (!goalNode) {
goalNode = CkwsNode::create(*inGoalConfig);
if (roadmap->addNode(goalNode) != KD_OK) {
hppDout(error,"Failed to add goal node in roadmap.");
goalNode.reset();
if (!goalNode) {
goalNode = CkwsNode::create(*goalConfig);
if (roadmap->addNode(goalNode) != KD_OK) {
hppDout(error,"Failed to add goal node in roadmap.");
goalNode.reset();
}
}
}
hppDout(info,"Number of edges in roadmap after adding nodes = "
<< roadmap->countEdges());
hppDout(info,"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 (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) {
hppDout(info,"Failed to add direct path in roadmap.");
if (roadmap->addEdge(startNode, goalNode, edge) == KD_ERROR) {
hppDout(info,"Failed to add direct path in roadmap.");
}
}
hppDout(info,
"Number edges in roadmap after attempt at adding edge = "
<< roadmap->countEdges());
}
hppDout(info,
"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;
}
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
*/
// solve the problem with the roadmapBuilder
// Set init and goal nodes.
roadmapBuilder_->resetStartNodes ();
roadmapBuilder_->resetGoalNodes ();
CkwsNodeShPtr initNode (roadmapBuilder_->roadmapNode (*initConf_));
roadmapBuilder_->addStartNode (initNode);
for (goalConfigIterator_t it = goalConfigurations_.begin ();
it != goalConfigurations_.end (); it++) {
CkwsConfigShPtr goalConfig = *it;
CkwsNodeShPtr goalNode (roadmapBuilder_->roadmapNode (*goalConfig));
roadmapBuilder_->addGoalNode (goalNode);
}
CkwsPathShPtr kwsPath = CkwsPath::create(getRobot());
if(KD_OK == roadmapBuilder()->solveProblem(*inInitConfig ,
*inGoalConfig , kwsPath)) {
std::list< CkwsEdgeShPtr > unusedEdges;
if(KD_OK == roadmapBuilder()->solveCurrentProblem(kwsPath, unusedEdges)) {
hppDout(info,"--- Problem solved.----");
} else {
hppDout(error,"---- Problem NOT solved.----");
......@@ -422,7 +442,6 @@ namespace hpp {
}
CkwsConfigShPtr initConf = initConfig();
CkwsConfigShPtr goalConf = goalConfig();
/*
Test that initial configuration is valid
*/
......@@ -432,7 +451,7 @@ namespace hpp {
return KD_ERROR;
}
if (planPath(initConf, goalConf, solutionPath) != KD_OK) {
if (planPath(initConf, solutionPath) != KD_OK) {
hppDout(error,"Failed to plan a path between init and goal config.");
return KD_ERROR;
}
......@@ -450,7 +469,7 @@ namespace hpp {
// optimizer for the path
CkwsPathShPtr optimizedPath;
if (shouldOptimize) {
if (pathOptimizer()->plan(solutionPath, CkwsPathPlanner::STABLE_ENDS,
optimizedPath)
== KD_OK) {
......
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