Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[11348] pkg/trunk
|
From: <is...@us...> - 2009-02-18 23:33:21
|
Revision: 11348
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11348&view=rev
Author: isucan
Date: 2009-02-18 22:16:43 +0000 (Wed, 18 Feb 2009)
Log Message:
-----------
small optimization for planning models forward kinematics: calling computeTransform() with the same argument twice in a row does not redo computation; projection definition for ompl was moved in base/ ; projections are better organized in kinematic_lanning added a projection for the end effector position
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPESTSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPIKSBLSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPKPIECESetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPPlannerSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPSBLSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPStateValidator.h
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
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/EST.cpp
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/kpiece/LBKPIECE1.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/src/LBKPIECE1.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
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/src/SBL.cpp
pkg/trunk/motion_planning/ompl/code/tests/samplingbased/kinematic/2dmap/2dmap.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/planning_display.cpp
pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
pkg/trunk/world_models/robot_models/planning_models/test/test_kinematic.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPDistanceEvaluators.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPProjectionEvaluators.h
pkg/trunk/motion_planning/ompl/code/ompl/base/ProjectionEvaluator.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluatorKinematic.h
Removed Paths:
-------------
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -125,7 +125,7 @@
void update(ompl::SpaceInformationKinematic::StateKinematic_t state) const
{
- m_model->kmodel->computeTransforms(static_cast<const ompl::SpaceInformationKinematic::StateKinematic_t>(state)->values, m_model->groupID);
+ m_model->kmodel->computeTransformsGroup(static_cast<const ompl::SpaceInformationKinematic::StateKinematic_t>(state)->values, m_model->groupID);
m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
}
Added: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPDistanceEvaluators.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPDistanceEvaluators.h (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPDistanceEvaluators.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -0,0 +1,48 @@
+/*********************************************************************
+* 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_DISTANCE_EVALUATORS
+#define KINEMATIC_PLANNING_RKP_DISTANCE_EVALUATORS
+
+#include <ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h>
+#include "kinematic_planning/RKPModelBase.h"
+
+namespace kinematic_planning
+{
+ // no additional distance evaluators at this point
+}
+
+#endif
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPESTSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPESTSetup.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPESTSetup.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -69,9 +69,6 @@
ompl::EST_t est = new ompl::EST(si);
mp = est;
- bool setDim = false;
- bool setProj = false;
-
if (options.find("range") != options.end())
{
double range = parseDouble(options["range"], est->getRange());
@@ -79,44 +76,11 @@
ROS_INFO("Range is set to %g", range);
}
- 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);
- }
-
- est->setProjectionEvaluator(new ompl::OrthogonalProjectionEvaluator(projection));
-
- ROS_INFO("Projection is set to %s", proj.c_str());
- setProj = true;
- }
+ est->setProjectionEvaluator(getProjectionEvaluator(model, options));
- if (options.find("celldim") != options.end())
+ if (est->getProjectionEvaluator() == NULL)
{
- std::string celldim = options["celldim"];
- std::vector<double> cdim;
- std::stringstream ss(celldim);
- while (ss.good())
- {
- double comp;
- ss >> comp;
- cdim.push_back(comp);
- }
-
- est->setCellDimensions(cdim);
- setDim = true;
- ROS_INFO("Cell dimensions set to %s", celldim.c_str());
- }
-
- if (!setDim || !setProj)
- {
- ROS_WARN("Adding EST failed: need to set both 'projection' and 'celldim' for %s", model->groupName.c_str());
+ ROS_WARN("Adding %s failed: need to set both 'projection' and 'celldim' for %s", name.c_str(), model->groupName.c_str());
return false;
}
else
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPIKSBLSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPIKSBLSetup.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPIKSBLSetup.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -69,53 +69,17 @@
ompl::IKSBL* sbl = new ompl::IKSBL(si);
mp = sbl;
- bool setDim = false;
- bool setProj = false;
-
if (options.find("range") != options.end())
{
double range = parseDouble(options["range"], sbl->getRange());
sbl->setRange(range);
ROS_INFO("Range is set to %g", range);
}
+
+ sbl->setProjectionEvaluator(getProjectionEvaluator(model, options));
- if (options.find("projection") != options.end())
+ if (sbl->getProjectionEvaluator() == NULL)
{
- 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));
-
- ROS_INFO("Projection is set to %s", proj.c_str());
- 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;
- ROS_INFO("Cell dimensions set to %s", celldim.c_str());
- }
-
- if (!setDim || !setProj)
- {
ROS_WARN("Adding %s failed: need to set both 'projection' and 'celldim' for %s", name.c_str(), model->groupName.c_str());
return false;
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPKPIECESetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPKPIECESetup.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPKPIECESetup.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -69,9 +69,6 @@
ompl::LBKPIECE1_t kpiece = new ompl::LBKPIECE1(si);
mp = kpiece;
- bool setDim = false;
- bool setProj = false;
-
if (options.find("range") != options.end())
{
double range = parseDouble(options["range"], kpiece->getRange());
@@ -79,43 +76,10 @@
ROS_INFO("Range is set to %g", range);
}
- 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);
- }
-
- kpiece->setProjectionEvaluator(new ompl::OrthogonalProjectionEvaluator(projection));
-
- ROS_INFO("Projection is set to %s", proj.c_str());
- setProj = true;
- }
+ kpiece->setProjectionEvaluator(getProjectionEvaluator(model, options));
- if (options.find("celldim") != options.end())
+ if (kpiece->getProjectionEvaluator() == NULL)
{
- std::string celldim = options["celldim"];
- std::vector<double> cdim;
- std::stringstream ss(celldim);
- while (ss.good())
- {
- double comp;
- ss >> comp;
- cdim.push_back(comp);
- }
-
- kpiece->setCellDimensions(cdim);
- setDim = true;
- ROS_INFO("Cell dimensions set to %s", celldim.c_str());
- }
-
- if (!setDim || !setProj)
- {
ROS_WARN("Adding %s failed: need to set both 'projection' and 'celldim' for %s", name.c_str(), model->groupName.c_str());
return false;
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPPlannerSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPPlannerSetup.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPPlannerSetup.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -40,12 +40,15 @@
#include "kinematic_planning/RKPModelBase.h"
#include "kinematic_planning/RKPStateValidator.h"
#include "kinematic_planning/RKPSpaceInformation.h"
+#include "kinematic_planning/RKPProjectionEvaluators.h"
+#include "kinematic_planning/RKPDistanceEvaluators.h"
#include <ompl/base/Planner.h>
#include <ompl/extension/samplingbased/kinematic/PathSmootherKinematic.h>
#include <ros/console.h>
#include <boost/lexical_cast.hpp>
+#include <boost/algorithm/string.hpp>
#include <cassert>
#include <vector>
@@ -89,6 +92,55 @@
sde["L2Square"] = new ompl::SpaceInformationKinematic::StateKinematicL2SquareDistanceEvaluator(si);
}
+ virtual ompl::ProjectionEvaluator_t getProjectionEvaluator(RKPModelBase *model, const std::map<std::string, std::string> &options) const
+ {
+ std::map<std::string, std::string>::const_iterator pit = options.find("projection");
+ std::map<std::string, std::string>::const_iterator cit = options.find("celldim");
+ ompl::ProjectionEvaluator_t pe = NULL;
+
+ if (pit != options.end() && cit != options.end())
+ {
+ std::string proj = pit->second;
+ boost::trim(proj);
+ std::string celldim = cit->second;
+ boost::trim(celldim);
+
+ if (proj.substr(0, 4) == "link")
+ {
+ std::string linkName = proj.substr(4);
+ boost::trim(linkName);
+ pe = new LinkPositionProjectionEvaluator(model, linkName);
+ }
+ else
+ {
+ std::vector<unsigned int> projection;
+ std::stringstream ss(proj);
+ while (ss.good())
+ {
+ unsigned int comp;
+ ss >> comp;
+ projection.push_back(comp);
+ }
+ pe = new ompl::OrthogonalProjectionEvaluator(projection);
+ }
+
+ std::vector<double> cdim;
+ std::stringstream ss(celldim);
+ while (ss.good())
+ {
+ double comp;
+ ss >> comp;
+ cdim.push_back(comp);
+ }
+
+ pe->setCellDimensions(cdim);
+ ROS_INFO("Projection is set to %s", proj.c_str());
+ ROS_INFO("Cell dimensions set to %s", celldim.c_str());
+ }
+
+ return pe;
+ }
+
virtual void preSetup(RKPModelBase *model, std::map<std::string, std::string> &options)
{
ROS_INFO("Adding %s instance for motion planning: %s", name.c_str(), model->groupName.c_str());
Added: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPProjectionEvaluators.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPProjectionEvaluators.h (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPProjectionEvaluators.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -0,0 +1,85 @@
+/*********************************************************************
+* 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_PROJECTION_EVALUATORS
+#define KINEMATIC_PLANNING_RKP_PROJECTION_EVALUATORS
+
+#include <ompl/extension/samplingbased/kinematic/ProjectionEvaluatorKinematic.h>
+#include <ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h>
+#include "kinematic_planning/RKPModelBase.h"
+
+namespace kinematic_planning
+{
+
+ class LinkPositionProjectionEvaluator : public ompl::ProjectionEvaluator
+ {
+ public:
+
+ LinkPositionProjectionEvaluator(RKPModelBase *model, const std::string &linkName) : ompl::ProjectionEvaluator()
+ {
+ m_model = model;
+ m_link = m_model->kmodel->getLink(linkName);
+ }
+
+ /** Return the dimension of the projection defined by this evaluator */
+ virtual unsigned int getDimension(void) const
+ {
+ return 3;
+ }
+
+ /** Compute the projection as an array of double values */
+ virtual void operator()(const ompl::SpaceInformation::State *state, double *projection) const
+ {
+ const ompl::SpaceInformationKinematic::StateKinematic *kstate = static_cast<const ompl::SpaceInformationKinematic::StateKinematic*>(state);
+ m_model->lock.lock();
+ m_model->kmodel->computeTransformsGroup(kstate->values, m_model->groupID);
+ const btVector3 &origin = m_link->globalTrans.getOrigin();
+ projection[0] = origin.x();
+ projection[1] = origin.y();
+ projection[2] = origin.z();
+ m_model->lock.unlock();
+ }
+
+ protected:
+
+ RKPModelBase *m_model;
+ planning_models::KinematicModel::Link *m_link;
+
+ };
+
+}
+
+#endif
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPSBLSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPSBLSetup.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPSBLSetup.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -69,9 +69,6 @@
ompl::SBL_t sbl = new ompl::SBL(si);
mp = sbl;
- bool setDim = false;
- bool setProj = false;
-
if (options.find("range") != options.end())
{
double range = parseDouble(options["range"], sbl->getRange());
@@ -79,43 +76,10 @@
ROS_INFO("Range is set to %g", range);
}
- 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));
-
- ROS_INFO("Projection is set to %s", proj.c_str());
- setProj = true;
- }
+ sbl->setProjectionEvaluator(getProjectionEvaluator(model, options));
- if (options.find("celldim") != options.end())
+ if (sbl->getProjectionEvaluator() == NULL)
{
- 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;
- ROS_INFO("Cell dimensions set to %s", celldim.c_str());
- }
-
- if (!setDim || !setProj)
- {
ROS_WARN("Adding %s failed: need to set both 'projection' and 'celldim' for %s", name.c_str(), model->groupName.c_str());
return false;
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPStateValidator.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPStateValidator.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPStateValidator.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -57,7 +57,7 @@
virtual bool operator()(const ompl::SpaceInformation::State_t state) const
{
m_model->lock.lock();
- m_model->kmodel->computeTransforms(static_cast<const ompl::SpaceInformationKinematic::StateKinematic_t>(state)->values, m_model->groupID);
+ m_model->kmodel->computeTransformsGroup(static_cast<const ompl::SpaceInformationKinematic::StateKinematic_t>(state)->values, m_model->groupID);
m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
bool valid = !m_model->collisionSpace->isCollision(m_model->collisionSpaceID);
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-18 22:16:43 UTC (rev 11348)
@@ -225,23 +225,31 @@
/** Free the memory */
virtual ~KinematicPlanning(void)
{
+ ROS_INFO("A");
+
+
stopReplanning();
+ ROS_INFO("B");
stopPublishingStatus();
+ ROS_INFO("C");
for (std::map<std::string, RKPModel*>::iterator i = m_models.begin() ; i != m_models.end() ; i++)
delete i->second;
+ ROS_INFO("D");
}
- bool isSafeToPlan(void)
+ bool isSafeToPlan(bool report)
{
if (!isMapUpdated(m_intervalCollisionMap))
{
- ROS_WARN("Planning is not safe: map is not up to date");
+ if (report)
+ ROS_WARN("Planning is not safe: map is not up to date");
return false;
}
if (!isStateUpdated(m_intervalKinematicState))
{
- ROS_WARN("Planning is not safe: kinematic state is not up to date");
+ if (report)
+ ROS_WARN("Planning is not safe: kinematic state is not up to date");
return false;
}
@@ -295,8 +303,11 @@
void startPublishingStatus(void)
{
- m_publishStatus = true;
- m_statusThread = new boost::thread(boost::bind(&KinematicPlanning::publishStatus, this));
+ if (loadedRobot())
+ {
+ m_publishStatus = true;
+ m_statusThread = new boost::thread(boost::bind(&KinematicPlanning::publishStatus, this));
+ }
}
void stopPublishingStatus(void)
@@ -358,7 +369,7 @@
bool result = false;
- res.value.unsafe = isSafeToPlan() ? 0 : 1;
+ res.value.unsafe = isSafeToPlan(true) ? 0 : 1;
result = m_requestState.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
res.value.id = -1;
res.value.done = trivial ? 1 : 0;
@@ -379,7 +390,7 @@
bool result = false;
- res.value.unsafe = isSafeToPlan() ? 0 : 1;
+ res.value.unsafe = isSafeToPlan(true) ? 0 : 1;
result = m_requestLinkPosition.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
res.value.id = -1;
@@ -455,7 +466,7 @@
{
ros::Time nextTime = ros::Time::now() + duration;
bool wait = true;
- while (wait && ros::Time::now() < nextTime)
+ while (m_publishStatus && wait && ros::Time::now() < nextTime)
{
delta.sleep();
if (m_statusLock.try_lock())
@@ -465,7 +476,7 @@
m_statusLock.unlock();
}
}
-
+
bool issueStop = false;
bool replan_inactive = m_continueReplanningLock.try_lock();
@@ -495,7 +506,7 @@
// the sent safety value is the one before the motion
// planning started
if (m_currentPlanStatus.path.states.empty())
- m_currentPlanStatus.unsafe = isSafeToPlan() ? 0 : 1;
+ m_currentPlanStatus.unsafe = isSafeToPlan(false) ? 0 : 1;
publish("kinematic_planning_status", m_currentPlanStatus);
@@ -579,7 +590,7 @@
boost::mutex::scoped_lock lock(m_continueReplanningLock);
m_collisionMonitorChange = false;
double distance = 0.0;
- bool safe = isSafeToPlan();
+ bool safe = isSafeToPlan(true);
currentState(m_currentPlanToStateRequest.start_state);
m_currentlyExecutedPathStart = m_currentPlanToStateRequest.start_state;
@@ -626,7 +637,7 @@
boost::mutex::scoped_lock lock(m_continueReplanningLock);
m_collisionMonitorChange = false;
double distance = 0.0;
- bool safe = isSafeToPlan();
+ bool safe = isSafeToPlan(true);
currentState(m_currentPlanToPositionRequest.start_state);
m_currentlyExecutedPathStart = m_currentPlanToPositionRequest.start_state;
Added: pkg/trunk/motion_planning/ompl/code/ompl/base/ProjectionEvaluator.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/ProjectionEvaluator.h (rev 0)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/ProjectionEvaluator.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -0,0 +1,85 @@
+/*********************************************************************
+* 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_BASE_PROJECTION_EVALUATOR_
+#define OMPL_BASE_PROJECTION_EVALUATOR_
+
+#include "ompl/base/SpaceInformation.h"
+
+namespace ompl
+{
+
+ /** Forward class declaration */
+ ForwardClassDeclaration(ProjectionEvaluator);
+
+ /** Abstract definition for a class computing projections */
+ class ProjectionEvaluator
+ {
+ public:
+ /** Destructor */
+ virtual ~ProjectionEvaluator(void)
+ {
+ }
+
+ /** 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;
+ }
+
+ /** 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 SpaceInformation::State *state, double *projection) const = 0;
+
+ protected:
+
+ std::vector<double> m_cellDimensions;
+
+ };
+
+}
+
+#endif
Deleted: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -1,96 +0,0 @@
-/*********************************************************************
-* 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) const = 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) const
- {
- 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
Copied: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluatorKinematic.h (from rev 11299, pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h)
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluatorKinematic.h (rev 0)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluatorKinematic.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -0,0 +1,79 @@
+/*********************************************************************
+* 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_KINEMATIC_
+#define OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_PROJECTION_EVALUATOR_KINEMATIC_
+
+#include "ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h"
+#include "ompl/base/ProjectionEvaluator.h"
+
+namespace ompl
+{
+
+ /** 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 SpaceInformation::State *state, double *projection) const
+ {
+ const SpaceInformationKinematic::StateKinematic *kstate = static_cast<const SpaceInformationKinematic::StateKinematic*>(state);
+ for (unsigned int i = 0 ; i < m_components.size() ; ++i)
+ projection[i] = kstate->values[m_components[i]];
+ }
+
+ protected:
+
+ std::vector<unsigned int> m_components;
+
+ };
+
+}
+
+#endif
Property changes on: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluatorKinematic.h
___________________________________________________________________
Added: svn:mergeinfo
+
Modified: 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 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/EST.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -38,9 +38,9 @@
#define OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_EST_
#include "ompl/base/Planner.h"
+#include "ompl/base/ProjectionEvaluator.h"
#include "ompl/datastructures/Grid.h"
#include "ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h"
-#include "ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h"
#include <vector>
namespace ompl
@@ -148,25 +148,12 @@
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);
+ m_projectionEvaluator->getCellDimensions(m_cellDimensions);
assert(m_cellDimensions.size() == m_projectionDimension);
m_tree.grid.setDimension(m_projectionDimension);
Planner::setup();
Modified: 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 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/src/EST.cpp 2009-02-18 22:16:43 UTC (rev 11348)
@@ -109,7 +109,7 @@
if (solved)
{
approxdif = dist;
- solution = motion;
+ solution = motion;
break;
}
if (dist < approxdif)
@@ -122,7 +122,7 @@
bool approximate = false;
if (solution == NULL)
- {
+ {
solution = approxsol;
approximate = true;
}
@@ -147,6 +147,9 @@
}
goal_r->setDifference(approxdif);
goal_r->setSolutionPath(path, approximate);
+
+ if (approximate)
+ m_msg.warn("EST: Found approximate solution");
}
delete xstate;
@@ -179,7 +182,7 @@
{
coord.resize(m_projectionDimension);
double projection[m_projectionDimension];
- (*m_projectionEvaluator)(motion->state, projection);
+ (*m_projectionEvaluator)(static_cast<SpaceInformation::State*>(motion->state), projection);
for (unsigned int i = 0 ; i < m_projectionDimension; ++i)
coord[i] = (int)trunc(projection[i]/m_cellDimensions[i]);
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-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/IKPlanner.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -159,6 +159,8 @@
/* copy solution to actual goal instance */
if (solved)
{
+ if (goal_r->isApproximate())
+ _P::m_msg.warn("IKPlanner: Found approximate solution");
goal_r->setSolutionPath(stateGoal->getSolutionPath(), goal_r->isApproximate());
stateGoal->forgetSolutionPath();
}
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/LBKPIECE1.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/LBKPIECE1.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/LBKPIECE1.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -38,9 +38,9 @@
#define OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_KPIECE_LBKPIECE1_
#include "ompl/base/Planner.h"
+#include "ompl/base/ProjectionEvaluator.h"
#include "ompl/datastructures/GridX.h"
#include "ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h"
-#include "ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h"
#include <vector>
@@ -103,20 +103,6 @@
{
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;
- }
/** Set the range the planner is supposed to use. This
parameter greatly influences the runtime of the
@@ -158,6 +144,7 @@
assert(m_projectionEvaluator);
m_projectionDimension = m_projectionEvaluator->getDimension();
assert(m_projectionDimension > 0);
+ m_projectionEvaluator->getCellDimensions(m_cellDimensions);
assert(m_cellDimensions.size() == m_projectionDimension);
m_tStart.grid.setDimension(m_projectionDimension);
m_tGoal.grid.setDimension(m_projectionDimension);
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/src/LBKPIECE1.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/src/LBKPIECE1.cpp 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/src/LBKPIECE1.cpp 2009-02-18 22:16:43 UTC (rev 11348)
@@ -226,7 +226,7 @@
{
coord.resize(m_projectionDimension);
double projection[m_projectionDimension];
- (*m_projectionEvaluator)(motion->state, projection);
+ (*m_projectionEvaluator)(static_cast<SpaceInformation::State*>(motion->state), projection);
for (unsigned int i = 0 ; i < m_projectionDimension; ++i)
coord[i] = (int)trunc(projection[i]/m_cellDimensions[i]);
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp 2009-02-18 22:16:43 UTC (rev 11348)
@@ -156,6 +156,9 @@
}
goal_r->setDifference(approxdif);
goal_r->setSolutionPath(path, approximate);
+
+ if (approximate)
+ m_msg.warn("RRT: Found approximate solution");
}
delete xstate;
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 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/sbl/SBL.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -38,9 +38,9 @@
#define OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_SBL_SBL_
#include "ompl/base/Planner.h"
+#include "ompl/base/ProjectionEvaluator.h"
#include "ompl/datastructures/Grid.h"
#include "ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h"
-#include "ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h"
#include <vector>
@@ -111,20 +111,6 @@
{
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;
- }
/** Set the range the planner is supposed to use. This
parameter greatly influences the runtime of the
@@ -151,6 +137,7 @@
assert(m_projectionEvaluator);
m_projectionDimension = m_projectionEvaluator->getDimension();
assert(m_projectionDimension > 0);
+ m_projectionEvaluator->getCellDimensions(m_cellDimensions);
assert(m_cellDimensions.size() == m_projectionDimension);
m_tStart.grid.setDimension(m_projectionDimension);
m_tGoal.grid.setDimension(m_projectionDimension);
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/sbl/src/SBL.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/sbl/src/SBL.cpp 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/sbl/src/SBL.cpp 2009-02-18 22:16:43 UTC (rev 11348)
@@ -223,7 +223,7 @@
{
coord.resize(m_projectionDimension);
double projection[m_projectionDimension];
- (*m_projectionEvaluator)(motion->state, projection);
+ (*m_projectionEvaluator)(static_cast<SpaceInformation::State*>(motion->state), projection);
for (unsigned int i = 0 ; i < m_projectionDimension; ++i)
coord[i] = (int)trunc(projection[i]/m_cellDimensions[i]);
Modified: pkg/trunk/motion_planning/ompl/code/tests/samplingbased/kinematic/2dmap/2dmap.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/tests/samplingbased/kinematic/2dmap/2dmap.cpp 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/ompl/code/tests/samplingbased/kinematic/2dmap/2dmap.cpp 2009-02-18 22:16:43 UTC (rev 11348)
@@ -37,6 +37,7 @@
#include <gtest/gtest.h>
#include "ompl/extension/samplingbased/kinematic/PathSmootherKinematic.h"
+#include "ompl/extension/samplingbased/kinematic/ProjectionEvaluatorKinematic.h"
#include "ompl/extension/samplingbased/kinematic/extension/kpiece/LBKPIECE1.h"
#include "ompl/extension/samplingbased/kinematic/extension/sbl/SBL.h"
#include "ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h"
@@ -311,15 +312,16 @@
SBL_t sbl = new SBL(si);
sbl->setRange(0.95);
+ std::vector<unsigned int> projection;
+ projection.push_back(0);
+ projection.push_back(1);
+ ope = new OrthogonalProjectionEvaluator(projection);
+
std::vector<double> cdim;
cdim.push_back(1);
cdim.push_back(1);
- sbl->setCellDimensions(cdim);
+ ope->setCellDimensions(cdim);
- std::vector<unsigned int> projection;
- projection.push_back(0);
- projection.push_back(1);
- ope = new OrthogonalProjectionEvaluator(projection);
sbl->setProjectionEvaluator(ope);
return sbl;
@@ -355,15 +357,16 @@
EST_t est = new EST(si);
est->setRange(0.75);
- std::vector<double> cdim;
- cdim.push_back(1);
- cdim.push_back(1);
- est->setCellDimensions(cdim);
-
std::vector<unsigned int> projection;
projection.push_back(0);
projection.push_back(1);
ope = new OrthogonalProjectionEvaluator(projection);
+
+ std::vector<double> cdim;
+ cdim.push_back(1);
+ cdim.push_back(1);
+ ope->setCellDimensions(cdim);
+
est->setProjectionEvaluator(ope);
return est;
@@ -399,15 +402,16 @@
LBKPIECE1_t kpiece = new LBKPIECE1(si);
kpiece->setRange(0.95);
- std::vector<double> cdim;
- cdim.push_back(1);
- cdim.push_back(1);
- kpiece->setCellDimensions(cdim);
-
std::vector<unsigned int> projection;
projection.push_back(0);
projection.push_back(1);
ope = new OrthogonalProjectionEvaluator(projection);
+
+ std::vector<double> cdim;
+ cdim.push_back(1);
+ cdim.push_back(1);
+ ope->setCellDimensions(cdim);
+
kpiece->setProjectionEvaluator(ope);
return kpiece;
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-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-02-18 22:16:43 UTC (rev 11348)
@@ -293,7 +293,7 @@
ros::Duration d(1.0);
d.sleep();
- while (1)
+ while (plan->ok())
{
ros::Duration d(10.0);
@@ -311,6 +311,7 @@
plan->runRightArmToCoordinates();
*/
+
plan->spin();
}
Modified: pkg/trunk/visualization/rviz/src/rviz/displays/planning_display.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/displays/planning_display.cpp 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/visualization/rviz/src/rviz/displays/planning_display.cpp 2009-02-18 22:16:43 UTC (rev 11348)
@@ -244,7 +244,7 @@
{
int group_id = kinematic_model_->getGroupID( displaying_kinematic_path_message_.model_name );
if (displaying_kinematic_path_message_.path.states[ current_state_ ].vals.size() > 0)
- kinematic_model_->computeTransforms(&displaying_kinematic_path_message_.path.states[ current_state_ ].vals[0], group_id);
+ kinematic_model_->computeTransformsGroup(&displaying_kinematic_path_message_.path.states[ current_state_ ].vals[0], group_id);
robot_->update( kinematic_model_, target_frame_ );
causeRender();
Modified: pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -177,7 +177,7 @@
/** Update varTrans if this joint is part of the group indicated by groupID
* and recompute globalTrans using varTrans */
- const double* computeTransform(const double *params, int groupID = -1);
+ const double* computeTransform(const double *params, int groupID);
/** Update the value of VarTrans using the information from params */
virtual void updateVariableTransform(const double *params) = 0;
@@ -371,7 +371,7 @@
unsigned int computeParameterNames(unsigned int pos);
/** recompute globalTrans */
- const double* computeTransform(const double *params, int groupID = -1);
+ const double* computeTransform(const double *params, int groupID);
/** Extract the information needed by the joint given the URDF description */
void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
@@ -611,10 +611,15 @@
m_ignoreSensors = false;
m_verbose = false;
m_built = false;
+ m_lastTransformParams = NULL;
+ m_lastTransformGroup = -2;
}
virtual ~KinematicModel(void)
{
+ if (m_lastTransformParams)
+ delete[] m_lastTransformParams;
+
for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
delete m_robots[i];
}
@@ -644,13 +649,19 @@
void getJoints(std::vector<Joint*> &joints) const;
/** Get the names of the joints in a specific group */
- void getJointsInGroup(std::vector<std::string> &names, int groupID = -1) const;
+ void getJointsInGroup(std::vector<std::string> &names, int groupID) const;
+ /** Get the names of the joints in a specific group */
+ void getJointsInGroup(std::vector<std::string> &names, const std::string &name) const;
/** Returns the dimension of the group (as a state, not number of joints) */
- unsigned int getGroupDimension(int groupID = -1) const;
+ unsigned int getGroupDimension(int groupID) const;
+
+ /** Returns the dimension of the group (as a state, not number of joints) */
+ unsigned int getGroupDimension(const std::string &name) const;
void defaultState(void);
- void computeTransforms(const double *params, int groupID = -1);
+ void computeTransformsGroup(const double *params, int groupID);
+ void computeTransforms(const double *params);
/** Add thansforms to the rootTransform such that the robot is in its planar/floating link frame */
bool reduceToRobotFrame(void);
@@ -676,6 +687,11 @@
bool m_verbose;
bool m_built;
+ /* Subsequent calls with the same argument should not redo computation;
+ * We simply store this state information and compare against it for next time */
+ double *m_lastTransformParams;
+ int m_lastTransformGroup;
+
private:
/** Build the needed datastructure for a joint */
Modified: pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp 2009-02-18 22:16:43 UTC (rev 11348)
@@ -73,14 +73,35 @@
params[i] = 0.0;
else
params[i] = (m_mi.stateBounds[2 * i] + m_mi.stateBounds[2 * i + 1]) / 2.0;
-
- computeTransforms(params);
+ m_lastTransformGroup = -2;
+ computeTransformsGroup(params, -1);
}
-void planning_models::KinematicModel::computeTransforms(const double *params, int groupID)
+void planning_models::KinematicModel::computeTransforms(const double *params)
{
+ computeTransformsGroup(params, -1);
+}
+
+void planning_models::KinematicModel::computeTransformsGroup(const double *params, int groupID)
+{
assert(m_built);
+ const unsigned int gdim = getGroupDimension(groupID);
+ if (m_lastTransformGroup == groupID)
+ {
+ bool same = true;
+ for (unsigned int i = 0 ; i < gdim ; ++i)
+ if (params[i] != m_lastTransformParams[i])
+ {
+ same = false;
+ break;
+ }
+ if (same)
+ return;
+ }
+ m_lastTransformGroup = groupID;
+ memcpy(m_lastTransformParams, params, gdim * sizeof(double));
+
if (groupID >= 0)
{
for (unsigned int i = 0 ; i < m_mi.groupChainStart[groupID].size() ; ++i)
@@ -425,6 +446,8 @@
for (unsigned int j = 0 ; j < m_robots[i]->joints.size() ; ++j)
m_jointMap[m_robots[i]->joints[j]->name] = m_robots[i]->joints[j];
}
+
+ m_lastTransformParams = new double[m_mi.stateDimension];
computeParameterNames();
defaultState();
@@ -494,6 +517,16 @@
return groupID >= 0 ? m_mi.groupStateIndexList[groupID].size() : m_mi.stateDimension;
}
+unsigned int planning_models::KinematicModel::getGroupDimension(const std::string &group) const
+{
+ return getGroupDimension(getGroupID(group));
+}
+
+void planning_models::KinematicModel::getJointsInGroup(std::vector<std::string> &names, const std::string &group) const
+{
+ getJointsInGroup(names, getGroupID(group));
+}
+
void planning_models::KinematicModel::getJointsInGroup(std::vector<std::string> &names, int groupID) const
{
std::vector<Joint*> joints;
Modified: pkg/trunk/world_models/robot_models/planning_models/test/test_kinematic.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/test/test_kinematic.cpp 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/world_models/robot_models/planning_models/test/test_kinematic.cpp 2009-02-18 22:16:43 UTC (rev 11348)
@@ -77,7 +77,7 @@
EXPECT_EQ(std::string("myrobot"), model->getModelName());
EXPECT_EQ((unsigned int)0, model->getRobotCount());
EXPECT_EQ((unsigned int)0, model->getModelInfo().stateDimension);
- EXPECT_EQ((unsigned int)0, model->getGroupDimension());
+ EXPECT_EQ((unsigned int)0, model->getGroupDimension(-1));
std::vector<planning_models::KinematicModel::Link*> links;
model->getLinks(links);
@@ -373,7 +373,7 @@
EXPECT_EQ((unsigned int)5, model->getModelInfo().stateDimension);
double param[5] = { 1, 1, 0.5, -0.5, 0.1 };
- model->computeTransforms(param, model->getGroupID("one_robot::base"));
+ model->computeTransformsGroup(param, model->getGroupID("one_robot::base"));
std::stringstream ss1;
model->printModelInfo(ss1);
@@ -743,7 +743,7 @@
std::stringstream ss;
model->printModelInfo(ss);
double param[8] = { -1, -1, 0, 1.57, 0.0, 5, 5, 0 };
- model->computeTransforms(param, model->getGroupID("more_robots::parts"));
+ model->computeTransformsGroup(param, model->getGroupID("more_robots::parts"));
EXPECT_TRUE(sameStringIgnoringWS(MODEL3_INFO, ss.str()));
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
