00001
00002
00003
00004
00005 PKG = 'pr2_pickup_object_demo'
00006
00007 import roslib; roslib.load_manifest(PKG)
00008 import sys
00009 import rospy
00010 import os
00011 import numpy
00012 import time
00013 from sensor_msgs.msg import LaserScan, Image, PointCloud
00014 from gazebo.msg import *
00015 from gazebo.srv import *
00016 from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Wrench, Vector3
00017
00018
00019 import actionlib
00020 from trajectory_msgs.msg import *
00021 from kinematics_msgs.msg import *
00022 from kinematics_msgs.srv import *
00023 from pr2_controllers_msgs.msg import *
00024 from pr2_controllers_msgs.srv import *
00025
00026 import rospy
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 def set_model_configuration_client(model_name, urdf_param_name, joint_names, joint_positions):
00039 print "Setting model configuration"
00040 rospy.wait_for_service('/gazebo/set_model_configuration')
00041 try:
00042 set_model_configuration = rospy.ServiceProxy('/gazebo/set_model_configuration',SetModelConfiguration)
00043 resp = set_model_configuration(model_name, urdf_param_name, joint_names, joint_positions)
00044 print "set configuration status: ", resp.status_message
00045 return resp.success
00046 except rospy.ServiceException, e:
00047 print "Service call failed: %s"%e
00048
00049 def set_model_state_client(model_name, pose, twist, reference_frame):
00050 print "Setting ",model_name," model state"
00051 rospy.wait_for_service('/gazebo/set_model_state')
00052 try:
00053 set_model_state = rospy.ServiceProxy('/gazebo/set_model_state',SetModelState)
00054 resp = set_model_state(ModelState(model_name, pose, twist, reference_frame))
00055 print "set state status: ", resp.status_message
00056 return resp.success
00057 except rospy.ServiceException, e:
00058 print "Service call failed: %s"%e
00059
00060 def arm_joint_trajectory_action_client(arm,trajectory):
00061 print "sending arm joint trajectory to ",arm
00062 client = actionlib.SimpleActionClient(arm,JointTrajectoryAction)
00063 client.wait_for_server()
00064 client.send_goal(trajectory)
00065 client.wait_for_result()
00066 return client.get_result()
00067
00068 def gripper_command_action_client(command):
00069 print "sending gripper command"
00070 client = actionlib.SimpleActionClient('l_gripper_controller/gripper_action',Pr2GripperCommandAction)
00071 client.wait_for_server()
00072 client.send_goal(command)
00073 client.wait_for_result()
00074 return client.get_result()
00075
00076 if __name__ == '__main__':
00077 rospy.init_node('gazebo_test_grasp')
00078
00079 joint_names = ['fl_caster_rotation_joint', 'fl_caster_l_wheel_joint', 'fl_caster_r_wheel_joint', 'fr_caster_rotation_joint', 'fr_caster_l_wheel_joint', 'fr_caster_r_wheel_joint', 'bl_caster_rotation_joint', 'bl_caster_l_wheel_joint', 'bl_caster_r_wheel_joint', 'br_caster_rotation_joint', 'br_caster_l_wheel_joint', 'br_caster_r_wheel_joint', 'head_pan_joint', 'head_tilt_joint', 'laser_tilt_mount_joint', 'r_upper_arm_roll_joint', 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_forearm_roll_joint', 'r_elbow_flex_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint', 'r_gripper_joint', 'r_gripper_l_finger_tip_joint', 'r_gripper_r_finger_tip_joint', 'r_gripper_l_finger_joint', 'r_gripper_r_finger_joint', 'l_upper_arm_roll_joint', 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint', 'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint', 'l_gripper_joint', 'l_gripper_l_finger_tip_joint', 'l_gripper_r_finger_tip_joint', 'l_gripper_l_finger_joint', 'l_gripper_r_finger_joint']
00080 joint_positions = [-1.2374515368840378e-05, 13.849011208135753, 13.854646834822727, -2.0182888337494376e-05, 13.867926659094984, 13.87465036000275, -3.0273366892430431e-05, 13.798391413070624, 13.803469733109218, -8.9169276339617909e-05, 13.763956446381279, 13.785982540514944, -0.028014984291522715, 0.522937305713703, 0.23430921400735369, 0.00031428941291178347, -0.40039691465768534, 1.0001709932508449, -0.0016722329173761707, -2.0498051406658426, -0.10004495846217587, -0.0011961845468899668, 0.021801808285971121, 0.01090090414298556, -0.01090090414298556, 0.17441446628776897, -0.17441446628776897, 0.63569515764238282, 0.77977415758334701, 1.0364984164611073, -3.9495641888643505, -1.7985134583697606, -0.28943329621010871, 3.6907943582700078, 0.085993919065644644, 0.042996959532822322, -0.042996959532822322, 0.68795135252515716, -0.68795135252515716]
00081
00082
00083 l_arm_joint_names = ['l_upper_arm_roll_joint', 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint', 'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint']
00084 l_arm_joint_positions = [0.63569515764238282, 0.77977415758334701, 1.0364984164611073, -3.9495641888643505, -1.7985134583697606, -0.80, 3.5]
00085 l_arm_trajectory = JointTrajectoryGoal()
00086 l_arm_trajectory.trajectory.joint_names = l_arm_joint_names
00087 point = JointTrajectoryPoint()
00088 point.positions = l_arm_joint_positions
00089 point.velocities = numpy.zeros(len(l_arm_joint_positions))
00090 point.time_from_start = rospy.Duration.from_sec(5)
00091 l_arm_trajectory.trajectory.points.append(point)
00092 result = arm_joint_trajectory_action_client('l_arm_controller/joint_trajectory_action',l_arm_trajectory)
00093 print "arm trajectory result: ",result
00094
00095 r_arm_joint_names = [ 'r_upper_arm_roll_joint', 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_forearm_roll_joint', 'r_elbow_flex_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint']
00096 r_arm_joint_positions = [ 0.00031428941291178347, -0.40039691465768534, 1.0001709932508449, -0.0016722329173761707, -2.0498051406658426, -0.10004495846217587, -0.0011961845468899668]
00097 r_arm_trajectory = JointTrajectoryGoal()
00098 r_arm_trajectory.trajectory.joint_names = r_arm_joint_names
00099 point = JointTrajectoryPoint()
00100 point.positions = r_arm_joint_positions
00101 point.velocities = numpy.zeros(len(r_arm_joint_positions))
00102 point.time_from_start = rospy.Duration.from_sec(5)
00103 r_arm_trajectory.trajectory.points.append(point)
00104 result = arm_joint_trajectory_action_client('r_arm_controller/joint_trajectory_action',r_arm_trajectory)
00105 print "arm trajectory result: ",result
00106
00107 gripper_command = Pr2GripperCommandGoal()
00108 gripper_command.command.position = 0.08
00109 gripper_command.command.max_effort = 100
00110 result = gripper_command_action_client(gripper_command)
00111 print "gripper command result: ",result
00112 time.sleep(3)
00113
00114
00115 model_name = 'pr2'
00116 pose = Pose(Point(1.030, 0, 0.0),Quaternion(0 ,0 ,0 , 1))
00117 twist = Twist(Vector3(0,0,0),Vector3(0,0,0))
00118 reference_frame = 'world'
00119 set_model_state_client(model_name, pose, twist, reference_frame)
00120
00121
00122 gripper_pub = rospy.Publisher('/l_gripper_controller/command', Pr2GripperCommand)
00123 gripper_command = Pr2GripperCommand()
00124 gripper_command.position = 0.0
00125 gripper_command.max_effort = 200.0
00126 timeout = time.time() + 5
00127 while (time.time() < timeout):
00128 gripper_pub.publish(gripper_command)
00129 time.sleep(1)
00130
00131 model_name = 'table_1'
00132 pose = Pose(Point(3,0,0.01),Quaternion(0,0,0,1))
00133 twist = Twist(Vector3(0,0,0),Vector3(0,0,0))
00134 reference_frame = 'world'
00135 set_model_state_client(model_name, pose, twist, reference_frame)
00136
00137 l_arm_joint_names = ['l_upper_arm_roll_joint', 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint', 'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint']
00138 l_arm_joint_positions = [0.3, 0.8, -0.3, 1.0, -1.0, 0.0, 0.0]
00139 l_arm_trajectory = JointTrajectoryGoal()
00140 l_arm_trajectory.trajectory.joint_names = l_arm_joint_names
00141 point = JointTrajectoryPoint()
00142 point.positions = l_arm_joint_positions
00143 point.velocities = numpy.zeros(len(l_arm_joint_positions))
00144 point.time_from_start = rospy.Duration.from_sec(5)
00145 l_arm_trajectory.trajectory.points.append(point)
00146 result = arm_joint_trajectory_action_client('l_arm_controller/joint_trajectory_action',l_arm_trajectory)
00147 print "arm trajectory result: ",result
00148