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
