def execute(self): # Get the bottle pose from the camera. rospy.loginfo('Moveing to view') m.move_to_named_pose('whole_arm', 'view_checkerboard') rospy.sleep(4) poses = [] qt = tf.transformations.quaternion_from_euler(0.0, 0.0, -3.141592653 / 2) for i in range(16): rospy.loginfo('GET OBJECT POSE') b_req = cartesian_calibration.srv.GetACoorRequest() b_req.data_set = "measured" b_req.object_idx = i b_res = self.pose_service.call(b_req) p = b_res.object_pose.pose # p.orientation.x = qt[0] # p.orientation.y = qt[1] # p.orientation.z = qt[2] # p.orientation.w = qt[3] object_global = t.convert_pose(p, self.camera_reference_frame, 'global_xyz_link') # object_global.position.z = min(0.80,p.position.z) poses.append(object_global) m.move_to_named_pose('realsense', 'tool_change_position') m.move_to_named_pose('wrist_only', 'sucker') for p in poses: if p.position.x == 0 and p.position.y == 0 and p.position.z == 0: rospy.logerr('Failed to find an object. %s' % i) # return 'failed' continue print("MOVE SUCKER TO ABOVE OBJECT") p = t.align_pose_orientation_to_frame(p, 'global_xyz_link', 'sucker_endpoint') t.publish_pose_as_transform(p, 'global_xyz_link', 'MOVE HERE', 0.5) res = m.move_to_global(p.position.x - 0.02, p.position.y + 0.01, p.position.z - 0.02, 'sucker', orientation=p.orientation) rospy.sleep(4)
def command_callback(target_pose, target_quaternion): global SERVO global CURR_Z, MIN_Z global CURR_DEPTH global pose_averager global GOAL_Z global GRIP_WIDTH_MM global VELO_COV #CURR_DEPTH = msg.data[5] if SERVO: # d = list(msg.data) # PBVS Method. #if d[2] > 0.150: # Min effective range of the realsense. # Convert width in pixels to mm. # 0.07 is distance from end effector (CURR_Z) to camera. # 0.1 is approx degrees per pixel for the realsense. # if d[2] > 0.25: # GRIP_WIDTH_PX = msg.data[4] # GRIP_WIDTH_MM = 2 * ((CURR_Z + 0.07)) * np.tan(0.1 * GRIP_WIDTH_PX / 2.0 / 180.0 * np.pi) * 1000 # Construct the Pose in the frame of the camera. # gp = geometry_msgs.msg.Pose() # gp.position.x = d[0] # gp.position.y = d[1] # gp.position.z = d[2] # q = tft.quaternion_from_euler(0, 0, -1 * d[3]) # gp.orientation.x = q[0] # gp.orientation.y = q[1] # gp.orientation.z = q[2] # gp.orientation.w = q[3] # Calculate Pose of Grasp in Robot Base Link Frame # Average over a few predicted poses to help combat noise. # x: 0.645162225459 # y: -0.318927784776 # z: 0.694286052858 # w: -0.00420092196819 # x: 0.81874143089 # y: -0.506091943919 # z: 0.374129838746 # gp_base = convert_pose(gp, 'camera_link', 'j2n6s300_link_base') # print(gp_base.orientation) # print(gp_base.position) #gpbo = gp_base.orientation e = tft.euler_from_quaternion(target_quaternion) # Only really care about rotation about x (e[0]). update is mean function av = pose_averager.update(np.array(target_pose + [e[0], e[1], e[2]])) # else: # gp_base = geometry_msgs.msg.Pose() # av = pose_averager.evaluate() # compute end_effector to j2n6s300_link_base's orientation and euler g_pose1 = geometry_msgs.msg.Pose() g_pose1.orientation.w = 1 try: end_effector = convert_pose(g_pose1, 'j2n6s300_end_effector', 'j2n6s300_link_base') end_effector_list = [ end_effector.orientation.x, end_effector.orientation.y, end_effector.orientation.z, end_effector.orientation.w ] except Exception as ex: template = "An exception of type {0} occurred. Arguments:{1!r}" message = template.format(type(ex).__name__, ex.args) print(message) return e1 = tft.euler_from_quaternion(end_effector_list) # Average pick pose in base frame. gp_base = geometry_msgs.msg.Pose() gp_base.position.x = av[0] gp_base.position.y = av[1] gp_base.position.z = av[2] GOAL_Z = av[2] ang = av[3] - np.pi / 2 # We don't want to align, we want to grip. q = tft.quaternion_from_euler( np.pi, 0, np.pi / 2 ) # 绕j2n6s300_base_link's fixed x y z轴转动到正的位置; np.pi, 0, np.pi/2 to left; np.pi/2, 0, np.pi/2 to forward;np.pi, np.pi/2, np.pi/2 to down q1 = tft.quaternion_from_euler(np.pi / 2, 0, 0) #将手指竖起来 q = tft.quaternion_multiply(q1, q) gp_base.orientation.x = q[0] gp_base.orientation.y = q[1] gp_base.orientation.z = q[2] gp_base.orientation.w = q[3] # Get the Position of the End Effector in the frame of the Robot base Link g_pose = geometry_msgs.msg.Pose() g_pose.position.z = 0.03 # Offset from the end_effector frame to the actual position of the fingers. g_pose.orientation.w = 1 p_gripper = geometry_msgs.msg.Pose() try: p_gripper = convert_pose(g_pose, 'j2n6s300_end_effector', 'j2n6s300_link_base') except Exception as ex: template = "An exception of type {0} occurred. Arguments:{1!r}" message = template.format(type(ex).__name__, ex.args) print(message) return publish_pose_as_transform(gp_base, 'j2n6s300_link_base', 'G', 0.0) # Calculate Position Error. pick pose - finger pose in base_frame dx = (gp_base.position.x - p_gripper.position.x) dy = (gp_base.position.y - p_gripper.position.y) dz = (gp_base.position.z - p_gripper.position.z) # Orientation velocity control is done in the frame of the gripper, # so figure out the rotation offset in the end effector frame. end_effector_inverse = tft.quaternion_inverse(end_effector_list) pgo = tft.quaternion_multiply(q, end_effector_inverse) q1 = [pgo[0], pgo[1], pgo[2], pgo[3]] # q1 = [pgo.x, pgo.y, pgo.z, pgo.w] e = tft.euler_from_quaternion(q1) dr = 1 * e[0] dp = 1 * e[1] dyaw = 1 * e[2] vx = max(min(dx * 2.5, MAX_VELO_X), -1.0 * MAX_VELO_X) vy = max(min(dy * 2.5, MAX_VELO_Y), -1.0 * MAX_VELO_Y) vz = max(min(dz - 0.04, MAX_VELO_Z), -1.0 * MAX_VELO_Z) # Apply a nonlinearity to the velocity v = np.array([vx, vy, vz]) vc = np.dot(v, VELO_COV) CURRENT_VELOCITY[0] = vc[0] CURRENT_VELOCITY[1] = vc[1] CURRENT_VELOCITY[2] = vc[2] CURRENT_VELOCITY[3] = 1 * dr #x: end effector self rotate CURRENT_VELOCITY[4] = 1 * dp #y: up and down rotate CURRENT_VELOCITY[ 5] = 1 * dyaw #max(min(1 * dyaw, MAX_ROTATION), -1 * MAX_ROTATION) #z: left and right rotate
def command_callback(msg): global SERVO global CURR_Z, MIN_Z global CURR_DEPTH global pose_averager global GOAL_Z global GRIP_WIDTH_MM global VELO_COV CURR_DEPTH = msg.data[5] if SERVO: d = list(msg.data) # PBVS Method. if d[2] > 0.150: # Min effective range of the realsense. # Convert width in pixels to mm. # 0.07 is distance from end effector (CURR_Z) to camera. # 0.1 is approx degrees per pixel for the realsense. if d[2] > 0.25: GRIP_WIDTH_PX = msg.data[4] GRIP_WIDTH_MM = 2 * ((CURR_Z + 0.07)) * np.tan( 0.1 * GRIP_WIDTH_PX / 2.0 / 180.0 * np.pi) * 1000 # Construct the Pose in the frame of the camera. gp = geometry_msgs.msg.Pose() gp.position.x = d[0] gp.position.y = d[1] gp.position.z = d[2] q = tft.quaternion_from_euler(0, 0, -1 * d[3]) gp.orientation.x = q[0] gp.orientation.y = q[1] gp.orientation.z = q[2] gp.orientation.w = q[3] # Calculate Pose of Grasp in Robot Base Link Frame # Average over a few predicted poses to help combat noise. gp_base = convert_pose(gp, 'camera_depth_frame', 'j2n6s300_link_base') gpbo = gp_base.orientation e = tft.euler_from_quaternion([gpbo.x, gpbo.y, gpbo.z, gpbo.w]) # Only really care about rotation about z (e[2]). av = pose_averager.update( np.array([ gp_base.position.x, gp_base.position.y, gp_base.position.z, e[2] ])) else: gp_base = geometry_msgs.msg.Pose() av = pose_averager.evaluate() # Average pose in base frame. gp_base.position.x = av[0] gp_base.position.y = av[1] gp_base.position.z = av[2] GOAL_Z = av[2] ang = av[3] - np.pi / 2 # We don't want to align, we want to grip. q = tft.quaternion_from_euler(np.pi, 0, ang) gp_base.orientation.x = q[0] gp_base.orientation.y = q[1] gp_base.orientation.z = q[2] gp_base.orientation.w = q[3] # Get the Position of the End Effector in the frame fo the Robot base Link g_pose = geometry_msgs.msg.Pose() g_pose.position.z = 0.03 # Offset from the end_effector frame to the actual position of the fingers. g_pose.orientation.w = 1 p_gripper = convert_pose(g_pose, 'j2n6s300_end_effector', 'j2n6s300_link_base') publish_pose_as_transform(gp_base, 'j2n6s300_link_base', 'G', 0.0) # Calculate Position Error. dx = (gp_base.position.x - p_gripper.position.x) dy = (gp_base.position.y - p_gripper.position.y) dz = (gp_base.position.z - p_gripper.position.z) # Orientation velocity control is done in the frame of the gripper, # so figure out the rotation offset in the end effector frame. gp_gripper = convert_pose(gp_base, 'j2n6s300_link_base', 'j2n6s300_end_effector') pgo = gp_gripper.orientation q1 = [pgo.x, pgo.y, pgo.z, pgo.w] e = tft.euler_from_quaternion(q1) dr = 1 * e[0] dp = 1 * e[1] dyaw = 1 * e[2] vx = max(min(dx * 2.5, MAX_VELO_X), -1.0 * MAX_VELO_X) vy = max(min(dy * 2.5, MAX_VELO_Y), -1.0 * MAX_VELO_Y) vz = max(min(dz - 0.04, MAX_VELO_Z), -1.0 * MAX_VELO_Z) # Apply a nonlinearity to the velocity v = np.array([vx, vy, vz]) vc = np.dot(v, VELO_COV) CURRENT_VELOCITY[0] = vc[0] CURRENT_VELOCITY[1] = vc[1] CURRENT_VELOCITY[2] = vc[2] CURRENT_VELOCITY[3] = -1 * dp CURRENT_VELOCITY[4] = 1 * dr CURRENT_VELOCITY[5] = max(min(1 * dyaw, MAX_ROTATION), -1 * MAX_ROTATION) print(CURRENT_VELOCITY)
def execute_grasp(): # Execute a grasp. global MOVING global CURR_Z global start_force_srv global stop_force_srv # Get the positions. msg = rospy.wait_for_message('/ggcnn/out/command', std_msgs.msg.Float32MultiArray) d = list(msg.data) # Calculate the gripper width. grip_width = d[4] # Convert width in pixels to mm. # 0.07 is distance from end effector (CURR_Z) to camera. # 0.1 is approx degrees per pixel for the realsense. #g_width = 2 * ((CURR_Z + 0.07)) * np.tan(0.1 * grip_width / 2.0 / 180.0 * np.pi) * 1000 # Convert into motor positions. #g = min((1 - (min(g_width, 70)/70)) * (6800-4000) + 4000, 5500) #set_finger_positions([g, g]) rospy.sleep(0.5) # Pose of the grasp (position only) in the camera frame. gp = geometry_msgs.msg.Pose() gp.position.x = d[0] gp.position.y = d[1] gp.position.z = d[2] gp.orientation.w = 1 # Convert to base frame, add the angle in (ensures planar grasp, camera isn't guaranteed to be perpendicular). gp_base = convert_pose(gp, 'camera_depth_optical_frame', 'm1n6s200_link_base') q = tft.quaternion_from_euler(np.pi, 0, d[3]) gp_base.orientation.x = q[0] gp_base.orientation.y = q[1] gp_base.orientation.z = q[2] gp_base.orientation.w = q[3] publish_pose_as_transform(gp_base, 'm1n6s200_link_base', 'G', 0.5) # Offset for initial pose. initial_offset = 0.20 gp_base.position.z += initial_offset # Disable force control, makes the robot more accurate. stop_force_srv.call(kinova_msgs.srv.StopRequest()) move_to_pose(gp_base) rospy.sleep(0.1) # Start force control, helps prevent bad collisions. start_force_srv.call(kinova_msgs.srv.StartRequest()) rospy.sleep(0.25) # Reset the position gp_base.position.z -= initial_offset # Flag to check for collisions. MOVING = True # Generate a nonlinearity for the controller. cart_cov = generate_cartesian_covariance(0) # Move straight down under velocity control. velo_pub = rospy.Publisher('/m1n6s200_driver/in/cartesian_velocity', kinova_msgs.msg.PoseVelocity, queue_size=1) while MOVING and CURR_Z - 0.02 > gp_base.position.z: dz = gp_base.position.z - CURR_Z - 0.03 # Offset by a few cm for the fingertips. MAX_VELO_Z = 0.08 dz = max(min(dz, MAX_VELO_Z), -1.0 * MAX_VELO_Z) v = np.array([0, 0, dz]) vc = list(np.dot(v, cart_cov)) + [0, 0, 0] velo_pub.publish(kinova_msgs.msg.PoseVelocity(*vc)) rospy.sleep(1 / 100.0) MOVING = False # close the fingers. rospy.sleep(0.1) set_finger_positions([8000, 8000]) rospy.sleep(0.5) # Move back up to initial position. gp_base.position.z += initial_offset gp_base.orientation.x = 1 gp_base.orientation.y = 0 gp_base.orientation.z = 0 gp_base.orientation.w = 0 move_to_pose(gp_base) stop_force_srv.call(kinova_msgs.srv.StopRequest()) return
def execute(self, userdata): first_attamept = True label = userdata.data['item_to_pick']['label'] suck_level = item_meta[label].get('suction_pressure', 3) set_suck_level(suck_level) USE_ANGLED = False position_limits = POSITION_LIMITS[ userdata.data['picking_from']]['sucker'] # Get the tote weight before the pick scales_topic = userdata.data['picking_from'] + '_scales/weight' if item_meta[userdata.data['item_to_pick'] ['label']]['grasp_type'] != 'grip_suck': # If we've just tried sucking, then don't re-weigh try: weight = rospy.wait_for_message(scales_topic, Int16, 2.0) userdata.data['weight_before'] = weight.data except: rospy.logerr('Unable to read weight from %s' % scales_topic) userdata.data['weight_before'] = -1 if item_meta[userdata.data['item_to_pick'] ['label']]['grasp_point_type'] == 'rgb_centroid': rospy.logerr( "Aborting RGB Centroid pick because there aren't any scales." ) return 'suck_failed' for object_global in userdata.data['item_to_pick']['grasp_poses']: USE_ANGLED = decide_angled(object_global, position_limits) object_global.position.x += 0.015 object_global.position.y -= 0.03 if USE_ANGLED: rospy.logerr('ANGLED PICK') # Make sure we're inside the tote. edge_offset = 0.0 object_global.position.x = max( object_global.position.x, position_limits['x_min'] - edge_offset) object_global.position.x = min( object_global.position.x, position_limits['x_max'] + edge_offset) object_global.position.y = max( object_global.position.y, position_limits['y_min'] - edge_offset) object_global.position.y = min( object_global.position.y, position_limits['y_max'] + edge_offset) object_global.position.z -= 0.02 # The endpoint is half way through the suction cup, so move up slightly #t.publish_pose_as_transform(object_global, 'global_xyz_link', 'ORIGINAL', seconds=2) translation = [ object_global.position.x, object_global.position.y, object_global.position.z ] qm = object_global.orientation q = (qm.x, qm.y, qm.z, qm.w) R = tft.quaternion_matrix(q) P = tft.projection_matrix([0, 0, 0], [0, 0, 1]) # XY plane pose_vec = R[:, 2] # Z portion of matrix. proj_vec = np.dot( P, pose_vec)[:3] # Projected onto XY plane as a unit vector. proj_vec_unit = tft.unit_vector(proj_vec) y_unit = [0, 1, 0] # Unit vector in the y-direction. yaw_angle = np.arccos(np.dot( proj_vec_unit, y_unit)) # Angle to the grasp around Z. pitch_angle = np.arcsin( tft.vector_norm(proj_vec)) # Angle to tilt the suction cup if pitch_angle > 1.2: pitch_angle = 1.2 # Only get absolute angle from above. Check the direction of the vector. if R[0, 2] > 0: yaw_angle = -1 * yaw_angle # Create a quaternion with the right angles (note rotations are applied in the rotated frame in zyx order) q = tft.quaternion_from_euler(yaw_angle, 0, -1 * pitch_angle, 'rzyx') if yaw_angle > 1.571 or yaw_angle < -1.571: # Rotate by 180 to normalise to the sucker workspace. q2 = tft.quaternion_from_euler(0, 0, 3.1415) q = tft.quaternion_multiply(q, q2) e = list(tft.euler_from_quaternion(q)) if e[2] > 1.5: # Stop gimbal lock and weird joint configurations. e[2] = 1.5 if e[2] < -1.5: e[2] = -1.5 q = tft.quaternion_from_euler(e[0], e[1], e[2]) object_global.orientation.x = q[0] object_global.orientation.y = q[1] object_global.orientation.z = q[2] object_global.orientation.w = q[3] R = tft.quaternion_matrix(q) t.publish_tf_quaterion_as_transform(translation, q, 'global_xyz_link', 'MOVE_HERE', seconds=0.5) # Create a pre-grasp pose away from the object. T = tft.translation_matrix(translation) T2 = tft.translation_matrix((0, 0, -0.05)) F = tft.concatenate_matrices(T, R, T2) pre_grasp_t = tft.translation_from_matrix(F) pre_grasp_q = tft.quaternion_from_matrix(F) #t.publish_tf_quaterion_as_transform(pre_grasp_t, pre_grasp_q, 'global_xyz_link', 'PRE', seconds=5.0) grasp_orientation = Quaternion() grasp_orientation.x = q[0] grasp_orientation.y = q[1] grasp_orientation.z = q[2] grasp_orientation.w = q[3] rospy.loginfo('MOVE TO TOOL CHANGE HEIGHT') if first_attamept: # Move the whole wrist and then select the sucker. res = m.move_to_global( min(object_global.position.x, REALSENSE_MAX_GLOBAL_X), max( min(object_global.position.y, SUCKER_MAX_Y_STRAIGHT) - 0.02, SUCKER_MIN_GLOBAL_Y + 0.02) - SUCKER_REALSENSE_Y_OFFSET, TOOL_CHANGE_REALSENSE_GLOBAL_Z, 'realsense') if not res: return 'failed' rospy.loginfo('SELECTING SUCKER') res = m.move_to_named_pose('wrist_only', 'sucker') if not res: return 'failed' first_attamept = False else: # Straighten up the sucker from the last attempt and move into position straight_q = Quaternion() straight_q.x = 0 straight_q.y = 0 straight_q.z = 0 straight_q.w = 1 res = m.move_to_global( min(object_global.position.x, SUCKER_MAX_GLOBAL_X), max( min(object_global.position.y, SUCKER_MAX_Y_STRAIGHT) - 0.02, SUCKER_MIN_GLOBAL_Y + 0.02), TOOL_CHANGE_REALSENSE_GLOBAL_Z + 0.35, 'sucker', orientation=straight_q) if not res: return 'failed' rospy.loginfo("MOVE TO PRE-GRASP") res = m.move_to_global(pre_grasp_t[0], pre_grasp_t[1], min(pre_grasp_t[2], SUCKER_MAX_GLOBAL_Z), 'sucker', orientation=grasp_orientation, velo_scale=0.4) if not res: return 'failed' rospy.loginfo("PUMP ON, MOVING TO OBJECT") suction.pump_on() (res, stopped, bail) = m.move_to_global_monitor_weight_and_suction( object_global.position.x, object_global.position.y, min(object_global.position.z, SUCKER_MAX_GLOBAL_Z), 'sucker', scales_topic, orientation=grasp_orientation, velo_scale=0.3) if not res: return 'failed' else: # Straight Down Suck rospy.logerr('STRAIGHT PICK') # Make sure we're inside the tote. edge_offset = 0.0 object_global.position.x = max( object_global.position.x, position_limits['x_min'] - edge_offset) object_global.position.x = min( object_global.position.x, position_limits['x_max'] + edge_offset) object_global.position.y = max( object_global.position.y, position_limits['y_min'] - edge_offset) object_global.position.y = min( object_global.position.y, position_limits['y_max'] + edge_offset) object_global.position.z = min(object_global.position.z, position_limits['z_max']) t.publish_pose_as_transform(object_global, 'global_xyz_link', 'MOVE_HERE', seconds=0.5) rospy.loginfo('MOVE TO TOOL CHANGE HEIGHT') # We only need to select the sucker on the first attempt. # Can cause it to break becasue the controllers don't switch properly # if there's no actual movment and we change controllers too quickly. Not sure why, something to do with timing. if first_attamept: res = m.move_to_global( object_global.position.x, max( min(object_global.position.y, SUCKER_MAX_Y_STRAIGHT), SUCKER_MIN_Y_STRAIGHT) - SUCKER_REALSENSE_Y_OFFSET, TOOL_CHANGE_REALSENSE_GLOBAL_Z, 'realsense') if not res: return 'failed' rospy.loginfo('SELECTING SUCKER') res = m.move_to_named_pose('wrist_only', 'sucker') if not res: return 'failed' first_attamept = False if item_meta[userdata.data['item_to_pick'] ['label']]['grasp_type'] == 'grip_suck': # Weird async movement bug. rospy.sleep(1.0) else: straight_q = Quaternion() straight_q.x = 0 straight_q.y = 0 straight_q.z = 0 straight_q.w = 1 res = m.move_to_global( object_global.position.x, max( min(object_global.position.y, SUCKER_MAX_Y_STRAIGHT), SUCKER_MIN_Y_STRAIGHT), TOOL_CHANGE_REALSENSE_GLOBAL_Z + 0.35, 'sucker', orientation=straight_q) if not res: return 'failed' # Correct the rotation if required. pca_yaw = userdata.data['item_to_pick']['pca_yaw'] q = (pca_yaw.x, pca_yaw.y, pca_yaw.z, pca_yaw.w) e = list(tft.euler_from_quaternion(q)) if e[2] > 1.57: e[2] -= 3.14 if e[2] < -1.57: e[2] += 3.14 if e[2] > 1.5: # Stop gimbal lock and weird joint configurations. e[2] = 1.5 if e[2] < -1.5: e[2] = -1.5 q = tft.quaternion_from_euler(0, 0, e[2]) pca_yaw.x = q[0] pca_yaw.y = q[1] pca_yaw.z = q[2] pca_yaw.w = q[3] rospy.loginfo("PUMP ON, MOVING TO OBJECT") suction.pump_on() v_scale = 0.25 if item_meta[userdata.data['item_to_pick'] ['label']]['grasp_point_type'] == 'rgb_centroid': v_scale = 0.1 (res, stopped, bail) = m.move_to_global_monitor_weight_and_suction( object_global.position.x, min(object_global.position.y, SUCKER_MAX_Y_STRAIGHT), min(object_global.position.z, SUCKER_MAX_GLOBAL_Z), 'sucker', scales_topic, orientation=pca_yaw, velo_scale=v_scale) if not res: return 'failed' if bail: m.move_relative(0, 0, -0.02, 'sucker_endpoint', 'sucker') rospy.sleep(0.5) elif not suction.check_pressure_sensor(): # If it's not already on, give it a sec. rospy.sleep(1.0) suction_state = suction.check_pressure_sensor( ) #query pressure sensor for state, has the object been sucked correctly rospy.loginfo("PRESSURE SENSOR STATUS: %s" % suction_state) if not suction_state and not bail: set_suck_level(3) # Just move down a touch and double check move_amount = 0.02 if USE_ANGLED: move_amount = 0.035 if object_global.position.z < (SUCKER_MAX_GLOBAL_Z): move_amount = min( move_amount, SUCKER_MAX_GLOBAL_Z - object_global.position.z) res = m.move_relative(0, 0, move_amount, 'sucker_endpoint', 'sucker', plan_time=0.5) else: res = True if res: rospy.sleep(1.0) suction_state = suction.check_pressure_sensor() if suction_state: #userdata.data['last_failed'] = None return 'succeeded' else: suction.pump_off() # Run out of grasp poses. suction.pump_off() if item_meta[userdata.data['item_to_pick'] ['label']]['grasp_type'] == 'suck_grip': return 'try_gripper' userdata.data['last_failed'][userdata.data['item_to_pick'] ['label']] = 0 return 'suck_failed'
def execute(self, userdata): gripper.open_gripper() position_limits = POSITION_LIMITS[userdata.data['picking_from']]['gripper'] # Get the weight before the pick scales_topic = userdata.data['picking_from'] + '_scales/weight' if item_meta[userdata.data['item_to_pick']['label']]['grasp_type'] != 'suck_grip': # If we've just tried sucking, then don't re-weigh try: weight = rospy.wait_for_message(scales_topic, Int16, 2.0) userdata.data['weight_before'] = weight.data except: rospy.logerr('Unable to read weight from %s' % scales_topic) userdata.data['weight_before'] = -1 if item_meta[userdata.data['item_to_pick']['label']]['grasp_point_type'] == 'rgb_centroid': rospy.logerr("Aborting RGB Centroid pick because there aren't any scales.") return 'grip_failed' # No way to detect failure yet, so just use one grasp point. object_global = userdata.data['item_to_pick']['grasp_poses'][0] object_global.position.x += 0.015 object_global.position.y -= 0.03 object_global.position.x = max(object_global.position.x, position_limits['x_min']) object_global.position.x = min(object_global.position.x, position_limits['x_max']) object_global.position.y = max(object_global.position.y, position_limits['y_min']) object_global.position.y = min(object_global.position.y, position_limits['y_max']) object_global.position.z = min(object_global.position.z, position_limits['z_max']) t.publish_pose_as_transform(object_global, 'global_xyz_link', 'MOVE HERE', 0.5) rospy.loginfo('MOVE TO TOOL CHANGE HEIGHT') curr = t.current_robot_pose('global_xyz_link', 'realsense_endpoint') # res = m.move_to_global(curr.position.x, curr.position.y, TOOL_CHANGE_REALSENSE_GLOBAL_Z, 'realsense') res = m.move_to_global(min(object_global.position.x, REALSENSE_MAX_GLOBAL_X), max(min(object_global.position.y, SUCKER_MAX_Y_STRAIGHT) - 0.02, SUCKER_MIN_GLOBAL_Y + 0.02) - SUCKER_REALSENSE_Y_OFFSET, TOOL_CHANGE_REALSENSE_GLOBAL_Z, 'realsense') if not res: return 'failed' rospy.loginfo('SELECTING GRIPPER') res = m.move_to_named_pose('wrist_only', 'gripper') if not res: return 'failed' object_global.position.z += 0.015 pca_yaw = userdata.data['item_to_pick']['pca_yaw'] q = (pca_yaw.x, pca_yaw.y, pca_yaw.z, pca_yaw.w) e = list(tft.euler_from_quaternion(q)) if e[2] > 1.57: e[2] -= 3.14 if e[2] < -1.57: e[2] += 3.14 if e[2] > 1.55: # Stop gimbal lock and weird joint configurations. e[2] = 1.55 if e[2] < -1.55: e[2] = -1.55 q = tft.quaternion_from_euler(0, 0, e[2]) object_global.orientation.x = q[0] object_global.orientation.y = q[1] object_global.orientation.z = q[2] object_global.orientation.w = q[3] rospy.loginfo('MOVE GRIPPER TO ABOVE OBJECT') res = m.move_to_global(object_global.position.x, object_global.position.y, max(object_global.position.z - 0.2, GRIPPER_MIN_GLOBAL_Z), 'gripper', orientation=object_global.orientation) if not res: return 'failed' rospy.loginfo('MOVING TO OBJECT') v_scale = 0.1 if item_meta[userdata.data['item_to_pick']['label']]['grasp_point_type'] == 'rgb_centroid': # Don't trust the depth. v_scale = 0.1 if userdata.data['item_to_pick']['label'] == 'mesh_cup': v_scale = 0.05 (res, stopped, bail) = m.move_to_global_monitor_weight(object_global.position.x, object_global.position.y, min(object_global.position.z, GRIPPER_MAX_GLOBAL_Z), 'gripper', scales_topic, velo_scale=v_scale, orientation=object_global.orientation) if not res: return 'failed' if bail: m.move_relative(0, 0, -0.02, 'gripper_endpoint', 'gripper') # rospy.sleep(0.5) rospy.loginfo('CLOSE THE GRIPPER') gripper.close_gripper() rospy.sleep(0.5) return('succeeded')
rospy.loginfo(move_success + str(res)) #MOVE TO STRAIGHT DROP POSITION curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint') pose = gmsg.Pose() qt = tf.transformations.quaternion_from_euler(0.0, 0.0, 1.57) pose.position.x = 0.05 pose.position.y = curr.position.y pose.position.z = 1.0 pose.orientation.x = qt[0] pose.orientation.y = qt[1] pose.orientation.z = qt[2] pose.orientation.w = qt[3] t.publish_pose_as_transform(pose, 'global_xyz_link', 'PINEAPPLE', seconds=5) rospy.loginfo('MOVE UP TO STRAIGHT DROP POSITION') res = m.move_to_global(pose.position.x, curr.position.y, pose.position.z, 'gripper', orientation=pose.orientation) if not res: rospy.logerr(move_error) rospy.loginfo(move_success + str(res)) elif tool is 'sucker': #TOOL CHANGE TO 45 DEGREES
def execute_grasp(): # 执行抓取。 global MOVING global CURR_Z global start_force_srv global stop_force_srv # 得到位置。 msg = rospy.wait_for_message('/ggcnn/out/command', std_msgs.msg.Float32MultiArray) d = list(msg.data) # 计算抓爪宽度。 grip_width = d[4] # 将以像素为单位的宽度转换为毫米。 # 0.07是从末端执行器(CURR_Z)到摄像机的距离。 #对于实感,每个像素约0.1度。 g_width = 2 * ((CURR_Z + 0.07)) * np.tan( 0.1 * grip_width / 2.0 / 180.0 * np.pi) * 1000 # 转换为电机位置。 g = min((1 - (min(g_width, 70) / 70)) * (6800 - 4000) + 4000, 5500) set_finger_positions([g, g]) rospy.sleep(0.5) # 握把的姿势(仅位置)在相机框架中。 gp = geometry_msgs.msg.Pose() gp.position.x = d[0] gp.position.y = d[1] gp.position.z = d[2] gp.orientation.w = 1 # 转换为基本框架,并添加角度(以确保平面抓地力,不保证相机垂直)。 gp_base = convert_pose(gp, 'camera_depth_optical_frame', 'm1n6s200_link_base') q = tft.quaternion_from_euler(np.pi, 0, d[3]) gp_base.orientation.x = q[0] gp_base.orientation.y = q[1] gp_base.orientation.z = q[2] gp_base.orientation.w = q[3] publish_pose_as_transform(gp_base, 'm1n6s200_link_base', 'G', 0.5) # 初始姿势的偏移量。 initial_offset = 0.20 gp_base.position.z += initial_offset # 禁用力控制,使机器人更加精确。 stop_force_srv.call(kinova_msgs.srv.StopRequest()) move_to_pose(gp_base) rospy.sleep(0.1) # 启动力控制,有助于防止不良碰撞。 start_force_srv.call(kinova_msgs.srv.StartRequest()) rospy.sleep(0.25) # 重设位置 gp_base.position.z -= initial_offset # 标记以检查碰撞。 MOVING = True # 为控制器生成非线性。 cart_cov = generate_cartesian_covariance(0) # 在速度控制下直线向下移动。 velo_pub = rospy.Publisher('/m1n6s200_driver/in/cartesian_velocity', kinova_msgs.msg.PoseVelocity, queue_size=1) while MOVING and CURR_Z - 0.02 > gp_base.position.z: dz = gp_base.position.z - CURR_Z - 0.03 # Offset by a few cm for the fingertips. MAX_VELO_Z = 0.08 dz = max(min(dz, MAX_VELO_Z), -1.0 * MAX_VELO_Z) v = np.array([0, 0, dz]) vc = list(np.dot(v, cart_cov)) + [0, 0, 0] velo_pub.publish(kinova_msgs.msg.PoseVelocity(*vc)) rospy.sleep(1 / 100.0) MOVING = False # 闭合手指。 rospy.sleep(0.1) set_finger_positions([8000, 8000]) rospy.sleep(0.5) # 移回初始位置。 gp_base.position.z += initial_offset gp_base.orientation.x = 1 gp_base.orientation.y = 0 gp_base.orientation.z = 0 gp_base.orientation.w = 0 move_to_pose(gp_base) stop_force_srv.call(kinova_msgs.srv.StopRequest()) return