Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008 import moveit_commander
00009 import rospy
00010 import geometry_msgs.msg
00011
00012 def main():
00013 rospy.init_node("moveit_command_sender")
00014
00015 robot = moveit_commander.RobotCommander()
00016
00017 print "=" * 10, " Robot Groups:"
00018 print robot.get_group_names()
00019
00020 print "=" * 10, " Printing robot state"
00021 print robot.get_current_state()
00022 print "=" * 10
00023
00024 rarm = moveit_commander.MoveGroupCommander("right_arm")
00025 larm = moveit_commander.MoveGroupCommander("left_arm")
00026
00027 print "=" * 15, " Right arm ", "=" * 15
00028 print "=" * 10, " Reference frame: %s" % rarm.get_planning_frame()
00029 print "=" * 10, " Reference frame: %s" % rarm.get_end_effector_link()
00030
00031 print "=" * 15, " Left ight arm ", "=" * 15
00032 print "=" * 10, " Reference frame: %s" % larm.get_planning_frame()
00033 print "=" * 10, " Reference frame: %s" % larm.get_end_effector_link()
00034
00035
00036 rarm_initial_pose = rarm.get_current_pose().pose
00037 print "=" * 10, " Printing Right Hand initial pose: "
00038 print rarm_initial_pose
00039
00040
00041 larm_initial_pose = larm.get_current_pose().pose
00042 print "=" * 10, " Printing Left Hand initial pose: "
00043 print larm_initial_pose
00044
00045 target_pose_r = geometry_msgs.msg.Pose()
00046 target_pose_r.position.x = 0.325471850974-0.01
00047 target_pose_r.position.y = -0.182271241593-0.3
00048 target_pose_r.position.z = 0.0676272396419+0.3
00049 target_pose_r.orientation.x = -0.000556712307053
00050 target_pose_r.orientation.y = -0.706576742941
00051 target_pose_r.orientation.z = -0.00102461782513
00052 target_pose_r.orientation.w = 0.707635461636
00053 rarm.set_pose_target(target_pose_r)
00054
00055 print "=" * 10," plan1 ..."
00056 rarm.go()
00057 rospy.sleep(1)
00058
00059 target_pose_l = [
00060 target_pose_r.position.x,
00061 -target_pose_r.position.y,
00062 target_pose_r.position.z,
00063 target_pose_r.orientation.x,
00064 target_pose_r.orientation.y,
00065 target_pose_r.orientation.z,
00066 target_pose_r.orientation.w
00067 ]
00068 larm.set_pose_target(target_pose_l)
00069
00070 print "=" * 10," plan2 ..."
00071 larm.go()
00072 rospy.sleep(1)
00073
00074
00075 rarm.clear_pose_targets()
00076
00077
00078 target_pose_r.position.x = 0.221486843301
00079 target_pose_r.position.y = -0.0746407547512
00080 target_pose_r.position.z = 0.642545484602
00081 target_pose_r.orientation.x = 0.0669013615474
00082 target_pose_r.orientation.y = -0.993519060661
00083 target_pose_r.orientation.z = 0.00834224628291
00084 target_pose_r.orientation.w = 0.0915122442864
00085 rarm.set_pose_target(target_pose_r)
00086
00087 print "=" * 10, " plan3..."
00088 rarm.go()
00089 rospy.sleep(1)
00090
00091 print "=" * 10,"Initial pose ..."
00092 rarm.set_pose_target(rarm_initial_pose)
00093 larm.set_pose_target(larm_initial_pose)
00094 rarm.go()
00095 larm.go()
00096 rospy.sleep(2)
00097
00098 if __name__ == '__main__':
00099 try:
00100 main()
00101 except rospy.ROSInterruptException:
00102 pass