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
00109 int main(int argc, char **argv)
00110 {
00111 ros::init(argc, argv, "move_group_interface_demo", ros::init_options::AnonymousName);
00112
00113 ros::AsyncSpinner spinner(1);
00114 spinner.start();
00115
00116 moveit::planning_interface::MoveGroup group(argc > 1 ? argv[1] : "right_arm");
00117 demoPlace(group);
00118
00119 sleep(2);
00120
00121 return 0;
00122 }