Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[9801] pkg/trunk
|
From: <is...@us...> - 2009-01-20 22:26:49
|
Revision: 9801
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9801&view=rev
Author: isucan
Date: 2009-01-20 21:40:36 +0000 (Tue, 20 Jan 2009)
Log Message:
-----------
check if the starting state is already a solution in a motion plan
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/CMakeLists.txt
pkg/trunk/motion_planning/ompl/code/ompl/base/Planner.h
pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv
pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h 2009-01-20 21:40:36 UTC (rev 9801)
@@ -168,56 +168,81 @@
static_cast<StateValidityPredicate*>(psetup->si->getStateValidityChecker())->setPoseConstraints(cstrs);
}
- /** Compute the actual motion plan */
- void computePlan(RKPPlannerSetup *psetup, int times, double allowed_time, bool interpolate,
+ /** Compute the actual motion plan. Return true if computed plan was trivial (start state already in goal region) */
+ bool computePlan(RKPPlannerSetup *psetup, int times, double allowed_time, bool interpolate,
ompl::SpaceInformationKinematic::PathKinematic_t &bestPath, double &bestDifference)
{
if (times <= 0)
{
ROS_ERROR("Request specifies motion plan cannot be computed %d times", times);
- return;
+ return false;
}
- /* do the planning */
- bestPath = NULL;
- bestDifference = 0.0;
- double totalTime = 0.0;
- ompl::SpaceInformation::Goal_t goal = psetup->si->getGoal();
+ unsigned int t_index = 0;
+ double t_distance = 0.0;
+ bool result = psetup->mp->isTrivial(&t_index, &t_distance);
- for (int i = 0 ; i < times ; ++i)
+ if (result)
{
- ros::Time startTime = ros::Time::now();
- bool ok = psetup->mp->solve(allowed_time);
- double tsolve = (ros::Time::now() - startTime).to_double();
- ROS_INFO("%s Motion planner spend %g seconds", (ok ? "[Success]" : "[Failure]"), tsolve);
- totalTime += tsolve;
+ ROS_INFO("Solution already achieved");
+ bestDifference = t_distance;
+
+ /* we want to maintain the invariant that a path will
+ at least consist of start & goal states, so we copy
+ the start state twice */
+ bestPath = new ompl::SpaceInformationKinematic::PathKinematic(psetup->si);
- /* do path smoothing */
- if (ok)
+ ompl::SpaceInformationKinematic::StateKinematic_t s0 = new ompl::SpaceInformationKinematic::StateKinematic(psetup->si->getStateDimension());
+ ompl::SpaceInformationKinematic::StateKinematic_t s1 = new ompl::SpaceInformationKinematic::StateKinematic(psetup->si->getStateDimension());
+ psetup->si->copyState(s0, static_cast<ompl::SpaceInformationKinematic::StateKinematic_t>(psetup->si->getStartState(t_index)));
+ psetup->si->copyState(s1, static_cast<ompl::SpaceInformationKinematic::StateKinematic_t>(psetup->si->getStartState(t_index)));
+ bestPath->states.push_back(s0);
+ bestPath->states.push_back(s1);
+ }
+ else
+ {
+ /* do the planning */
+ bestPath = NULL;
+ bestDifference = 0.0;
+ double totalTime = 0.0;
+ ompl::SpaceInformation::Goal_t goal = psetup->si->getGoal();
+
+ for (int i = 0 ; i < times ; ++i)
{
ros::Time startTime = ros::Time::now();
- ompl::SpaceInformationKinematic::PathKinematic_t path = static_cast<ompl::SpaceInformationKinematic::PathKinematic_t>(goal->getSolutionPath());
- psetup->smoother->smoothMax(path);
- double tsmooth = (ros::Time::now() - startTime).to_double();
- ROS_INFO(" Smoother spent %g seconds (%g seconds in total)", tsmooth, tsmooth + tsolve);
- if (interpolate)
- psetup->si->interpolatePath(path);
- if (bestPath == NULL || bestDifference > goal->getDifference() ||
- (bestPath && bestDifference == goal->getDifference() && bestPath->states.size() > path->states.size()))
+ bool ok = psetup->mp->solve(allowed_time);
+ double tsolve = (ros::Time::now() - startTime).to_double();
+ ROS_INFO("%s Motion planner spend %g seconds", (ok ? "[Success]" : "[Failure]"), tsolve);
+ totalTime += tsolve;
+
+ /* do path smoothing */
+ if (ok)
{
- if (bestPath)
- delete bestPath;
- bestPath = path;
- bestDifference = goal->getDifference();
- goal->forgetSolutionPath();
- ROS_INFO(" Obtained better solution");
+ ros::Time startTime = ros::Time::now();
+ ompl::SpaceInformationKinematic::PathKinematic_t path = static_cast<ompl::SpaceInformationKinematic::PathKinematic_t>(goal->getSolutionPath());
+ psetup->smoother->smoothMax(path);
+ double tsmooth = (ros::Time::now() - startTime).to_double();
+ ROS_INFO(" Smoother spent %g seconds (%g seconds in total)", tsmooth, tsmooth + tsolve);
+ if (interpolate)
+ psetup->si->interpolatePath(path);
+ if (bestPath == NULL || bestDifference > goal->getDifference() ||
+ (bestPath && bestDifference == goal->getDifference() && bestPath->states.size() > path->states.size()))
+ {
+ if (bestPath)
+ delete bestPath;
+ bestPath = path;
+ bestDifference = goal->getDifference();
+ goal->forgetSolutionPath();
+ ROS_INFO(" Obtained better solution");
+ }
}
+ psetup->mp->clear();
}
- psetup->mp->clear();
+
+ ROS_INFO("Total planning time: %g; Average planning time: %g", totalTime, (totalTime / (double)times));
}
-
- ROS_INFO("\nTotal planning time: %g; Average planning time: %g", totalTime, (totalTime / (double)times));
+ return result;
}
void fillSolution(RKPPlannerSetup *psetup, ompl::SpaceInformationKinematic::PathKinematic_t bestPath, double bestDifference,
@@ -252,7 +277,7 @@
psetup->si->clearStartStates();
}
- bool execute(ModelMap &models, _R &req, robot_msgs::KinematicPath &path, double &distance)
+ bool execute(ModelMap &models, _R &req, robot_msgs::KinematicPath &path, double &distance, bool &trivial)
{
// make sure the same motion planner instance is not used by other calls
boost::mutex::scoped_lock(m_lock);
@@ -286,7 +311,7 @@
double bestDifference = 0.0;
m->collisionSpace->lock();
- computePlan(psetup, req.times, req.allowed_time, req.interpolate, bestPath, bestDifference);
+ trivial = computePlan(psetup, req.times, req.allowed_time, req.interpolate, bestPath, bestDifference);
m->collisionSpace->unlock();
/* fill in the results */
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-01-20 21:40:36 UTC (rev 9801)
@@ -256,21 +256,24 @@
{
robot_msgs::KinematicPath solution;
unsigned int step = 0;
- while (m_replanning)
+ bool trivial = false;
+
+ while (m_replanning && !trivial)
{
step++;
ROS_INFO("Replanning step %d", step);
- m_continueReplanningLock.lock();
+ boost::mutex::scoped_lock lock(m_continueReplanningLock);
m_collisionMonitorChange = false;
double distance = 0.0;
const double *start_state = m_robotState->getParams();
for (unsigned int i = 0 ; i < m_kmodel->stateDimension ; ++i)
m_currentPlanToStateRequest.start_state.vals[i] = start_state[i];
- m_requestState.execute(m_models, m_currentPlanToStateRequest, solution, distance);
+ m_requestState.execute(m_models, m_currentPlanToStateRequest, solution, distance, trivial);
publish("path_to_goal", solution);
+ if (trivial)
+ break;
while (!m_collisionMonitorChange)
m_collisionMonitorCondition.wait(m_continueReplanningLock);
- m_continueReplanningLock.unlock();
}
}
@@ -279,22 +282,24 @@
{
robot_msgs::KinematicPath solution;
unsigned int step = 0;
-
+ bool trivial = false;
+
while (m_replanning)
{
step++;
ROS_INFO("Replanning step %d", step);
- m_continueReplanningLock.lock();
+ boost::mutex::scoped_lock lock(m_continueReplanningLock);
m_collisionMonitorChange = false;
double distance = 0.0;
const double *start_state = m_robotState->getParams();
for (unsigned int i = 0 ; i < m_kmodel->stateDimension ; ++i)
m_currentPlanToPositionRequest.start_state.vals[i] = start_state[i];
- m_requestLinkPosition.execute(m_models, m_currentPlanToPositionRequest, solution, distance);
+ m_requestLinkPosition.execute(m_models, m_currentPlanToPositionRequest, solution, distance, trivial);
publish("path_to_goal", solution);
+ if (trivial)
+ break;
while (!m_collisionMonitorChange)
m_collisionMonitorCondition.wait(m_continueReplanningLock);
- m_continueReplanningLock.unlock();
}
}
@@ -441,7 +446,8 @@
//Acutally run the service.
- bool r = m_requestState.execute(m_models, req, res.path, res.distance);
+ bool trivial = false;
+ bool r = m_requestState.execute(m_models, req, res.path, res.distance, trivial);
//Copy the path.
@@ -483,14 +489,20 @@
bool planToState(robot_srvs::KinematicPlanState::request &req, robot_srvs::KinematicPlanState::response &res)
{
- ROS_INFO("\nRequest for planning to a state");
- return m_requestState.execute(m_models, req, res.path, res.distance);
+ ROS_INFO("Request for planning to a state");
+ bool trivial = false;
+ bool result = m_requestState.execute(m_models, req, res.path, res.distance, trivial);
+ res.trivial = trivial ? 1 : 0;
+ return result;
}
bool planToPosition(robot_srvs::KinematicPlanLinkPosition::request &req, robot_srvs::KinematicPlanLinkPosition::response &res)
{
- ROS_INFO("\nRequest for planning to a position");
- return m_requestLinkPosition.execute(m_models, req, res.path, res.distance);
+ ROS_INFO("Request for planning to a position");
+ bool trivial = false;
+ bool result = m_requestLinkPosition.execute(m_models, req, res.path, res.distance, trivial);
+ res.trivial = trivial ? 1 : 0;
+ return result;
}
virtual void setRobotDescription(robot_desc::URDF *file)
Modified: pkg/trunk/motion_planning/ompl/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/ompl/CMakeLists.txt 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/motion_planning/ompl/CMakeLists.txt 2009-01-20 21:40:36 UTC (rev 9801)
@@ -4,6 +4,7 @@
include_directories(${PROJECT_SOURCE_DIR}/code)
rospack_add_library(ompl code/ompl/base/util/src/time.cpp
code/ompl/base/util/src/output.cpp
+ code/ompl/base/src/Planner.cpp
code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
code/ompl/extension/samplingbased/kinematic/src/PathSmootherKinematic.cpp
code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
Modified: pkg/trunk/motion_planning/ompl/code/ompl/base/Planner.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/Planner.h 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/Planner.h 2009-01-20 21:40:36 UTC (rev 9801)
@@ -86,6 +86,13 @@
return m_type;
}
+ /** A problem is trivial if the given starting state already
+ in the goal region, so we need no motion planning. startID
+ will be set to the index of the starting state that
+ satisfies the goal. The distance to the goal can
+ optionally be returned as well. */
+ virtual bool isTrivial(unsigned int *startID = NULL, double *distance = NULL);
+
protected:
SpaceInformation_t m_si;
Modified: pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h 2009-01-20 21:40:36 UTC (rev 9801)
@@ -39,6 +39,7 @@
#include "ompl/base/General.h"
#include <vector>
+#include <cassert>
/** Main namespace */
namespace ompl
@@ -357,9 +358,16 @@
/** Perform additional setup tasks (run once, before use) */
virtual void setup(void)
{
+ assert(m_stateValidityChecker);
m_setup = true;
}
+ /** Check if a given state is valid or not */
+ bool isValid(const State_t state)
+ {
+ return (*m_stateValidityChecker)(state);
+ }
+
protected:
bool m_setup;
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2009-01-20 21:40:36 UTC (rev 9801)
@@ -249,9 +249,6 @@
/** Insert states in a path, at the collision checking resolution */
virtual void interpolatePath(PathKinematic_t path, double factor = 1.0);
-
- /** Check if a given state is valid or not */
- bool isValid(const StateKinematic_t state);
/** Print information about the current instance of the state space */
virtual void printSettings(std::ostream &out = std::cout) const;
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2009-01-20 21:40:36 UTC (rev 9801)
@@ -107,7 +107,6 @@
{
assert(m_stateDimension > 0);
assert(m_stateComponent.size() == m_stateDimension);
- assert(m_stateValidityChecker);
assert(m_stateDistanceEvaluator);
SpaceInformation::setup();
}
@@ -298,11 +297,6 @@
{
return (*m_stateDistanceEvaluator)(static_cast<const State_t>(s1), static_cast<const State_t>(s2));
}
-
-bool ompl::SpaceInformationKinematic::isValid(const StateKinematic_t state)
-{
- return (*m_stateValidityChecker)(static_cast<const State_t>(state));
-}
void ompl::SpaceInformationKinematic::printSettings(std::ostream &out) const
{
Modified: pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv 2009-01-20 21:40:36 UTC (rev 9801)
@@ -50,3 +50,7 @@
# it may return a distance larger than the desired threshold. The user may choose to
# discard the path
float64 distance
+
+# If the starting state was already in the goal region,
+# this is set to 1. Otherwise, it will be set to 0
+byte trivial
Modified: pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanState.srv 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanState.srv 2009-01-20 21:40:36 UTC (rev 9801)
@@ -52,3 +52,8 @@
# it may return a distance larger than the desired threshold. The user may choose to
# discard the path
float64 distance
+
+
+# If the starting state was already in the goal region,
+# this is set to 1. Otherwise, it will be set to 0
+byte trivial
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
