Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[11392] pkg/trunk
|
From: <is...@us...> - 2009-02-19 07:28:56
|
Revision: 11392
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11392&view=rev
Author: isucan
Date: 2009-02-19 07:28:48 +0000 (Thu, 19 Feb 2009)
Log Message:
-----------
separated hillclimbig and added kpiece as default planner
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl/CMakeLists.txt
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/GAIK.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/IKPlanner.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/GAIK.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/KPIECE1.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/src/KPIECE1.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups.xml
Added Paths:
-----------
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/HCIK.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/HCIK.cpp
Modified: pkg/trunk/motion_planning/ompl/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/ompl/CMakeLists.txt 2009-02-19 07:16:15 UTC (rev 11391)
+++ pkg/trunk/motion_planning/ompl/CMakeLists.txt 2009-02-19 07:28:48 UTC (rev 11392)
@@ -3,13 +3,14 @@
rospack(ompl)
rospack_add_boost_directories()
include_directories(${PROJECT_SOURCE_DIR}/code)
-set(CMAKE_BUILD_TYPE Release)
+#set(CMAKE_BUILD_TYPE Release)
rospack_add_library(ompl code/ompl/base/util/src/time.cpp
code/ompl/base/util/src/output.cpp
code/ompl/base/util/src/random_utils.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/ik/src/HCIK.cpp
code/ompl/extension/samplingbased/kinematic/extension/ik/src/GAIK.cpp
code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
code/ompl/extension/samplingbased/kinematic/extension/rrt/src/LazyRRT.cpp
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-02-19 07:16:15 UTC (rev 11391)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2009-02-19 07:28:48 UTC (rev 11392)
@@ -67,7 +67,7 @@
}
/** Destructor */
- ~SpaceInformationKinematic(void)
+ virtual ~SpaceInformationKinematic(void)
{
}
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/GAIK.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/GAIK.h 2009-02-19 07:16:15 UTC (rev 11391)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/GAIK.h 2009-02-19 07:28:48 UTC (rev 11392)
@@ -34,11 +34,11 @@
/* \author Ioan Sucan */
-#ifndef OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_GAIK_
-#define OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_GAIK_
+#ifndef OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_IK_GAIK_
+#define OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_IK_GAIK_
-#include "ompl/base/Planner.h"
#include "ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h"
+#include "ompl/extension/samplingbased/kinematic/extension/ik/HCIK.h"
namespace ompl
{
@@ -56,17 +56,17 @@
@par External documentation
*/
- class GAIK : public Planner
+ class GAIK
{
public:
- GAIK(SpaceInformation_t si) : Planner(si)
- {
- m_type = PLAN_TO_GOAL_REGION;
+ GAIK(SpaceInformationKinematic_t si) : m_hcik(si)
+ {
+ m_si = si;
m_rho = 0.04;
m_poolSize = 80;
m_poolExpansion = 100;
- m_maxImproveSteps = 3;
+ m_hcik.setMaxImproveSteps(3);
m_checkValidity = true;
}
@@ -74,20 +74,16 @@
{
}
- virtual bool solve(double solveTime);
+ virtual bool solve(double solveTime, SpaceInformationKinematic::StateKinematic_t result);
- virtual void clear(void)
- {
- }
-
void setMaxImproveSteps(unsigned int maxSteps)
{
- m_maxImproveSteps = maxSteps;
+ m_hcik.setMaxImproveSteps(maxSteps);
}
unsigned int getMaxImproveSteps(void) const
{
- return m_maxImproveSteps;
+ return m_hcik.getMaxImproveSteps();
}
void setValidityCheck(bool valid)
@@ -135,8 +131,7 @@
protected:
- bool tryToImprove(SpaceInformationKinematic::StateKinematic_t state, double *distance);
- bool tryToImproveAux(double add, SpaceInformationKinematic::StateKinematic_t state, double *distance);
+ bool tryToImprove(SpaceInformationKinematic::StateKinematic_t state, double distance);
bool valid(SpaceInformationKinematic::StateKinematic_t state);
struct Individual
@@ -153,12 +148,16 @@
}
};
- unsigned int m_poolSize;
- unsigned int m_poolExpansion;
- unsigned int m_maxImproveSteps;
- bool m_checkValidity;
+ HCIK m_hcik;
+ SpaceInformationKinematic_t m_si;
+ unsigned int m_poolSize;
+ unsigned int m_poolExpansion;
+ unsigned int m_maxImproveSteps;
+ bool m_checkValidity;
- double m_rho;
+ double m_rho;
+
+ msg::Interface m_msg;
};
}
Added: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/HCIK.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/HCIK.h (rev 0)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/HCIK.h 2009-02-19 07:28:48 UTC (rev 11392)
@@ -0,0 +1,92 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* \author Ioan Sucan */
+
+#ifndef OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_IK_HCIK_
+#define OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_IK_HCIK_
+
+#include "ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h"
+
+namespace ompl
+{
+
+ /** Forward class declaration */
+ ForwardClassDeclaration(HCIK);
+
+ /**
+ @subsubsection HCIK Inverse Kinematics with Hill Climbing
+
+ @par Short description
+
+ HCIK does inverse kinematics with hill climbing, starting from a given state.
+
+ @par External documentation
+ */
+ class HCIK
+ {
+ public:
+
+ HCIK(SpaceInformationKinematic_t si)
+ {
+ m_si = si;
+ m_maxImproveSteps = 2;
+ }
+
+ virtual ~HCIK(void)
+ {
+ }
+
+ bool tryToImprove(SpaceInformationKinematic::StateKinematic_t state, double add, double *distance = NULL) const;
+
+ void setMaxImproveSteps(unsigned int steps)
+ {
+ m_maxImproveSteps = steps;
+ }
+
+ unsigned int getMaxImproveSteps(void) const
+ {
+ return m_maxImproveSteps;
+ }
+
+ protected:
+
+ SpaceInformationKinematic_t m_si;
+ unsigned int m_maxImproveSteps;
+
+ };
+
+}
+
+#endif
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/IKPlanner.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/IKPlanner.h 2009-02-19 07:16:15 UTC (rev 11391)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/IKPlanner.h 2009-02-19 07:28:48 UTC (rev 11392)
@@ -62,7 +62,7 @@
public:
IKPlanner(SpaceInformation_t si) : _P(si),
- m_gaik(si)
+ m_gaik(dynamic_cast<SpaceInformationKinematic_t>(si))
{
_P::m_type = _P::m_type | PLAN_TO_GOAL_REGION;
}
@@ -80,12 +80,6 @@
{
return m_gaik.getRange();
}
-
- virtual void setup(void)
- {
- m_gaik.setup();
- _P::setup();
- }
virtual bool solve(double solveTime)
{
@@ -128,7 +122,6 @@
bool solved = false;
unsigned int step = 0;
- m_gaik.clear();
while (!solved)
{
@@ -136,18 +129,14 @@
double time_left = (endTime - time_utils::Time::now()).toSeconds();
if (time_left <= 0.0)
break;
- if (m_gaik.solve(time_left * 0.6))
+ if (m_gaik.solve(time_left * 0.5, stateGoal->state))
{
- SpaceInformationKinematic::PathKinematic_t foundPath = static_cast<SpaceInformationKinematic::PathKinematic_t>(goal_r->getSolutionPath());
- assert(foundPath && foundPath->states.size() == 1);
-
/* change goal to a state one */
si->forgetGoal();
si->setGoal(stateGoal);
- si->copyState(stateGoal->state, foundPath->states[0]);
/* run _P on the new goal */
- clear();
+ _P::clear();
time_left = (endTime - time_utils::Time::now()).toSeconds();
_P::m_msg.inform("IKPlanner: Using GAIK goal state for the planner (step %u, %g seconds remaining)", step, time_left);
solved = _P::solve(time_left);
@@ -159,9 +148,12 @@
/* copy solution to actual goal instance */
if (solved)
{
- if (goal_r->isApproximate())
+ double dist = -1.0;
+ bool approx = !goal_r->isSatisfied(stateGoal->state, &dist);
+ if (approx)
_P::m_msg.warn("IKPlanner: Found approximate solution");
- goal_r->setSolutionPath(stateGoal->getSolutionPath(), goal_r->isApproximate());
+ goal_r->setSolutionPath(stateGoal->getSolutionPath(), approx);
+ goal_r->setDifference(dist);
stateGoal->forgetSolutionPath();
}
else
@@ -172,14 +164,7 @@
delete stateGoal;
return solved;
}
-
-
- virtual void clear(void)
- {
- m_gaik.clear();
- _P::clear();
- }
-
+
protected:
GAIK m_gaik;
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/GAIK.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/GAIK.cpp 2009-02-19 07:16:15 UTC (rev 11391)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/GAIK.cpp 2009-02-19 07:28:48 UTC (rev 11392)
@@ -39,14 +39,13 @@
bool ompl::GAIK::valid(SpaceInformationKinematic::StateKinematic_t state)
{
- return m_checkValidity ? m_si->isValid(static_cast<SpaceInformationKinematic::StateKinematic_t>(state)) : true;
+ return m_checkValidity ? m_si->isValid(static_cast<SpaceInformation::State_t>(state)) : true;
}
-bool ompl::GAIK::solve(double solveTime)
+bool ompl::GAIK::solve(double solveTime, SpaceInformationKinematic::StateKinematic_t result)
{
- SpaceInformationKinematic_t si = dynamic_cast<SpaceInformationKinematic_t>(m_si);
- SpaceInformationKinematic::GoalRegionKinematic_t goal_r = dynamic_cast<SpaceInformationKinematic::GoalRegionKinematic_t>(si->getGoal());
- unsigned int dim = si->getStateDimension();
+ SpaceInformationKinematic::GoalRegionKinematic_t goal_r = dynamic_cast<SpaceInformationKinematic::GoalRegionKinematic_t>(m_si->getGoal());
+ unsigned int dim = m_si->getStateDimension();
if (!goal_r)
{
@@ -71,7 +70,7 @@
for (unsigned int i = 0 ; i < maxPoolSize ; ++i)
{
pool[i].state = new SpaceInformationKinematic::StateKinematic(dim);
- si->sample(pool[i].state);
+ m_si->sample(pool[i].state);
if (goal_r->isSatisfied(pool[i].state, &(pool[i].distance)))
{
if (valid(pool[i].state))
@@ -86,7 +85,7 @@
std::vector<double> range(dim);
for (unsigned int i = 0 ; i < dim ; ++i)
- range[i] = m_rho * (si->getStateComponent(i).maxValue - si->getStateComponent(i).minValue);
+ range[i] = m_rho * (m_si->getStateComponent(i).maxValue - m_si->getStateComponent(i).minValue);
while (!solved && time_utils::Time::now() < endTime)
{
@@ -95,7 +94,7 @@
for (unsigned int i = m_poolSize ; i < maxPoolSize ; ++i)
{
- si->sampleNear(pool[i].state, pool[i%m_poolSize].state, range);
+ m_si->sampleNear(pool[i].state, pool[i%m_poolSize].state, range);
if (goal_r->isSatisfied(pool[i].state, &(pool[i].distance)))
{
if (valid(pool[i].state))
@@ -113,24 +112,14 @@
if (solved)
{
// set the solution
- SpaceInformationKinematic::PathKinematic_t path = new SpaceInformationKinematic::PathKinematic(m_si);
- SpaceInformationKinematic::StateKinematic_t st = new SpaceInformationKinematic::StateKinematic(dim);
- si->copyState(st, pool[solution].state);
- path->states.push_back(st);
- goal_r->setDifference(pool[solution].distance);
- goal_r->setSolutionPath(path);
+ m_si->copyState(result, pool[solution].state);
// try to improve the solution
- double dist = goal_r->getDifference();
- tryToImprove(st, &dist);
- goal_r->setDifference(dist);
+ tryToImprove(result, pool[solution].distance);
// if improving the state made it invalid, revert
- if (!valid(st))
- {
- si->copyState(st, pool[solution].state);
- goal_r->setDifference(pool[solution].distance);
- }
+ if (!valid(result))
+ m_si->copyState(result, pool[solution].state);
}
else
{
@@ -142,30 +131,15 @@
if (valid(pool[i].state))
{
// set the solution
- SpaceInformationKinematic::PathKinematic_t path = new SpaceInformationKinematic::PathKinematic(m_si);
- SpaceInformationKinematic::StateKinematic_t st = new SpaceInformationKinematic::StateKinematic(dim);
- si->copyState(st, pool[i].state);
- path->states.push_back(st);
+ m_si->copyState(result, pool[i].state);
// try to improve the state
- double dist = pool[i].distance;
- tryToImprove(st, &dist);
+ tryToImprove(result, pool[i].distance);
- // if the improved state is still valid, we have a solution
- if (valid(st))
- {
- // the solution may no longer be just approximate, so we check
- bool approx = goal_r->isSatisfied(st, &dist);
- goal_r->setSolutionPath(path, approx);
- goal_r->setDifference(dist);
- }
- else
- {
- // if the improvement made the state no longer valid, revert to previous one
- si->copyState(st, pool[i].state);
- goal_r->setSolutionPath(path, true);
- goal_r->setDifference(pool[i].distance);
- }
+ // if the improvement made the state no longer valid, revert to previous one
+ if (!valid(result))
+ m_si->copyState(result, pool[i].state);
+ solved = true;
break;
}
}
@@ -174,101 +148,24 @@
for (unsigned int i = 0 ; i < maxPoolSize ; ++i)
delete pool[i].state;
- return goal_r->isAchieved();
+ return solved;
}
-bool ompl::GAIK::tryToImprove(SpaceInformationKinematic::StateKinematic_t state, double *distance)
+bool ompl::GAIK::tryToImprove(SpaceInformationKinematic::StateKinematic_t state, double distance)
{
- m_msg.inform("GAIK: Distance to goal before improvement: %g", *distance);
+ m_msg.inform("GAIK: Distance to goal before improvement: %g", distance);
time_utils::Time start = time_utils::Time::now();
- tryToImproveAux(0.1, state, distance);
- tryToImproveAux(0.05, state, distance);
- tryToImproveAux(0.01, state, distance);
- tryToImproveAux(0.005, state, distance);
- tryToImproveAux(0.001, state, distance);
- tryToImproveAux(0.0005, state, distance);
- tryToImproveAux(0.0001, state, distance);
- tryToImproveAux(0.00005, state, distance);
- tryToImproveAux(0.000025, state, distance);
- tryToImproveAux(0.00001, state, distance);
- tryToImproveAux(0.000005, state, distance);
+ m_hcik.tryToImprove(state, 0.1, &distance);
+ m_hcik.tryToImprove(state, 0.05, &distance);
+ m_hcik.tryToImprove(state, 0.01, &distance);
+ m_hcik.tryToImprove(state, 0.005, &distance);
+ m_hcik.tryToImprove(state, 0.001, &distance);
+ m_hcik.tryToImprove(state, 0.005, &distance);
+ m_hcik.tryToImprove(state, 0.0001, &distance);
+ m_hcik.tryToImprove(state, 0.00005, &distance);
+ m_hcik.tryToImprove(state, 0.000025, &distance);
+ m_hcik.tryToImprove(state, 0.000005, &distance);
m_msg.inform("GAIK: Improvement took %g seconds", (time_utils::Time::now() - start).toSeconds());
- m_msg.inform("GAIK: Distance to goal after improvement: %g", *distance);
+ m_msg.inform("GAIK: Distance to goal after improvement: %g", distance);
return true;
}
-
-bool ompl::GAIK::tryToImproveAux(double add, SpaceInformationKinematic::StateKinematic_t state, double *distance)
-{
- SpaceInformationKinematic_t si = dynamic_cast<SpaceInformationKinematic_t>(m_si);
- SpaceInformationKinematic::GoalRegionKinematic_t goal_r = dynamic_cast<SpaceInformationKinematic::GoalRegionKinematic_t>(si->getGoal());
- unsigned int dim = si->getStateDimension();
-
- bool wasSatisfied = goal_r->isSatisfied(state, distance);
- double bestDist = *distance;
-
- bool change = true;
- unsigned int steps = 0;
-
- while (change && steps < m_maxImproveSteps)
- {
- change = false;
- steps++;
-
- for (unsigned int i = 0 ; i < dim ; ++i)
- {
- bool better = true;
- bool increased = false;
- while (better)
- {
- better = false;
- double backup = state->values[i];
- state->values[i] += add;
- bool isS = goal_r->isSatisfied(state, distance);
- if ((wasSatisfied && !isS) || !si->satisfiesBounds(state))
- state->values[i] = backup;
- else
- {
- wasSatisfied = isS;
- if (*distance < bestDist)
- {
- better = true;
- change = true;
- increased = true;
- bestDist = *distance;
- }
- else
- state->values[i] = backup;
- }
- }
-
- if (!increased)
- {
- better = true;
- while (better)
- {
- better = false;
- double backup = state->values[i];
- state->values[i] -= add;
- bool isS = goal_r->isSatisfied(state, distance);
- if ((wasSatisfied && !isS) || !si->satisfiesBounds(state))
- state->values[i] = backup;
- else
- {
- wasSatisfied = isS;
- if (*distance < bestDist)
- {
- better = true;
- change = true;
- bestDist = *distance;
- }
- else
- state->values[i] = backup;
- }
- }
- }
- }
- }
-
- *distance = bestDist;
- return wasSatisfied && valid(state);
-}
Added: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/HCIK.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/HCIK.cpp (rev 0)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/HCIK.cpp 2009-02-19 07:28:48 UTC (rev 11392)
@@ -0,0 +1,119 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* \author Ioan Sucan */
+
+#include "ompl/extension/samplingbased/kinematic/extension/ik/HCIK.h"
+#include <algorithm>
+
+bool ompl::HCIK::tryToImprove(SpaceInformationKinematic::StateKinematic_t state, double add, double *distance) const
+{
+ SpaceInformationKinematic::GoalRegionKinematic_t goal_r = dynamic_cast<SpaceInformationKinematic::GoalRegionKinematic_t>(m_si->getGoal());
+ unsigned int dim = m_si->getStateDimension();
+
+ if (!goal_r)
+ return false;
+
+ double tempDistance;
+ double initialDistance;
+ bool wasSatisfied = goal_r->isSatisfied(state, &initialDistance);
+ double bestDist = initialDistance;
+
+ bool change = true;
+ unsigned int steps = 0;
+
+ while (change && steps < m_maxImproveSteps)
+ {
+ change = false;
+ steps++;
+
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ {
+ bool better = true;
+ bool increased = false;
+ while (better)
+ {
+ better = false;
+ double backup = state->values[i];
+ state->values[i] += add;
+ bool isS = goal_r->isSatisfied(state, &tempDistance);
+ if ((wasSatisfied && !isS) || !m_si->satisfiesBounds(state))
+ state->values[i] = backup;
+ else
+ {
+ wasSatisfied = isS;
+ if (tempDistance < bestDist)
+ {
+ better = true;
+ change = true;
+ increased = true;
+ bestDist = tempDistance;
+ }
+ else
+ state->values[i] = backup;
+ }
+ }
+
+ if (!increased)
+ {
+ better = true;
+ while (better)
+ {
+ better = false;
+ double backup = state->values[i];
+ state->values[i] -= add;
+ bool isS = goal_r->isSatisfied(state, &tempDistance);
+ if ((wasSatisfied && !isS) || !m_si->satisfiesBounds(state))
+ state->values[i] = backup;
+ else
+ {
+ wasSatisfied = isS;
+ if (tempDistance < bestDist)
+ {
+ better = true;
+ change = true;
+ bestDist = tempDistance;
+ }
+ else
+ state->values[i] = backup;
+ }
+ }
+ }
+ }
+ }
+
+ if (distance)
+ *distance = bestDist;
+ return bestDist < initialDistance;
+}
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/KPIECE1.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/KPIECE1.h 2009-02-19 07:16:15 UTC (rev 11391)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/KPIECE1.h 2009-02-19 07:28:48 UTC (rev 11392)
@@ -41,6 +41,7 @@
#include "ompl/base/ProjectionEvaluator.h"
#include "ompl/datastructures/GridX.h"
#include "ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h"
+#include "ompl/extension/samplingbased/kinematic/extension/ik/HCIK.h"
#include <vector>
namespace ompl
@@ -73,7 +74,8 @@
{
public:
- KPIECE1(SpaceInformation_t si) : Planner(si)
+ KPIECE1(SpaceInformation_t si) : Planner(si),
+ m_hcik(dynamic_cast<SpaceInformationKinematic_t>(si))
{
m_type = PLAN_TO_GOAL_STATE | PLAN_TO_GOAL_REGION;
m_projectionEvaluator = NULL;
@@ -85,6 +87,7 @@
m_minValidPathPercentage = 0.2;
m_rho = 0.5;
m_tree.grid.onCellUpdate(computeImportance, NULL);
+ m_hcik.setMaxImproveSteps(50);
}
virtual ~KPIECE1(void)
@@ -264,6 +267,7 @@
bool selectMotion(Motion* &smotion, Grid::Cell* &scell);
void computeCoordinates(const Motion* motion, Grid::Coord &coord);
+ HCIK m_hcik;
TreeData m_tree;
ProjectionEvaluator *m_projectionEvaluator;
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/src/KPIECE1.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/src/KPIECE1.cpp 2009-02-19 07:16:15 UTC (rev 11391)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/src/KPIECE1.cpp 2009-02-19 07:28:48 UTC (rev 11392)
@@ -83,7 +83,8 @@
Motion *approxsol = NULL;
double approxdif = INFINITY;
SpaceInformationKinematic::StateKinematic_t xstate = new SpaceInformationKinematic::StateKinematic(dim);
-
+
+ double improveValue = 0.01;
unsigned int iteration = 1;
while (time_utils::Time::now() < endTime)
@@ -97,8 +98,25 @@
assert(existing);
/* sample random state (with goal biasing) */
- if (goal_s && m_rng.uniform(0.0, 1.0) < m_goalBias)
- si->copyState(xstate, goal_s->state);
+ if (m_rng.uniform(0.0, 1.0) < m_goalBias)
+ {
+ if (goal_s)
+ si->copyState(xstate, goal_s->state);
+ else
+ {
+ if (approxsol)
+ {
+ si->copyState(xstate, approxsol->state);
+ if (!m_hcik.tryToImprove(xstate, improveValue))
+ {
+ si->sampleNear(xstate, existing->state, range);
+ improveValue /= 2.0;
+ }
+ }
+ else
+ si->sampleNear(xstate, existing->state, range);
+ }
+ }
else
si->sampleNear(xstate, existing->state, range);
@@ -184,7 +202,7 @@
if (scell && !scell->data->motions.empty())
{
scell->data->selections++;
- smotion = scell->data->motions[m_rng.uniformInt(0, scell->data->motions.size() - 1)];
+ smotion = scell->data->motions[m_rng.halfNormalInt(0, scell->data->motions.size() - 1)];
return true;
}
else
@@ -221,7 +239,7 @@
cell->data->coverage = 1.0;
cell->data->iteration = iteration;
cell->data->selections = 1;
- cell->data->score = 1.0/ (1e-6 + dist);
+ cell->data->score = 1.0 / (1e-3 + dist);
m_tree.grid.add(cell);
created = 1;
}
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-02-19 07:16:15 UTC (rev 11391)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-02-19 07:28:48 UTC (rev 11392)
@@ -90,7 +90,7 @@
req.params.model_id = GROUPNAME;
req.params.distance_metric = "L2Square";
- req.params.planner_id = "SBL";
+ req.params.planner_id = "KPIECE";
req.threshold = 0.1;
req.interpolate = 1;
req.times = 1;
@@ -140,14 +140,15 @@
req.goal_constraints[0].z = 0.829675;
req.goal_constraints[0].roll = 0.0;
+ req.goal_constraints[0].pitch = 0.0;
req.goal_constraints[0].yaw = 0.0;
- req.goal_constraints[0].position_distance = 0.001;
- req.goal_constraints[0].orientation_distance = 0.03;
- req.goal_constraints[0].orientation_importance = 0.01;
+ req.goal_constraints[0].position_distance = 0.0001;
+ req.goal_constraints[0].orientation_distance = 0.1;
+ req.goal_constraints[0].orientation_importance = 0.0001;
// allow 1 second computation time
- req.allowed_time = 1.0;
+ req.allowed_time = 0.5;
// define the service messages
robot_srvs::KinematicReplanLinkPosition::Request s_req;
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups.xml 2009-02-19 07:16:15 UTC (rev 11391)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups.xml 2009-02-19 07:28:48 UTC (rev 11392)
@@ -87,6 +87,11 @@
<elem key="celldim" value="0.1 0.1"/>
</map>
+ <map name="KPIECE" flag="planning">
+ <elem key="projection" value="link r_wrist_roll_link" />
+ <elem key="celldim" value="0.1 0.1 0.1"/>
+ </map>
+
<map name="IKSBL" flag="planning">
<elem key="projection" value="0 1"/>
<elem key="celldim" value="1 1"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
