Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21900] pkg/trunk
|
From: <is...@us...> - 2009-08-14 22:24:08
|
Revision: 21900
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21900&view=rev
Author: isucan
Date: 2009-08-14 22:23:53 +0000 (Fri, 14 Aug 2009)
Log Message:
-----------
splitting genetic algo search from planning
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
pkg/trunk/highlevel/move_arm/src/move_arm_action.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/src/helpers/Model.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp
pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h
pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
pkg/trunk/sandbox/3dnav_pr2/manifest.xml
Added Paths:
-----------
pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/
pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/
pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/chomp_planner_longrange.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/chomp_planner_shortrange.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_planning.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_search.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/sbpl_planning.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_chomp_planning.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_fake_planning.launch
pkg/trunk/sandbox/ompl_search/
pkg/trunk/sandbox/ompl_search/CMakeLists.txt
pkg/trunk/sandbox/ompl_search/Makefile
pkg/trunk/sandbox/ompl_search/mainpage.dox
pkg/trunk/sandbox/ompl_search/manifest.xml
pkg/trunk/sandbox/ompl_search/src/
pkg/trunk/sandbox/ompl_search/src/SearchRequestHandler.cpp
pkg/trunk/sandbox/ompl_search/src/SearchRequestHandler.h
pkg/trunk/sandbox/ompl_search/src/search_valid_state.cpp
pkg/trunk/sandbox/ompl_search/state_search.launch
Removed Paths:
-------------
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/IKSetup.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/IKSetup.cpp
pkg/trunk/sandbox/3dnav_pr2/launch/perception/components/
pkg/trunk/sandbox/3dnav_pr2/launch/planning/chomp_planner_longrange.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/chomp_planner_shortrange.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_planning.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/sbpl_planning.launch
Modified: pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-08-14 22:23:53 UTC (rev 21900)
@@ -1,13 +1,12 @@
<launch>
<node pkg="move_arm" type="move_arm_action" output="screen">
- <remap from="robot_description" to="robot_description" />
<remap from="collision_map" to="collision_map_occ" />
<remap from="collision_map_update" to="collision_map_occ_update" />
<remap from="arm_ik" to="pr2_ik_right_arm/ik_service" />
- <remap from="get_valid_state" to="find_valid_state" />
+ <remap from="get_valid_state" to="ompl_search/find_valid_state" />
<remap from="controller_start" to="/r_arm_joint_waypoint_controller/TrajectoryStart" />
<remap from="controller_query" to="/r_arm_joint_waypoint_controller/TrajectoryQuery" />
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-14 22:23:53 UTC (rev 21900)
@@ -1041,7 +1041,7 @@
private:
ros::NodeHandle nh_;
- MoveArmSetup &setup_;
+ MoveArmSetup &setup_;
actionlib::SingleGoalActionServer<MoveArmAction> as_;
planning_environment::PlanningMonitor *planningMonitor_;
Modified: pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt 2009-08-14 22:23:53 UTC (rev 21900)
@@ -18,7 +18,6 @@
src/helpers/ompl_planner/kinematicpSBLSetup.cpp
src/helpers/ompl_planner/dynamicRRTSetup.cpp
src/helpers/ompl_planner/dynamicKPIECESetup.cpp
- src/helpers/ompl_planner/IKSetup.cpp
src/helpers/Model.cpp
)
rospack_link_boost(ompl_planning_helpers thread)
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h 2009-08-14 22:23:53 UTC (rev 21900)
@@ -40,7 +40,6 @@
#include "ompl_ros/ModelKinematic.h"
#include "ompl_ros/ModelDynamic.h"
#include "ompl_planning/planners/PlannerSetup.h"
-#include "ompl_planning/planners/IKSetup.h"
#include <boost/shared_ptr.hpp>
#include <string>
@@ -56,7 +55,6 @@
{
planningMonitor = pMonitor;
groupName = gName;
- ik = NULL;
createMotionPlanningInstances(planningMonitor->getCollisionModels()->getGroupPlannersConfig(groupName));
}
@@ -65,15 +63,12 @@
for (std::map<std::string, PlannerSetup*>::iterator i = planners.begin(); i != planners.end() ; ++i)
if (i->second)
delete i->second;
- if (ik)
- delete ik;
}
planning_environment::PlanningMonitor *planningMonitor;
std::string groupName;
std::map<std::string, PlannerSetup*> planners;
- IKSetup *ik;
protected:
Deleted: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/IKSetup.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/IKSetup.h 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/IKSetup.h 2009-08-14 22:23:53 UTC (rev 21900)
@@ -1,62 +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_PLANNING_PLANNERS_IK_SETUP_
-#define OMPL_PLANNING_PLANNERS_IK_SETUP_
-
-#include "ompl_ros/ModelKinematic.h"
-#include <ompl/extension/kinematic/extension/ik/GAIK.h>
-#include <boost/shared_ptr.hpp>
-
-namespace ompl_planning
-{
- class IKSetup
- {
- public:
- IKSetup(void);
- ~IKSetup(void);
-
- void setup(planning_environment::PlanningMonitor *planningMonitor, const std::string &groupName,
- boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
-
- ompl::kinematic::GAIK *gaik; // ik with genetic algorithms
- ompl_ros::ModelBase *ompl_model;
-
- };
-
-}
-
-#endif
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp 2009-08-14 22:23:53 UTC (rev 21900)
@@ -90,13 +90,7 @@
else
if (type == "GAIK")
{
- if (ik)
- {
- ROS_WARN("Re-definition of '%s'", type.c_str());
- delete ik;
- }
- ik = new IKSetup();
- ik->setup(planningMonitor, groupName, cfgs[i]);
+ // skip this, but don't output error
}
else
ROS_WARN("Unknown planner type: %s", type.c_str());
Deleted: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/IKSetup.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/IKSetup.cpp 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/IKSetup.cpp 2009-08-14 22:23:53 UTC (rev 21900)
@@ -1,92 +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 */
-
-#include "ompl_planning/planners/IKSetup.h"
-#include <ros/console.h>
-
-ompl_planning::IKSetup::IKSetup(void)
-{
- ompl_model = NULL;
- gaik = NULL;
-}
-
-ompl_planning::IKSetup::~IKSetup(void)
-{
- if (gaik)
- delete gaik;
- if (ompl_model)
- delete ompl_model;
-}
-
-void ompl_planning::IKSetup::setup(planning_environment::PlanningMonitor *planningMonitor, const std::string &groupName,
- boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
- ROS_DEBUG("Adding IK instance for '%s'", groupName.c_str());
- ompl_model = new ompl_ros::ModelKinematic(planningMonitor, groupName);
- ompl_model->configure();
-
- gaik = new ompl::kinematic::GAIK(dynamic_cast<ompl::kinematic::SpaceInformationKinematic*>(ompl_model->si));
-
- if (options->hasParam("max_improve_steps"))
- {
- gaik->setMaxImproveSteps(options->getParamInt("max_improve_steps", gaik->getMaxImproveSteps()));
- ROS_DEBUG("Max improve steps is set to %u", gaik->getMaxImproveSteps());
- }
-
- if (options->hasParam("range"))
- {
- gaik->setRange(options->getParamDouble("range", gaik->getRange()));
- ROS_DEBUG("Range is set to %g", gaik->getRange());
- }
-
- if (options->hasParam("pool_size"))
- {
- gaik->setPoolSize(options->getParamInt("pool_size", gaik->getPoolSize()));
- ROS_DEBUG("Pool size is set to %u", gaik->getPoolSize());
- }
-
- if (options->hasParam("pool_expansion_size"))
- {
- gaik->setPoolExpansionSize(options->getParamInt("pool_expansion_size", gaik->getPoolExpansionSize()));
- ROS_DEBUG("Pool expansion size is set to %u", gaik->getPoolExpansionSize());
- }
-
- if (options->hasParam("max_improve_steps"))
- {
- gaik->setMaxImproveSteps(options->getParamInt("max_improve_steps", gaik->getMaxImproveSteps()));
- ROS_DEBUG("Max improve steps is set to %u", gaik->getMaxImproveSteps());
- }
-}
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-08-14 22:23:53 UTC (rev 21900)
@@ -58,7 +58,6 @@
nodeHandle_.param<double>("~state_delay", stateDelay_, 0.01);
planKinematicPathService_ = nodeHandle_.advertiseService("~plan_kinematic_path", &OMPLPlanning::planToGoal, this);
- findValidStateService_ = nodeHandle_.advertiseService("find_valid_state", &OMPLPlanning::findValidState, this);
}
/** Free the memory */
@@ -178,33 +177,12 @@
return st;
}
- bool findValidState(motion_planning_msgs::ConvertToJointConstraint::Request &req, motion_planning_msgs::ConvertToJointConstraint::Response &res)
- {
- ROS_INFO("Received request for searching a valid state");
- bool st = false;
-
- planning_models::StateParams *startState = fillStartState(req.start_state);
- if (startState)
- {
- std::stringstream ss;
- startState->print(ss);
- ROS_DEBUG("Complete starting state:\n%s", ss.str().c_str());
- st = requestHandler_.findState(models_, startState, req, res);
- delete startState;
- }
- else
- ROS_ERROR("Starting robot state is unknown. Cannot start search.");
-
- return st;
- }
-
// ROS interface
ros::NodeHandle nodeHandle_;
planning_environment::CollisionModels *collisionModels_;
planning_environment::PlanningMonitor *planningMonitor_;
tf::TransformListener tf_;
ros::ServiceServer planKinematicPathService_;
- ros::ServiceServer findValidStateService_;
double stateDelay_;
// planning data
Modified: pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp 2009-08-14 22:23:53 UTC (rev 21900)
@@ -44,68 +44,6 @@
onFinishPlan_ = onFinishPlan;
}
-bool ompl_planning::RequestHandler::isRequestValid(ModelMap &models, motion_planning_msgs::ConvertToJointConstraint::Request &req)
-{
- ModelMap::const_iterator pos = models.find(req.params.model_id);
-
- if (pos == models.end())
- {
- ROS_ERROR("Cannot search for '%s'. Model does not exist", req.params.model_id.c_str());
- return false;
- }
-
- /* find the model */
- Model *m = pos->second;
-
- IKSetup *iksetup = m->ik;
- if (iksetup == NULL)
- {
- ROS_ERROR("No IK setup available for model '%s'", pos->first.c_str());
- return false;
- }
-
- /* check if the desired distance metric is defined */
- if (iksetup->ompl_model->sde.find(req.params.distance_metric) == iksetup->ompl_model->sde.end())
- {
- ROS_ERROR("Distance evaluator not found: '%s'", req.params.distance_metric.c_str());
- return false;
- }
-
- // check headers
- for (unsigned int i = 0 ; i < req.constraints.pose_constraint.size() ; ++i)
- if (!m->planningMonitor->getTransformListener()->frameExists(req.constraints.pose_constraint[i].pose.header.frame_id))
- {
- ROS_ERROR("Frame '%s' is not defined for pose constraint message %u", req.constraints.pose_constraint[i].pose.header.frame_id.c_str(), i);
- return false;
- }
-
- for (unsigned int i = 0 ; i < req.constraints.joint_constraint.size() ; ++i)
- if (!m->planningMonitor->getTransformListener()->frameExists(req.constraints.joint_constraint[i].header.frame_id))
- {
- ROS_ERROR("Frame '%s' is not defined for joint constraint message %u", req.constraints.joint_constraint[i].header.frame_id.c_str(), i);
- return false;
- }
-
- if (!req.states.empty())
- {
- // make sure all joints are in the group
- for (unsigned int i = 0 ; i < req.names.size() ; ++i)
- {
- int idx = m->planningMonitor->getKinematicModel()->getJointIndexInGroup(req.names[i], m->groupName);
- if (idx < 0)
- return false;
- }
-
- if (iksetup->ompl_model->si->getStateDimension() != m->planningMonitor->getKinematicModel()->getJointsDimension(req.names))
- {
- ROS_ERROR("The state dimension for model '%s' does not match the dimension of the joints defining the hint states", req.params.model_id.c_str());
- return false;
- }
- }
-
- return true;
-}
-
bool ompl_planning::RequestHandler::isRequestValid(ModelMap &models, motion_planning_msgs::GetMotionPlan::Request &req)
{
ModelMap::const_iterator pos = models.find(req.params.model_id);
@@ -190,30 +128,6 @@
return true;
}
-void ompl_planning::RequestHandler::configure(const planning_models::StateParams *startState, motion_planning_msgs::ConvertToJointConstraint::Request &req, IKSetup *iksetup)
-{
- /* clear memory */
- iksetup->ompl_model->si->clearGoal();
-
- // clear clones of environments
- iksetup->ompl_model->clearEnvironmentDescriptions();
-
- setWorkspaceBounds(req.params, iksetup->ompl_model);
- iksetup->ompl_model->si->setStateDistanceEvaluator(iksetup->ompl_model->sde[req.params.distance_metric]);
-
- /* set the pose of the whole robot */
- ompl_ros::EnvironmentDescription *ed = iksetup->ompl_model->getEnvironmentDescription();
- ed->kmodel->computeTransforms(startState->getParams());
- ed->collisionSpace->updateRobotModel();
-
- /* add goal state */
- iksetup->ompl_model->planningMonitor->transformConstraintsToFrame(req.constraints, iksetup->ompl_model->planningMonitor->getFrameId());
- iksetup->ompl_model->si->setGoal(computeGoalFromConstraints(iksetup->ompl_model, req.constraints));
-
- /* print some information */
- printSettings(iksetup->ompl_model->si);
-}
-
void ompl_planning::RequestHandler::setWorkspaceBounds(motion_planning_msgs::KinematicSpaceParameters ¶ms, ompl_ros::ModelBase *ompl_model)
{
/* set the workspace volume for planning */
@@ -341,87 +255,6 @@
return result;
}
-bool ompl_planning::RequestHandler::findState(ModelMap &models, const planning_models::StateParams *start, motion_planning_msgs::ConvertToJointConstraint::Request &req,
- motion_planning_msgs::ConvertToJointConstraint::Response &res)
-{
- if (!isRequestValid(models, req))
- return false;
-
- // find the data we need
- Model *m = models[req.params.model_id];
- IKSetup *iksetup = m->ik;
-
- // copy the hint states to OMPL datastructures
- const unsigned int dim = iksetup->ompl_model->si->getStateDimension();
- ompl::base::State *goal = new ompl::base::State(dim);
- std::vector<ompl::base::State*> hints;
- planning_models::StateParams hs(*start);
- unsigned int sdim = m->planningMonitor->getKinematicModel()->getJointsDimension(req.names);
- for (unsigned int i = 0 ; i < req.states.size() ; ++i)
- {
- if (req.states[i].vals.size() != sdim)
- {
- ROS_ERROR("Incorrect number of parameters for hint state at index %u. Expected %u, got %d.", i, sdim, (int)req.states[i].vals.size());
- continue;
- }
- ompl::base::State *st = new ompl::base::State(dim);
- hs.setParamsJoints(req.states[i].vals, req.names);
- hs.copyParamsGroup(st->values, iksetup->ompl_model->groupID);
- std::stringstream ss;
- ss << "Hint state: ";
- iksetup->ompl_model->si->printState(st, ss);
- ROS_DEBUG("%s", ss.str().c_str());
- hints.push_back(st);
- }
-
- m->planningMonitor->getEnvironmentModel()->lock();
- m->planningMonitor->getKinematicModel()->lock();
-
- configure(start, req, iksetup);
- ros::WallTime startTime = ros::WallTime::now();
- bool found = iksetup->gaik->solve(req.allowed_time, goal, hints);
- double stime = (ros::WallTime::now() - startTime).toSec();
- m->planningMonitor->getEnvironmentModel()->unlock();
- m->planningMonitor->getKinematicModel()->unlock();
-
- ROS_DEBUG("Spent %f seconds searching for state", stime);
-
- if (found)
- {
- int u = 0;
- std::vector<std::string> jnames;
- std::stringstream ss;
- m->planningMonitor->getKinematicModel()->getJointsInGroup(jnames, iksetup->ompl_model->groupID);
- res.joint_constraint.resize(jnames.size());
- for (unsigned int i = 0; i < jnames.size() ; ++i)
- {
- res.joint_constraint[i].header.frame_id = m->planningMonitor->getFrameId();
- res.joint_constraint[i].header.stamp = m->planningMonitor->lastMapUpdate();
- res.joint_constraint[i].joint_name = jnames[i];
- planning_models::KinematicModel::Joint *joint = m->planningMonitor->getKinematicModel()->getJoint(jnames[i]);
- res.joint_constraint[i].value.resize(joint->usedParams);
- res.joint_constraint[i].tolerance_above.resize(joint->usedParams, 0.0);
- res.joint_constraint[i].tolerance_below.resize(joint->usedParams, 0.0);
- for (unsigned int j = 0 ; j < joint->usedParams ; ++j)
- {
- res.joint_constraint[i].value[j] = goal->values[j + u];
- ss << goal->values[j + u] << " ";
- }
- u += joint->usedParams;
- }
- ROS_DEBUG("Solution was found: %s", ss.str().c_str());
- }
- else
- ROS_DEBUG("No solution was found");
-
- iksetup->ompl_model->si->clearGoal();
- delete goal;
- for (unsigned int i = 0 ; i < hints.size() ; ++i)
- delete hints[i];
-
- return true;
-}
-
bool ompl_planning::RequestHandler::computePlan(ModelMap &models, const planning_models::StateParams *start, double stateDelay,
motion_planning_msgs::GetMotionPlan::Request &req, motion_planning_msgs::GetMotionPlan::Response &res)
{
Modified: pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h 2009-08-14 22:23:53 UTC (rev 21900)
@@ -41,7 +41,6 @@
#include <ros/ros.h>
#include <motion_planning_msgs/GetMotionPlan.h>
-#include <motion_planning_msgs/ConvertToJointConstraint.h>
#include <boost/bind.hpp>
/** \brief Main namespace */
@@ -67,16 +66,10 @@
/** \brief Check if the request is valid */
bool isRequestValid(ModelMap &models, motion_planning_msgs::GetMotionPlan::Request &req);
- /** \brief Check if the request is valid */
- bool isRequestValid(ModelMap &models, motion_planning_msgs::ConvertToJointConstraint::Request &req);
-
/** \brief Check and compute a motion plan. Return true if the plan was succesfully computed */
bool computePlan(ModelMap &models, const planning_models::StateParams *start, double stateDelay,
motion_planning_msgs::GetMotionPlan::Request &req, motion_planning_msgs::GetMotionPlan::Response &res);
- /** \brief Find a state in the specified goal region. Return true if state was found */
- bool findState(ModelMap &models, const planning_models::StateParams *start, motion_planning_msgs::ConvertToJointConstraint::Request &req, motion_planning_msgs::ConvertToJointConstraint::Response &res);
-
/** \brief Enable callback for when a motion plan computation is completed */
void setOnFinishPlan(const boost::function<void(PlannerSetup*)> &onFinishPlan);
@@ -92,9 +85,6 @@
/** \brief Set up all the data needed by motion planning based on a request */
void configure(const planning_models::StateParams *startState, motion_planning_msgs::GetMotionPlan::Request &req, PlannerSetup *psetup);
- /** \brief Set up all the data needed by inverse kinematics based on a request */
- void configure(const planning_models::StateParams *startState, motion_planning_msgs::ConvertToJointConstraint::Request &req, IKSetup *iksetup);
-
/** \brief Compute the actual motion plan. Return true if computed plan was trivial (start state already in goal region) */
bool callPlanner(PlannerSetup *psetup, int times, double allowed_time, Solution &sol);
Property changes on: pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/sandbox/3dnav_pr2/launch/perception/components:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch 2009-08-14 22:23:53 UTC (rev 21900)
@@ -30,7 +30,7 @@
<param name="object_padd" type="double" value="0.05" />
</node>
- <include file="$(find 3dnav_pr2)/launch/perception/components/tilt_laser_self_filter.launch" />
+ <include file="$(find 3dnav_pr2)/launch/perception/bits/tilt_laser_self_filter.launch" />
<!-- assemble pointcloud into a full world view -->
<node machine="four" pkg="point_cloud_assembler" type="point_cloud_assembler_srv" output="screen" name="point_cloud_assembler">
@@ -50,6 +50,6 @@
</node>
<!-- start collision map -->
- <include file="$(find 3dnav_pr2)/launch/perception/components/collision_map_self_occ.launch" />
+ <include file="$(find 3dnav_pr2)/launch/perception/bits/collision_map_self_occ.launch" />
</launch>
Copied: pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/chomp_planner_longrange.launch (from rev 21868, pkg/trunk/sandbox/3dnav_pr2/launch/planning/chomp_planner_longrange.launch)
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/chomp_planner_longrange.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/chomp_planner_longrange.launch 2009-08-14 22:23:53 UTC (rev 21900)
@@ -0,0 +1,47 @@
+
+<launch>
+ <include file="$(find pr2_alpha)/$(env ROBOT).machine" />
+
+ <rosparam ns="/robot_description_planning" command="load" file="$(find pr2_defs)/planning/planning.yaml" />
+ <node machine="two" pkg="chomp_motion_planner" name="chomp_planner_longrange" type="chomp_planner_node" args="collision_map:=collision_map_occ" respawn="true" output="screen" clear_params="true">
+ <param name="collision_clearance" value="0.07"/>
+
+ <param name="collision_links/r_gripper_l_finger_link/link_radius" value="0.03"/>
+ <param name="collision_links/r_gripper_r_finger_link/link_radius" value="0.03"/>
+ <param name="collision_links/r_gripper_palm_link/link_radius" value="0.05"/>
+ <param name="collision_links/r_forearm_link/link_radius" value="0.065"/>
+ <param name="collision_links/r_upper_arm_link/link_radius" value="0.08"/>
+
+ <param name="collision_links/r_gripper_l_finger_link/link_clearance" value="0.13"/>
+ <param name="collision_links/r_gripper_r_finger_link/link_clearance" value="0.13"/>
+ <param name="collision_links/r_gripper_palm_link/link_clearance" value="0.1"/>
+ <param name="collision_links/r_forearm_link/link_clearance" value="0.085"/>
+ <param name="collision_links/r_upper_arm_link/link_clearance" value="0.07"/>
+
+ <param name="animate_path" value="false"/>
+ <param name="reference_frame" value="base_link"/>
+
+ <param name="collision_space/size_x" value="2.0" />
+ <param name="collision_space/size_y" value="3.0" />
+ <param name="collision_space/size_z" value="4.0" />
+ <param name="collision_space/origin_x" value="0.1" />
+ <param name="collision_space/origin_y" value="-1.5" />
+ <param name="collision_space/origin_z" value="-2.0" />
+ <param name="collision_space/resolution" value="0.02" />
+
+ <param name="trajectory_duration" value="4.5"/>
+ <param name="trajectory_discretization" value="0.03"/>
+ <param name="learning_rate" value="3.0" />
+ <param name="max_iterations" value="500" />
+ <param name="max_iterations_after_collision_free" value="100" />
+ <param name="smoothness_cost_weight" value="0.000005"/>
+ <param name="obstacle_cost_weight" value="1.0" />
+ <param name="joint_update_limit" value="0.01" />
+
+ <param name="smoothness_cost_velocity" value="0.01" />
+ <param name="smoothness_cost_acceleration" value="1.0" />
+ <param name="smoothness_cost_jerk" value="0.000001" />
+
+ <param name="add_randomness" value="false" />
+ </node>
+</launch>
Property changes on: pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/chomp_planner_longrange.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/sandbox/3dnav_pr2/launch/planning/chomp_planner_longrange.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/chomp_planner_shortrange.launch (from rev 21868, pkg/trunk/sandbox/3dnav_pr2/launch/planning/chomp_planner_shortrange.launch)
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/chomp_planner_shortrange.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/chomp_planner_shortrange.launch 2009-08-14 22:23:53 UTC (rev 21900)
@@ -0,0 +1,47 @@
+
+<launch>
+ <include file="$(find pr2_alpha)/$(env ROBOT).machine" />
+
+ <rosparam ns="/robot_description_planning" command="load" file="$(find pr2_defs)/planning/planning.yaml" />
+ <node machine="two" pkg="chomp_motion_planner" name="chomp_planner_shortrange" type="chomp_planner_node" args="collision_map:=collision_map_occ" respawn="true" output="screen" clear_params="true">
+ <param name="collision_clearance" value="0.07"/>
+
+ <param name="collision_links/r_gripper_l_finger_link/link_radius" value="0.02"/>
+ <param name="collision_links/r_gripper_r_finger_link/link_radius" value="0.02"/>
+ <param name="collision_links/r_gripper_palm_link/link_radius" value="0.05"/>
+ <param name="collision_links/r_forearm_link/link_radius" value="0.065"/>
+ <param name="collision_links/r_upper_arm_link/link_radius" value="0.08"/>
+
+ <param name="collision_links/r_gripper_l_finger_link/link_clearance" value="0.13"/>
+ <param name="collision_links/r_gripper_r_finger_link/link_clearance" value="0.13"/>
+ <param name="collision_links/r_gripper_palm_link/link_clearance" value="0.1"/>
+ <param name="collision_links/r_forearm_link/link_clearance" value="0.085"/>
+ <param name="collision_links/r_upper_arm_link/link_clearance" value="0.07"/>
+
+ <param name="animate_path" value="false"/>
+ <param name="reference_frame" value="base_link"/>
+
+ <param name="collision_space/size_x" value="2.0" />
+ <param name="collision_space/size_y" value="3.0" />
+ <param name="collision_space/size_z" value="4.0" />
+ <param name="collision_space/origin_x" value="0.1" />
+ <param name="collision_space/origin_y" value="-1.5" />
+ <param name="collision_space/origin_z" value="-2.0" />
+ <param name="collision_space/resolution" value="0.02" />
+
+ <param name="trajectory_duration" value="4.0"/>
+ <param name="trajectory_discretization" value="0.04"/>
+ <param name="learning_rate" value="1.0" />
+ <param name="max_iterations" value="500" />
+ <param name="max_iterations_after_collision_free" value="100" />
+ <param name="smoothness_cost_weight" value="0.000005"/>
+ <param name="obstacle_cost_weight" value="1.0" />
+ <param name="joint_update_limit" value="0.01" />
+
+ <param name="smoothness_cost_velocity" value="0.01" />
+ <param name="smoothness_cost_acceleration" value="1.0" />
+ <param name="smoothness_cost_jerk" value="0.000001" />
+
+ <param name="add_randomness" value="false" />
+ </node>
+</launch>
Property changes on: pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/chomp_planner_shortrange.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/sandbox/3dnav_pr2/launch/planning/chomp_planner_shortrange.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_planning.launch (from rev 21868, pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_planning.launch)
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_planning.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_planning.launch 2009-08-14 22:23:53 UTC (rev 21900)
@@ -0,0 +1,19 @@
+<launch>
+
+ <include file="$(find 3dnav_pr2)/launch/prX.machine" />
+
+ <node machine="two" pkg="ompl_planning" type="motion_planner" respawn="true" output="screen">
+ <remap from="collision_map" to="collision_map_occ" />
+ <remap from="collision_map_update" to="collision_map_occ_update" />
+
+ <!-- if we are moving the base, it is best to maintain the map in a fixed frame since identified objects that are being published
+ are not necessarily updated -->
+ <!-- <param name="planner_frame_id" type="string" value="odom_combined" /> -->
+
+ <param name="refresh_interval_collision_map" type="double" value="5.0" />
+ <param name="refresh_interval_kinematic_state" type="double" value="1.0" />
+ <param name="bounding_planes" type="string" value="0 0 1 -0.01" />
+ <param name="verbose_collisions" type="bool" value="false" />
+ </node>
+
+</launch>
Added: pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_search.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_search.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_search.launch 2009-08-14 22:23:53 UTC (rev 21900)
@@ -0,0 +1,19 @@
+<launch>
+
+ <include file="$(find 3dnav_pr2)/launch/prX.machine" />
+
+ <node machine="two" pkg="ompl_search" type="search_state" respawn="true" output="screen">
+ <remap from="collision_map" to="collision_map_occ" />
+ <remap from="collision_map_update" to="collision_map_occ_update" />
+
+ <!-- if we are moving the base, it is best to maintain the map in a fixed frame since identified objects that are being published
+ are not necessarily updated -->
+ <!-- <param name="planner_frame_id" type="string" value="odom_combined" /> -->
+
+ <param name="refresh_interval_collision_map" type="double" value="5.0" />
+ <param name="refresh_interval_kinematic_state" type="double" value="1.0" />
+ <param name="bounding_planes" type="string" value="0 0 1 -0.01" />
+ <param name="verbose_collisions" type="bool" value="false" />
+ </node>
+
+</launch>
Copied: pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/sbpl_planning.launch (from rev 21868, pkg/trunk/sandbox/3dnav_pr2/launch/planning/sbpl_planning.launch)
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/sbpl_planning.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/sbpl_planning.launch 2009-08-14 22:23:53 UTC (rev 21900)
@@ -0,0 +1,41 @@
+<launch>
+
+ <node pkg="sbpl_arm_planner_node" type="sbpl_arm_planner_node" name="right_arm/sbpl_planner" output="screen" respawn="false">
+ <!-- <node launch-prefix="gdb" pkg="sbpl_arm_planner_node" type="sbpl_arm_planner_node" name="right_arm/sbpl_planner" output="screen" respawn="false"/> -->
+
+ <param name="env_config" textfile="$(find sbpl_arm_planner)/env_examples/robarm3d/r_arm.cfg" />
+ <param name="planner_config" textfile="$(find sbpl_arm_planner)/params.cfg" />
+
+ <param name="allocated_time" value="20.0" />
+ <param name="forward_search" value="true" />
+ <param name="search_mode" value="true" />
+ <param name="num_joints" value="7" />
+ <param name="planner_type" value="cartesian" />
+ <param name="use_dijkstra_heuristic" value="true" />
+
+ <param name="torso_arm_offset_x" value="0.0" />
+ <param name="torso_arm_offset_y" value="-0.188" />
+ <param name="torso_arm_offset_z" value="0.0" />
+
+ <param name="collision_map_topic" value="collision_map_occ" />
+ <param name="arm_name" value="right_arm" />
+ <param name="lowres_cc" value="true" />
+ <param name="use_collision_map" value="true" />
+ <param name="use_collision_map_occ_to_update_voxel" value="true" />
+ <param name="use_exact_gripper_collision_checking" value="true" />
+ <param name="use_voxel3d_grid" value="true" />
+
+ <!-- planning monitor -->
+ <remap from="robot_description" to="robot_description" />
+ <remap from="collision_map" to="collision_map_occ" />
+ <remap from="collision_map_update" to="collision_map_occ_update" />
+
+ <param name="arm" type="string" value="right_arm" />
+ <param name="planning_frame_id" type="string" value="torso_lift_link" />
+ <param name="refresh_interval_collision_map" type="double" value="5.0" />
+ <param name="refresh_interval_kinematic_state" type="double" value="1.0" />
+ <param name="bounding_planes" type="string" value="0 0 1 -0.01" />
+
+ </node>
+
+</launch>
Property changes on: pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/sbpl_planning.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/sandbox/3dnav_pr2/launch/planning/sbpl_planning.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Deleted: pkg/trunk/sandbox/3dnav_pr2/launch/planning/chomp_planner_longrange.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/planning/chomp_planner_longrange.launch 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/planning/chomp_planner_longrange.launch 2009-08-14 22:23:53 UTC (rev 21900)
@@ -1,47 +0,0 @@
-
-<launch>
- <include file="$(find pr2_alpha)/$(env ROBOT).machine" />
-
- <rosparam ns="/robot_description_planning" command="load" file="$(find pr2_defs)/planning/planning.yaml" />
- <node machine="two" pkg="chomp_motion_planner" name="chomp_planner_longrange" type="chomp_planner_node" args="collision_map:=collision_map_occ" respawn="true" output="screen" clear_params="true">
- <param name="collision_clearance" value="0.07"/>
-
- <param name="collision_links/r_gripper_l_finger_link/link_radius" value="0.03"/>
- <param name="collision_links/r_gripper_r_finger_link/link_radius" value="0.03"/>
- <param name="collision_links/r_gripper_palm_link/link_radius" value="0.05"/>
- <param name="collision_links/r_forearm_link/link_radius" value="0.065"/>
- <param name="collision_links/r_upper_arm_link/link_radius" value="0.08"/>
-
- <param name="collision_links/r_gripper_l_finger_link/link_clearance" value="0.13"/>
- <param name="collision_links/r_gripper_r_finger_link/link_clearance" value="0.13"/>
- <param name="collision_links/r_gripper_palm_link/link_clearance" value="0.1"/>
- <param name="collision_links/r_forearm_link/link_clearance" value="0.085"/>
- <param name="collision_links/r_upper_arm_link/link_clearance" value="0.07"/>
-
- <param name="animate_path" value="false"/>
- <param name="reference_frame" value="base_link"/>
-
- <param name="collision_space/size_x" value="2.0" />
- <param name="collision_space/size_y" value="3.0" />
- <param name="collision_space/size_z" value="4.0" />
- <param name="collision_space/origin_x" value="0.1" />
- <param name="collision_space/origin_y" value="-1.5" />
- <param name="collision_space/origin_z" value="-2.0" />
- <param name="collision_space/resolution" value="0.02" />
-
- <param name="trajectory_duration" value="4.5"/>
- <param name="trajectory_discretization" value="0.03"/>
- <param name="learning_rate" value="3.0" />
- <param name="max_iterations" value="500" />
- <param name="max_iterations_after_collision_free" value="100" />
- <param name="smoothness_cost_weight" value="0.000005"/>
- <param name="obstacle_cost_weight" value="1.0" />
- <param name="joint_update_limit" value="0.01" />
-
- <param name="smoothness_cost_velocity" value="0.01" />
- <param name="smoothness_cost_acceleration" value="1.0" />
- <param name="smoothness_cost_jerk" value="0.000001" />
-
- <param name="add_randomness" value="false" />
- </node>
-</launch>
Deleted: pkg/trunk/sandbox/3dnav_pr2/launch/planning/chomp_planner_shortrange.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/planning/chomp_planner_shortrange.launch 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/planning/chomp_planner_shortrange.launch 2009-08-14 22:23:53 UTC (rev 21900)
@@ -1,47 +0,0 @@
-
-<launch>
- <include file="$(find pr2_alpha)/$(env ROBOT).machine" />
-
- <rosparam ns="/robot_description_planning" command="load" file="$(find pr2_defs)/planning/planning.yaml" />
- <node machine="two" pkg="chomp_motion_planner" name="chomp_planner_shortrange" type="chomp_planner_node" args="collision_map:=collision_map_occ" respawn="true" output="screen" clear_params="true">
- <param name="collision_clearance" value="0.07"/>
-
- <param name="collision_links/r_gripper_l_finger_link/link_radius" value="0.02"/>
- <param name="collision_links/r_gripper_r_finger_link/link_radius" value="0.02"/>
- <param name="collision_links/r_gripper_palm_link/link_radius" value="0.05"/>
- <param name="collision_links/r_forearm_link/link_radius" value="0.065"/>
- <param name="collision_links/r_upper_arm_link/link_radius" value="0.08"/>
-
- <param name="collision_links/r_gripper_l_finger_link/link_clearance" value="0.13"/>
- <param name="collision_links/r_gripper_r_finger_link/link_clearance" value="0.13"/>
- <param name="collision_links/r_gripper_palm_link/link_clearance" value="0.1"/>
- <param name="collision_links/r_forearm_link/link_clearance" value="0.085"/>
- <param name="collision_links/r_upper_arm_link/link_clearance" value="0.07"/>
-
- <param name="animate_path" value="false"/>
- <param name="reference_frame" value="base_link"/>
-
- <param name="collision_space/size_x" value="2.0" />
- <param name="collision_space/size_y" value="3.0" />
- <param name="collision_space/size_z" value="4.0" />
- <param name="collision_space/origin_x" value="0.1" />
- <param name="collision_space/origin_y" value="-1.5" />
- <param name="collision_space/origin_z" value="-2.0" />
- <param name="collision_space/resolution" value="0.02" />
-
- <param name="trajectory_duration" value="4.0"/>
- <param name="trajectory_discretization" value="0.04"/>
- <param name="learning_rate" value="1.0" />
- <param name="max_iterations" value="500" />
- <param name="max_iterations_after_collision_free" value="100" />
- <param name="smoothness_cost_weight" value="0.000005"/>
- <param name="obstacle_cost_weight" value="1.0" />
- <param name="joint_update_limit" value="0.01" />
-
- <param name="smoothness_cost_velocity" value="0.01" />
- <param name="smoothness_cost_acceleration" value="1.0" />
- <param name="smoothness_cost_jerk" value="0.000001" />
-
- <param name="add_randomness" value="false" />
- </node>
-</launch>
Added: pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_chomp_planning.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_chomp_planning.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_chomp_planning.launch 2009-08-14 22:23:53 UTC (rev 21900)
@@ -0,0 +1,7 @@
+<launch>
+
+ <include file="$(find 3dnav_pr2)/launch/planning/bits/ompl_planning.launch" />
+ <include file="$(find 3dnav_pr2)/launch/planning/bits/chomp_planner_shortrange.launch" />
+ <include file="$(find 3dnav_pr2)/launch/planning/bits/ompl_search.launch" />
+
+</launch>
Added: pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_fake_planning.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_fake_planning.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_fake_planning.launch 2009-08-14 22:23:53 UTC (rev 21900)
@@ -0,0 +1,7 @@
+<launch>
+
+ <include file="$(find 3dnav_pr2)/launch/planning/bits/ompl_planning.launch" />
+ <include file="$(find 3dnav_pr2)/launch/planning/bits/fake_planning.launch" />
+ <include file="$(find 3dnav_pr2)/launch/planning/bits/ompl_search.launch" />
+
+</launch>
Deleted: pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_planning.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_planning.launch 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_planning.launch 2009-08-14 22:23:53 UTC (rev 21900)
@@ -1,19 +0,0 @@
-<launch>
-
- <include file="$(find 3dnav_pr2)/launch/prX.machine" />
-
- <node machine="two" pkg="ompl_planning" type="motion_planner" respawn="true" output="screen">
- <remap from="collision_map" to="collision_map_occ" />
- <remap from="collision_map_update" to="collision_map_occ_update" />
-
- <!-- if we are moving the base, it is best to maintain the map in a fixed frame since identified objects that are being published
- are not necessarily updated -->
- <!-- <param name="planner_frame_id" type="string" value="odom_combined" /> -->
-
- <param name="refresh_interval_collision_map" type="double" value="5.0" />
- <param name="refresh_interval_kinematic_state" type="double" value="1.0" />
- <param name="bounding_planes" type="string" value="0 0 1 -0.01" />
- <param name="verbose_collisions" type="bool" value="false" />
- </node>
-
-</launch>
Deleted: pkg/trunk/sandbox/3dnav_pr2/launch/planning/sbpl_planning.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/planning/sbpl_planning.launch 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/planning/sbpl_planning.launch 2009-08-14 22:23:53 UTC (rev 21900)
@@ -1,41 +0,0 @@
-<launch>
-
- <node pkg="sbpl_arm_planner_node" type="sbpl_arm_planner_node" name="right_arm/sbpl_planner" output="screen" respawn="false">
- <!-- <node launch-prefix="gdb" pkg="sbpl_arm_planner_node" type="sbpl_arm_planner_node" name="right_arm/sbpl_planner" output="screen" respawn="false"/> -->
-
- <param name="env_config" textfile="$(find sbpl_arm_planner)/env_examples/robarm3d/r_arm.cfg" />
- <param name="planner_config" textfile="$(find sbpl_arm_planner)/params.cfg" />
-
- <param name="allocated_time" value="20.0" />
- <param name="forward_search" value="true" />
- <param name="search_mode" value="true" />
- <param name="num_joints" value="7" />
- <param name="planner_type" value="cartesian" />
- <param name="use_dijkstra_heuristic" value="true" />
-
- <param name="torso_arm_offset_x" value="0.0" />
- <param name="torso_arm_offset_y" value="-0.188" />
- <param name="torso_arm_offset_z" value="0.0" />
-
- <param name="collision_map_topic" value="collision_map_occ" />
- <param name="arm_name" value="right_arm" />
- <param name="lowres_cc" value="true" />
- <param name="use_collision_map" value="true" />
- <param name="use_collision_map_occ_to_update_voxel" value="true" />
- <param name="use_exact_gripper_collision_checking" value="true" />
- <param name="use_voxel3d_grid" value="true" />
-
- <!-- planning monitor -->
- <remap from="robot_description" to="robot_description" />
- <remap from="collision_map" to="collision_map_occ" />
- <remap from="collision_map_update" to="collision_map_occ_update" />
-
- <param name="arm" type="string" value="right_arm" />
- <param name="planning_frame_id" type="string" value="torso_lift_link" />
- <param name="refresh_interval_collision_map" type="double" value="5.0" />
- <param name="refresh_interval_kinematic_state" type="double" value="1.0" />
- <param name="bounding_planes" type="string" value="0 0 1 -0.01" />
-
- </node>
-
-</launch>
Modified: pkg/trunk/sandbox/3dnav_pr2/manifest.xml
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/manifest.xml 2009-08-14 22:14:40 UTC (rev 21899)
+++ pkg/trunk/sandbox/3dnav_pr2/manifest.xml 2009-08-14 22:23:53 UTC (rev 21900)
@@ -41,6 +41,7 @@
<depend package="planning_environment" />
<depend package="ompl_planning" />
+ <depend package="ompl_search" />
<depend package="fake_motion_planning" />
<depend package="pr2_ik" />
<depend package="move_arm" />
Added: pkg/trunk/sandbox/ompl_search/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/ompl_search/CMakeLists.txt (rev 0)
+++ pkg/trunk/sandbox/ompl_search/CMakeLists.txt 2009-08-14 22:23:53 UTC (rev 21900)
@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+set(ROS_BUILD_TYPE Debug)
+
+rospack(ompl_search)
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+
+rospack_add_executable(search_state src/search_valid_state.cpp src/SearchRequestHandler.cpp)
Added: pkg/trunk/sandbox/ompl_search/Makefile
===================================================================
--- pkg/trunk/sandbox/ompl_search/Makefile (rev 0)
+++ pkg/trunk/sandbox/ompl_search/Makefile 2009-08-14 22:23:53 UTC (rev 21900)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/sandbox/ompl_search/mainpage.dox
===================================================================
--- pkg/trunk/sandbox/ompl_search/mainpage.dox (rev 0)
+++ pkg/trunk/sandbox/ompl_search/mainpage.dox 2009-08-14 22:23:53 UTC (rev 21900)
@@ -0,0 +1,119 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b ompl_search is ...
+
+<!--
+In addition to providing an overview of your package,
+this is the section where the specification and design/architecture
+should be detailed. While the original specification may be done on the
+wiki, it should be transferred here once your package starts to take shape.
+You can then link to this documentation page from the Wiki.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+\section rosapi ROS API
+
+<!--
+Names are very important in ROS because they can be remapped on the
+command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
+APPEAR IN THE CODE. You should list names of every topic, service and
+parameter used in your code. There is a template below that you can
+use to document each node separately.
+
+List of nodes:
+- \b node_name1
+- \b node_name2
+-->
+
+<!-- START: copy from here to 'END' for each node
+
+<hr>
+
+\subsection node_name node_name
+
+node_name does (provide a basic description of your node)
+
+\subsubsection Usage
+\verbatim
+$ node_type1 [standard ROS args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ node_type1
+\endverbatim
+
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- \b "in": [std_msgs/FooType] description of in
+
+Publishes to:
+- \b "out": [std_msgs/FooType] description of out
+
+
+\subsubsection parameters ROS parameters
+
+Reads the following parameters from the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+- \b "~my_param" : \b [string] description of my_param
+
+Sets the following parameters on the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+
+
+\subsubsection services ROS services
+- \b "foo_service": [std_srvs/FooType] description of foo_service
+
+
+END: copy for each node -->
+
+
+<!-- START: Uncomment if you have any command-line tools
+
+\section commandline Command-line tools
+
+This section is a catch-all for any additional tools that your package
+provides or uses that may be of use to the reader. For example:
+
+- tools/scripts (e.g. rospack, roscd)
+- roslaunch .launch files
+- xmlparam files
+
+\subsection script_name script_name
+
+Description of what this script/file does.
+
+\subsubsection Usage
+\verbatim
+$ ./script_name [args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ ./script_name foo bar
+\endverbatim
+
+END: Command-Line Tools Section -->
+
+*/
\ No newline at end of file
Added: pkg/trunk/sandbox/ompl_search/manifest.xml
===================================================================
--- pkg/trunk/sandbox/ompl_search/manifest.xml (rev 0)
+++ pkg/trunk/sandbox/ompl_search/manifest.xml 2009-08-14 22:23:53 UTC (rev 21900)
@@ -0,0 +1,18 @@
+<package>
+ <description brief="ompl_search">
+
+ ompl_search
+
+ </description>
+ <author>Ioan Sucan</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/ompl_search</url>
+
+ <depend package="ompl_ros" />
+ <depend package="roscpp" />
+ <depend package="motion_planning_msgs" />
+
+</package>
+
+
Added: pkg/trunk/sandbox/ompl_search/src/SearchRequestHandler.cpp
===================================================================
--- pkg/trunk/sandbox/ompl_search/src/SearchRequestHandler.cpp (rev 0)
+++ pkg/trunk/sandbox/ompl_search/src/SearchRequestHandler.cpp 2009-08-14 22:23:53 UTC (rev 21900)
@@ -0,0 +1,240 @@
+/*********************************************************************
+* 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 "SearchRequestHandler.h"
+#include <ompl_ros/base/GoalDefinitions.h>
+#include <ros/console.h>
+#include <sstream>
+#include <cstdlib>
+
+bool ompl_search::SearchRequestHandler::isRequestValid(ModelMap &models, motion_planning_msgs::ConvertToJointConstraint::Request &req)
+{
+ ModelMap::const_iterator pos = models.find(req.params.model_id);
+
+ if (pos == models.end())
+ {
+ ROS_ERROR("Cannot search for '%s'. Model does not exist", req.params.model_id.c_str());
+ return false;
+ }
+
+ /* find the model */
+ SearchModel *m = pos->second;
+
+ /* check if the desired distance metric is defined */
+ if (m->mk->sde.find(req.params.distance_metric) == m->mk->sde.end())
+ {
+ ROS_ERROR("Distance evaluator not found: '%s'", req.params.distance_metric.c_str());
+ return false;
+ }
+
+ // check headers
+ for (unsigned int i = 0 ; i < req.constraints.pose_constraint.size() ; ++i)
+ if (!m->mk->planningMonitor->getTransformListener()->frameExists(req.constraints.pose_constraint[i].pose.header.frame_id))
+ {
+ ROS_ERROR("Frame '%s' is not defined for pose constraint message %u", req.constraints.pose_constraint[i].pose.header.frame_id.c_str(), i);
+ return false;
+ }
+
+ for (unsigned int i = 0 ; i < req.constraints.joint_constraint.size() ; ++i)
+ if (!m->mk->planningMonitor->getTransformListener()->frameExists(req.constraints.joint_constraint[i].header.frame_id))
+ {
+ ROS_ERROR("Frame '%s' is not defined for joint constraint message %u", req.constraints.joint_constraint[i].header.frame_id.c_str(), i);
+ return false;
+ }
+
+ if (!req.states.empty())
+ {
+ // make sure all joints are in the group
+ for (unsigned int i = 0 ; i < req.names.size() ; ++i)
+ {
+ int idx = m->mk->planningMonitor->getKinematicModel()->getJointIndexInGroup(req.names[i], m->mk->groupID);
+ if (idx < 0)
+ return false;
+ }
+
+ if (m->mk->si->getStateDimension() != m->mk->planningMonitor->getKinematicModel()->getJointsDimension(req.names))
+ {
+ ROS_ERROR("The state dimension for model '%s' does not match the dimension of the joints defining the hint states", req.params.model_id.c_str());
+ return false;
+ }
+ }
+
+ return true;
+}
+
+void ompl_search::SearchRequestHandler::configure(const planning_models::StateParams *startState, motion_planning_msgs::ConvertToJointConstraint::Request &req, SearchModel *model)
+{
+ /* clear memory */
+ ...
[truncated message content] |
