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 import roslib; roslib.load_manifest('move_arm_tutorials')
00033
00034 import rospy
00035 import actionlib
00036
00037 import geometric_shapes_msgs
00038
00039 from move_arm_msgs.msg import MoveArmAction
00040 from move_arm_msgs.msg import MoveArmGoal
00041
00042 from motion_planning_msgs.msg import JointConstraint
00043
00044 from actionlib_msgs.msg import GoalStatus
00045
00046
00047 if __name__ == '__main__':
00048 import hrl_lib.transforms as tr
00049
00050 rospy.init_node('move_arm_joint_goal_test')
00051
00052 move_arm = actionlib.SimpleActionClient('move_right_arm', MoveArmAction)
00053 move_arm.wait_for_server()
00054 rospy.loginfo('Connected to server')
00055
00056 goalB = MoveArmGoal()
00057
00058 names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint',
00059 'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
00060 'r_forearm_roll_joint', 'r_wrist_flex_joint',
00061 'r_wrist_roll_joint']
00062
00063 goalB.motion_plan_request.group_name = 'right_arm'
00064 goalB.motion_plan_request.num_planning_attempts = 1
00065 goalB.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
00066
00067 goalB.motion_plan_request.planner_id = ''
00068 goalB.planner_service_name = 'ompl_planning/plan_kinematic_path'
00069
00070
00071 import roslib; roslib.load_manifest('darpa_m3')
00072 import sandbox_advait.pr2_arms as pa
00073 pr2_arms = pa.PR2Arms()
00074 raw_input('Move arm to goal location and hit ENTER')
00075 q = pr2_arms.get_joint_angles(0)
00076 raw_input('Move arm to start location and hit ENTER')
00077
00078 q[6] = tr.angle_within_mod180(q[6])
00079 q[4] = tr.angle_within_mod180(q[4])
00080 for i in range(7):
00081 jc = JointConstraint()
00082 jc.joint_name = names[i]
00083 jc.position = q[i]
00084 jc.tolerance_below = 0.1
00085 jc.tolerance_above = 0.1
00086 goalB.motion_plan_request.goal_constraints.joint_constraints.append(jc)
00087
00088 move_arm.send_goal(goalB)
00089 finished_within_time = move_arm.wait_for_result(rospy.Duration(200.0))
00090
00091 if not finished_within_time:
00092 move_arm.cancel_goal()
00093 rospy.loginfo("Timed out achieving goal A")
00094 else:
00095 state = move_arm.get_state()
00096
00097 if state == GoalStatus.SUCCEEDED:
00098 rospy.loginfo('Action finished and was successful.')
00099 else:
00100 rospy.loginfo('Action failed: %d'%(state))
00101
00102