overhead_grasp_behavior.py
Go to the documentation of this file.
00001 #! /usr/bin/python
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 # 3cm resolution
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     # Return random grasp configuration in entire space.
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     # Attempt to slightly adjust grasp parameters to get a close configuration
00080     # which will hopefully find an IK solution.
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.) # +/- 15 degrees
00086         dp = random.uniform(-np.pi/12., np.pi/12.) # +/- 15 degrees
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     # Returns a quaternion for the gripper pose given a gripper rotation
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 # Simple functionality and tests
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()


pr2_grasp_behaviors
Author(s): Kelsey Hawkins, Advisor: Prof. Charlie Kemp (Healthcare Robotics Lab at Georgia Tech)
autogenerated on Wed Nov 27 2013 11:40:53