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
00033 roslib.load_manifest('hrl_simple_arm_goals')
00034
00035 import rospy
00036 import actionlib
00037
00038 from move_arm_msgs.msg import MoveArmGoal
00039 from move_arm_msgs.msg import MoveArmAction
00040 from motion_planning_msgs.msg import PositionConstraint
00041 from motion_planning_msgs.msg import OrientationConstraint
00042 from geometric_shapes_msgs.msg import Shape
00043 from actionlib_msgs.msg import GoalStatus
00044
00045
00046 if __name__ == '__main__':
00047
00048 rospy.init_node('arm_cartesian_goal_sender')
00049
00050 move_arm = actionlib.SimpleActionClient('move_right_arm', MoveArmAction)
00051 move_arm.wait_for_server()
00052 rospy.logout('Connected to server')
00053
00054 goalA = MoveArmGoal()
00055 goalA.motion_plan_request.group_name = 'right_arm'
00056 goalA.motion_plan_request.num_planning_attempts = 1
00057 goalA.motion_plan_request.planner_id = ''
00058 goalA.planner_service_name = 'ompl_planning/plan_kinematic_path'
00059 goalA.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
00060
00061
00062 pc = PositionConstraint()
00063 pc.header.stamp = rospy.Time.now()
00064 pc.header.frame_id = 'torso_lift_link'
00065 pc.link_name = 'r_wrist_roll_link'
00066 pc.position.x = 0.75
00067 pc.position.y = -0.188
00068 pc.position.z = 0
00069
00070 pc.constraint_region_shape.type = Shape.BOX
00071 pc.constraint_region_shape.dimensions = [0.02, 0.02, 0.02]
00072 pc.constraint_region_orientation.w = 1.0
00073
00074 goalA.motion_plan_request.goal_constraints.position_constraints.append(pc)
00075
00076 oc = OrientationConstraint()
00077 oc.header.stamp = rospy.Time.now()
00078 oc.header.frame_id = 'torso_lift_link'
00079 oc.link_name = 'r_wrist_roll_link'
00080 oc.orientation.x = 0.
00081 oc.orientation.y = 0.
00082 oc.orientation.z = 0.
00083 oc.orientation.w = 1.
00084
00085 oc.absolute_roll_tolerance = 0.04
00086 oc.absolute_pitch_tolerance = 0.04
00087 oc.absolute_yaw_tolerance = 0.04
00088 oc.weight = 1.
00089
00090 goalA.motion_plan_request.goal_constraints.orientation_constraints.append(oc)
00091
00092 move_arm.send_goal(goalA)
00093 finished_within_time = move_arm.wait_for_result()
00094 if not finished_within_time:
00095 move_arm.cancel_goal()
00096 rospy.logout('Timed out achieving goal A')
00097 else:
00098 state = move_arm.get_state()
00099 if state == GoalStatus.SUCCEEDED:
00100 rospy.logout('Action finished with SUCCESS')
00101 else:
00102 rospy.logout('Action failed')
00103
00104
00105