def solveIk(target_pose): tip_hand_transform = xyzrpy_from_xyzquat([-0.0053791, 0.0085135, -0.023009, 0.70582, 0.70832, 0.0027417, 0.0095863]) planner = IK(target_tip_pos = target_pose[0:3], target_tip_ori = target_pose[3:7], tip_hand_transform=tip_hand_transform, target_link='link_5', target_joint_bb=[[6, 0, 0]], ori_tol=0.1, pos_tol=0.01, ik_only=True) plan = planner.plan() if plan.success(): print 'success' plan.visualize() #plan.execute() # else: print 'failed'
target_pose[0] += 0.03 # x target_pose[2] -= 0.04 # z elif i < 9: target_pose = target_pose_7_hori else: target_pose = target_pose_7_hori target_pose[2] += 0.1 # z target_pose[1] += (bin_cnstr[i][0] + bin_cnstr[i][1]) / 2 - ( bin_cnstr[refind][0] + bin_cnstr[refind][1]) / 2 # y target_pose[2] += (bin_cnstr[i][4] + bin_cnstr[i][5]) / 2 - ( bin_cnstr[refind][4] + bin_cnstr[refind][5]) / 2 # z #target_pose[1] += 0.08 target_poses.append(target_pose) tip_hand_transform = xyzrpy_from_xyzquat( [-0.062, 0.021, -0.025, -0.007, 0.704, 0.710, -0.002]) rospy.init_node('listener', anonymous=True) capsen.capsen.init() # obj_list = ["champion_copper_plus_spark_plug", "kong_duck_dog_toy", "kong_sitting_frog_dog_toy", # "first_years_take_and_toss_straw_cup", "mead_index_cards", "champion_copper_plus_spark_plug", # "stanley_66_052", "dr_browns_bottle_brush", "sharpie_accent_tank_style_highlighters", # "champion_copper_plus_spark_plug", "sharpie_accent_tank_style_highlighters", "mommys_helper_outlet_plugs"] obj_list = [ "elmers_washable_no_run_school_glue", "paper_mate_12_count_mirado_black_warrior", "feline_greenies_dental_treats", "champion_copper_plus_spark_plug", "paper_mate_12_count_mirado_black_warrior", "mark_twain_huckleberry_finn", "laugh_out_loud_joke_book", "laugh_out_loud_joke_book", "mark_twain_huckleberry_finn", "stanley_66_052", "mead_index_cards",
elif i<6: target_pose = target_pose_7_hori target_pose[0] += 0.03 # x target_pose[2] -= 0.04 # z elif i<9: target_pose = target_pose_7_hori else: target_pose = target_pose_7_hori target_pose[2] += 0.1 # z target_pose[1] += (bin_cnstr[i][0]+bin_cnstr[i][1])/2 - (bin_cnstr[refind][0]+bin_cnstr[refind][1])/2 # y target_pose[2] += (bin_cnstr[i][4]+bin_cnstr[i][5])/2 - (bin_cnstr[refind][4]+bin_cnstr[refind][5])/2 # z #target_pose[1] += 0.08 target_poses.append(target_pose) tip_hand_transform = xyzrpy_from_xyzquat([-0.062, 0.021, -0.025, -0.007, 0.704, 0.710, -0.002]) rospy.init_node('listener', anonymous=True) capsen.capsen.init() # obj_list = ["champion_copper_plus_spark_plug", "kong_duck_dog_toy", "kong_sitting_frog_dog_toy", # "first_years_take_and_toss_straw_cup", "mead_index_cards", "champion_copper_plus_spark_plug", # "stanley_66_052", "dr_browns_bottle_brush", "sharpie_accent_tank_style_highlighters", # "champion_copper_plus_spark_plug", "sharpie_accent_tank_style_highlighters", "mommys_helper_outlet_plugs"] obj_list = ["elmers_washable_no_run_school_glue", "paper_mate_12_count_mirado_black_warrior", "feline_greenies_dental_treats", "champion_copper_plus_spark_plug", "paper_mate_12_count_mirado_black_warrior", "mark_twain_huckleberry_finn", "laugh_out_loud_joke_book", "laugh_out_loud_joke_book", "mark_twain_huckleberry_finn", "stanley_66_052", "mead_index_cards", "expo_dry_erase_board_eraser"] # obj_list = ["champion_copper_plus_spark_plug", "kong_duck_dog_toy", "kong_sitting_frog_dog_toy", # "first_years_take_and_toss_straw_cup", "mead_index_cards", "champion_copper_plus_spark_plug",
def percept_hand(obj_pose = None, binNum = 0, obj_id = None, bin_contents = None, isExecute = True, withPause = True): capsen.capsen.init() home_joint_pose = [0, -0.2, 0.2, 0.01, 1, 1.4] planner = IKJoint(target_joint_pos=home_joint_pose); #plan = Plan() #plan.q_traj = [home_joint_pose]; plan = planner.plan() plan.visualize(); plan.setSpeedByName('yolo'); if withPause: print '[Percept] Going back to percept_hand home' pause() plan.execute() bin_cnstr = get_bin_inner_cnstr() # 1. init target poses target_poses = [] refind = 7 for i in range(12): target_pose_7_vert = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(math.pi, 1.4, math.pi).tolist() # for bin 7 vertical target_pose_7_hori = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(2.2, 0, math.pi/2).tolist() target_pose_7_invhori = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(-math.pi/2, 0, -math.pi/2).tolist() if i < 3: target_pose = copy.deepcopy(target_pose_7_invhori) if i == 0: target_pose[1] -= 0.06 target_pose[2] -= 0.08 # z elif i<6: target_pose = copy.deepcopy(target_pose_7_hori) target_pose[0] += 0.03 # x target_pose[2] -= 0.04 # z elif i<9: target_pose = copy.deepcopy(target_pose_7_hori) else: target_pose = copy.deepcopy(target_pose_7_hori) target_pose[2] += 0.1 # z target_pose[1] += (bin_cnstr[i][0]+bin_cnstr[i][1])/2 - (bin_cnstr[refind][0]+bin_cnstr[refind][1])/2 # y target_pose[2] += (bin_cnstr[i][4]+bin_cnstr[i][5])/2 - (bin_cnstr[refind][4]+bin_cnstr[refind][5])/2 # z target_poses.append(target_pose) y_deltas = [0.08, -0.08] caption = ['left', 'right'] max_score = -100 max_obj_pose = None for i in range(len(y_deltas)): # 2. plan to target tip_hand_transform = xyzrpy_from_xyzquat([-0.067, 0.027, -0.012, -0.007, 0.704, 0.710, -0.002]) # need to be updated target_pose = copy.deepcopy(target_poses[binNum]) target_pose[1] += y_deltas[i] plan = None planner = IK(target_tip_pos = target_pose[0:3], target_tip_ori = target_pose[3:7], tip_hand_transform=tip_hand_transform, target_link='link_5', target_joint_bb=[[6, 1.2, 1.2]]) for j in range(15): planner.ori_tol = 0.5*j/10.0 planner.pos_tol = 0.01*j/10.0 newplan = planner.plan() if newplan.success(): plan = newplan print '[Percept] hand planning success at the trial %d' % j break if plan: plan.setSpeedByName('fastest') plan.visualize() if withPause: print '[Percept] Going to percept_hand %s ' % caption[i] pause() plan.execute() else: print '[Percept] hand planning failed' continue # 3. percept rospy.sleep(0.8) # wait for new pointcloud obj_pose, score = capsen.capsen.detectOneObject(obj_id, bin_contents, binNum, mode = 1, withScore = True) if score > max_score: max_obj_pose = obj_pose max_score = score # 4. move back to percept home planner = IKJoint(target_joint_pos=home_joint_pose) plan = planner.plan() plan.visualize(); plan.setSpeedByName('yolo'); if withPause: print '[Percept] Going back to percept_hand home' pause() plan.execute() goToHome.goToHome(robotConfig=None, homePos = [1,0,1.2], isExecute = isExecute, withPause = withPause) if max_obj_pose is None: return None, None, None pose_type = find_object_pose_type(obj_id, max_obj_pose) return max_obj_pose, pose_type, max_score
def percept_hand(obj_pose=None, binNum=0, obj_id=None, bin_contents=None, isExecute=True, withPause=True): capsen.capsen.init() home_joint_pose = [0, -0.2, 0.2, 0.01, 1, 1.4] planner = IKJoint(target_joint_pos=home_joint_pose) #plan = Plan() #plan.q_traj = [home_joint_pose]; plan = planner.plan() plan.visualize() plan.setSpeedByName('yolo') if withPause: print '[Percept] Going back to percept_hand home' pause() plan.execute() bin_cnstr = get_bin_inner_cnstr() # 1. init target poses target_poses = [] refind = 7 for i in range(12): target_pose_7_vert = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler( math.pi, 1.4, math.pi).tolist() # for bin 7 vertical target_pose_7_hori = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler( 2.2, 0, math.pi / 2).tolist() target_pose_7_invhori = [1.1756, 0, 0.79966 ] + tfm.quaternion_from_euler( -math.pi / 2, 0, -math.pi / 2).tolist() if i < 3: target_pose = copy.deepcopy(target_pose_7_invhori) if i == 0: target_pose[1] -= 0.06 target_pose[2] -= 0.08 # z elif i < 6: target_pose = copy.deepcopy(target_pose_7_hori) target_pose[0] += 0.03 # x target_pose[2] -= 0.04 # z elif i < 9: target_pose = copy.deepcopy(target_pose_7_hori) else: target_pose = copy.deepcopy(target_pose_7_hori) target_pose[2] += 0.1 # z target_pose[1] += (bin_cnstr[i][0] + bin_cnstr[i][1]) / 2 - ( bin_cnstr[refind][0] + bin_cnstr[refind][1]) / 2 # y target_pose[2] += (bin_cnstr[i][4] + bin_cnstr[i][5]) / 2 - ( bin_cnstr[refind][4] + bin_cnstr[refind][5]) / 2 # z target_poses.append(target_pose) y_deltas = [0.08, -0.08] caption = ['left', 'right'] max_score = -100 max_obj_pose = None for i in range(len(y_deltas)): # 2. plan to target tip_hand_transform = xyzrpy_from_xyzquat( [-0.067, 0.027, -0.012, -0.007, 0.704, 0.710, -0.002]) # need to be updated target_pose = copy.deepcopy(target_poses[binNum]) target_pose[1] += y_deltas[i] plan = None planner = IK(target_tip_pos=target_pose[0:3], target_tip_ori=target_pose[3:7], tip_hand_transform=tip_hand_transform, target_link='link_5', target_joint_bb=[[6, 1.2, 1.2]]) for j in range(15): planner.ori_tol = 0.5 * j / 10.0 planner.pos_tol = 0.01 * j / 10.0 newplan = planner.plan() if newplan.success(): plan = newplan print '[Percept] hand planning success at the trial %d' % j break if plan: plan.setSpeedByName('fastest') plan.visualize() if withPause: print '[Percept] Going to percept_hand %s ' % caption[i] pause() plan.execute() else: print '[Percept] hand planning failed' continue # 3. percept rospy.sleep(0.8) # wait for new pointcloud obj_pose, score = capsen.capsen.detectOneObject(obj_id, bin_contents, binNum, mode=1, withScore=True) if score > max_score: max_obj_pose = obj_pose max_score = score # 4. move back to percept home planner = IKJoint(target_joint_pos=home_joint_pose) plan = planner.plan() plan.visualize() plan.setSpeedByName('yolo') if withPause: print '[Percept] Going back to percept_hand home' pause() plan.execute() goToHome.goToHome(robotConfig=None, homePos=[1, 0, 1.2], isExecute=isExecute, withPause=withPause) if max_obj_pose is None: return None, None, None pose_type = find_object_pose_type(obj_id, max_obj_pose) return max_obj_pose, pose_type, max_score