Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <pluginlib/class_loader.h>
00039 #include <ros/ros.h>
00040
00041
00042 #include <moveit/robot_model_loader/robot_model_loader.h>
00043 #include <moveit/planning_interface/planning_interface.h>
00044 #include <moveit/kinematic_constraints/utils.h>
00045 #include <moveit_msgs/DisplayTrajectory.h>
00046 #include <moveit_msgs/PlanningScene.h>
00047
00048 int main(int argc, char **argv)
00049 {
00050 ros::init (argc, argv, "move_group_tutorial");
00051 ros::AsyncSpinner spinner(1);
00052 spinner.start();
00053 ros::NodeHandle node_handle("~");
00054
00055
00056
00057 robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
00058
00059
00060 robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
00061
00062
00063
00064
00065 planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
00066
00067
00068 boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;
00069 planning_interface::PlannerManagerPtr planner_instance;
00070 std::string planner_plugin_name;
00071
00072
00073 if (!node_handle.getParam("planning_plugin", planner_plugin_name))
00074 ROS_FATAL_STREAM("Could not find planner plugin name");
00075
00076
00077 try
00078 {
00079 planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager"));
00080 }
00081 catch(pluginlib::PluginlibException& ex)
00082 {
00083 ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
00084 }
00085 try
00086 {
00087 planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
00088 if (!planner_instance->initialize(robot_model, node_handle.getNamespace()))
00089 ROS_FATAL_STREAM("Could not initialize planner instance");
00090 ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
00091 }
00092 catch(pluginlib::PluginlibException& ex)
00093 {
00094 const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses();
00095 std::stringstream ss;
00096 for (std::size_t i = 0 ; i < classes.size() ; ++i)
00097 ss << classes[i] << " ";
00098 ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl
00099 << "Available plugins: " << ss.str());
00100 }
00101
00102
00103 ros::WallDuration sleep_time(8.0);
00104 sleep_time.sleep();
00105
00106
00107
00108 planning_interface::MotionPlanRequest req;
00109 planning_interface::MotionPlanResponse res;
00110
00111
00112 geometry_msgs::PoseStamped pose;
00113 pose.header.frame_id = "torso_lift_link";
00114 pose.pose.position.x = 0.75;
00115 pose.pose.position.y = 0.0;
00116 pose.pose.position.z = 0.0;
00117 pose.pose.orientation.w = 1.0;
00118
00119
00120 std::vector<double> tolerance_pose(3, 0.01);
00121 std::vector<double> tolerance_angle(3, 0.01);
00122
00123
00124 req.group_name = "right_arm";
00125 moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("r_wrist_roll_link", pose, tolerance_pose, tolerance_angle);
00126 req.goal_constraints.push_back(pose_goal);
00127
00128
00129 planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
00130
00131
00132 context->solve(res);
00133
00134
00135 if(res.error_code_.val != res.error_code_.SUCCESS)
00136 {
00137 ROS_ERROR("Could not compute plan successfully");
00138 return 0;
00139 }
00140
00141
00142
00143 ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
00144 moveit_msgs::DisplayTrajectory display_trajectory;
00145
00146
00147 ROS_INFO("Visualizing the trajectory");
00148 moveit_msgs::MotionPlanResponse response;
00149 res.getMessage(response);
00150
00151 display_trajectory.trajectory_start = response.trajectory_start;
00152 display_trajectory.trajectory.push_back(response.trajectory);
00153 display_publisher.publish(display_trajectory);
00154
00155 sleep_time.sleep();
00156
00157
00158
00159 robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst();
00160 planning_scene->setCurrentState(response.trajectory_start);
00161 robot_state::JointStateGroup* joint_state_group = robot_state.getJointStateGroup("right_arm");
00162 joint_state_group->setVariableValues(response.trajectory.joint_trajectory.points.back().positions);
00163
00164
00165 robot_state::RobotState goal_state(robot_model);
00166 robot_state::JointStateGroup* goal_group = goal_state.getJointStateGroup("right_arm");
00167 std::vector<double> joint_values(7, 0.0);
00168 joint_values[0] = -2.0;
00169 joint_values[3] = -0.2;
00170 joint_values[5] = -0.15;
00171 goal_group->setVariableValues(joint_values);
00172 moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_group);
00173
00174 req.goal_constraints.clear();
00175 req.goal_constraints.push_back(joint_goal);
00176
00177
00178 context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
00179
00180
00181 context->solve(res);
00182
00183
00184 if(res.error_code_.val != res.error_code_.SUCCESS)
00185 {
00186 ROS_ERROR("Could not compute plan successfully");
00187 return 0;
00188 }
00189
00190
00191 ROS_INFO("Visualizing the trajectory");
00192 res.getMessage(response);
00193
00194 display_trajectory.trajectory_start = response.trajectory_start;
00195 display_trajectory.trajectory.push_back(response.trajectory);
00196
00197 display_publisher.publish(display_trajectory);
00198
00199
00200
00201 joint_state_group->setVariableValues(response.trajectory.joint_trajectory.points.back().positions);
00202
00203
00204 req.goal_constraints.clear();
00205 req.goal_constraints.push_back(pose_goal);
00206 context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
00207 context->solve(res);
00208 res.getMessage(response);
00209 display_trajectory.trajectory.push_back(response.trajectory);
00210 display_publisher.publish(display_trajectory);
00211
00212
00213 pose.pose.position.x = 0.65;
00214 pose.pose.position.y = -0.2;
00215 pose.pose.position.z = -0.1;
00216 moveit_msgs::Constraints pose_goal_2 = kinematic_constraints::constructGoalConstraints("r_wrist_roll_link", pose, tolerance_pose, tolerance_angle);
00217
00218
00219 joint_state_group->setVariableValues(response.trajectory.joint_trajectory.points.back().positions);
00220
00221
00222 req.goal_constraints.clear();
00223 req.goal_constraints.push_back(pose_goal_2);
00224
00225
00226
00227 geometry_msgs::QuaternionStamped quaternion;
00228 quaternion.header.frame_id = "torso_lift_link";
00229 quaternion.quaternion.w = 1.0;
00230
00231 req.path_constraints = kinematic_constraints::constructGoalConstraints("r_wrist_roll_link", quaternion);
00232
00233
00234
00235
00236
00237
00238
00239
00240 req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y = req.workspace_parameters.min_corner.z = -2.0;
00241 req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y = req.workspace_parameters.max_corner.z = 2.0;
00242
00243
00244 context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
00245 context->solve(res);
00246 res.getMessage(response);
00247 display_trajectory.trajectory.push_back(response.trajectory);
00248
00249 display_publisher.publish(display_trajectory);
00250
00251 sleep_time.sleep();
00252 ROS_INFO("Done");
00253 planner_instance.reset();
00254
00255 return 0;
00256 }