Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[4153] pkg/trunk
|
From: <is...@us...> - 2008-09-10 21:39:15
|
Revision: 4153
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4153&view=rev
Author: isucan
Date: 2008-09-10 21:39:21 +0000 (Wed, 10 Sep 2008)
Log Message:
-----------
added EST (Expansive Space Trees) algorithm
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/RKPModel.h
pkg/trunk/motion_planning/kinematic_planning/include/RKPSBLSetup.h
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/CMakeLists.txt
pkg/trunk/motion_planning/ompl/code/examples/samplingbased/kinematic/grid/grid.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/sbl/SBL.h
pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
Added Paths:
-----------
pkg/trunk/motion_planning/kinematic_planning/include/RKPESTSetup.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/EST.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/src/
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/src/EST.cpp
Added: pkg/trunk/motion_planning/kinematic_planning/include/RKPESTSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RKPESTSetup.h (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RKPESTSetup.h 2008-09-10 21:39:21 UTC (rev 4153)
@@ -0,0 +1,137 @@
+/*********************************************************************
+* 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 KINEMATIC_PLANNING_RKP_EST_SETUP_
+#define KINEMATIC_PLANNING_RKP_EST_SETUP_
+
+#include "RKPPlannerSetup.h"
+#include <ompl/extension/samplingbased/kinematic/extension/est/EST.h>
+
+class RKPESTSetup : public RKPPlannerSetup
+{
+ public:
+
+ RKPESTSetup(void) : RKPPlannerSetup()
+ {
+ }
+
+ virtual ~RKPESTSetup(void)
+ {
+ if (dynamic_cast<ompl::EST_t>(mp))
+ {
+ ompl::ProjectionEvaluator_t pe = dynamic_cast<ompl::EST_t>(mp)->getProjectionEvaluator();
+ if (pe)
+ delete pe;
+ }
+ }
+
+ virtual bool setup(RKPModelBase *model, std::map<std::string, std::string> &options)
+ {
+ std::cout << "Adding EST instance for motion planning: " << model->groupName << std::endl;
+
+ si = new SpaceInformationRKPModel(model);
+ svc = new StateValidityPredicate(model);
+ si->setStateValidityChecker(svc);
+
+ smoother = new ompl::PathSmootherKinematic(si);
+ smoother->setMaxSteps(50);
+ smoother->setMaxEmptySteps(4);
+
+ ompl::EST_t sbl = new ompl::EST(si);
+ mp = sbl;
+
+ bool setDim = false;
+ bool setProj = false;
+
+ if (options.find("range") != options.end())
+ {
+ double range = string_utils::fromString<double>(options["range"]);
+ sbl->setRange(range);
+ std::cout << "Range is set to " << range << std::endl;
+ }
+
+ if (options.find("projection") != options.end())
+ {
+ std::string proj = options["projection"];
+ std::vector<unsigned int> projection;
+ std::stringstream ss(proj);
+ while (ss.good())
+ {
+ unsigned int comp;
+ ss >> comp;
+ projection.push_back(comp);
+ }
+
+ sbl->setProjectionEvaluator(new ompl::OrthogonalProjectionEvaluator(projection));
+
+ std::cout << "Projection is set to " << proj << std::endl;
+ setProj = true;
+ }
+
+ if (options.find("celldim") != options.end())
+ {
+ std::string celldim = options["celldim"];
+ std::vector<double> cdim;
+ std::stringstream ss(celldim);
+ while (ss.good())
+ {
+ double comp;
+ ss >> comp;
+ cdim.push_back(comp);
+ }
+
+ sbl->setCellDimensions(cdim);
+ setDim = true;
+ std::cout << "Cell dimensions set to " << celldim << std::endl;
+ }
+
+ if (!setDim || !setProj)
+ {
+ std::cout << "Adding EST failed: need to set both 'projection' and 'cellldim' for " << model->groupName << std::endl;
+ return false;
+ }
+ else
+ {
+ setupDistanceEvaluators();
+ si->setup();
+ mp->setup();
+ return true;
+ }
+ }
+
+};
+
+#endif
Modified: pkg/trunk/motion_planning/kinematic_planning/include/RKPModel.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RKPModel.h 2008-09-10 21:32:47 UTC (rev 4152)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RKPModel.h 2008-09-10 21:39:21 UTC (rev 4153)
@@ -42,6 +42,7 @@
#include "RKPRRTSetup.h"
#include "RKPLazyRRTSetup.h"
#include "RKPSBLSetup.h"
+#include "RKPESTSetup.h"
#include <string>
#include <map>
@@ -78,6 +79,14 @@
delete rrt;
}
+ void addEST(std::map<std::string, std::string> &options)
+ {
+ RKPPlannerSetup *est = new RKPESTSetup();
+ if (est->setup(dynamic_cast<RKPModelBase*>(this), options))
+ planners["EST"] = est;
+ else
+ delete est;
+ }
void addSBL(std::map<std::string, std::string> &options)
{
RKPPlannerSetup *sbl = new RKPSBLSetup();
Modified: pkg/trunk/motion_planning/kinematic_planning/include/RKPSBLSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RKPSBLSetup.h 2008-09-10 21:32:47 UTC (rev 4152)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RKPSBLSetup.h 2008-09-10 21:39:21 UTC (rev 4153)
@@ -52,7 +52,7 @@
{
if (dynamic_cast<ompl::SBL_t>(mp))
{
- ompl::SBL::ProjectionEvaluator_t pe = dynamic_cast<ompl::SBL_t>(mp)->getProjectionEvaluator();
+ ompl::ProjectionEvaluator_t pe = dynamic_cast<ompl::SBL_t>(mp)->getProjectionEvaluator();
if (pe)
delete pe;
}
@@ -95,7 +95,7 @@
projection.push_back(comp);
}
- sbl->setProjectionEvaluator(new ompl::SBL::OrthogonalProjectionEvaluator(projection));
+ sbl->setProjectionEvaluator(new ompl::OrthogonalProjectionEvaluator(projection));
std::cout << "Projection is set to " << proj << std::endl;
setProj = true;
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-09-10 21:32:47 UTC (rev 4152)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-09-10 21:39:21 UTC (rev 4153)
@@ -228,6 +228,14 @@
options = data.getMapTagValues("planning", "SBL");
}
model->addSBL(options);
+
+ options.clear();
+ if (group)
+ {
+ const robot_desc::URDF::Map &data = group->data;
+ options = data.getMapTagValues("planning", "EST");
+ }
+ model->addEST(options);
}
ModelMap m_models;
Modified: pkg/trunk/motion_planning/ompl/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/ompl/CMakeLists.txt 2008-09-10 21:32:47 UTC (rev 4152)
+++ pkg/trunk/motion_planning/ompl/CMakeLists.txt 2008-09-10 21:39:21 UTC (rev 4153)
@@ -8,6 +8,7 @@
code/ompl/extension/samplingbased/kinematic/src/PathSmootherKinematic.cpp
code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
code/ompl/extension/samplingbased/kinematic/extension/rrt/src/LazyRRT.cpp
+ code/ompl/extension/samplingbased/kinematic/extension/est/src/EST.cpp
code/ompl/extension/samplingbased/kinematic/extension/sbl/src/SBL.cpp
)
rospack_add_compile_flags(ompl -Wextra)
Modified: pkg/trunk/motion_planning/ompl/code/examples/samplingbased/kinematic/grid/grid.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/examples/samplingbased/kinematic/grid/grid.cpp 2008-09-10 21:32:47 UTC (rev 4152)
+++ pkg/trunk/motion_planning/ompl/code/examples/samplingbased/kinematic/grid/grid.cpp 2008-09-10 21:39:21 UTC (rev 4153)
@@ -315,13 +315,13 @@
std::vector<unsigned int> projection;
projection.push_back(0);
projection.push_back(1);
- ope = new SBL::OrthogonalProjectionEvaluator(projection);
+ ope = new OrthogonalProjectionEvaluator(projection);
sbl->setProjectionEvaluator(ope);
return sbl;
}
- SBL::OrthogonalProjectionEvaluator_t ope;
+ OrthogonalProjectionEvaluator_t ope;
};
Added: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h (rev 0)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h 2008-09-10 21:39:21 UTC (rev 4153)
@@ -0,0 +1,96 @@
+/*********************************************************************
+* 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_PROJECTION_EVALUATOR_
+#define OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_PROJECTION_EVALUATOR_
+
+#include "ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h"
+
+namespace ompl
+{
+
+ /** Forward class declaration */
+ ForwardClassDeclaration(ProjectionEvaluator);
+
+ /** Abstract definition for a class computing projections */
+ class ProjectionEvaluator
+ {
+ public:
+ /** Destructor */
+ virtual ~ProjectionEvaluator(void)
+ {
+ }
+
+ /** Return the dimension of the projection defined by this evaluator */
+ virtual unsigned int getDimension(void) const = 0;
+
+ /** Compute the projection as an array of double values */
+ virtual void operator()(const SpaceInformationKinematic::StateKinematic_t state, double *projection) = 0;
+ };
+
+ /** Forward class declaration */
+ ForwardClassDeclaration(OrthogonalProjectionEvaluator);
+
+ /** Definition for a class computing orthogonal projections */
+ class OrthogonalProjectionEvaluator : public ProjectionEvaluator
+ {
+ public:
+
+ OrthogonalProjectionEvaluator(const std::vector<unsigned int> &components) : ProjectionEvaluator()
+ {
+ m_components = components;
+ }
+
+ virtual unsigned int getDimension(void) const
+ {
+ return m_components.size();
+ }
+
+ virtual void operator()(const SpaceInformationKinematic::StateKinematic_t state, double *projection)
+ {
+ for (unsigned int i = 0 ; i < m_components.size() ; ++i)
+ projection[i] = state->values[m_components[i]];
+ }
+
+ protected:
+
+ std::vector<unsigned int> m_components;
+
+ };
+
+}
+
+#endif
Added: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/EST.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/EST.h (rev 0)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/EST.h 2008-09-10 21:39:21 UTC (rev 4153)
@@ -0,0 +1,237 @@
+/*********************************************************************
+* 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_EST_
+#define OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_EST_
+
+#include "ompl/base/Planner.h"
+#include "ompl/datastructures/Grid.h"
+#include "ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h"
+#include "ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h"
+#include <vector>
+
+namespace ompl
+{
+
+ /** Forward class declaration */
+ ForwardClassDeclaration(EST);
+
+ /**
+ @subsubsection EST Expansive Space Trees (EST)
+
+ @par Short description
+
+
+ */
+ class EST : public Planner
+ {
+ public:
+
+ EST(SpaceInformation_t si) : Planner(si)
+ {
+ m_type = PLAN_TO_GOAL_STATE | PLAN_TO_GOAL_REGION;
+ random_utils::init(&m_rngState);
+ m_projectionEvaluator = NULL;
+ m_projectionDimension = 0;
+ m_goalBias = 0.05;
+ m_rho = 0.5;
+ }
+
+ virtual ~EST(void)
+ {
+ freeMemory();
+ }
+
+ virtual bool solve(double solveTime);
+
+ virtual void clear(void)
+ {
+ freeMemory();
+ m_tree.grid.clear();
+ m_tree.size = 0;
+ }
+
+ /** In the process of randomly selecting states in the state
+ space to attempt to go towards, the algorithm may in fact
+ choose the actual goal state, if it knows it, with some
+ probability. This probability is a real number between 0.0
+ and 1.0; its value should usually be around 0.05 and
+ should not be too large. It is probably a good idea to use
+ the default value. */
+ void setGoalBias(double goalBias)
+ {
+ m_goalBias = goalBias;
+ }
+
+ /** Get the goal bias the planner is using */
+ double getGoalBias(void) const
+ {
+ 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;
+ }
+
+ /** Get the range the planner is using */
+ double getRange(void) const
+ {
+ return m_rho;
+ }
+
+ /** 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
+ OrthogonalProjectionEvaluator */
+ void setProjectionEvaluator(ProjectionEvaluator_t projectionEvaluator)
+ {
+ m_projectionEvaluator = projectionEvaluator;
+ }
+
+ ProjectionEvaluator_t getProjectionEvaluator(void) const
+ {
+ return m_projectionEvaluator;
+ }
+
+ /** Define the dimension (each component) of a grid cell. The
+ number of dimensions set here must be the same as the
+ dimension of the projection computed by the projection
+ evaluator. */
+ void setCellDimensions(const std::vector<double> &cellDimensions)
+ {
+ m_cellDimensions = cellDimensions;
+ }
+
+ void getCellDimensions(std::vector<double> &cellDimensions) const
+ {
+ cellDimensions = m_cellDimensions;
+ }
+
+ virtual void setup(void)
+ {
+ assert(m_projectionEvaluator);
+ m_projectionDimension = m_projectionEvaluator->getDimension();
+ assert(m_projectionDimension > 0);
+ assert(m_cellDimensions.size() == m_projectionDimension);
+ m_tree.grid.setDimension(m_projectionDimension);
+ Planner::setup();
+ }
+
+ protected:
+
+ ForwardClassDeclaration(Motion);
+
+ class Motion
+ {
+ public:
+
+ Motion(void)
+ {
+ parent = NULL;
+ state = NULL;
+ }
+
+ Motion(unsigned int dimension)
+ {
+ state = new SpaceInformationKinematic::StateKinematic(dimension);
+ parent = NULL;
+ }
+
+ virtual ~Motion(void)
+ {
+ if (state)
+ delete state;
+ }
+
+ SpaceInformationKinematic::StateKinematic_t state;
+ Motion_t parent;
+
+ };
+
+ typedef std::vector<Motion_t> MotionSet;
+
+ struct TreeData
+ {
+ TreeData(void)
+ {
+ size = 0;
+ }
+
+ Grid<MotionSet> grid;
+ unsigned int size;
+ };
+
+ void freeMemory(void)
+ {
+ for (Grid<MotionSet>::iterator it = m_tree.grid.begin(); it != m_tree.grid.end() ; ++it)
+ {
+ for (unsigned int i = 0 ; i < it->second->data.size() ; ++i)
+ delete it->second->data[i];
+ }
+ }
+
+ void addMotion(Motion_t motion);
+ Motion_t selectMotion(void);
+ void computeCoordinates(const Motion_t motion, Grid<MotionSet>::Coord &coord);
+
+ TreeData m_tree;
+
+ ProjectionEvaluator *m_projectionEvaluator;
+ unsigned int m_projectionDimension;
+ std::vector<double> m_cellDimensions;
+
+ double m_goalBias;
+ double m_rho;
+ random_utils::rngState m_rngState;
+ };
+
+}
+
+#endif
Added: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/src/EST.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/src/EST.cpp (rev 0)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/src/EST.cpp 2008-09-10 21:39:21 UTC (rev 4153)
@@ -0,0 +1,202 @@
+/*********************************************************************
+* 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/est/EST.h"
+
+bool ompl::EST::solve(double solveTime)
+{
+ SpaceInformationKinematic_t si = dynamic_cast<SpaceInformationKinematic_t>(m_si);
+ SpaceInformationKinematic::GoalRegionKinematic_t goal_r = dynamic_cast<SpaceInformationKinematic::GoalRegionKinematic_t>(si->getGoal());
+ SpaceInformationKinematic::GoalStateKinematic_t goal_s = dynamic_cast<SpaceInformationKinematic::GoalStateKinematic_t>(si->getGoal());
+ unsigned int dim = si->getStateDimension();
+
+ if (!goal_s && !goal_r)
+ {
+ m_msg.error("Unknown type of goal (or goal undefined)");
+ return false;
+ }
+
+ time_utils::Time endTime = time_utils::Time::now() + time_utils::Duration(solveTime);
+
+ if (m_tree.grid.size() == 0)
+ {
+ for (unsigned int i = 0 ; i < m_si->getStartStateCount() ; ++i)
+ {
+ Motion_t motion = new Motion(dim);
+ si->copyState(motion->state, dynamic_cast<SpaceInformationKinematic::StateKinematic_t>(si->getStartState(i)));
+ if (si->isValid(motion->state))
+ addMotion(motion);
+ else
+ {
+ m_msg.error("Initial state is in collision!");
+ delete motion;
+ }
+ }
+ }
+
+ if (m_tree.grid.size() == 0)
+ {
+ m_msg.error("There are no valid initial states!");
+ return false;
+ }
+
+ m_msg.inform("Starting with %u states", m_tree.size);
+
+ 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);
+
+ Motion_t solution = NULL;
+ Motion_t approxsol = NULL;
+ double approxdif = INFINITY;
+ SpaceInformationKinematic::StateKinematic_t xstate = new SpaceInformationKinematic::StateKinematic(dim);
+
+ while (time_utils::Time::now() < endTime)
+ {
+ /* Decide on a state to expand from */
+ Motion_t existing = selectMotion();
+ assert(existing);
+
+ /* sample random state (with goal biasing) */
+ if (goal_s && random_utils::uniform(&m_rngState, 0.0, 1.0) < m_goalBias)
+ si->copyState(xstate, goal_s->state);
+ else
+ si->sampleNear(xstate, existing->state, range);
+
+ if (si->checkMotionSubdivision(existing->state, xstate))
+ {
+ /* create a motion */
+ Motion_t motion = new Motion(dim);
+ si->copyState(motion->state, xstate);
+ motion->parent = existing;
+
+ addMotion(motion);
+ double dist = 0.0;
+ bool solved = goal_r->isSatisfied(motion->state, &dist);
+ if (solved)
+ {
+ approxdif = dist;
+ solution = motion;
+ break;
+ }
+ if (dist < approxdif)
+ {
+ approxdif = dist;
+ approxsol = motion;
+ }
+ }
+ }
+
+ bool approximate = false;
+ if (solution == NULL)
+ {
+ solution = approxsol;
+ approximate = true;
+ }
+
+ if (solution != NULL)
+ {
+ /* construct the solution path */
+ std::vector<Motion_t> mpath;
+ while (solution != NULL)
+ {
+ mpath.push_back(solution);
+ solution = solution->parent;
+ }
+
+ /* set the solution path */
+ SpaceInformationKinematic::PathKinematic_t path = new SpaceInformationKinematic::PathKinematic(m_si);
+ for (int i = mpath.size() - 1 ; i >= 0 ; --i)
+ {
+ SpaceInformationKinematic::StateKinematic_t st = new SpaceInformationKinematic::StateKinematic(dim);
+ si->copyState(st, mpath[i]->state);
+ path->states.push_back(st);
+ }
+ goal_r->setDifference(approxdif);
+ goal_r->setSolutionPath(path, approximate);
+ }
+
+ delete xstate;
+
+ m_msg.inform("Created %u states in %u cells", m_tree.size, m_tree.grid.size());
+
+ return goal_r->isAchieved();
+}
+
+ompl::EST::Motion_t ompl::EST::selectMotion(void)
+{
+ double sum = 0.0;
+ Grid<MotionSet>::Cell_t cell = NULL;
+ double prob = random_utils::uniform(&m_rngState) * (m_tree.grid.size() - 1);
+ for (Grid<MotionSet>::iterator it = m_tree.grid.begin(); it != m_tree.grid.end() ; ++it)
+ {
+ sum += (double)(m_tree.size - it->second->data.size()) / (double)m_tree.size;
+ if (prob < sum)
+ {
+ cell = it->second;
+ break;
+ }
+ }
+ if (!cell && m_tree.grid.size() > 0)
+ cell = m_tree.grid.begin()->second;
+ return cell && !cell->data.empty() ? cell->data[random_utils::uniformInt(&m_rngState, 0, cell->data.size() - 1)] : NULL;
+}
+
+void ompl::EST::computeCoordinates(const Motion_t motion, Grid<MotionSet>::Coord &coord)
+{
+ coord.resize(m_projectionDimension);
+ double projection[m_projectionDimension];
+ (*m_projectionEvaluator)(motion->state, projection);
+
+ for (unsigned int i = 0 ; i < m_projectionDimension; ++i)
+ coord[i] = (int)trunc(projection[i]/m_cellDimensions[i]);
+}
+
+void ompl::EST::addMotion(Motion_t motion)
+{
+ Grid<MotionSet>::Coord coord;
+ computeCoordinates(motion, coord);
+ Grid<MotionSet>::Cell_t cell = m_tree.grid.getCell(coord);
+ if (cell)
+ cell->data.push_back(motion);
+ else
+ {
+ cell = m_tree.grid.create(coord);
+ cell->data.push_back(motion);
+ m_tree.grid.add(cell);
+ }
+ m_tree.size++;
+}
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/sbl/SBL.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/sbl/SBL.h 2008-09-10 21:32:47 UTC (rev 4152)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/sbl/SBL.h 2008-09-10 21:39:21 UTC (rev 4153)
@@ -40,6 +40,7 @@
#include "ompl/base/Planner.h"
#include "ompl/datastructures/Grid.h"
#include "ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h"
+#include "ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h"
#include <vector>
/** Main namespace */
@@ -83,57 +84,6 @@
{
public:
- /** Forward class declaration */
- ForwardClassDeclaration(ProjectionEvaluator);
-
-
- /** Abstract definition for a class computing projections */
- class ProjectionEvaluator
- {
- public:
- /** Destructor */
- virtual ~ProjectionEvaluator(void)
- {
- }
-
- /** Return the dimension of the projection defined by this evaluator */
- virtual unsigned int getDimension(void) const = 0;
-
- /** Compute the projection as an array of double values */
- virtual void operator()(const SpaceInformationKinematic::StateKinematic_t state, double *projection) = 0;
- };
-
- /** Forward class declaration */
- ForwardClassDeclaration(OrthogonalProjectionEvaluator);
-
- /** Definition for a class computing orthogonal projections */
- class OrthogonalProjectionEvaluator : public ProjectionEvaluator
- {
- public:
-
- OrthogonalProjectionEvaluator(const std::vector<unsigned int> &components) : ProjectionEvaluator()
- {
- m_components = components;
- }
-
- virtual unsigned int getDimension(void) const
- {
- return m_components.size();
- }
-
- virtual void operator()(const SpaceInformationKinematic::StateKinematic_t state, double *projection)
- {
- for (unsigned int i = 0 ; i < m_components.size() ; ++i)
- projection[i] = state->values[m_components[i]];
- }
-
- protected:
-
- std::vector<unsigned int> m_components;
-
- };
-
-
SBL(SpaceInformation_t si) : Planner(si)
{
m_type = PLAN_TO_GOAL_STATE;
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-09-10 21:32:47 UTC (rev 4152)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-09-10 21:39:21 UTC (rev 4153)
@@ -15,6 +15,12 @@
<elem key="range" value="0.5"/>
</map>
+ <map name="EST" flag="planning">
+ <elem key="projection" value="0 1"/>
+ <elem key="celldim" value="1 1"/>
+ <elem key="range" value="0.5"/>
+ </map>
+
<map name="LazyRRT" flag="planning">
<elem key="range" value="0.75" />
</map>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
