tfm.quaternion_from_euler(*tip_hand_transform[3:6]),
rospy.Time.now(), 'tip', "link_5")
br.sendTransform(tuple(target_pose[0:3]), tuple(target_pose[3:7]),
rospy.Time.now(), 'target_pose', "world")
# user_reply=raw_input('Execute? [(y)es/(n)o]: ')
# if user_reply == 'y':
# plan.setSpeedByName('fastest')
plan.execute()
raw_input('[NIMA] test realsense now: ')
rospy.sleep(0.8) # wait for new pointcloud
obj_pose = capsen.capsen.detectOneObject(obj_list[i], [obj_list[i]],
i,
mode=1)
pause()
# go home
plan = Plan()
plan.q_traj = [home_joint_pose]
plan.execute()
# if obj_pose is None:
# print 'Detection failed'
# continue
# pubFrame(br, obj_pose, 'obj', 'world')
plan = Plan()
plan.q_traj = [home_joint_pose]
plan.execute()
br.sendTransform(tuple(tip_hand_transform[0:3]),
tfm.quaternion_from_euler(*tip_hand_transform[3:6]),
rospy.Time.now(), 'tip', "link_5")
br.sendTransform(tuple(target_pose[0:3]), tuple(target_pose[3:7]),
rospy.Time.now(), 'target_pose', "world")
# user_reply=raw_input('Execute? [(y)es/(n)o]: ')
# if user_reply == 'y':
# plan.setSpeedByName('fastest')
plan.execute()
raw_input('[NIMA] test realsense now: ')
rospy.sleep(0.8) # wait for new pointcloud
obj_pose = capsen.capsen.detectOneObject(obj_list[i], [obj_list[i]], i, mode = 1)
pause()
# go home
plan = Plan(); plan.q_traj = [home_joint_pose];
plan.execute()
# if obj_pose is None:
# print 'Detection failed'
# continue
# pubFrame(br, obj_pose, 'obj', 'world')
plan = Plan(); plan.q_traj = [home_joint_pose];
plan.execute()
def main(argv=None):
global br
if argv is None:
argv = sys.argv
rospy.init_node('capsen_test', anonymous=True)
rospy.sleep(0.1)
init()
rospy.sleep(0.1)
#retObjects, retScore = detectObjects('expo_dry_erase_board_eraser', ['expo_dry_erase_board_eraser', 'elmers_washable_no_run_school_glue'], bin_num = 0, mode = 0)
# retObjects, retScore = detectObjects('expo_dry_erase_board_eraser', ['expo_dry_erase_board_eraser', 'elmers_washable_no_run_school_glue'], bin_num = 0, mode = 0)
# pose = pose2list(retObjects[0].pose)
# pubFrame(br, pose, 'obj', 'map')
# print 'Objects', retObjects
# pause()
obj_list = ['mommys_helper_outlet_plugs',
'kong_duck_dog_toy',
'first_years_take_and_toss_straw_cup',
'champion_copper_plus_spark_plug',
'mead_index_cards',
'laugh_out_loud_joke_book',
'highland_6539_self_stick_notes',
'elmers_washable_no_run_school_glue',
'stanley_66_052',
'genuine_joe_plastic_stir_sticks',
'safety_works_safety_glasses',
'munchkin_white_hot_duck_bath_toy'
# 'crayola_64_ct',
# 'dr_browns_bottle_brush',
# 'kyjen_squeakin_eggs_plush_puppies',
# 'expo_dry_erase_board_eraser',
# 'cheezit_big_original',
# 'kong_air_dog_squeakair_tennis_ball',
# 'safety_works_safety_glasses',
# 'genuine_joe_plastic_stir_sticks'
]
bin_contents_all = [
[ "mommys_helper_outlet_plugs", "mark_twain_huckleberry_finn" ],
[ "feline_greenies_dental_treats", "kong_duck_dog_toy" ],
[ "first_years_take_and_toss_straw_cup","kong_sitting_frog_dog_toy" ],
[ "paper_mate_12_count_mirado_black_warrior", "champion_copper_plus_spark_plug" ],
[ "mead_index_cards", "sharpie_accent_tank_style_highlighters" ],
[ "mommys_helper_outlet_plugs", "laugh_out_loud_joke_book" ],
[ "kyjen_squeakin_eggs_plush_puppies", "highland_6539_self_stick_notes" ],
[ "elmers_washable_no_run_school_glue", "champion_copper_plus_spark_plug" ],
[ "crayola_64_ct", "stanley_66_052" ],
[ "genuine_joe_plastic_stir_sticks", "expo_dry_erase_board_eraser" ],
[ "safety_works_safety_glasses" ],
[ "kong_air_dog_squeakair_tennis_ball", "munchkin_white_hot_duck_bath_toy" ]]
#for i, obj_id in enumerate(obj_list):
for i in range(5,12):
pose = detectOneObject(obj_list[i], bin_contents_all[i], i)
pubFrame(br, pose, 'obj_final', 'map')
print 'Pose', pose
pause()
def main(argv=None):
global br
if argv is None:
argv = sys.argv
rospy.init_node('capsen_test', anonymous=True)
rospy.sleep(0.1)
init()
rospy.sleep(0.1)
#retObjects, retScore = detectObjects('expo_dry_erase_board_eraser', ['expo_dry_erase_board_eraser', 'elmers_washable_no_run_school_glue'], bin_num = 0, mode = 0)
# retObjects, retScore = detectObjects('expo_dry_erase_board_eraser', ['expo_dry_erase_board_eraser', 'elmers_washable_no_run_school_glue'], bin_num = 0, mode = 0)
# pose = pose2list(retObjects[0].pose)
# pubFrame(br, pose, 'obj', 'map')
# print 'Objects', retObjects
# pause()
obj_list = [
'mommys_helper_outlet_plugs', 'kong_duck_dog_toy',
'first_years_take_and_toss_straw_cup',
'champion_copper_plus_spark_plug', 'mead_index_cards',
'laugh_out_loud_joke_book', 'highland_6539_self_stick_notes',
'elmers_washable_no_run_school_glue', 'stanley_66_052',
'genuine_joe_plastic_stir_sticks', 'safety_works_safety_glasses',
'munchkin_white_hot_duck_bath_toy'
# 'crayola_64_ct',
# 'dr_browns_bottle_brush',
# 'kyjen_squeakin_eggs_plush_puppies',
# 'expo_dry_erase_board_eraser',
# 'cheezit_big_original',
# 'kong_air_dog_squeakair_tennis_ball',
# 'safety_works_safety_glasses',
# 'genuine_joe_plastic_stir_sticks'
]
bin_contents_all = [
["mommys_helper_outlet_plugs", "mark_twain_huckleberry_finn"],
["feline_greenies_dental_treats", "kong_duck_dog_toy"],
["first_years_take_and_toss_straw_cup", "kong_sitting_frog_dog_toy"],
[
"paper_mate_12_count_mirado_black_warrior",
"champion_copper_plus_spark_plug"
], ["mead_index_cards", "sharpie_accent_tank_style_highlighters"],
["mommys_helper_outlet_plugs", "laugh_out_loud_joke_book"],
[
"kyjen_squeakin_eggs_plush_puppies",
"highland_6539_self_stick_notes"
],
[
"elmers_washable_no_run_school_glue",
"champion_copper_plus_spark_plug"
], ["crayola_64_ct", "stanley_66_052"],
["genuine_joe_plastic_stir_sticks", "expo_dry_erase_board_eraser"],
["safety_works_safety_glasses"],
[
"kong_air_dog_squeakair_tennis_ball",
"munchkin_white_hot_duck_bath_toy"
]
]
#for i, obj_id in enumerate(obj_list):
for i in range(5, 12):
pose = detectOneObject(obj_list[i], bin_contents_all[i], i)
pubFrame(br, pose, 'obj_final', 'map')
print 'Pose', pose
pause()
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
