def gentle_move(dir_index, q_initial, pos, gripperOri_list, tip_hand_transform, listener, point_counter): plans = [] axis_list = [[0, 1, 0], [-1, 0, 0], [0, -1, 0], [1, 0, 0], [0, 0, -1], [0, 0, 1]] axis = axis_list[dir_index] if point_counter == 4: bin_counter = 0 elif point_counter == 9: bin_counter = 1 elif point_counter == 14: bin_counter = 2 elif point_counter == 19: bin_counter = 3 for i in range(0, 10000): for i in range(0, 3): pos[i] = pos[i] + 0.002 * axis[i] with Timer('generatePlan'): plan, qf, plan_possible = generatePlan(q_initial, pos, gripperOri_list, tip_hand_transform, 'fastest') if plan_possible: plans.append(plan) q_initial = qf else: return '[IK] Error' #~execute plan with Timer('executePlanForward'): executePlanForward(plans, False, False) value = raw_input( "Enter: Move 2mm closer to wall, s: Stop robot and record value ") #save data if s if value == 's': fpath = os.environ[ 'CODE_BASE'] + '/catkin_ws/src/apc_config/storage_corners.yaml' stream = open(fpath, 'r') data = yaml.load(stream) tcp_pose = get_tcp_pose(listener) if dir_index == 4: terms = ['z'] else: terms = ['x', 'y'] for i in range(0, len(terms)): if dir_index == 4: param_name = '/bin' + str(bin_counter) + '/' + terms[i] value_tmp = tcp_pose[2] - rospy.get_param( '/gripper/spatula_tip_to_tcp_dist') else: param_name = '/corner' + str( point_counter) + '/' + terms[i] value_tmp = tcp_pose[i] value = value_tmp.astype(type('float', (float, ), {})) data[param_name] = value with open(fpath, 'w') as yaml_file: yaml_file.write(yaml.dump(data, default_flow_style=False)) break else: plans = []
def release_safe(listener, isExecute=True, withPause=False): plans = [] #Initialize parameters spatula_tip_to_tcp_dist = rospy.get_param( "/gripper/spatula_tip_to_tcp_dist") l1 = 0.0 l2 = 0.0 l3 = spatula_tip_to_tcp_dist tip_hand_transform = [l1, l2, l3, 0, 0, 0] bin_pose = ik.helper.get_params_yaml('bin' + str(0) + '_pose') UpdistFromBinFast = 0.15 - 0.1 #Initial configuration of robot q_initial = ik.helper.get_joints() targetPose = ik.helper.get_tcp_pose(listener, tcp_offset=spatula_tip_to_tcp_dist) #Desired new safe position above binNum targetPose[2] = bin_pose[2] + UpdistFromBinFast ################# ## Build plans ## ################# #~1. Open gripper grasp_plan = EvalPlan('helper.moveGripper(%f, 200)' % 0.11) plans.append(grasp_plan) #~2. Open spatula grasp_plan = EvalPlan('spatula.open()') plans.append(grasp_plan) #~3. Move up plan, qf, plan_possible = generatePlan(q_initial, targetPose[0:3], targetPose[3:7], tip_hand_transform, 'superSaiyan', plan_name='go_safe_bin') if plan_possible: plans.append(plan) q_initial = qf else: print('[Release Safe] Plans failed:') #execute plan_name if isExecute and plan_possible: executePlanForward(plans, withPause)
def grasp(objInput, listener, br, isExecute=True, objId='expo_eraser', binId=0, flag=0, withPause=False, rel_pose=None, BoxBody=None, place_pose=None, viz_pub=None, is_drop=True, update_command=None, recorder=None): ######################################################### # fhogan and nikhilcd # Description: #~2017 picking primitive # #~Usage #~ pick(objInput, listener, br, isExecute = True, objId = 'cheezit_big_original', flag = 1, withPause = False, rel_pose=None, BoxBody=None, place_pose=None, viz_pub=None): # #~Parameters: # #~Inputs: # 1) objInput: Input from vision or RViz (list of 12, or 7) # 2) listener: Input from vision or RViz (list of 12, or 7) # 3) br: Input from vision or RViz (list of 12, or 7) # 4) isExecute: execute the robot motion if possible or not? (Bool) # 5) objId: name of object (str) # 6) binId: bin in which we need to place the objects. (It is useless when flag 0 and 1) # 7) flag: Type of action directed by planning (0:run picking, 1:go to home (camera), 2: drop the object at target location) # 8) withPause: Pause between robot motions (Bool) # 9) rel_pose: object pose relative to link_6 # 10) BoxBody: Bounding box points resolved in link 6 frame # 11) place_pose: desired position of bounding box # 12) viz_pub: desired position of bounding box # #~Output: # Dictionary of following keys: # 1) grasp_possible: Does the object fit in the hand? (True/False) # 2) plan_possible: Has the plan succeeded? (True/False) # 3) execution_possible: Is there anything in the grasp? (True/False) # 4) gripper_opening: gripper opening after grasp attempt (in meters) # 5) graspPose: pose of the gripper at grasp (7X1 - position and quaternion) # 6) gelsight_data: [] , dummy for now ######################################################### #rospy.logdebug(msg, *args) #rospy.loginfo(msg, *args) #rospy.logwarn(msg, *args) #rospy.logerr(msg, *args) #rospy.logfatal(msg, *args) ############################### ## Pring input variables for ## ############################### rospy.logdebug('[Picking] Starting primitive with flag: %d' % flag) rospy.logdebug('[Picking] objInput %s', objInput) rospy.logdebug('[Picking] objId %s', objId) rospy.logdebug('[Picking] flag %s', flag) rospy.logdebug('[Picking] withPause %s', withPause) rospy.logdebug('[Picking] rel_pose %s', rel_pose) rospy.logdebug('[Picking] BoxBody %s', BoxBody) rospy.logdebug('[Picking] place_pose %s', place_pose) ######################## ## Initialize values ## ######################## q_initial = ik.helper.get_joints() bin_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose') spatula_tip_to_tcp_dist = rospy.get_param( "/gripper/spatula_tip_to_tcp_dist") delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking') vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3]) plans = [] plans_grasping = [] plans_grasping2 = [] plans_guarded = [] plans_placing = [] graspPose = [] plan_possible = False execution_possible = False gripper_opening = 0.0 grasp_possible = True gelsight_data = [] collision = False final_object_pose = None rospy.set_param('is_record', False) rospy.set_param('is_contact', False) weightSensor = ws_prob.WeightSensor() liftoff_pub = rospy.Publisher('/liftoff_time', std_msgs.msg.String, queue_size=10, latch=False) def compose_output(): return { 'collision': collision, 'grasp_possible': grasp_possible, 'plan_possible': plan_possible, 'execution_possible': execution_possible, 'gripper_opening': gripper_opening, 'graspPose': graspPose, 'gelsight_data': gelsight_data, 'final_object_pose': final_object_pose } ######################### ## Constant parameters ## ######################### UpdistFromBinFast = 0.15 # Margin above object during "move to a location" UpdistFromBinGuarded = 0.05 # Margin above object during "move to a location" gripperOriHome = [0, 1, 0, 0] l1 = 0.0 l2 = 0.0 l3 = spatula_tip_to_tcp_dist tip_hand_transform = [l1, l2, l3, 0, 0, 0] vision_pos[2] = 0.17786528 ############################### ## Picking primitive flag: 0 ## ############################### if flag == 0: # ik.visualize_helper.visualize_grasping_proposals(viz_pub, np.asarray([objInput]), listener, br, False) #~Define reference frames world_X, world_Y, world_Z, tote_X, tote_Y, tote_Z, tote_pose_pos = ik.helper.reference_frames( listener=listener, br=br) #~get grasp pose and gripper opening from vision if len(objInput) == 12: graspPos, hand_X, hand_Y, hand_Z, grasp_width = ik.helper.get_picking_params_from_12( objInput) elif len(objInput) == 7: graspPos, hand_X, hand_Y, hand_Z, grasp_width = ik.helper.get_picking_params_from_7( objInput, objId, listener, br) #~build gripper orientation matrix 3x3 hand_orient_norm = np.vstack([hand_X, hand_Y, hand_Z]) hand_orient_norm = hand_orient_norm.transpose() #~Grasp relocation script hand_orient_quat = ik.helper.mat2quat(hand_orient_norm) euler = tf.transformations.euler_from_quaternion(hand_orient_quat) theta = euler[2] - np.pi colFreePose = collisionFree(graspPos, binId=binId, listener=listener, br=br, finger_opening=grasp_width, safety_margin=0.00, theta=theta) graspPos[0:2] = colFreePose[0:2] ################################# ## GENERATE PLANS OF PRIMITIVE ## #################################. #~ Start from home position # if isExecute: # q_initial = collision_free_placing(binId, listener, isSuction = False, with_object = False, hand_orientation=[0,1,0,0], projected_target_position = bin_pose[0:3], isReturn = True) # scorpion.back() #~0. Move to a location above the bin, Rotate gripper to grasp orientation pregrasp_targetPosition = graspPos pregrasp_targetPosition[2] = bin_pose[2] + UpdistFromBinFast plan, qf, plan_possible = generatePlan(q_initial, pregrasp_targetPosition, hand_orient_quat, tip_hand_transform, 'superSaiyan', plan_name='go_safe_bin') if plan_possible: plans.append(plan) q_initial = qf else: return compose_output() #~1. Move to to surface of bin pregrasp_targetPosition[2] = bin_pose[2] + UpdistFromBinGuarded plan, qf, plan_possible = generatePlan(q_initial, pregrasp_targetPosition, hand_orient_quat, tip_hand_transform, 'superSaiyan', plan_name='go_top_bin') if plan_possible: plans.append(plan) q_initial = qf else: return compose_output() #~2. Open gripper grasp_plan = EvalPlan('helper.moveGripper(%f, 200)' % grasp_width) plans.append(grasp_plan) #~ 3. Open spatula grasp_plan = EvalPlan('spatula.open()') plans.append(grasp_plan) grasp_targetPosition = deepcopy(graspPos) # rospy.loginfo('[Picking] Grasp Target %s', grasp_targetPosition) #~4. sleep sleep_plan = EvalPlan('rospy.sleep(0.2)') plans.append(sleep_plan) #~5. perform guarded move down grasp_targetPosition[2] = bin_pose[2] - rospy.get_param( '/tote/height') + 0.000 #~frank hack for safety if binId == 0: grasp_targetPosition[2] = grasp_targetPosition[2] + 0.005 plan, qf, plan_possible = generatePlan(q_initial, grasp_targetPosition, hand_orient_quat, tip_hand_transform, 'faster', guard_on=WeightGuard( binId, threshold=100), plan_name='guarded_pick') if plan_possible: plans_guarded.append(plan) q_initial = qf else: return compose_output() #~7. Close spatula grasp_plan = EvalPlan('spatula.close()') plans_guarded.append(grasp_plan) #~8. Close gripper grasp_plan = EvalPlan('helper.graspinGripper(%f,%f)' % (800, 50)) plans_guarded.append(grasp_plan) #~9. sleep sleep_plan = EvalPlan('rospy.sleep(0.2)') plans_guarded.append(sleep_plan) #~10. Move to a location above the bin plan, qf, plan_possible = generatePlan(q_initial, vision_pos, delta_vision_pose[3:7], tip_hand_transform, 'fastest', plan_name='retrieve_object') if plan_possible: plans_grasping.append(plan) q_initial = qf else: return compose_output() ################ ## EXECUTION ### ################ if isExecute and plan_possible: executePlanForward(plans, withPause) ###We start recording now lasers.start(binId) recorder.start_recording( action='grasping', action_id=str( datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")), tote_num=binId, frame_rate_ratio=5, image_size=-1) #Execute guarded move rospy.set_param('is_record', True) executePlanForward(plans_guarded, withPause) rospy.set_param('is_record', False) #Publish liftoff time liftoff_msgs = std_msgs.msg.String() liftoff_msgs.data = 'Liftoff (Robot command)' liftoff_pub.publish(liftoff_msgs) #Execute non-guarded grasp plan move executePlanForward(plans_grasping, withPause) ###We stop recording recorder.stop_recording(save_action=True) lasers.stop(binId) #~Check if picking success low_threshold = 0.0035 high_threshold = 0.015 if isExecute and plan_possible: rospy.sleep(2) gripper_opening = gripper.getGripperopening() if gripper_opening > high_threshold: rospy.loginfo('[Picking] ***************') rospy.loginfo( '[Picking] Pick Successful (Gripper Opening Test)') rospy.loginfo('[Picking] ***************') execution_possible = True else: rospy.loginfo('[Picking] ***************') rospy.loginfo( '[Picking] Pick Inconclusive (Gripper Opening Test)') rospy.loginfo('[Picking] ***************') execution_possible = None elif not isExecute: execution_possible = plan_possible return compose_output() ############# ## Placing ## ############# elif flag == 2: #if primitive called without placing arguments, go to center of bin if rel_pose == None: drop_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose') drop_pose[3:7] = gripperOriHome else: drop_pose = ik.helper.drop_pose_transform(binId, rel_pose, BoxBody, place_pose, viz_pub, listener, br) #~ Adjust height according to weigth if is_drop: drop_offset = 0.15 else: drop_offset = 0.002 #~set drop pose target z position to be bottom of bin drop_pose[2] = bin_pose[2] - rospy.get_param( 'tote/height') + drop_offset #~Predrop: go to top middle bin surface fast without guarded move predrop_pos = np.array(drop_pose[0:3]) predrop_pos[2] = bin_pose[2] + 0.05 ###################### ## Generate plans ## ###################### #~move safe to placing bin number rospy.logdebug( '[Picking] Collision free placing to binId %s and position %s', binId, predrop_pos) # if isExecute: # q_initial = collision_free_placing(binId, listener, obj_ID=objId, BoxBody=BoxBody, isSuction = False,with_object = True, hand_orientation=drop_pose[3:7],projected_target_position = predrop_pos, isReturn = True) # if update_command is not None: # update_command.execute() #~0.go up to avoid collision with tote walls current_pose = ik.helper.get_tcp_pose(listener, tcp_offset=l3) current_pose[2] = current_pose[2] + 0.15 plan, qf, plan_possible = generatePlan(q_initial, current_pose[0:3], current_pose[3:7], tip_hand_transform, 'faster', plan_name='go_up') if plan_possible: plans.append(plan) q_initial = qf else: return compose_output() #~0.1.fast motion to bin surface high predrop_pos_high = predrop_pos predrop_pos_high[2] = current_pose[2] plan, qf, plan_possible = generatePlan(q_initial, predrop_pos_high, drop_pose[3:7], tip_hand_transform, 'faster', plan_name='go_bin_surface_high') if plan_possible: plans.append(plan) q_initial = qf else: return compose_output() #~1.fast motion to bin surface predrop_pos[2] = bin_pose[2] + 0.05 plan, qf, plan_possible = generatePlan(q_initial, predrop_pos, drop_pose[3:7], tip_hand_transform, 'faster', plan_name='go_bin_surface') if plan_possible: plans.append(plan) q_initial = qf else: return compose_output() #~2. Guarded move down slow plan, qf, plan_possible = generatePlan(q_initial, drop_pose[0:3], drop_pose[3:7], tip_hand_transform, 'fast', guard_on=WeightGuard(binId), backwards_speed='superSaiyan', plan_name='guarded_drop') if plan_possible: plans_placing.append(plan) q_initial = qf else: return compose_output() #~3. open gripper grasp_plan = EvalPlan('helper.moveGripper(0.110, 200)') plans_placing.append(grasp_plan) #~4. open spatula grasp_plan = EvalPlan('spatula.open()') plans_placing.append(grasp_plan) #~5. go to predrop_pos plan, qf, plan_possible = generatePlan(q_initial, predrop_pos[0:3], drop_pose[3:7], tip_hand_transform, 'superSaiyan', plan_name='predrop_pos') if plan_possible: plans_placing.append(plan) q_initial = qf else: return compose_output() #~6. go to final pose final_pos = bin_pose[0:3] final_pos[2] = bin_pose[2] + 0.3 plan, qf, plan_possible = generatePlan(q_initial, final_pos, drop_pose[3:7], tip_hand_transform, 'superSaiyan', plan_name='final_pose') if plan_possible: plans_placing.append(plan) q_initial = qf else: return compose_output() #~ Execute plans if plan_possible: if isExecute: executePlanForward(plans, withPause) #We start recording now lasers.start(binId) recorder.start_recording( action='placing', action_id=str( datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")), tote_num=binId, frame_rate_ratio=5, image_size=-1) rospy.set_param('is_record', True) executePlanForward(plans_placing, withPause) rospy.set_param('is_record', False) recorder.stop_recording(save_action=True) lasers.stop(binId) execution_possible = True return compose_output()
def calibrate_gelsight(isExecute=True, withPause=False): arc_ori = np.array([0, 1, 0, 0]) ori_1 = np.array([-0.70710678118654757, 0.70710678118654757, 0, 0]) ori_2 = np.array([0.70710678118654757, 0.70710678118654757, 0, 0]) #initialize Parameters tip_hand_transform = [0, 0, 0, 0, 0, 0] #read initial robot poses q_initial = ik.helper.get_joints() #go to arc_1 # goToHome.goToARC() plans_list = [] pose_list = [] #arc position (high) pose_list.append(np.array([1.0, 0., 0.792, 0, 1, 0, 0])) pose_list.append( np.array([.642, .72005, .95168, .69934, .70796, -0.06744, .07189])) pose_list.append( np.array([.642, .72005, .75559, .69934, .70796, -0.06744, .07189])) pose_list.append( np.array([.56577, .72005, .75559, .69934, .70796, -0.06744, .07189])) pose_list.append( np.array([.48138, .72005, .75559, .69934, .70796, -0.06744, .07189])) pose_list.append( np.array([.48138, .72005, .95168, .69934, .70796, -0.06744, .07189])) pose_list.append(np.array([1.0, 0., 0.792, 0, 1, 0, 0])) speed_list = [ 'superSaiyan', 'superSaiyan', 'slow', 'slow', 'slow', 'slow', 'superSaiyan' ] #gripper hand_commands # is_gripper_list = [0,0,0,0,0,0,0] is_gripper_list = [0, 0, 1, 1, 1, 0, 0] #~1. Open gripper grasp_plan = EvalPlan('helper.moveGripper(%f, 200)' % 0.11) plans_list.append(grasp_plan) #2. generate robot motions for i in range(len(pose_list)): print ' speed_list[i]', speed_list[i] plan, qf, plan_possible = generatePlan(q_initial, pose_list[i][0:3], pose_list[i][3:7], tip_hand_transform, speed_list[i], plan_name=str(i)) if plan_possible: plans_list.append(plan) q_initial = qf else: print('[Release Safe] Plans failed:') if is_gripper_list[i]: #sleep sleep_plan = EvalPlan('rospy.sleep(1.0)') plans_list.append(sleep_plan) #close gripper grasp_plan = EvalPlan('helper.graspinGripper(%f,%f)' % (800, 30)) plans_list.append(grasp_plan) #sleep sleep_plan = EvalPlan('rospy.sleep(1.5)') plans_list.append(sleep_plan) #capture image gelsight_plan = EvalPlan('helper.capture_gelsight()') plans_list.append(gelsight_plan) #sleep sleep_plan = EvalPlan('rospy.sleep(.5)') plans_list.append(sleep_plan) #open gripper grasp_plan = EvalPlan('helper.moveGripper(%f, 200)' % 0.11) plans_list.append(grasp_plan) #sleep sleep_plan = EvalPlan('rospy.sleep(2.)') plans_list.append(sleep_plan) #execute plan_name if isExecute and plan_possible: executePlanForward(plans_list, withPause)
def unit_test_centers(): listener = tf.TransformListener() br = tf.TransformBroadcaster() #~ ~Build hand transform l1 = 0.0 l2 = 0.0 l3 = rospy.get_param("/gripper/spatula_tip_to_tcp_dist") tip_hand_transform = [l1, l2, l3, 0, 0, 0] gripperOriHome = [0, 1, 0, 0] q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014] plans = [] withPause = True #~generate plans #loop through sections (bins) plans = [] goToHome.prepGripperPicking() goToHome.goToARC() for i in range(0, 9): binId = i collision_free_placing(binId, listener, isSuction=False) #~READ robot position rospy.sleep(.3) tcpPose = get_tcp_pose(listener, tcp_offset=l3) tcpPose[2] = rospy.get_param( '/bin' + str(i) + '_pose/z') #~- rospy.get_param('/bin'+str(i)+'/height') rospy.sleep(.3) q_initial = get_joints() rospy.sleep(1.0) ##move to edge of structure map_bin_id_to_ws_id = { 0: 0, 1: 1, 2: 2, 3: 3, 4: 0, 5: 0, 6: 4, 7: 4, 8: 5 } plan, qf, plan_possible = generatePlan(q_initial, tcpPose[0:3], gripperOriHome, tip_hand_transform, 'fast') if plan_possible: plans.append(plan) q_initial = qf else: return '[IK] Error' ##move to edge of structure map_bin_id_to_ws_id = { 0: 0, 1: 1, 2: 2, 3: 3, 4: 0, 5: 0, 6: 4, 7: 4, 8: 5 } plan, qf, plan_possible = generatePlan( q_initial, tcpPose[0:3] - np.array([0, 0, 0.2]), gripperOriHome, tip_hand_transform, 'fast', guard_on=WeightGuard(map_bin_id_to_ws_id[binId], threshold=100)) if plan_possible: plans.append(plan) q_initial = qf else: return '[IK] Error' executePlanForward(plans, False) if binId > 5: collision_free_placing(binId, listener, isSuction=False) plans = []
def unit_test_collision(flag=0): #~ ~Build hand transform l1 = 0.0 l2 = 0.0 l3 = rospy.get_param("/gripper/calibration_to_tcp_dist") tip_hand_transform = [l1, l2, l3, 0, 0, 0] gripperOriHome = [0, 1, 0, 0] q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014] plans = [] withPause = True #~generate plans #loop through sections (bins) counter = 0 section_points_list = [[0, 1, 2, 3], [0, 1, 2, 3], [0, 1, 2, 3], [0, 2, 3]] #~bin corners for i in range(0, 4): #~get corner points from yaml points_warped_list, points_list = get_bin_corners(i) points_warped = np.asarray(points_warped_list) points = np.asarray(points_list) box1 = points box2 = points_warped #################### ## middle point ## ################### point_middle_list = get_bin_middle(i) point_middle = np.asarray(point_middle_list[0]) #~transform to warped frame point_middle_warped = rect2quad(point_middle, box1, box2) #~ point_middle_warped = quad2quad(point_middle, box1, box2) #~ point_middle_warped = interp2d(point_middle, box1, box2) target_middle = np.hstack((point_middle_warped, 0.05)) #################### ## corner points ## ################### for term in section_points_list[i]: ##move to middle plan, qf, plan_possible = generatePlan(q_initial, target_middle, gripperOriHome, tip_hand_transform, 'fastest') counter += 1 if plan_possible: plans.append(plan) q_initial = qf else: return '[IK] Error' #~ target_position2d_warped = np.array(points_warped_list[term]) target_position2d = np.array(points_list[term]) #~convert to warped frame target_position2d = rect2quad(target_position2d, box1, box2) #~ target_position2d = quad2quad(target_position2d,box1,box2) #~ target_position2d = interp2d(target_position2d,box1,box2) target_position = np.hstack((target_position2d, 0.05)) ##move to edge of structure target_middle2 = np.array( [target_middle[0], target_position[1], target_middle[2]]) ##move to middle edge plan, qf, plan_possible = generatePlan(q_initial, target_middle2, gripperOriHome, tip_hand_transform, 'fastest') counter += 1 if plan_possible: plans.append(plan) q_initial = qf else: return '[IK] Error' #~generate plan plan, qf, plan_possible = generatePlan(q_initial, target_position, gripperOriHome, tip_hand_transform, 'fastest') counter += 1 if plan_possible: plans.append(plan) q_initial = qf else: return '[IK] Error' ##move to edge of structure plan, qf, plan_possible = generatePlan(q_initial, target_middle2, gripperOriHome, tip_hand_transform, 'fastest') counter += 1 if plan_possible: plans.append(plan) q_initial = qf else: return '[IK] Error' #~Execute plans goToARC() executePlanForward(plans, withPause)
def calibrate_storage(): ################################################# #~ Generate Plans gripperOri = [] gripperOri.append([-1., 0., 0., 0.]) gripperOri.append([-0.70710678118654757, -0.70710678118654757, 0., 0.]) gripperOri.append([0., -1., 0., 0.]) gripperOri.append([-0.70710678118654757, 0.70710678118654757, 0., 0.]) #~build total list plan gripperOri_list = [] ori_list = [ 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0 ] for i in range(0, len(ori_list)): gripperOri_list.append(gripperOri[ori_list[i]]) #~positions pos = [] #~bin0 bin_pose = get_params_yaml('bin0_pose') pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([1.00 + .12, -0.46 + 0.19647, 0.41]) pos.append([0.75 + .12, bin_pose[1], 0.41]) pos.append([1. + .12, -0.697281360626 + 0.19647, 0.41]) pos.append([1.19 + .12, bin_pose[1], 0.41]) pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append( [1.00278019905 + .12, -0.763054132462 + 0.19647, 0.447774887085 + .05]) pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) #~bin1 bin_pose = get_params_yaml('bin1_pose') pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([1.00 + .12, -0.0943171977997 + 0.19647, 0.41]) pos.append([0.75 + .12, bin_pose[1], 0.41]) pos.append([1. + .12, -0.330679833889 + 0.19647, 0.41]) pos.append([1.19 + .12, bin_pose[1], 0.41]) pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([ 0.961158931255 + .12, -0.0221027284861 + 0.19647, 0.447779148817 + .05 ]) pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) #~bin2 bin_pose = get_params_yaml('bin2_pose') pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([1.00 + .12, 0.307756274939 + 0.19647, 0.41]) pos.append([0.8 + .12, bin_pose[1], 0.41]) pos.append([1 + .12, 0.0618322640657 + 0.19647, 0.41]) pos.append([1.18 + .12, bin_pose[1], 0.41]) pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append( [0.940047621727 + .12, 0.353745698929 + 0.19647, 0.447779148817 + .05]) pos.append([ bin_pose[0] + .12, bin_pose[1], 0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) #################### ## Generate Plans ## #################### q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014] tip_hand_transform = [0., 0., 0., 0., 0., 0.] dir_list = [ None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None ] point_counter = 0 #~generate plans plans = [] listener = tf.TransformListener() br = tf.TransformBroadcaster() rospy.sleep(0.5) goToHome.goToARC() #~ goToHome.prepGripperPicking() for i in range(0, len(pos)): plan, qf, plan_possible = generatePlan(q_initial, pos[i], gripperOri_list[i], tip_hand_transform, 'fastest') if plan_possible: plans.append(plan) q_initial = qf else: return '[IK] Error' #~execute plan executePlanForward(plans, True) plans = [] if dir_list[i] is not None: gentle_move(dir_list[i], q_initial, pos[i], gripperOri_list[i], tip_hand_transform, listener, point_counter) point_counter += 1
def calibrate_boxes(): ################################################# #~ Generate Plans gripperOri = [] gripperOri.append([-1., 0., 0., 0.]) gripperOri.append([-0.715217411518, -0.715217411518, 0., 0.]) gripperOri.append([0., -1., 0., 0.]) gripperOri.append([-0.715217411518, 0.715217411518, 0., 0.]) #~build total list plan gripperOri_list = [] ori_list = [ 0, 0, 1, 2, 3, 3, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0 ] for i in range(0, len(ori_list)): gripperOri_list.append(gripperOri[ori_list[i]]) #~positions pos = [] #~bin4 bin_pose = get_params_yaml('bin4_pose') pos.append([ bin_pose[0], bin_pose[1], 0.15 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([1.1, -0.498443692923, 0.5512911677361 - .15]) pos.append([1.05, -0.599031984806, 0.5512911677361 - .15]) pos.append([1.14, -0.70, 0.5512911677361 - .15]) pos.append([1.2, -0.618239402771, 0.5512911677361 - .15]) pos.append([ bin_pose[0], bin_pose[1], 0.30 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([1.08824729919, -0.808329701424, 0.5512911677361 - 0.05]) pos.append([ bin_pose[0], bin_pose[1], 0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) #~bin5 bin_pose = get_params_yaml('bin5_pose') pos.append([ bin_pose[0], bin_pose[1], 0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([0.79457449913, -0.498443692923, 0.5512911677361 - .15]) pos.append([0.757445454597, -0.599031984806, 0.5512911677361 - .15]) pos.append([0.79457449913, -0.70, 0.5512911677361 - .15]) pos.append([0.889182715416, -0.618239402771, 0.5512911677361 - .15]) pos.append([ bin_pose[0], bin_pose[1], 0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([0.79457449913, -0.788900852203, 0.5512911677361 - .05]) pos.append([ bin_pose[0], bin_pose[1], 0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) #~bin6 bin_pose = get_params_yaml('bin6_pose') pos.append([ bin_pose[0], bin_pose[1], 0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([0.09962657094, -0.55, 0.539945185184 + 0.0]) pos.append([0.0830265730619, -0.6, 0.539945185184 + 0.0]) pos.append([0.099, -0.68, 0.539945185184 + 0.0]) pos.append([0.15, -0.6, 0.539945185184 + 0.0]) pos.append([ bin_pose[0], bin_pose[1], 0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([0.145160362124, -0.846666872501, 0.65]) pos.append([ bin_pose[0], bin_pose[1], 0.4 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) #~bin7 bin_pose = get_params_yaml('bin7_pose') pos.append([ bin_pose[0], bin_pose[1], 0.4 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([-0.199928298593, -0.5, 0.539945185184 + 0.0]) pos.append([-0.272910028696, -0.6, 0.539945185184 + 0.0]) pos.append([-0.195086687803, -0.75, 0.539945185184 + 0.0]) pos.append([-0.100125610828, -0.6, 0.539945185184 + 0.0]) pos.append([ bin_pose[0], bin_pose[1], 0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([-0.203513264656, -0.86, 0.65]) pos.append([ bin_pose[0], bin_pose[1], 0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) #~bin8 bin_pose = get_params_yaml('bin8_pose') pos.append([ bin_pose[0], bin_pose[1], 0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([-0.09, 0.68, 0.716840863228]) pos.append([-0.22, 0.62, 0.716840863228]) pos.append([-0.09, 0.5, 0.716840863228]) pos.append([0.1, 0.62, 0.716840863228]) pos.append([ bin_pose[0], bin_pose[1], 0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) pos.append([0.106256209314, 0.78, 0.8]) pos.append([ bin_pose[0], bin_pose[1], 0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist') ]) #################### ## Generate Plans ## #################### q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014] tip_hand_transform = [0., 0., 0., 0., 0., 0.] dir_list = [ None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None ] point_counter = 0 #~generate plans plans = [] listener = tf.TransformListener() br = tf.TransformBroadcaster() rospy.sleep(0.5) goToHome.goToARC() goToHome.prepGripperPicking() ############################### ## Unwrap angles example use ## ############################### #~1) function to unwarp an angle to the range [-pi,pi] def unwrap(angle): if angle > np.pi: angle = np.mod(angle, 2 * np.pi) if angle > np.pi: angle = angle - 2 * np.pi elif angle < -np.pi: angle = np.mod(angle, -2 * np.pi) if angle < -np.pi: angle = angle + 2 * np.pi return angle for i in range(0, 16): plan, qf, plan_possible = generatePlan(q_initial, pos[i], gripperOri_list[i], tip_hand_transform, 'fastest') if plan_possible: #~2) unwrap angle of joint 6 only plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5]) plans.append(plan) #~3) set q_initial (for next plan) with the unwrapped angle joint state q_initial = plan.q_traj[-1] else: return '[IK] Error' #~execute plan executePlanForward(plans, True) plans = [] if dir_list[i] is not None: gentle_move_boxes(dir_list[i], q_initial, pos[i], gripperOri_list[i], tip_hand_transform, listener, point_counter) point_counter += 1 ################################## plans_col_free = [] plans_col_free, q_initial, plan_possible, motion = collision_free_plan( q_initial, plans_col_free, [0, 1, 0, 0], False, 6) executePlanForward(plans_col_free, True) for i in range(16, 32): #~ for i in range(0,16): plan, qf, plan_possible = generatePlan(q_initial, pos[i], gripperOri_list[i], tip_hand_transform, 'fastest') if plan_possible: plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5]) plans.append(plan) q_initial = plan.q_traj[-1] else: return '[IK] Error' #~execute plan executePlanForward(plans, True) plans = [] if dir_list[i] is not None: gentle_move_boxes(dir_list[i], q_initial, pos[i], gripperOri_list[i], tip_hand_transform, listener, point_counter) point_counter += 1 plans_col_free = [] plans_col_free, q_initial, plan_possible, motion = collision_free_plan( q_initial, plans_col_free, [0, 1, 0, 0], False, 8) executePlanForward(plans_col_free, True) for i in range(32, 40): #~ for i in range(16,24): plan, qf, plan_possible = generatePlan(q_initial, pos[i], gripperOri_list[i], tip_hand_transform, 'fastest') if plan_possible: plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5]) plans.append(plan) q_initial = plan.q_traj[-1] else: return '[IK] Error' #~execute plan executePlanForward(plans, True) plans = [] if dir_list[i] is not None: gentle_move_boxes(dir_list[i], q_initial, pos[i], gripperOri_list[i], tip_hand_transform, listener, point_counter) point_counter += 1
def place(listener, br, isExecute=True, binId=0, withPause=False, rel_pose=None, BoxBody=None, place_pose=None, viz_pub=None, is_drop=True, recorder=None): ######################################################### # fhogan and nikhilcd # Description: #~2017 picking primitive # #~Usage #~ pick(objInput, listener, br, isExecute = True, objId = 'cheezit_big_original', flag = 1, withPause = False, rel_pose=None, BoxBody=None, place_pose=None, viz_pub=None): # #~Parameters: # #~Inputs: # 1) objInput: Input from vision or RViz (list of 12, or 7) # 2) listener: Input from vision or RViz (list of 12, or 7) # 3) br: Input from vision or RViz (list of 12, or 7) # 4) isExecute: execute the robot motion if possible or not? (Bool) # 5) objId: name of object (str) # 6) binId: bin in which we need to place the objects. (It is useless when flag 0 and 1) # 7) flag: Type of action directed by planning (0:run picking, 1:go to home (camera), 2: drop the object at target location) # 8) withPause: Pause between robot motions (Bool) # 9) rel_pose: object pose relative to link_6 # 10) BoxBody: Bounding box points resolved in link 6 frame # 11) place_pose: desired position of bounding box # 12) viz_pub: desired position of bounding box # #~Output: # Dictionary of following keys: # 1) grasp_possible: Does the object fit in the hand? (True/False) # 2) plan_possible: Has the plan succeeded? (True/False) # 3) execution_possible: Is there anything in the grasp? (True/False) # 4) gripper_opening: gripper opening after grasp attempt (in meters) # 5) graspPose: pose of the gripper at grasp (7X1 - position and quaternion) # 6) gelsight_data: [] , dummy for now ######################################################### #rospy.logdebug(msg, *args) #rospy.loginfo(msg, *args) #rospy.logwarn(msg, *args) #rospy.logerr(msg, *args) #rospy.logfatal(msg, *args) ############################### ## Pring input variables for ## ############################### rospy.logdebug('[Picking] withPause %s', withPause) rospy.logdebug('[Picking] rel_pose %s', rel_pose) rospy.logdebug('[Picking] BoxBody %s', BoxBody) rospy.logdebug('[Picking] place_pose %s', place_pose) ######################## ## Initialize values ## ######################## q_initial = ik.helper.get_joints() bin_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose') spatula_tip_to_tcp_dist = rospy.get_param( "/gripper/spatula_tip_to_tcp_dist") delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking') vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3]) plans = [] plans_grasping = [] plans_grasping2 = [] plans_guarded = [] plans_guarded2 = [] plans_placing = [] plans_placing2 = [] graspPose = [] plan_possible = False execution_possible = False gripper_opening = 0.0 grasp_possible = True gelsight_data = [] collision = False final_object_pose = None rospy.set_param('is_record', False) rospy.set_param('is_contact', False) liftoff_pub = rospy.Publisher('/liftoff_time', std_msgs.msg.String, queue_size=10, latch=False) def compose_output(): return { 'collision': collision, 'grasp_possible': grasp_possible, 'plan_possible': plan_possible, 'execution_possible': execution_possible, 'gripper_opening': gripper_opening, 'graspPose': graspPose, 'gelsight_data': gelsight_data, 'final_object_pose': final_object_pose } ######################### ## Constant parameters ## ######################### UpdistFromBinFast = 0.15 # Margin above object during "move to a location" UpdistFromBinGuarded = 0.05 # Margin above object during "move to a location" gripperOriHome = [0, 1, 0, 0] l1 = 0.0 l2 = 0.0 l3 = spatula_tip_to_tcp_dist tip_hand_transform = [l1, l2, l3, 0, 0, 0] vision_pos[2] = 0.17786528 ############# ## Placing ## ############# #if primitive called without placing arguments, go to center of bin if rel_pose == None: drop_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose') drop_pose[3:7] = gripperOriHome else: drop_pose = ik.helper.drop_pose_transform(binId, rel_pose, BoxBody, place_pose, viz_pub, listener, br) #~ Adjust height according to weigth if is_drop: drop_offset = 0.15 else: drop_offset = 0.025 #~set drop pose target z position to be bottom of bin drop_pose[2] = bin_pose[2] - rospy.get_param('tote/height') + drop_offset #~Predrop: go to top middle bin surface fast without guarded move predrop_pos = np.array(drop_pose[0:3]) predrop_pos[2] = bin_pose[2] + 0.05 ###################### ## Generate plans ## ###################### #~move safe to placing bin number rospy.logdebug( '[Picking] Collision free placing to binId %s and position %s', binId, predrop_pos) # if isExecute: # q_initial = collision_free_placing(binId, listener, obj_ID=objId, BoxBody=BoxBody, isSuction = False,with_object = True, hand_orientation=drop_pose[3:7],projected_target_position = predrop_pos, isReturn = True) # if update_command is not None: # update_command.execute() #~0.go up to avoid collision with tote walls current_pose = ik.helper.get_tcp_pose(listener, tcp_offset=l3) current_pose[2] = current_pose[2] + 0.15 plan, qf, plan_possible = generatePlan(q_initial, current_pose[0:3], current_pose[3:7], tip_hand_transform, 'faster', plan_name='go_up') if plan_possible: plans.append(plan) q_initial = qf else: return compose_output() #~0.1.fast motion to bin surface high predrop_pos_high = predrop_pos predrop_pos_high[2] = current_pose[2] plan, qf, plan_possible = generatePlan(q_initial, predrop_pos_high, drop_pose[3:7], tip_hand_transform, 'faster', plan_name='go_bin_surface_high') if plan_possible: plans.append(plan) q_initial = qf else: return compose_output() #~1.fast motion to bin surface predrop_pos[2] = bin_pose[2] + 0.05 plan, qf, plan_possible = generatePlan(q_initial, predrop_pos, drop_pose[3:7], tip_hand_transform, 'faster', plan_name='go_bin_surface') if plan_possible: plans.append(plan) q_initial = qf else: return compose_output() #~2. Guarded move down slow plan, qf, plan_possible = generatePlan(q_initial, drop_pose[0:3], drop_pose[3:7], tip_hand_transform, 'fast', guard_on=WeightGuard(binId, threshold=100), backwards_speed='superSaiyan', plan_name='guarded_drop') if plan_possible: plans_placing.append(plan) q_initial = qf else: return compose_output() #~3. open gripper grasp_plan = EvalPlan('helper.moveGripper(0.110, 200)') plans_placing2.append(grasp_plan) #~4. open spatula grasp_plan = EvalPlan('spatula.open()') plans_placing2.append(grasp_plan) #~5. go to predrop_pos plan, qf, plan_possible = generatePlan(q_initial, predrop_pos[0:3], drop_pose[3:7], tip_hand_transform, 'superSaiyan', plan_name='predrop_pos') if plan_possible: plans_placing2.append(plan) q_initial = qf else: return compose_output() #~6. go to final pose final_pos = bin_pose[0:3] final_pos[2] = bin_pose[2] + 0.3 plan, qf, plan_possible = generatePlan(q_initial, final_pos, drop_pose[3:7], tip_hand_transform, 'superSaiyan', plan_name='final_pose') if plan_possible: plans_placing2.append(plan) q_initial = qf else: return compose_output() #~ Execute plans if plan_possible: if isExecute: executePlanForward(plans, withPause) #We start recording now if recorder is not None: lasers.start(binId) recorder.start_recording( action='placing', action_id=str( datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")), tote_num=binId, frame_rate_ratio=5, image_size=-1) rospy.set_param('is_record', True) executePlanForward(plans_placing, withPause) if rospy.get_param('is_contact'): ik.helper.move_cart(dz=0.015) executePlanForward(plans_placing2, withPause) rospy.set_param('is_record', False) if recorder is not None: recorder.stop_recording(save_action=True) lasers.stop(binId) execution_possible = True return compose_output()
def retrieve(listener, br, isExecute=True, objId='expo_eraser', binId=0, withPause=False, viz_pub=None, recorder=None, ws_object=None, isShake=True): liftoff_pub = rospy.Publisher('/liftoff_time', std_msgs.msg.String, queue_size=10, latch=False) def compose_output(): return { 'collision': collision, 'grasp_possible': grasp_possible, 'plan_possible': plan_possible, 'execution_possible': execution_possible, 'gripper_opening': gripper_opening, 'graspPose': graspPose, 'gelsight_data': gelsight_data, 'final_object_pose': final_object_pose } q_initial = ik.helper.get_joints() bin_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose') spatula_tip_to_tcp_dist = rospy.get_param( "/gripper/spatula_tip_to_tcp_dist") l1 = 0.0 l2 = 0.0 l3 = spatula_tip_to_tcp_dist tip_hand_transform = [l1, l2, l3, 0, 0, 0] delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking') delta_vision_pose[3:7] = np.array([0, 1, 0, 0]) vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3]) vision_pos[2] = 0.17786528 - 0.1 plans = [] plans_grasping = [] graspPose = [] plan_possible = False execution_possible = False gripper_opening = 0.0 grasp_possible = True gelsight_data = [] collision = False final_object_pose = None rospy.set_param('is_record', False) rospy.set_param('is_contact', False) # ~10. Move to a location above the bin plan, qf, plan_possible = generatePlan(q_initial, vision_pos, delta_vision_pose[3:7], tip_hand_transform, 'fastest', plan_name='retrieve_object') if plan_possible: plans_grasping.append(plan) q_initial = qf else: return compose_output() if isExecute and plan_possible: #Publish liftoff time liftoff_msgs = std_msgs.msg.String() liftoff_msgs.data = 'Liftoff (Robot command)' liftoff_pub.publish(liftoff_msgs) #Execute non-guarded grasp plan move executePlanForward(plans_grasping, withPause) #~Check if picking success low_threshold = 0.0035 high_threshold = 0.027 #shake robot rospy.sleep(1.5) if isShake and gripper.getGripperopening() > high_threshold and isExecute: # We initialize drop listener ws_object.start_listening_for_drop(bin_num=binId) ik.ik.shake() rospy.sleep(2.) # We stop drop listener ws_object.stop_listening_for_drop() ### We stop recording if recorder is not None: recorder.stop_recording(save_action=True) lasers.stop(binId) if isExecute and plan_possible: rospy.sleep(2) gripper_opening = gripper.getGripperopening() if gripper_opening > high_threshold: rospy.loginfo('[Picking] ***************') rospy.loginfo('[Picking] Pick Successful (Gripper Opening Test)') rospy.loginfo('[Picking] ***************') execution_possible = True #temporary hack else: rospy.loginfo('[Picking] ***************') rospy.loginfo('[Picking] Pick Inconclusive (Gripper Opening Test)') rospy.loginfo('[Picking] ***************') execution_possible = None elif not isExecute: execution_possible = plan_possible return compose_output()
def grasp(objInput, listener, br, isExecute=True, objId='expo_eraser', binId=0, withPause=False, viz_pub=None, recorder=None, is_regrasp=False, open_hand=True): ######################################################### # fhogan and nikhilcd # Description: #~2017 picking primitive # #~Usage #~ pick(objInput, listener, br, isExecute = True, objId = 'cheezit_big_original', flag = 1, withPause = False, rel_pose=None, BoxBody=None, place_pose=None, viz_pub=None): # #~Parameters: # #~Inputs: # 1) objInput: Input from vision or RViz (list of 12, or 7) # 2) listener: Input from vision or RViz (list of 12, or 7) # 3) br: Input from vision or RViz (list of 12, or 7) # 4) isExecute: execute the robot motion if possible or not? (Bool) # 5) objId: name of object (str) # 6) binId: bin in which we need to place the objects. (It is useless when flag 0 and 1) # 7) flag: Type of action directed by planning (0:run picking, 1:go to home (camera), 2: drop the object at target location) # 8) withPause: Pause between robot motions (Bool) # 9) rel_pose: object pose relative to link_6 # 10) BoxBody: Bounding box points resolved in link 6 frame # 11) place_pose: desired position of bounding box # 12) viz_pub: desired position of bounding box # #~Output: # Dictionary of following keys: # 1) grasp_possible: Does the object fit in the hand? (True/False) # 2) plan_possible: Has the plan succeeded? (True/False) # 3) execution_possible: Is there anything in the grasp? (True/False) # 4) gripper_opening: gripper opening after grasp attempt (in meters) # 5) graspPose: pose of the gripper at grasp (7X1 - position and quaternion) # 6) gelsight_data: [] , dummy for now ######################################################### #rospy.logdebug(msg, *args) #rospy.loginfo(msg, *args) #rospy.logwarn(msg, *args) #rospy.logerr(msg, *args) #rospy.logfatal(msg, *args) ############################### ## Pring input variables for ## ############################### rospy.logdebug('[Picking] objInput %s', objInput) rospy.logdebug('[Picking] objId %s', objId) rospy.logdebug('[Picking] withPause %s', withPause) ######################## ## Initialize values ## ######################## q_initial = ik.helper.get_joints() bin_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose') spatula_tip_to_tcp_dist = rospy.get_param( "/gripper/spatula_tip_to_tcp_dist") delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking') vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3]) plans = [] plans_grasping = [] plans_grasping2 = [] plans_guarded = [] plans_guarded2 = [] plans_placing = [] plans_placing2 = [] graspPos = [] plan_possible = False execution_possible = False gripper_opening = 0.0 grasp_possible = True gelsight_data = [] collision = False final_object_pose = None rospy.set_param('is_record', False) rospy.set_param('is_contact', False) def compose_output(): return { 'collision': collision, 'grasp_possible': grasp_possible, 'plan_possible': plan_possible, 'execution_possible': execution_possible, 'gripper_opening': gripper_opening, 'graspPos': graspPos, 'gelsight_data': gelsight_data, 'final_object_pose': final_object_pose } ######################### ## Constant parameters ## ######################### UpdistFromBinFast = 0.15 # Margin above object during "move to a location" UpdistFromBinGuarded = 0.05 # Margin above object during "move to a location" gripperOriHome = [0, 1, 0, 0] l1 = 0.0 l2 = 0.0 l3 = spatula_tip_to_tcp_dist tip_hand_transform = [l1, l2, l3, 0, 0, 0] vision_pos[2] = 0.17786528 ############################### ## Picking primitive ## ############################### # ik.visualize_helper.visualize_grasping_proposals(viz_pub, np.asarray([objInput]), listener, br, False) #~Define reference frames world_X, world_Y, world_Z, tote_X, tote_Y, tote_Z, tote_pose_pos = ik.helper.reference_frames( listener=listener, br=br) #~get grasp pose and gripper opening from vision if len(objInput) == 12: graspPos, hand_X, hand_Y, hand_Z, grasp_width = ik.helper.get_picking_params_from_12( objInput) if not is_regrasp: graspPos = graspPos - hand_X * 0.015 * 1 elif len(objInput) == 7: graspPos, hand_X, hand_Y, hand_Z, grasp_width = ik.helper.get_picking_params_from_7( objInput, objId, listener, br) #frank hack for stationary spatula: grasp_width = 0.11 #~build gripper orientation matrix 3x3 hand_orient_norm = np.vstack([hand_X, hand_Y, hand_Z]) hand_orient_norm = hand_orient_norm.transpose() #~Grasp relocation script hand_orient_quat = ik.helper.mat2quat(hand_orient_norm) euler = tf.transformations.euler_from_quaternion(hand_orient_quat) theta = euler[2] - np.pi colFreePose = collisionFree(graspPos, binId=binId, listener=listener, br=br, finger_opening=grasp_width, safety_margin=0.00, theta=theta) graspPos[0:2] = colFreePose[0:2] ################################# ## GENERATE PLANS OF PRIMITIVE ## #################################. #~ Start from home position # if isExecute: # q_initial = collision_free_placing(binId, listener, isSuction = False, with_object = False, hand_orientation=[0,1,0,0], projected_target_position = bin_pose[0:3], isReturn = True) # scorpion.back() #~0. Move to a location above the bin, Rotate gripper to grasp orientation pregrasp_targetPosition = graspPos pregrasp_targetPosition[2] = bin_pose[2] + UpdistFromBinFast - 0.1 plan, qf, plan_possible = generatePlan(q_initial, pregrasp_targetPosition, hand_orient_quat, tip_hand_transform, 'superSaiyan', plan_name='go_safe_bin') if plan_possible: plans.append(plan) q_initial = qf else: return compose_output() #~1. Move to to surface of bin pregrasp_targetPosition[2] = bin_pose[2] + UpdistFromBinGuarded - 0.1 plan, qf, plan_possible = generatePlan(q_initial, pregrasp_targetPosition, hand_orient_quat, tip_hand_transform, 'superSaiyan', plan_name='go_top_bin') if plan_possible: plans.append(plan) q_initial = qf else: return compose_output() if open_hand: #~2. Open gripper grasp_plan = EvalPlan('helper.moveGripper(%f, 200)' % grasp_width) plans.append(grasp_plan) #~ 3. Open spatula grasp_plan = EvalPlan('spatula.open()') plans.append(grasp_plan) grasp_targetPosition = deepcopy(graspPos) # rospy.loginfo('[Picking] Grasp Target %s', grasp_targetPosition) #~4. sleep sleep_plan = EvalPlan('rospy.sleep(0.2)') plans.append(sleep_plan) #~5. perform guarded move down grasp_targetPosition[2] = bin_pose[2] - rospy.get_param( '/tote/height') + 0.015 #~frank hack for safety plan, qf, plan_possible = generatePlan(q_initial, grasp_targetPosition, hand_orient_quat, tip_hand_transform, 'faster', guard_on=WeightGuard(binId, threshold=140), plan_name='guarded_pick') if plan_possible: plans_guarded.append(plan) q_initial = qf else: return compose_output() #~7. Close spatula grasp_plan = EvalPlan('spatula.close()') plans_guarded2.append(grasp_plan) #~8. Close gripper grasp_plan = EvalPlan('helper.graspinGripper(%f,%f)' % (800, 30)) plans_guarded2.append(grasp_plan) #~9. sleep sleep_plan = EvalPlan('rospy.sleep(0.6)') plans_guarded2.append(sleep_plan) #~10. Move to a location above the bin # plan, qf, plan_possible = generatePlan(q_initial, vision_pos, delta_vision_pose[3:7], tip_hand_transform, 'fastest', plan_name = 'retrieve_object') # if plan_possible: # plans_grasping.append(plan) # q_initial = qf # else: # return compose_output() ################ ## EXECUTION ### ################ if isExecute and plan_possible: executePlanForward(plans, withPause) ###We start recording now if recorder is not None: lasers.start(binId) recorder.start_recording( action='grasping', action_id=str( datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")), tote_num=binId, frame_rate_ratio=5, image_size=-1) #Execute guarded move rospy.set_param('is_record', True) executePlanForward(plans_guarded, withPause) if rospy.get_param('is_contact'): ik.helper.move_cart(dz=0.005) executePlanForward(plans_guarded2, withPause) rospy.set_param('is_record', False) elif not isExecute: execution_possible = plan_possible return compose_output()