00001
00002
00003 import sys
00004 import numpy as np, math
00005 import random
00006
00007 import roslib; roslib.load_manifest('pr2_grasp_behaviors')
00008 import rospy
00009 from tf.transformations import quaternion_about_axis, quaternion_multiply, quaternion_matrix
00010
00011 from grasp_manager import GraspBehavior
00012 from grasp_behavior_server import GraspBehaviorServer
00013
00014 class OverheadGrasp(GraspBehavior):
00015 def __init__(self, arm, use_coll_detection=False):
00016 super(OverheadGrasp, self).__init__(arm, use_coll_detection)
00017
00018 self.HOVER_Z = -0.10
00019 self.GRASP_DIST = 0.30
00020 self.GRASP_VELOCITY = 0.3
00021 self.BIAS_RADIUS = 0.012
00022 self.GRASP_TIME = 2.0
00023 self.SETUP_VELOCITY = 0.5
00024 self.JOINTS_BIAS = [0.0, -0.25, -1.0, 0.0, 0.0, 0.5, 0.0]
00025 self.JIGGLE_RESOLUTION = 0.03
00026 if arm == 'l':
00027 for i in [0, 2, 4]:
00028 self.JOINTS_BIAS[i] *= -1
00029
00030 def grasp_preparation_move(self):
00031 joints = [-1.32734204881265387, -0.34601608409943324, -1.4620635485239604, -1.2729772622637399, -7.5123303230158518, -1.5570651396529178, -5.5929916630672727]
00032 if self.arm == 'l':
00033 for i in [0, 2, 4]:
00034 joints[i] *= -1
00035 else:
00036 joints[6] -= 3.14/2
00037 self.cm.command_joint_trajectory([joints], max_joint_vel=0.30, blocking=False)
00038
00039 def grasp_setup_move(self, params):
00040 self.xyrp = params
00041 rospy.loginfo("Moving to grasp position (%1.2f, %1.2f, %1.2f, %1.2f)" %
00042 (self.xyrp[0], self.xyrp[1], self.xyrp[2], self.xyrp[3]))
00043 self.tool_rot_quat = self.overhead_gripper_quat(self.xyrp[2], self.xyrp[3])
00044 grasp_pose = self.create_goal_pose(self.xyrp[0], self.xyrp[1], self.HOVER_Z,
00045 self.tool_rot_quat)
00046 return self.cm.move_arm_pose_biased(grasp_pose, self.JOINTS_BIAS,
00047 self.SETUP_VELOCITY, blocking = True)
00048
00049 def execute_approach(self, block):
00050 rospy.loginfo("Moving arm down")
00051 goal_pose = self.create_goal_pose(self.xyrp[0], self.xyrp[1],
00052 self.HOVER_Z - self.GRASP_DIST,
00053 self.tool_rot_quat)
00054 goal_pose.header.stamp = rospy.Time.now()
00055 return self.cm.move_cartesian_ik(goal_pose, collision_aware = False,
00056 blocking = block,
00057 step_size = .005, pos_thres = .005, rot_thres = .1,
00058 settling_time = rospy.Duration(self.GRASP_TIME),
00059 joints_bias = self.JOINTS_BIAS, bias_radius = self.BIAS_RADIUS,
00060 vel = self.GRASP_VELOCITY)
00061
00062 def execute_retreat(self):
00063 retreat_pose = self.create_goal_pose(self.xyrp[0], self.xyrp[1],
00064 self.HOVER_Z,
00065 self.tool_rot_quat)
00066 self.cm.move_arm_pose_biased(retreat_pose, self.JOINTS_BIAS,
00067 self.SETUP_VELOCITY, blocking = True)
00068
00069
00070
00071 def random_generator(self):
00072 x = random.uniform(0.40, 0.75)
00073 y = random.uniform(-0.35, 0.35)
00074 r = random.uniform(0., np.pi)
00075 p = random.uniform(-np.pi/3, np.pi/6)
00076 return x, y, r, p
00077
00078
00079
00080
00081 def jiggle_grasp_params(self, grasp_params):
00082 x, y, r, p = grasp_params
00083 dir_ang = random.uniform(0., 2. * np.pi)
00084 dx, dy = self.JIGGLE_RESOLUTION * np.cos(dir_ang), self.JIGGLE_RESOLUTION * np.sin(dir_ang)
00085 dr = random.uniform(-np.pi/12., np.pi/12.)
00086 dp = random.uniform(-np.pi/12., np.pi/12.)
00087 x += dx
00088 y += dy
00089 r += dr
00090 p += dp
00091 r = self.normalize_rot(r)
00092 return (x, y, r, p)
00093
00094
00095
00096 def overhead_gripper_quat(self, gripper_roll, gripper_pitch=0.):
00097 gripper_roll = self.normalize_rot(gripper_roll)
00098 quat1 = quaternion_about_axis(np.pi/2. + gripper_pitch, (0, 1, 0))
00099 quat2 = quaternion_about_axis(gripper_roll, (0, 0, 1))
00100 quat = quaternion_multiply(quat2, quat1)
00101 return quat
00102
00103
00104
00105 def main():
00106 rospy.init_node("overhead_grasp")
00107 if sys.argv[1] not in ['r', 'l']:
00108 print "Must specify arm [r, l]"
00109 return
00110 if sys.argv[2] == "test":
00111 og = OverheadGrasp(sys.argv[1], use_coll_detection=True)
00112 while not rospy.is_shutdown():
00113 params = og.random_generator()
00114 print "Grasp Result:", og.perform_grasp(params, is_place=False, collide=False,
00115 behavior_name="overhead_grasp", sig_level=0.999)
00116 params = og.random_generator()
00117 print "Place Result:", og.perform_grasp(params, is_place=True, collide=False,
00118 behavior_name="overhead_grasp", sig_level=0.999)
00119 return
00120
00121 if sys.argv[2] == "data":
00122 og = OverheadGrasp(sys.argv[1], use_coll_detection=True)
00123 num_times = 0
00124 while not rospy.is_shutdown():
00125 params = og.random_generator()
00126 og.perform_grasp(params, is_place=False, collide=True, data_collecting=True)
00127 rospy.loginfo("Number of grasps performed: %d", num_times)
00128 num_times += 1
00129 return
00130
00131 if sys.argv[2] == "server":
00132 og = OverheadGrasp(sys.argv[1], use_coll_detection=True)
00133 gbs = GraspBehaviorServer(sys.argv[1], og)
00134 gbs.start_grasping_server(sys.argv[1] + "_overhead_grasp", sys.argv[1] + "_overhead_grasp_setup")
00135 rospy.spin()
00136 return
00137
00138 print "Usage: python overhead_grasp_behavior.py [r|l] [test|data|server]"
00139 print "args:", sys.argv
00140 return
00141
00142 if __name__ == "__main__":
00143 main()