Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[18163] pkg/trunk
|
From: <is...@us...> - 2009-07-02 04:49:31
|
Revision: 18163
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18163&view=rev
Author: isucan
Date: 2009-07-02 04:49:29 +0000 (Thu, 02 Jul 2009)
Log Message:
-----------
more testing on planning with dynamics
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/kpiece/KPIECE1.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/kpiece/src/KPIECE1.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/rrt/RRT.h
pkg/trunk/motion_planning/ompl/code/tests/dynamic/2dmap/2dmap.cpp
pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/PR2PropagationModels.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/dynamicRRTSetup.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/kinematicKPIECESetup.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
pkg/trunk/stacks/pr2/pr2_defs/planning/planning.yaml
Added Paths:
-----------
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/dynamicKPIECESetup.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/dynamicKPIECESetup.cpp
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/kpiece/KPIECE1.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/kpiece/KPIECE1.h 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/kpiece/KPIECE1.h 2009-07-02 04:49:29 UTC (rev 18163)
@@ -82,11 +82,10 @@
m_projectionEvaluator = NULL;
m_projectionDimension = 0;
m_goalBias = 0.05;
- m_selectBorderPercentage = 0.9;
- m_badScoreFactor = 0.5;
+ m_selectBorderPercentage = 0.7;
+ m_badScoreFactor = 0.3;
m_goodScoreFactor = 0.9;
m_minValidPathStates = 3;
- m_rho = 0.5;
m_tree.grid.onCellUpdate(computeImportance, NULL);
}
@@ -123,30 +122,6 @@
return m_goalBias;
}
- /** Set the range the planner is supposed to use. This
- parameter greatly influences the runtime of the
- algorithm. It is probably a good idea to find what a good
- value is for each model the planner is used for. The range
- parameter influences how this @b qm along the path between
- @b qc and @b qr is chosen. @b qr may be too far, and it
- may not be best to have @b qm = @b qr all the time (range
- = 1.0 implies @b qm = @b qr. range should be less than
- 1.0). However, in a large space, it is also good to leave
- the neighborhood of @b qc (range = 0.0 implies @b qm = @b
- qc and no progress is made. rande should be larger than
- 0.0). Multiple values of this range parameter should be
- tried until a suitable one is found. */
- void setRange(double rho)
- {
- m_rho = rho;
- }
-
- /** \brief Get the range the planner is using */
- double getRange(void) const
- {
- return m_rho;
- }
-
/** \brief Set the projection evaluator. This class is able to
compute the projection of a given state. The simplest
option is to use an orthogonal projection; see
@@ -293,7 +268,6 @@
double m_badScoreFactor;
double m_selectBorderPercentage;
double m_goalBias;
- double m_rho;
random_utils::RNG m_rng;
};
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/kpiece/src/KPIECE1.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/kpiece/src/KPIECE1.cpp 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/kpiece/src/KPIECE1.cpp 2009-07-02 04:49:29 UTC (rev 18163)
@@ -82,10 +82,21 @@
double approxdif = INFINITY;
base::Control *rctrl = new base::Control(cdim);
+ std::vector<Grid::Coord> coords(si->getMaxControlDuration() + 1);
std::vector<base::State*> states(si->getMaxControlDuration() + 1);
for (unsigned int i = 0 ; i < states.size() ; ++i)
states[i] = new base::State(sdim);
+ // coordinates of the goal state and the best state seen so far
+ Grid::Coord best_coord, better_coord;
+ bool haveBestCoord = false;
+ bool haveBetterCoord = false;
+ if (goal_s)
+ {
+ m_projectionEvaluator->computeCoordinates(goal_s->state, best_coord);
+ haveBestCoord = true;
+ }
+
while (time_utils::Time::now() < endTime)
{
m_tree.iteration++;
@@ -93,7 +104,20 @@
/* Decide on a state to expand from */
Motion *existing = NULL;
Grid::Cell *ecell = NULL;
- selectMotion(existing, ecell);
+
+ if (m_rng.uniform(0.0, 1.0) < m_goalBias)
+ {
+ if (haveBestCoord)
+ ecell = m_tree.grid.getCell(best_coord);
+ if (!ecell && haveBetterCoord)
+ ecell = m_tree.grid.getCell(better_coord);
+ if (ecell)
+ existing = ecell->data->motions[m_rng.halfNormalInt(0, ecell->data->motions.size() - 1)];
+ else
+ selectMotion(existing, ecell);
+ }
+ else
+ selectMotion(existing, ecell);
assert(existing);
@@ -113,16 +137,63 @@
cd = 0;
}
- /* if we have */
+ /* if we have enough steps */
if (cd >= m_minValidPathStates)
{
- /* create a motion */
+
+ // split the motion into smaller ones, so we do not cross cell boundaries
+ for (unsigned int i = 0 ; i <= cd ; ++i)
+ m_projectionEvaluator->computeCoordinates(states[i], coords[i]);
+
+ unsigned int start = 0;
+ unsigned int curr = 1;
+ while (curr < cd)
+ {
+ // we have reached into a new cell
+ if (coords[start] != coords[curr])
+ {
+ /* create a motion */
+ Motion *motion = new Motion(sdim, cdim);
+ si->copyState(motion->state, states[curr - 1]);
+ si->copyControl(motion->control, rctrl);
+ motion->steps = curr - start;
+ motion->parent = existing;
+
+ double dist = 0.0;
+ bool solved = goal_r->isSatisfied(motion->state, &dist);
+ addMotion(motion, dist);
+
+ if (solved)
+ {
+ approxdif = dist;
+ solution = motion;
+ break;
+ }
+ if (dist < approxdif)
+ {
+ approxdif = dist;
+ approxsol = motion;
+ better_coord = coords[start];
+ haveBetterCoord = true;
+ }
+
+ // new parent will be the newly created motion
+ existing = motion;
+ start = curr;
+ }
+ curr++;
+ }
+
+ if (solution)
+ break;
+
+ /* create the last segment of the motion */
Motion *motion = new Motion(sdim, cdim);
si->copyState(motion->state, states[cd]);
si->copyControl(motion->control, rctrl);
- motion->steps = cd;
+ motion->steps = cd - start;
motion->parent = existing;
-
+
double dist = 0.0;
bool solved = goal_r->isSatisfied(motion->state, &dist);
addMotion(motion, dist);
@@ -138,6 +209,8 @@
approxdif = dist;
approxsol = motion;
}
+
+ // update cell score
ecell->data->score *= m_goodScoreFactor;
}
else
@@ -219,7 +292,7 @@
if (cell)
{
cell->data->motions.push_back(motion);
- cell->data->coverage += 1.0;
+ cell->data->coverage += motion->steps;
m_tree.grid.update(cell);
}
else
@@ -227,7 +300,7 @@
cell = m_tree.grid.createCell(coord);
cell->data = new CellData();
cell->data->motions.push_back(motion);
- cell->data->coverage = 1.0;
+ cell->data->coverage = motion->steps;
cell->data->iteration = m_tree.iteration;
cell->data->selections = 1;
cell->data->score = 1.0 / (1e-3 + dist);
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/rrt/RRT.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/rrt/RRT.h 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/rrt/RRT.h 2009-07-02 04:49:29 UTC (rev 18163)
@@ -75,7 +75,6 @@
m_nn.setDistanceFunction(boost::bind(&RRT::distanceFunction, this, _1, _2));
m_goalBias = 0.05;
m_hintBias = 0.75;
- m_rho = 0.5;
}
virtual ~RRT(void)
@@ -127,30 +126,6 @@
return m_hintBias;
}
- /** Set the range the planner is supposed to use. This
- parameter greatly influences the runtime of the
- algorithm. It is probably a good idea to find what a good
- value is for each model the planner is used for. The range
- parameter influences how this @b qm along the path between
- @b qc and @b qr is chosen. @b qr may be too far, and it
- may not be best to have @b qm = @b qr all the time (range
- = 1.0 implies @b qm = @b qr. range should be less than
- 1.0). However, in a large space, it is also good to leave
- the neighborhood of @b qc (range = 0.0 implies @b qm = @b
- qc and no progress is made. rande should be larger than
- 0.0). Multiple values of this range parameter should be
- tried until a suitable one is found. */
- void setRange(double rho)
- {
- m_rho = rho;
- }
-
- /** \brief Get the range the planner is using */
- double getRange(void) const
- {
- return m_rho;
- }
-
virtual void getStates(std::vector<const base::State*> &states) const;
protected:
@@ -210,7 +185,6 @@
double m_goalBias;
double m_hintBias;
- double m_rho;
random_utils::RNG m_rng;
};
Modified: pkg/trunk/motion_planning/ompl/code/tests/dynamic/2dmap/2dmap.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/tests/dynamic/2dmap/2dmap.cpp 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl/code/tests/dynamic/2dmap/2dmap.cpp 2009-07-02 04:49:29 UTC (rev 18163)
@@ -313,7 +313,6 @@
base::Planner* newPlanner(dynamic::SpaceInformationControlsIntegrator *si)
{
dynamic::RRT *rrt = new dynamic::RRT(si);
- rrt->setRange(0.95);
return rrt;
}
};
@@ -343,7 +342,6 @@
base::Planner* newPlanner(dynamic::SpaceInformationControlsIntegrator *si)
{
dynamic::KPIECE1 *kpiece = new dynamic::KPIECE1(si);
- kpiece->setRange(0.95);
std::vector<unsigned int> projection;
projection.push_back(0);
Modified: pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt 2009-07-02 04:49:29 UTC (rev 18163)
@@ -15,6 +15,7 @@
src/helpers/ompl_planner/kinematicLBKPIECESetup.cpp
src/helpers/ompl_planner/kinematicSBLSetup.cpp
src/helpers/ompl_planner/dynamicRRTSetup.cpp
+ src/helpers/ompl_planner/dynamicKPIECESetup.cpp
src/helpers/ompl_extensions/GoalDefinitions.cpp
src/helpers/ompl_extensions/SpaceInformation.cpp
src/helpers/ompl_extensions/ForwardPropagationModels.cpp
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h 2009-07-02 04:49:29 UTC (rev 18163)
@@ -78,6 +78,7 @@
void add_kIKKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
void add_dRRT(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
+ void add_dKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
};
typedef std::map<std::string, Model*> ModelMap;
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/PR2PropagationModels.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/PR2PropagationModels.h 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/PR2PropagationModels.h 2009-07-02 04:49:29 UTC (rev 18163)
@@ -58,15 +58,15 @@
*dimension = 2;
component.resize(*dimension);
component[0].type = ompl::base::ControlComponent::LINEAR;
- component[0].minValue = -0.2;
+ component[0].minValue = -1.0;
component[0].maxValue = 1.5;
component[1].type = ompl::base::ControlComponent::LINEAR;
- component[1].minValue = -0.5;
- component[1].maxValue = 0.5;
+ component[1].minValue = -0.9;
+ component[1].maxValue = 0.9;
- *resolution = 0.1;
- *minDuration = 1;
- *maxDuration = 5;
+ *resolution = 0.05;
+ *minDuration = 5;
+ *maxDuration = 10;
}
virtual void operator()(const ompl::base::State *begin, const ompl::base::Control *ctrl, double resolution, ompl::base::State *end) const
Copied: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/dynamicKPIECESetup.h (from rev 18088, pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/kinematicKPIECESetup.h)
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/dynamicKPIECESetup.h (rev 0)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/dynamicKPIECESetup.h 2009-07-02 04:49:29 UTC (rev 18163)
@@ -0,0 +1,58 @@
+/*********************************************************************
+* 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_PLANNING_PLANNERS_DYNAMIC_KPIECE_SETUP_
+#define OMPL_PLANNING_PLANNERS_DYNAMIC_KPIECE_SETUP_
+
+#include "ompl_planning/planners/PlannerSetup.h"
+#include <ompl/extension/dynamic/extension/kpiece/KPIECE1.h>
+
+namespace ompl_planning
+{
+
+ class dynamicKPIECESetup : public PlannerSetup
+ {
+ public:
+
+ dynamicKPIECESetup(ModelBase *m);
+ virtual ~dynamicKPIECESetup(void);
+ virtual bool setup(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
+ };
+
+} // ompl_planning
+
+#endif
+
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp 2009-07-02 04:49:29 UTC (rev 18163)
@@ -44,6 +44,7 @@
#include "ompl_planning/planners/kinematicLBKPIECESetup.h"
#include "ompl_planning/planners/dynamicRRTSetup.h"
+#include "ompl_planning/planners/dynamicKPIECESetup.h"
/* instantiate the planners that can be used */
void ompl_planning::Model::createMotionPlanningInstances(std::vector< boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> > cfgs)
@@ -76,6 +77,9 @@
if (type == "dynamic::RRT")
add_dRRT(cfgs[i]);
else
+ if (type == "dynamic::KPIECE")
+ add_dKPIECE(cfgs[i]);
+ else
ROS_WARN("Unknown planner type: %s", type.c_str());
}
}
@@ -143,6 +147,15 @@
delete kpiece;
}
+void ompl_planning::Model::add_dKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
+{
+ PlannerSetup *kpiece = new dynamicKPIECESetup(dynamic_cast<ModelBase*>(this));
+ if (kpiece->setup(options))
+ planners[kpiece->name] = kpiece;
+ else
+ delete kpiece;
+}
+
void ompl_planning::Model::add_kLBKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
{
PlannerSetup *kpiece = new kinematicLBKPIECESetup(dynamic_cast<ModelBase*>(this));
Copied: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/dynamicKPIECESetup.cpp (from rev 18088, pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/kinematicKPIECESetup.cpp)
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/dynamicKPIECESetup.cpp (rev 0)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/dynamicKPIECESetup.cpp 2009-07-02 04:49:29 UTC (rev 18163)
@@ -0,0 +1,81 @@
+/*********************************************************************
+* 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_planning/planners/dynamicKPIECESetup.h"
+
+ompl_planning::dynamicKPIECESetup::dynamicKPIECESetup(ModelBase *m) : PlannerSetup(m)
+{
+ name = "dynamic::KPIECE";
+ priority = 3;
+}
+
+ompl_planning::dynamicKPIECESetup::~dynamicKPIECESetup(void)
+{
+ if (dynamic_cast<ompl::dynamic::KPIECE1*>(mp))
+ {
+ ompl::base::ProjectionEvaluator *pe = dynamic_cast<ompl::dynamic::KPIECE1*>(mp)->getProjectionEvaluator();
+ if (pe)
+ delete pe;
+ }
+}
+
+bool ompl_planning::dynamicKPIECESetup::setup(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
+{
+ preSetup(options);
+
+ ompl::dynamic::KPIECE1 *kpiece = new ompl::dynamic::KPIECE1(dynamic_cast<ompl::dynamic::SpaceInformationControlsIntegrator*>(si));
+ mp = kpiece;
+
+ if (options->hasParam("goal_bias"))
+ {
+ kpiece->setGoalBias(options->getParamDouble("goal_bias", kpiece->getGoalBias()));
+ ROS_DEBUG("Goal bias is set to %g", kpiece->getGoalBias());
+ }
+
+ kpiece->setProjectionEvaluator(getProjectionEvaluator(options));
+
+ if (kpiece->getProjectionEvaluator() == NULL)
+ {
+ ROS_WARN("Adding %s failed: need to set both 'projection' and 'celldim' for %s", name.c_str(), model->groupName.c_str());
+ return false;
+ }
+ else
+ {
+ postSetup(options);
+ return true;
+ }
+}
+
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/dynamicRRTSetup.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/dynamicRRTSetup.cpp 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/dynamicRRTSetup.cpp 2009-07-02 04:49:29 UTC (rev 18163)
@@ -53,18 +53,18 @@
ompl::dynamic::RRT *rrt = new ompl::dynamic::RRT(dynamic_cast<ompl::dynamic::SpaceInformationControlsIntegrator*>(si));
mp = rrt;
- if (options->hasParam("range"))
- {
- rrt->setRange(options->getParamDouble("range", rrt->getRange()));
- ROS_DEBUG("Range is set to %g", rrt->getRange());
- }
-
if (options->hasParam("goal_bias"))
{
rrt->setGoalBias(options->getParamDouble("goal_bias", rrt->getGoalBias()));
ROS_DEBUG("Goal bias is set to %g", rrt->getGoalBias());
}
+ if (options->hasParam("hint_bias"))
+ {
+ rrt->setHintBias(options->getParamDouble("hint_bias", rrt->getHintBias()));
+ ROS_DEBUG("Goal bias is set to %g", rrt->getHintBias());
+ }
+
postSetup(options);
return true;
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/kinematicKPIECESetup.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/kinematicKPIECESetup.cpp 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/kinematicKPIECESetup.cpp 2009-07-02 04:49:29 UTC (rev 18163)
@@ -39,7 +39,7 @@
ompl_planning::kinematicKPIECESetup::kinematicKPIECESetup(ModelBase *m) : PlannerSetup(m)
{
name = "kinematic::KPIECE";
- priority = 3;
+ priority = 4;
}
ompl_planning::kinematicKPIECESetup::~kinematicKPIECESetup(void)
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-07-02 04:49:29 UTC (rev 18163)
@@ -45,6 +45,9 @@
OMPLPlanning(void)
{
+ // display the first 3 coordinates of states in diffusion trees
+ requestHandler_.enableDebugMode(0, 1);
+
// register with ROS
collisionModels_ = new planning_environment::CollisionModels("robot_description");
if (nodeHandle_.hasParam("~planning_frame_id"))
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-07-02 04:49:29 UTC (rev 18163)
@@ -131,14 +131,14 @@
req.params.model_id = "base";
req.params.distance_metric = "L2Square";
- req.params.planner_id = "dynamic::RRT";
+ req.params.planner_id = "dynamic::KPIECE";
- req.params.volumeMin.x = -8;
- req.params.volumeMin.y = -8;
+ req.params.volumeMin.x = -3;
+ req.params.volumeMin.y = -3;
req.params.volumeMin.z = 0;
- req.params.volumeMax.x = 8;
- req.params.volumeMax.y = 8;
+ req.params.volumeMax.x = 3;
+ req.params.volumeMax.y = 3;
req.params.volumeMax.z = 0;
req.times = 1;
@@ -153,20 +153,20 @@
req.goal_constraints.joint_constraint[0].toleranceBelow.resize(3);
req.goal_constraints.joint_constraint[0].value[0] = 0.0;
- req.goal_constraints.joint_constraint[0].toleranceBelow[0] = 0.0;
- req.goal_constraints.joint_constraint[0].toleranceAbove[0] = 0.0;
+ req.goal_constraints.joint_constraint[0].toleranceBelow[0] = 0.05;
+ req.goal_constraints.joint_constraint[0].toleranceAbove[0] = 0.05;
req.goal_constraints.joint_constraint[0].value[1] = 1.0;
- req.goal_constraints.joint_constraint[0].toleranceBelow[1] = 0.0;
- req.goal_constraints.joint_constraint[0].toleranceAbove[1] = 0.0;
+ req.goal_constraints.joint_constraint[0].toleranceBelow[1] = 0.05;
+ req.goal_constraints.joint_constraint[0].toleranceAbove[1] = 0.05;
req.goal_constraints.joint_constraint[0].value[2] = 0.0;
- req.goal_constraints.joint_constraint[0].toleranceBelow[2] = 0.0;
- req.goal_constraints.joint_constraint[0].toleranceAbove[2] = 0.0;
+ req.goal_constraints.joint_constraint[0].toleranceBelow[2] = 0.1;
+ req.goal_constraints.joint_constraint[0].toleranceAbove[2] = 0.1;
// allow 1 second computation time
- req.allowed_time = 10.0;
+ req.allowed_time = 20.0;
// define the service messages
motion_planning_srvs::MotionPlan::Response res;
Modified: pkg/trunk/stacks/pr2/pr2_defs/planning/planning.yaml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/planning/planning.yaml 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/stacks/pr2/pr2_defs/planning/planning.yaml 2009-07-02 04:49:29 UTC (rev 18163)
@@ -14,7 +14,7 @@
links:
base_link
planner_configs:
- RRTkConfig1 RRTdConfig1 LazyRRTkConfig1 SBLkConfig1 KPIECEkConfig1
+ RRTkConfig1 RRTdConfig1 LazyRRTkConfig1 SBLkConfig1 KPIECEkConfig1 KPIECEdConfig1
left_arm:
links:
@@ -79,7 +79,6 @@
RRTdConfig1:
type: dynamic::RRT
- range: 0.75
LazyRRTkConfig1:
type: kinematic::LazyRRT
@@ -96,7 +95,12 @@
projection: 0 1
celldim: 1 1
range: 0.5
-
+
+ KPIECEdConfig1:
+ type: dynamic::KPIECE
+ projection: 0 1
+ celldim: 0.5 0.5
+
RRTkConfig2:
type: kinematic::RRT
range: 0.75
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
