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 #include <moveit/move_group_interface/move_group.h>
00038 #include <ros/ros.h>
00039
00040 void demoPick(moveit::planning_interface::MoveGroup& group)
00041 {
00042 std::vector<moveit_msgs::Grasp> grasps;
00043 for (std::size_t i = 0; i < 20; ++i)
00044 {
00045 geometry_msgs::PoseStamped p = group.getRandomPose();
00046 p.pose.orientation.x = 0;
00047 p.pose.orientation.y = 0;
00048 p.pose.orientation.z = 0;
00049 p.pose.orientation.w = 1;
00050 moveit_msgs::Grasp g;
00051 g.grasp_pose = p;
00052 g.pre_grasp_approach.direction.vector.x = 1.0;
00053 g.post_grasp_retreat.direction.vector.z = 1.0;
00054 g.post_grasp_retreat.direction.header = p.header;
00055 g.pre_grasp_approach.min_distance = 0.2;
00056 g.pre_grasp_approach.desired_distance = 0.4;
00057 g.post_grasp_retreat.min_distance = 0.1;
00058 g.post_grasp_retreat.desired_distance = 0.27;
00059 g.pre_grasp_posture.joint_names.resize(1, "r_gripper_joint");
00060 g.pre_grasp_posture.points.resize(1);
00061 g.pre_grasp_posture.points[0].positions.resize(1);
00062 g.pre_grasp_posture.points[0].positions[0] = 1;
00063
00064 g.grasp_posture.joint_names.resize(1, "r_gripper_joint");
00065 g.grasp_posture.points.resize(1);
00066 g.grasp_posture.points[0].positions.resize(1);
00067 g.grasp_posture.points[0].positions[0] = 0;
00068
00069 grasps.push_back(g);
00070 }
00071 group.pick("bubu", grasps);
00072 }
00073
00074 void demoPlace(moveit::planning_interface::MoveGroup& group)
00075 {
00076 std::vector<moveit_msgs::PlaceLocation> loc;
00077 for (std::size_t i = 0; i < 20; ++i)
00078 {
00079 geometry_msgs::PoseStamped p = group.getRandomPose();
00080 p.pose.orientation.x = 0;
00081 p.pose.orientation.y = 0;
00082 p.pose.orientation.z = 0;
00083 p.pose.orientation.w = 1;
00084 moveit_msgs::PlaceLocation g;
00085 g.place_pose = p;
00086 g.pre_place_approach.direction.vector.x = 1.0;
00087 g.post_place_retreat.direction.vector.z = 1.0;
00088 g.post_place_retreat.direction.header = p.header;
00089 g.pre_place_approach.min_distance = 0.2;
00090 g.pre_place_approach.desired_distance = 0.4;
00091 g.post_place_retreat.min_distance = 0.1;
00092 g.post_place_retreat.desired_distance = 0.27;
00093
00094 g.post_place_posture.joint_names.resize(1, "r_gripper_joint");
00095 g.post_place_posture.points.resize(1);
00096 g.post_place_posture.points[0].positions.resize(1);
00097 g.post_place_posture.points[0].positions[0] = 0;
00098
00099 loc.push_back(g);
00100 }
00101 group.place("bubu", loc);
00102 }
00103
00104 void attachObject(void)
00105 {
00106 }
00107
00108 int main(int argc, char** argv)
00109 {
00110 ros::init(argc, argv, "move_group_interface_demo", ros::init_options::AnonymousName);
00111
00112 ros::AsyncSpinner spinner(1);
00113 spinner.start();
00114
00115 moveit::planning_interface::MoveGroup group(argc > 1 ? argv[1] : "right_arm");
00116 demoPlace(group);
00117
00118 sleep(2);
00119
00120 return 0;
00121 }