Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[20094] pkg/trunk
|
From: <is...@us...> - 2009-07-30 00:50:35
|
Revision: 20094
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20094&view=rev
Author: isucan
Date: 2009-07-30 00:50:20 +0000 (Thu, 30 Jul 2009)
Log Message:
-----------
bugfixes for planning environment, gaik; removed motion_planning_srvs package ; added service for searching for valid states using GAIK ; using this service in move_arm (seeded with ik solutions) to find valid states
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
pkg/trunk/highlevel/move_arm/launch/move_larm.launch
pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
pkg/trunk/highlevel/move_arm/manifest.xml
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/motion_planning/motion_planning_msgs/CMakeLists.txt
pkg/trunk/motion_planning/motion_planning_msgs/manifest.xml
pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicSpaceParameters.msg
pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/GAIK.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/src/GAIK.cpp
pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/PlannerSetup.h
pkg/trunk/motion_planning/ompl_planning/manifest.xml
pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/GoalDefinitions.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/plan_ompl_path/manifest.xml
pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/util/kinematic_state_constraint_evaluator.cpp
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_planner_node.h
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/manifest.xml
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp
pkg/trunk/motion_planning/planning_research/sbpl_pm_wrapper/include/sbpl_pm_wrapper/pm_wrapper.h
pkg/trunk/motion_planning/planning_research/sbpl_pm_wrapper/manifest.xml
pkg/trunk/motion_planning/planning_research/sbpl_pm_wrapper/src/pm_wrapper.cpp
pkg/trunk/sandbox/3dnav_pr2/manifest.xml
pkg/trunk/sandbox/3dnav_pr2/params/planning/planning.yaml
pkg/trunk/sandbox/chomp_motion_planner/include/chomp_motion_planner/chomp_planner_node.h
pkg/trunk/sandbox/chomp_motion_planner/manifest.xml
pkg/trunk/sandbox/chomp_motion_planner/src/chomp_planner_node.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/motion_planning_msgs/srv/
pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertPoseToJointConstraint.srv
pkg/trunk/motion_planning/motion_planning_msgs/srv/GetMotionPlan.srv
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/IKSetup.cpp
pkg/trunk/motion_planning/ompl_planning/src/request_handler/
pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp
pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h
pkg/trunk/motion_planning/plan_ompl_path/ROS_BUILD_BLACKLIST
Removed Paths:
-------------
pkg/trunk/motion_planning/motion_planning_srvs/
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/RequestHandler.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
Modified: pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
===================================================================
--- pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-07-30 00:50:20 UTC (rev 20094)
@@ -44,7 +44,7 @@
#include <manipulation_msgs/JointTraj.h>
#include <pr2_robot_actions/MoveArmState.h>
#include <pr2_robot_actions/MoveArmGoal.h>
-#include <motion_planning_srvs/MotionPlan.h>
+#include <motion_planning_msgs/GetMotionPlan.h>
#include <ros/ros.h>
#include <planning_environment/monitors/planning_monitor.h>
@@ -104,8 +104,8 @@
bool fixState(planning_models::StateParams &sp, bool atGoal);
bool fillStartState(std::vector<motion_planning_msgs::KinematicJoint> &start);
void fillTrajectoryPath(const motion_planning_msgs::KinematicPath &path, manipulation_msgs::JointTraj &traj);
- bool alterRequestUsingIK(motion_planning_srvs::MotionPlan::Request &req);
- bool computeIK(ros::ServiceClient &client, const robot_msgs::PoseStamped &pose_stamped_msg, int attempts, std::vector<double> &solution);
+ bool alterRequestUsingIK(motion_planning_msgs::GetMotionPlan::Request &req);
+ bool computeIK(ros::ServiceClient &client, const robot_msgs::PoseStamped &pose_stamped_msg, std::vector<double> &solution);
};
}
Modified: pkg/trunk/highlevel/move_arm/launch/move_larm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/move_larm.launch 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/highlevel/move_arm/launch/move_larm.launch 2009-07-30 00:50:20 UTC (rev 20094)
@@ -6,13 +6,14 @@
<remap from="collision_map_update" to="collision_map_occ_update" />
<remap from="arm_ik" to="pr2_ik_left_arm/ik_service" />
- <remap from="arm_ik_query" to="pr2_ik_left_arm/ik_query" />
+
+ <remap from="get_valid_state" to="find_valid_state" />
<remap from="controller_start" to="l_arm_joint_trajectory_controller/TrajectoryStart" />
<remap from="controller_query" to="l_arm_joint_trajectory_controller/TrajectoryQuery" />
<remap from="controller_cancel" to="l_arm_joint_trajectory_controller/TrajectoryCancel" />
- <remap from="motion_plan" to="plan_kinematic_path" />
+ <remap from="get_motion_plan" to="plan_kinematic_path" />
<param name="arm" type="string" value="left_arm" />
<param name="show_collisions" type="bool" value="true" />
Modified: pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-07-30 00:50:20 UTC (rev 20094)
@@ -6,13 +6,14 @@
<remap from="collision_map_update" to="collision_map_occ_update" />
<remap from="arm_ik" to="pr2_ik_right_arm/ik_service" />
- <remap from="arm_ik_query" to="pr2_ik_right_arm/ik_query" />
+
+ <remap from="get_valid_state" to="find_valid_state" />
<remap from="controller_start" to="r_arm_joint_trajectory_controller/TrajectoryStart" />
<remap from="controller_query" to="r_arm_joint_trajectory_controller/TrajectoryQuery" />
<remap from="controller_cancel" to="r_arm_joint_trajectory_controller/TrajectoryCancel" />
- <remap from="motion_plan" to="plan_kinematic_path" />
+ <remap from="get_motion_plan" to="plan_kinematic_path" />
<param name="arm" type="string" value="right_arm" />
<param name="show_collisions" type="bool" value="true" />
Modified: pkg/trunk/highlevel/move_arm/manifest.xml
===================================================================
--- pkg/trunk/highlevel/move_arm/manifest.xml 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/highlevel/move_arm/manifest.xml 2009-07-30 00:50:20 UTC (rev 20094)
@@ -14,7 +14,6 @@
<depend package="std_msgs"/>
<depend package="robot_msgs"/>
<depend package="motion_planning_msgs"/>
- <depend package="motion_planning_srvs"/>
<depend package="manipulation_srvs"/>
<depend package="visualization_msgs"/>
<depend package="planning_models" />
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-07-30 00:50:20 UTC (rev 20094)
@@ -43,6 +43,7 @@
#include <manipulation_srvs/IKService.h>
#include <manipulation_srvs/IKQuery.h>
+#include <motion_planning_msgs/ConvertPoseToJointConstraint.h>
#include <visualization_msgs/Marker.h>
#include <cstdlib>
@@ -53,14 +54,13 @@
{
// these are the strings used internally to access services
// they should be remaped in the launch file
- static const std::string CONTROL_START_NAME = "controller_start";
- static const std::string CONTROL_QUERY_NAME = "controller_query";
- static const std::string CONTROL_CANCEL_NAME = "controller_cancel";
- static const std::string MOTION_PLAN_NAME = "motion_plan";
+ static const std::string CONTROL_START_NAME = "controller_start";
+ static const std::string CONTROL_QUERY_NAME = "controller_query";
+ static const std::string CONTROL_CANCEL_NAME = "controller_cancel";
+ static const std::string MOTION_PLAN_NAME = "get_motion_plan";
+ static const std::string SEARCH_VALID_STATE_NAME = "get_valid_state";
+ static const std::string ARM_IK_NAME = "arm_ik";
- static const std::string ARM_IK_NAME = "arm_ik";
- static const std::string ARM_IK_QUERY_NAME = "arm_ik_query";
-
MoveArm::MoveArm(const::std::string &arm_name) : Action<pr2_robot_actions::MoveArmGoal, int32_t>("move_" + arm_name)
{
valid_ = true;
@@ -120,8 +120,8 @@
return robot_actions::ABORTED;
}
- motion_planning_srvs::MotionPlan::Request req;
- motion_planning_srvs::MotionPlan::Response res;
+ motion_planning_msgs::GetMotionPlan::Request req;
+ motion_planning_msgs::GetMotionPlan::Response res;
req.params.model_id = arm_; // the model to plan for (should be defined in planning.yaml)
@@ -140,17 +140,19 @@
// tell the planning monitor about the constraints we will be following
planningMonitor_->setPathConstraints(req.path_constraints);
planningMonitor_->setGoalConstraints(req.goal_constraints);
+
+ // fill the staring state
+ fillStartState(req.start_state);
if (perform_ik_)
alterRequestUsingIK(req);
-
ResultStatus result = robot_actions::SUCCESS;
feedback = pr2_robot_actions::MoveArmState::PLANNING;
update(feedback);
- ros::ServiceClient clientPlan = node_handle_.serviceClient<motion_planning_srvs::MotionPlan>(MOTION_PLAN_NAME, true);
+ ros::ServiceClient clientPlan = node_handle_.serviceClient<motion_planning_msgs::GetMotionPlan>(MOTION_PLAN_NAME, true);
ros::ServiceClient clientStart = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryStart>(CONTROL_START_NAME, true);
ros::ServiceClient clientQuery = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryQuery>(CONTROL_QUERY_NAME, true);
ros::ServiceClient clientCancel = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryCancel>(CONTROL_CANCEL_NAME, true);
@@ -369,19 +371,28 @@
void MoveArm::fillTrajectoryPath(const motion_planning_msgs::KinematicPath &path, manipulation_msgs::JointTraj &traj)
{
- /// \todo Joint controller does not take joint names; make sure we set them when the controller is updated
traj.names = arm_joint_names_;
traj.points.resize(path.states.size());
+ planning_models::StateParams *sp = planningMonitor_->getKinematicModel()->newStateParams();
for (unsigned int i = 0 ; i < path.states.size() ; ++i)
{
- traj.points[i].positions = path.states[i].vals;
traj.points[i].time = path.times[i];
+ traj.points[i].positions.reserve(path.states[i].vals.size());
+
+ sp->setParamsGroup(path.states[i].vals, arm_);
+ for (unsigned int j = 0 ; j < arm_joint_names_.size() ; ++j)
+ {
+ planning_models::KinematicModel::Joint *joint = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[j]);
+ const double *params = sp->getParamsJoint(arm_joint_names_[j]);
+ for (unsigned int k = 0 ; k < joint->usedParams ; ++k)
+ traj.points[i].positions.push_back(params[k]);
+ }
}
+ delete sp;
}
void MoveArm::printPath(const motion_planning_msgs::KinematicPath &path)
{
- ROS_DEBUG("Received path with %d states", (int)path.states.size());
for (unsigned int i = 0 ; i < path.states.size() ; ++i)
{
std::stringstream ss;
@@ -426,17 +437,17 @@
ros::ServiceClient client_query = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryQuery>(CONTROL_QUERY_NAME);
pr2_mechanism_controllers::TrajectoryQuery::Request req_query;
pr2_mechanism_controllers::TrajectoryQuery::Response res_query;
- req_query.trajectoryid = -1;
+ req_query.trajectoryid = pr2_mechanism_controllers::TrajectoryQuery::Request::Query_Joint_Names;
bool result = client_query.call(req_query, res_query);
if (!result)
{
- ROS_WARN("Unable to retrieve controller joint names from control query service. Waiting a bit and retrying...");
+ ROS_INFO("Querying controller for joint names ...");
ros::Duration(5.0).sleep();
result = client_query.call(req_query, res_query);
if (result)
- ROS_WARN("Retrieved controller joints on second attempt");
+ ROS_INFO("Joint names received");
}
if (!result)
@@ -467,13 +478,10 @@
planningMonitor_->getKinematicModel()->getJointsInGroup(groupNames, arm_);
if (groupNames.size() != joint_names.size())
{
- ROS_ERROR("The group '%s' has more joints than the controller can handle", arm_.c_str());
+ ROS_ERROR("The group '%s' does not have the same number of joints as the controller can handle", arm_.c_str());
return false;
}
- if (groupNames != joint_names)
- ROS_WARN("The group joints are not in the same order as the controller expects");
-
return true;
}
@@ -548,7 +556,7 @@
return (upper_bound - lower_bound) * drand48() + lower_bound;
}
- bool MoveArm::alterRequestUsingIK(motion_planning_srvs::MotionPlan::Request &req)
+ bool MoveArm::alterRequestUsingIK(motion_planning_msgs::GetMotionPlan::Request &req)
{
bool result = false;
@@ -566,28 +574,24 @@
ROS_INFO("Converting pose constraint to joint constraint using IK...");
planningMonitor_->getEnvironmentModel()->setVerbose(false);
- ros::ServiceClient client = node_handle_.serviceClient<manipulation_srvs::IKService>(ARM_IK_NAME, true);
- for (int t = 0 ; t < 2 ; ++t)
+ ros::ServiceClient ik_client = node_handle_.serviceClient<manipulation_srvs::IKService>(ARM_IK_NAME, true);
+
+ // find an IK solution
+ std::vector<double> solution;
+ if (computeIK(ik_client, req.goal_constraints.pose_constraint[0].pose, solution))
{
- robot_msgs::PoseStamped tpose = req.goal_constraints.pose_constraint[0].pose;
- if (t > 0)
+ // check if it is a valid state
+ planning_models::StateParams spTest(*planningMonitor_->getRobotState());
+ spTest.setParamsJoints(solution, arm_joint_names_);
+
+ if (planningMonitor_->isStateValidAtGoal(&spTest))
{
- tpose.pose.position.x = uniformDouble(tpose.pose.position.x - req.goal_constraints.pose_constraint[0].position_tolerance_below.x,
- tpose.pose.position.x + req.goal_constraints.pose_constraint[0].position_tolerance_above.x);
- tpose.pose.position.y = uniformDouble(tpose.pose.position.y - req.goal_constraints.pose_constraint[0].position_tolerance_below.y,
- tpose.pose.position.y + req.goal_constraints.pose_constraint[0].position_tolerance_above.y);
- tpose.pose.position.z = uniformDouble(tpose.pose.position.z - req.goal_constraints.pose_constraint[0].position_tolerance_below.z,
- tpose.pose.position.z + req.goal_constraints.pose_constraint[0].position_tolerance_above.z);
- }
- std::vector<double> solution;
- if (computeIK(client, tpose, 5, solution))
- {
unsigned int n = 0;
for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
{
motion_planning_msgs::JointConstraint jc;
jc.joint_name = arm_joint_names_[i];
- jc.header.frame_id = tpose.header.frame_id;
+ jc.header.frame_id = req.goal_constraints.pose_constraint[0].pose.header.frame_id;
jc.header.stamp = planningMonitor_->lastMechanismStateUpdate();
unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
for (unsigned int j = 0 ; j < u ; ++j)
@@ -600,22 +604,92 @@
req.goal_constraints.joint_constraint.push_back(jc);
}
req.goal_constraints.pose_constraint.clear();
-
+
// update the goal constraints for the planning monitor as well
planningMonitor_->setGoalConstraints(req.goal_constraints);
result = true;
}
+ else
+ {
+ // if it is not, we try to fix it
+ motion_planning_msgs::ConvertPoseToJointConstraint::Request c_req;
+ c_req.params = req.params;
+ c_req.start_state = req.start_state;
+ c_req.constraints = req.goal_constraints;
+ c_req.names = arm_joint_names_;
+ c_req.states.resize(1);
+ c_req.states[0].vals = solution;
+ c_req.allowed_time = 0.3;
+
+ // add a few more guesses
+ for (int t = 0 ; t < 4 ; ++t)
+ {
+ robot_msgs::PoseStamped tpose = req.goal_constraints.pose_constraint[0].pose;
+ tpose.pose.position.x = uniformDouble(tpose.pose.position.x - req.goal_constraints.pose_constraint[0].position_tolerance_below.x,
+ tpose.pose.position.x + req.goal_constraints.pose_constraint[0].position_tolerance_above.x);
+ tpose.pose.position.y = uniformDouble(tpose.pose.position.y - req.goal_constraints.pose_constraint[0].position_tolerance_below.y,
+ tpose.pose.position.y + req.goal_constraints.pose_constraint[0].position_tolerance_above.y);
+ tpose.pose.position.z = uniformDouble(tpose.pose.position.z - req.goal_constraints.pose_constraint[0].position_tolerance_below.z,
+ tpose.pose.position.z + req.goal_constraints.pose_constraint[0].position_tolerance_above.z);
+ std::vector<double> sol;
+ if (computeIK(ik_client, tpose, sol))
+ {
+ unsigned int sz = c_req.states.size();
+ c_req.states.resize(sz + 1);
+ c_req.states[sz].vals = sol;
+ }
+ }
+
+ motion_planning_msgs::ConvertPoseToJointConstraint::Response c_res;
+ ros::ServiceClient s_client = node_handle_.serviceClient<motion_planning_msgs::ConvertPoseToJointConstraint>(SEARCH_VALID_STATE_NAME);
+ if (s_client.call(c_req, c_res))
+ {
+ if (!c_res.joint_constraint.empty())
+ {
+ req.goal_constraints.joint_constraint = c_res.joint_constraint;
+ req.goal_constraints.pose_constraint.clear();
+
+ // update the goal constraints for the planning monitor as well
+ planningMonitor_->setGoalConstraints(req.goal_constraints);
+ result = true;
+ }
+ }
+ else
+ ROS_ERROR("Service for searching for valid states failed");
+ }
}
- planningMonitor_->getEnvironmentModel()->setVerbose(true);
- if (!result)
- ROS_WARN("Unable to compute IK");
+ else
+ {
+ motion_planning_msgs::ConvertPoseToJointConstraint::Request c_req;
+ c_req.params = req.params;
+ c_req.start_state = req.start_state;
+ c_req.constraints = req.goal_constraints;
+ c_req.names = arm_joint_names_;
+ c_req.allowed_time = 0.3;
+ motion_planning_msgs::ConvertPoseToJointConstraint::Response c_res;
+ ros::ServiceClient s_client = node_handle_.serviceClient<motion_planning_msgs::ConvertPoseToJointConstraint>(SEARCH_VALID_STATE_NAME);
+ if (s_client.call(c_req, c_res))
+ {
+ if (!c_res.joint_constraint.empty())
+ {
+ req.goal_constraints.joint_constraint = c_res.joint_constraint;
+ req.goal_constraints.pose_constraint.clear();
+
+ // update the goal constraints for the planning monitor as well
+ planningMonitor_->setGoalConstraints(req.goal_constraints);
+ result = true;
+ }
+ }
+ else
+ ROS_ERROR("Service for searching for valid states failed");
+ }
}
}
return result;
}
- bool MoveArm::computeIK(ros::ServiceClient &client, const robot_msgs::PoseStamped &pose_stamped_msg, int attempts, std::vector<double> &solution)
+ bool MoveArm::computeIK(ros::ServiceClient &client, const robot_msgs::PoseStamped &pose_stamped_msg, std::vector<double> &solution)
{
// define the service messages
manipulation_srvs::IKService::Request request;
@@ -624,82 +698,36 @@
request.data.pose_stamped = pose_stamped_msg;
request.data.joint_names = arm_joint_names_;
- bool validSolution = false;
- int ikSteps = 0;
- while (ikSteps < attempts && !validSolution)
+ planning_models::StateParams *sp = planningMonitor_->getKinematicModel()->newStateParams();
+ sp->randomStateGroup(arm_);
+ for(unsigned int i = 0; i < arm_joint_names_.size() ; ++i)
{
- request.data.positions.clear();
- planning_models::StateParams *sp = NULL;
- if (ikSteps == 0)
- sp = new planning_models::StateParams(*planningMonitor_->getRobotState());
- else
+ const double *params = sp->getParamsJoint(arm_joint_names_[i]);
+ const unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
+ for (unsigned int j = 0 ; j < u ; ++j)
+ request.data.positions.push_back(params[j]);
+ }
+ delete sp;
+
+ if (client.call(request, response))
+ {
+ ROS_DEBUG("Obtained IK solution");
+ solution = response.solution;
+ if (solution.size() != request.data.positions.size())
{
- sp = planningMonitor_->getKinematicModel()->newStateParams();
- sp->randomStateGroup(arm_);
- }
- ikSteps++;
-
- for(unsigned int i = 0; i < arm_joint_names_.size() ; ++i)
- {
- const double *params = sp->getParamsJoint(arm_joint_names_[i]);
- const unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
- for (unsigned int j = 0 ; j < u ; ++j)
- request.data.positions.push_back(params[j]);
- }
- delete sp;
-
- if (client.call(request, response))
- {
- ROS_DEBUG("Obtained IK solution");
- solution = response.solution;
- if (solution.size() != request.data.positions.size())
- {
- ROS_ERROR("Incorrect number of elements in IK output");
- return false;
- }
-
- for(unsigned int i = 0; i < solution.size() ; ++i)
- ROS_DEBUG("IK[%d] = %f", (int)i, solution[i]);
-
- // if IK did not fail, check if the state is valid
- planning_models::StateParams spTest(*planningMonitor_->getRobotState());
- unsigned int n = 0;
- for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
- {
- unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
- for (unsigned int j = 0 ; j < u ; ++j)
- {
- std::vector<double> params(solution.begin() + n, solution.begin() + n + u);
- spTest.setParamsJoint(params, arm_joint_names_[i]);
- }
- n += u;
- }
-
- // if state is not valid, we try to fix it
- fixState(spTest, true);
-
- if (planningMonitor_->isStateValidAtGoal(&spTest))
- {
- validSolution = true;
-
- // update solution
- solution.clear();
- for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
- {
- std::vector<double> params;
- spTest.copyParamsJoint(params, arm_joint_names_[i]);
- solution.insert(solution.end(), params.begin(), params.end());
- }
- }
- }
- else
- {
- ROS_ERROR("IK service failed");
+ ROS_ERROR("Incorrect number of elements in IK output");
return false;
}
+ for(unsigned int i = 0; i < solution.size() ; ++i)
+ ROS_DEBUG("IK[%d] = %f", (int)i, solution[i]);
}
-
- return validSolution;
+ else
+ {
+ ROS_ERROR("IK service failed");
+ return false;
+ }
+
+ return true;
}
}
Modified: pkg/trunk/motion_planning/motion_planning_msgs/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/CMakeLists.txt 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/motion_planning/motion_planning_msgs/CMakeLists.txt 2009-07-30 00:50:20 UTC (rev 20094)
@@ -2,3 +2,4 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(motion_planning_msgs)
genmsg()
+gensrv()
Modified: pkg/trunk/motion_planning/motion_planning_msgs/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/manifest.xml 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/motion_planning/motion_planning_msgs/manifest.xml 2009-07-30 00:50:20 UTC (rev 20094)
@@ -13,7 +13,7 @@
<depend package="robot_msgs"/>
<export>
- <cpp cflags="-I${prefix}/msg/cpp"/>
+ <cpp cflags="-I${prefix}/msg/cpp -I${prefix}/srv/cpp"/>
</export>
</package>
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicSpaceParameters.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicSpaceParameters.msg 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicSpaceParameters.msg 2009-07-30 00:50:20 UTC (rev 20094)
@@ -5,12 +5,6 @@
# for which the motion planner should plan for
string model_id
-
-# The name of the motion planner to use. If no name is specified,
-# a default motion planner will be used
-string planner_id
-
-
# A string that identifies which distance metric the planner should use
string distance_metric
Added: pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertPoseToJointConstraint.srv
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertPoseToJointConstraint.srv (rev 0)
+++ pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertPoseToJointConstraint.srv 2009-07-30 00:50:20 UTC (rev 20094)
@@ -0,0 +1,30 @@
+# Parameters for the state space
+motion_planning_msgs/KinematicSpaceParameters params
+
+
+# Starting state updates. If certain joints should be considered
+# at positions other than the current ones, these positions should
+# be set here
+motion_planning_msgs/KinematicJoint[] start_state
+
+
+# The joint names, in the same order as the values in the state
+string[] names
+
+# A list of states, each with the dimension of the requested model.
+# The dimension of the model may be different from the number of joints
+# (when there are joints that use more than one parameter)
+# Each state is to be used as a hint (initialization) for the search
+# if the valid state
+motion_planning_msgs/KinematicState[] states
+
+# The input constraints, to be converted to a set of joint constraints
+motion_planning_msgs/KinematicConstraints constraints
+
+# The maximum amount of time the search is to be run for
+float64 allowed_time
+
+---
+
+# The set of joint constraints that correspond to the pose constraint
+motion_planning_msgs/JointConstraint[] joint_constraint
Added: pkg/trunk/motion_planning/motion_planning_msgs/srv/GetMotionPlan.srv
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/srv/GetMotionPlan.srv (rev 0)
+++ pkg/trunk/motion_planning/motion_planning_msgs/srv/GetMotionPlan.srv 2009-07-30 00:50:20 UTC (rev 20094)
@@ -0,0 +1,40 @@
+# This service contains the definition for a request to the motion
+# planner and the output it provides
+
+# Parameters for the state space
+motion_planning_msgs/KinematicSpaceParameters params
+
+# Starting state updates. If certain joints should be considered
+# at positions other than the current ones, these positions should
+# be set here
+motion_planning_msgs/KinematicJoint[] start_state
+
+# The goal state for the model to plan for. The goal is achieved
+# if all constraints are satisfied
+motion_planning_msgs/KinematicConstraints goal_constraints
+
+# No state in the produced motion plan will violate these constraints
+motion_planning_msgs/KinematicConstraints path_constraints
+
+# The name of the motion planner to use. If no name is specified,
+# a default motion planner will be used
+string planner_id
+
+# The number of times this plan is to be computed. Shortest solution
+# will be reported.
+int32 times
+
+# The maximum amount of time the motion planner is allowed to plan for
+float64 allowed_time
+
+---
+
+# A solution path, if found
+motion_planning_msgs/KinematicPath path
+
+# distance to goal as computed by internal planning heuristic
+# this should be 0.0 if the solution was achieved exaclty
+float64 distance
+
+# set to 1 if the computed path was approximate
+byte approximate
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/GAIK.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/GAIK.h 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/GAIK.h 2009-07-30 00:50:20 UTC (rev 20094)
@@ -122,7 +122,7 @@
}
/** \brief Get the number of individuals to add to the population in each generation */
- unsigned int getPoolExtensionSize(void) const
+ unsigned int getPoolExpansionSize(void) const
{
return m_poolExpansion;
}
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/src/GAIK.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/src/GAIK.cpp 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/src/GAIK.cpp 2009-07-30 00:50:20 UTC (rev 20094)
@@ -163,7 +163,7 @@
bool ompl::kinematic::GAIK::tryToImprove(base::State *state, double distance)
{
- m_msg.inform("GAIK: Distance to goal before improvement: %g", distance);
+ m_msg.message("GAIK: Distance to goal before improvement: %g", distance);
time_utils::Time start = time_utils::Time::now();
m_hcik.tryToImprove(state, 0.1, &distance);
m_hcik.tryToImprove(state, 0.05, &distance);
@@ -175,7 +175,7 @@
m_hcik.tryToImprove(state, 0.00005, &distance);
m_hcik.tryToImprove(state, 0.000025, &distance);
m_hcik.tryToImprove(state, 0.000005, &distance);
- m_msg.inform("GAIK: Improvement took %g seconds", (time_utils::Time::now() - start).toSeconds());
- m_msg.inform("GAIK: Distance to goal after improvement: %g", distance);
+ m_msg.message("GAIK: Improvement took %g seconds", (time_utils::Time::now() - start).toSeconds());
+ m_msg.message("GAIK: Distance to goal after improvement: %g", distance);
return true;
}
Modified: pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt 2009-07-30 00:50:20 UTC (rev 20094)
@@ -18,17 +18,16 @@
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/ompl_extensions/GoalDefinitions.cpp
src/helpers/ompl_extensions/SpaceInformation.cpp
src/helpers/ompl_extensions/StateValidator.cpp
src/helpers/ompl_extensions/ForwardPropagationModels.cpp
- src/helpers/RequestHandler.cpp
src/helpers/Model.cpp
src/helpers/ModelBase.cpp
)
-
rospack_link_boost(ompl_planning_helpers thread)
-rospack_add_executable(motion_planner src/motion_planner.cpp)
+rospack_add_executable(motion_planner src/motion_planner.cpp src/request_handler/RequestHandler.cpp)
rospack_link_boost(motion_planner thread)
target_link_libraries(motion_planner ompl_planning_helpers)
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h 2009-07-30 00:50:20 UTC (rev 20094)
@@ -39,6 +39,7 @@
#include "ompl_planning/ModelBase.h"
#include "ompl_planning/planners/PlannerSetup.h"
+#include "ompl_planning/planners/IKSetup.h"
#include <boost/shared_ptr.hpp>
#include <string>
@@ -52,6 +53,7 @@
public:
Model(void) : ModelBase()
{
+ ik = NULL;
}
virtual ~Model(void)
@@ -59,12 +61,15 @@
for (std::map<std::string, PlannerSetup*>::iterator i = planners.begin(); i != planners.end() ; ++i)
if (i->second)
delete i->second;
+ if (ik)
+ delete ik;
}
/* instantiate the planners that can be used */
void createMotionPlanningInstances(std::vector< boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> > cfgs);
- std::map<std::string, PlannerSetup*> planners;
+ std::map<std::string, PlannerSetup*> planners;
+ IKSetup *ik;
protected:
@@ -75,6 +80,15 @@
typedef std::map<std::string, Model*> ModelMap;
+ /** \brief Create all the instances needed by OMPL using the planning parameters from the ROS server */
+ void setupPlanningModels(planning_environment::PlanningMonitor *planningMonitor, ModelMap &models);
+
+ /** \brief Get a list of known models */
+ std::vector<std::string> knownModels(ModelMap &models);
+
+ /** \brief Free all allocated memory */
+ void destroyPlanningModels(ModelMap &models);
+
} // ompl_planning
#endif
Deleted: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/RequestHandler.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/RequestHandler.h 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/RequestHandler.h 2009-07-30 00:50:20 UTC (rev 20094)
@@ -1,130 +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_REQUEST_HANDLER_
-#define OMPL_PLANNING_REQUEST_HANDLER_
-
-#include "ompl_planning/Model.h"
-#include <ros/ros.h>
-#include <motion_planning_srvs/MotionPlan.h>
-
-/** \brief Main namespace */
-namespace ompl_planning
-{
-
- /** \brief This class represents a basic request to a motion
- planner. */
-
- class RequestHandler
- {
- public:
-
- RequestHandler(void)
- {
- debug_ = false;
- px_ = py_ = pz_ = -1;
- }
-
- ~RequestHandler(void)
- {
- }
-
- /** \brief Enable debug mode. Display markers consisting of a 1-D orthogonal projection of states */
- void enableDebugMode(int idx)
- {
- enableDebugMode(idx, -1, -1);
- }
-
- /** \brief Enable debug mode. Display markers consisting of a 2-D orthogonal projection of states */
- void enableDebugMode(int idx1, int idx2)
- {
- enableDebugMode(idx1, idx2, -1);
- }
-
- /** \brief Enable debug mode. Display markers consisting of a 3-D orthogonal projection of states */
- void enableDebugMode(int idx1, int idx2, int idx3);
-
- /** \brief Disable debug mode */
- void disableDebugMode(void);
-
- /** \brief Check if the request is valid */
- bool isRequestValid(ModelMap &models, motion_planning_srvs::MotionPlan::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, motion_planning_srvs::MotionPlan::Request &req, motion_planning_srvs::MotionPlan::Response &res);
-
- /** \brief Print the planner setup settings as debug messages */
- void printSettings(PlannerSetup *psetup);
-
- private:
-
- struct Solution
- {
- ompl::base::Path *path;
- double difference;
- bool approximate;
- };
-
- /** \brief Set up all the data needed by motion planning based on a request and lock the planner setup
- * using this data */
- void configure(const planning_models::StateParams *startState, motion_planning_srvs::MotionPlan::Request &req, PlannerSetup *psetup);
-
- /** \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);
-
- /** \brief Fill the response with solution data */
- void fillResult(PlannerSetup *psetup, const planning_models::StateParams *start, motion_planning_srvs::MotionPlan::Response &res, const Solution &sol);
-
- /** \brief Fix the input states, if they are not valid */
- bool fixInputStates(PlannerSetup *psetup, double value, unsigned int count);
-
- /** \brief Send visualization markers */
- void display(PlannerSetup *psetup);
-
- /** \brief If true, broadcast tree structure as a set of visualization markers */
- bool debug_;
-
- /** \brief The orthogonal projection to take when displaying states */
- int px_, py_, pz_;
-
- ros::Publisher displayPublisher_;
- ros::NodeHandle nh_;
-
- };
-
-} // ompl_planning
-
-#endif
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/PlannerSetup.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/PlannerSetup.h 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/PlannerSetup.h 2009-07-30 00:50:20 UTC (rev 20094)
@@ -49,8 +49,6 @@
#include <ompl/base/Planner.h>
#include <ompl/extension/kinematic/PathSmootherKinematic.h>
-#include <planning_environment/models/robot_models.h>
-
#include <ros/console.h>
#include <string>
#include <map>
Modified: pkg/trunk/motion_planning/ompl_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/manifest.xml 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/motion_planning/ompl_planning/manifest.xml 2009-07-30 00:50:20 UTC (rev 20094)
@@ -15,11 +15,14 @@
<depend package="robot_srvs" />
<depend package="visualization_msgs" />
<depend package="motion_planning_msgs" />
- <depend package="motion_planning_srvs" />
<depend package="mapping_msgs" />
<depend package="tabletop_msgs" />
<depend package="planning_environment"/>
<depend package="ompl" />
+ <export>
+ <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lompl_planning_helpers `rosboost-cfg --lflags thread`"/>
+ </export>
+
</package>
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp 2009-07-30 00:50:20 UTC (rev 20094)
@@ -51,7 +51,6 @@
/* instantiate the planners that can be used */
void ompl_planning::Model::createMotionPlanningInstances(std::vector< boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> > cfgs)
{
-
for (unsigned int i = 0 ; i < cfgs.size() ; ++i)
{
std::string type = cfgs[i]->getParamString("type");
@@ -89,6 +88,17 @@
if (type == "dynamic::KPIECE")
add_planner<dynamicKPIECESetup>(cfgs[i]);
else
+ if (type == "GAIK")
+ {
+ if (ik)
+ {
+ ROS_WARN("Re-definition of '%s'", type.c_str());
+ delete ik;
+ }
+ ik = new IKSetup(dynamic_cast<ModelBase*>(this));
+ ik->setup(cfgs[i]);
+ }
+ else
ROS_WARN("Unknown planner type: %s", type.c_str());
}
}
@@ -98,8 +108,52 @@
{
PlannerSetup *p = new _T(dynamic_cast<ModelBase*>(this));
if (p->setup(options))
- planners[p->name + "[" + options->getName() + "]"] = p;
+ {
+ std::string location = p->name + "[" + options->getName() + "]";
+ if (planners.find(location) != planners.end())
+ {
+ ROS_WARN("Re-definition of '%s'", location.c_str());
+ delete planners[location];
+ }
+ planners[location] = p;
+ }
else
delete p;
}
+void ompl_planning::setupPlanningModels(planning_environment::PlanningMonitor *planningMonitor, ompl_planning::ModelMap &models)
+{
+ ROS_DEBUG("=======================================");
+ std::stringstream ss;
+ planningMonitor->getKinematicModel()->printModelInfo(ss);
+ ROS_DEBUG("%s", ss.str().c_str());
+ ROS_DEBUG("=======================================");
+
+ /* create a model for each group */
+ std::map< std::string, std::vector<std::string> > groups = planningMonitor->getCollisionModels()->getPlanningGroups();
+
+ for (std::map< std::string, std::vector<std::string> >::iterator it = groups.begin(); it != groups.end() ; ++it)
+ {
+ Model *model = new Model();
+ model->planningMonitor = planningMonitor;
+ model->groupID = planningMonitor->getKinematicModel()->getGroupID(it->first);
+ model->groupName = it->first;
+ model->createMotionPlanningInstances(planningMonitor->getCollisionModels()->getGroupPlannersConfig(model->groupName));
+ models[model->groupName] = model;
+ }
+}
+
+std::vector<std::string> ompl_planning::knownModels(ompl_planning::ModelMap &models)
+{
+ std::vector<std::string> model_ids;
+ for (std::map<std::string, Model*>::const_iterator i = models.begin() ; i != models.end() ; ++i)
+ model_ids.push_back(i->first);
+ return model_ids;
+}
+
+void ompl_planning::destroyPlanningModels(ompl_planning::ModelMap &models)
+{
+ for (std::map<std::string, Model*>::iterator i = models.begin() ; i != models.end() ; i++)
+ delete i->second;
+ models.clear();
+}
Deleted: pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-30 00:50:20 UTC (rev 20094)
@@ -1,509 +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/RequestHandler.h"
-#include <visualization_msgs/Marker.h>
-#include <ros/console.h>
-#include <sstream>
-#include <cstdlib>
-
-bool ompl_planning::RequestHandler::isRequestValid(ModelMap &models, motion_planning_srvs::MotionPlan::Request &req)
-{
- ModelMap::const_iterator pos = models.find(req.params.model_id);
-
- if (pos == models.end())
- {
- ROS_ERROR("Cannot plan for '%s'. Model does not exist", req.params.model_id.c_str());
- return false;
- }
-
- /* find the model */
- Model *m = pos->second;
-
- /* if the user did not specify a planner, use the first available one */
- if (req.params.planner_id.empty())
- for (std::map<std::string, PlannerSetup*>::const_iterator it = m->planners.begin() ; it != m->planners.end() ; ++it)
- if ((req.goal_constraints.pose_constraint.empty() && (it->second->mp->getType() & ompl::base::PLAN_TO_GOAL_STATE) != 0) ||
- (!req.goal_constraints.pose_constraint.empty() && (it->second->mp->getType() & ompl::base::PLAN_TO_GOAL_REGION) != 0))
- {
- if (req.params.planner_id.empty())
- req.params.planner_id = it->first;
- else
- if (m->planners[req.params.planner_id]->priority < it->second->priority ||
- (m->planners[req.params.planner_id]->priority == it->second->priority && rand() % 2 == 1))
- req.params.planner_id = it->first;
- }
-
- /* check if desired planner exists */
- std::map<std::string, PlannerSetup*>::iterator plannerIt = m->planners.end();
- for (std::map<std::string, PlannerSetup*>::iterator it = m->planners.begin() ; it != m->planners.end() ; ++it)
- if (it->first.find(req.params.planner_id) != std::string::npos)
- {
- req.params.planner_id = it->first;
- plannerIt = it;
- break;
- }
-
- if (plannerIt == m->planners.end())
- {
- ROS_ERROR("Motion planner not found: '%s'", req.params.planner_id.c_str());
- return false;
- }
-
- PlannerSetup *psetup = plannerIt->second;
-
- /* check if the desired distance metric is defined */
- if (psetup->sde.find(req.params.distance_metric) == psetup->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.goal_constraints.pose_constraint.size() ; ++i)
- if (!m->planningMonitor->getTransformListener()->frameExists(req.goal_constraints.pose_constraint[i].pose.header.frame_id))
- {
- ROS_ERROR("Frame '%s' is not defined for goal pose constraint message %u", req.goal_constraints.pose_constraint[i].pose.header.frame_id.c_str(), i);
- return false;
- }
-
- for (unsigned int i = 0 ; i < req.goal_constraints.joint_constraint.size() ; ++i)
- if (!m->planningMonitor->getTransformListener()->frameExists(req.goal_constraints.joint_constraint[i].header.frame_id))
- {
- ROS_ERROR("Frame '%s' is not defined for goal joint constraint message %u", req.goal_constraints.joint_constraint[i].header.frame_id.c_str(), i);
- return false;
- }
-
- for (unsigned int i = 0 ; i < req.path_constraints.pose_constraint.size() ; ++i)
- if (!m->planningMonitor->getTransformListener()->frameExists(req.path_constraints.pose_constraint[i].pose.header.frame_id))
- {
- ROS_ERROR("Frame '%s' is not defined for path pose constraint message %u", req.path_constraints.pose_constraint[i].pose.header.frame_id.c_str(), i);
- return false;
- }
-
- for (unsigned int i = 0 ; i < req.path_constraints.joint_constraint.size() ; ++i)
- if (!m->planningMonitor->getTransformListener()->frameExists(req.path_constraints.joint_constraint[i].header.frame_id))
- {
- ROS_ERROR("Frame '%s' is not defined for path joint constraint message %u", req.path_constraints.joint_constraint[i].header.frame_id.c_str(), i);
- return false;
- }
-
- return true;
-}
-
-void ompl_planning::RequestHandler::configure(const planning_models::StateParams *startState, motion_planning_srvs::MotionPlan::Request &req, PlannerSetup *psetup)
-{
- /* clear memory */
- psetup->si->clearGoal(); // goal definitions
- psetup->si->clearStartStates(); // start states
-
- // clear clones of environments
- psetup->model->clearEnvironmentDescriptions();
-
- /* before configuring, we may need to update bounds on the state space */
- static_cast<StateValidityPredicate*>(psetup->si->getStateValidityChecker())->setConstraints(req.path_constraints);
-
- /* set the workspace volume for planning */
- if (psetup->model->planningMonitor->getTransformListener()->frameExists(req.params.volumeMin.header.frame_id) &&
- psetup->model->planningMonitor->getTransformListener()->frameExists(req.params.volumeMax.header.frame_id))
- {
- bool err = false;
- try
- {
- psetup->model->planningMonitor->getTransformListener()->transformPoint(psetup->model->planningMonitor->getFrameId(), req.params.volumeMin, req.params.volumeMin);
- psetup->model->planningMonitor->getTransformListener()->transformPoint(psetup->model->planningMonitor->getFrameId(), req.params.volumeMax, req.params.volumeMax);
- }
- catch(...)
- {
- err = true;
- }
- if (err)
- ROS_ERROR("Unable to transform workspace bounds to planning frame");
- else
- {
- // only area or volume should go through
- if (SpaceInformationKinematicModel *s = dynamic_cast<SpaceInformationKinematicModel*>(psetup->si))
- {
- s->setPlanningArea(req.params.volumeMin.point.x, req.params.volumeMin.point.y,
- req.params.volumeMax.point.x, req.params.volumeMax.point.y);
- s->setPlanningVolume(req.params.volumeMin.point.x, req.params.volumeMin.point.y, req.params.volumeMin.point.z,
- req.params.volumeMax.point.x, req.params.volumeMax.point.y, req.params.volumeMax.point.z);
- }
- if (SpaceInformationDynamicModel *s = dynamic_cast<SpaceInformationDynamicModel*>(psetup->si))
- {
- s->setPlanningArea(req.params.volumeMin.point.x, req.params.volumeMin.point.y,
- req.params.volumeMax.point.x, req.params.volumeMax.point.y);
- s->setPlanningVolume(req.params.volumeMin.point.x, req.params.volumeMin.point.y, req.params.volumeMin.point.z,
- req.params.volumeMax.point.x, req.params.volumeMax.point.y, req.params.volumeMax.point.z);
- }
- }
- }
- else
- ROS_DEBUG("No workspace bounding volume was set");
-
- psetup->si->setStateDistanceEvaluator(psetup->sde[req.params.distance_metric]);
-
- /* set the starting state */
- const unsigned int dim = psetup->si->getStateDimension();
- ompl::base::State *start = new ompl::base::State(dim);
-
- if (psetup->model->groupID >= 0)
- {
- /* set the pose of the whole robot */
- EnvironmentDescription *ed = psetup->model->getEnvironmentDescription();
- ed->kmodel->computeTransforms(startState->getParams());
- ed->collisionSpace->updateRobotModel();
-
- /* extract the components needed for the start state of the desired group */
- startState->copyParamsGroup(start->values, psetup->model->groupID);
- }
- else
- /* the start state is the complete state */
- startState->copyParams(start->values);
-
- psetup->si->addStartState(start);
-
- /* add goal state */
- psetup->si->setGoal(computeGoalFromConstraints(psetup->si, psetup->model, req.goal_constraints));
-
- /* fix invalid input states, if we have any */
- fixInputStates(psetup, 0.02, 50);
- fixInputStates(psetup, 0.05, 50);
-
- /* print some information */
- printSettings(psetup);
-}
-
-void ompl_planning::RequestHandler::printSettings(PlannerSetup *psetup)
-{
- ROS_DEBUG("=======================================");
- std::stringstream ss;
- psetup->si->printSettings(ss);
- static_cast<StateValidityPredicate*>(psetup->si->getStateValidityChecker())->printSettings(ss);
- ROS_DEBUG("%s", ss.str().c_str());
- ROS_DEBUG("=======================================");
-}
-
-bool ompl_planning::RequestHandler::fixInputStates(PlannerSetup *psetup, double value, unsigned int count)
-{
- /* add bounds for automatic state fixing (in case a state is invalid) */
- std::vector<double> rhoStart(psetup->si->getStateDimension(), value);
- std::vector<double> rhoGoal(rhoStart);
-
- // in case we have large bounds, we may have a larger area to sample,
- // so we increase it if we can
- GoalToState *gs = dynamic_cast<GoalToState*>(psetup->si->getGoal());
- if (gs)
- {
- std::vector< std::pair<double, double> > bounds = gs->getBounds();
- for (unsigned int i = 0 ; i < rhoGoal.size() ; ++i)
- {
- double dif = bounds[i].second - bounds[i].first;
- if (dif > rhoGoal[i])
- rhoGoal[i] = dif;
- }
- }
-
- psetup->model->planningMonitor->getEnvironmentModel()->setVerbose(true);
- bool result = psetup->si->fixInvalidInputStates(rhoStart, rhoGoal, count);
- psetup->model->planningMonitor->getEnvironmentModel()->setVerbose(false);
-
- return result;
-}
-
-bool ompl_planning::RequestHandler::computePlan(ModelMap &models, const planning_models::StateParams *start,
- motion_planning_srvs::MotionPlan::Request &req, motion_planning_srvs::MotionPlan::Response &res)
-{
- if (!isRequestValid(models, req))
- return false;
-
- // find the data we need
- Model *m = models[req.params.model_id];
-
- // get the planner setup
- PlannerSetup *psetup = m->planners[req.params.planner_id];
-
- ROS_INFO("Selected motion planner: '%s', with priority %d", req.params.planner_id.c_str(), psetup->priority);
-
- psetup->model->planningMonitor->getEnvironmentModel()->lock();
- psetup->model->planningMonitor->getKinematicModel()->lock();
-
- // configure the planner
- configure(start, req, psetup);
-
- /* compute actual motion plan */
- Solution sol;
- sol.path = NULL;
- sol.difference = 0.0;
- sol.approximate = false;
- callPlanner(psetup, req.times, req.allowed_time, sol);
-
- psetup->model->planningMonitor->getEnvironmentModel()->unlock();
- psetup->model->planningMonitor->getKinematicModel()->unlock();
-
- psetup->si->clearGoal();
- psetup->si->clearStartStates();
-
- /* copy the solution to the result */
- if (sol.path)
- {
- fillResult(psetup, start, res, sol);
- delete sol.path;
- if (!sol.approximate)
- {
- psetup->priority++;
- if (psetup->priority > (int)m->planners.size())
- psetup->priority = m->planners.size();
- }
- }
-
- if (!sol.path || sol.approximate)
- {
- psetup->priority--;
- if (psetup->priority < -(int)m->planners.size())
- psetup->priority = -m->planners.size();
- }
- ROS_DEBUG("New motion priority for '%s' is %d", req.params.planner_id.c_str(), psetup->priority);
- return true;
-}
-
-void ompl_planning::RequestHandler::fillResult(PlannerSetup *psetup, const planning_models::StateParams *start, motion_planning_srvs::MotionPlan::Response &res, const Solution &sol)
-{
- std::vector<planning_models::KinematicModel::Joint*> joints;
- psetup->model->planningMonitor->getKinematicModel()->getJoints(joints);
- res.path.start_state.resize(joints.size());
- for (unsigned int i = 0 ; i < joints.size() ; ++i)
- {
- res.path.start_state[i].header = res.path.header;
- res.path.start_state[i].joint_name = joints[i]->name;
- start->copyParamsJoint(res.path.start_state[i].value, joints[i]->name);
- }
-
- ompl::kinematic::PathKinematic *kpath = dynamic_cast<ompl::kinematic::PathKinematic*>(sol.path);
- if (kpath)
- {
- res.path.states.resize(kpath->states.size());
- res.path.times.resize(kpath->states.size());
- res.path.names.clear();
- psetup->model->planningMonitor->getKinematicModel()->getJointsInGroup(res.path.names, psetup->model->groupID);
-
- const unsigned int dim = psetup->si->getStateDimension();
- for (unsigned int i = 0 ; i < kpath->states.size() ; ++i)
- {
- res.path.times[i] = i * 0.02;
- res.path.states[i].vals.resize(dim);
- for (unsigned int j = 0 ; j < dim ; ++j)
- res.path.states[i].vals[j] = kpath->states[i]->values[j];
- }
- }
-
- ompl::dynamic::PathDynamic *dpath = dynamic_cast<ompl::dynamic::PathDynamic*>(sol.path);
- if (dpath)
- {
- res.path.states.resize(dpath->states.size());
- res.path.times.resize(dpath->states.size());
- res.path.names.clear();
- psetup->model->planningMonitor->getKinematicModel()->getJointsInGroup(res.path.names, psetup->model->groupID);
-
- const unsigned int dim = psetup->si->getStateDimension();
- for (unsigned int i = 0 ; i < dpath->states.size() ; ++i)
- {
- res.path.times[i] = i * 0.02;
- res.path.states[i].vals.resize(dim);
- for (unsigned int j = 0 ; j < dim ; ++j)
- res.path.states[i].vals[j] = dpath->states[i]->values[j];
- }
- }
- assert(kpath || dpath);
-
- res.distance = sol.difference;
- res.approximate = sol.approximate ? 1 : 0;
-}
-
-bool ompl_planning::RequestHandler::callPlanner(PlannerSetup *psetup, int times, double allowed_time, Solution &sol)
-{
- if (times <= 0)
- {
- ROS_ERROR("Motion plan cannot be computed %d times", times);
- return false;
- }
-
- if (dynamic_cast<ompl::base::GoalRegion*>(psetup->si->getGoal()))
- ROS_DEBUG("Goal threshold is %g", dynamic_cast<ompl::base::GoalRegion*>(psetup->si->getGoal())->threshold);
-
- unsigned int t_index = 0;
- double t_distance = 0.0;
- bool result = psetup->mp->isTrivial(&t_index, &t_distance);
-
- if (result)
- {
- ROS_INFO("Solution already achieved");
- sol.difference = t_distance;
- sol.approximate = false;
-
- /* we want to maintain the invariant that a path will
- at least consist of start & goal states, so we copy
- the start state twice */
- ompl::kinematic::PathKinematic *kpath = new ompl::kinematic::PathKinematic(psetup->si);
- ompl::base::State *s0 = new ompl::base::State(psetup->si->getStateDimension());
- ompl::base::State *s1 = new ompl::base::State(psetup->si->getStateDimension());
- psetup->si->copyState(s...
[truncated message content] |
