class ExpiredTask:
def __init__(self, _name, en_sim):
self.name = _name
self.en_sim = en_sim
self.state = State.init
self.dual_arm = DualArmTask(self.name, self.en_sim)
self.camara = GetObjInfo()
self.left_cpose_queue = queue.Queue()
self.right_cpose_queue = queue.Queue()
self.place_pose_queue = queue.Queue()
self.object_queue = queue.Queue()
self.object_list = []
self.left_tar_obj = queue.Queue()
self.right_tar_obj = queue.Queue()
self.retry_obj_queue_left = queue.Queue()
self.retry_obj_queue_right = queue.Queue()
self.target_obj_queue = {
'left': self.left_tar_obj,
'right': self.right_tar_obj
}
self.target_obj = {'left': None, 'right': None}
self.retry_obj_queue = {
'left': self.retry_obj_queue_left,
'right': self.retry_obj_queue_right
}
self.obj_done = np.zeros((100), dtype=bool)
self.obj_retry = np.zeros((100), dtype=bool)
self.next_level = {'left': False, 'right': False}
self.init()
def init(self):
for pose in place_pose:
self.place_pose_queue.put(pose)
def get_obj_inf(self, side):
fb = self.dual_arm.get_feedback(side)
ids, mats, names, exps, side_ids = self.camara.get_obj_info(
side, fb.orientation)
if ids is None:
return
for _id, mat, name, exp, side_id in zip(ids, mats, names, exps,
side_ids):
obj = ObjInfo()
obj['id'] = _id
obj['name'] = name
obj['expired'] = exp
obj['side_id'] = side_id
obj['pos'] = mat[0:3, 3]
obj['vector'] = mat[0:3, 2]
obj['sucang'], roll = self.dual_arm.suc2vector(
mat[0:3, 2], [0, 1.57, 0])
obj['euler'] = [roll, 90, 0]
if obj['vector'][2] > -0.2:
self.object_queue.put(obj)
print('f**k+++++============--------------', obj['pos'])
else:
print('f**k < -0.2 ', obj['vector'])
print('fuckkkkkkkkkkkkkkkkkkkkkkk', obj['id'])
def arrange_obj(self, side):
pass
def check_pose(self, side):
self.target_obj[side] = self.target_obj_queue[side].get()
fb = self.dual_arm.get_feedback(side)
ids, mats, _, _, _ = self.camara.get_obj_info(side, fb.orientation)
if ids is None:
return
for _id, mat in zip(ids, mats):
if _id == self.target_obj[side]['id']:
self.target_obj[side]['pos'] = mat[0:3, 3]
if mat[2, 2] > -0.1:
self.target_obj[side][
'sucang'], roll = self.dual_arm.suc2vector(
mat[0:3, 2], [0, 1.57, 0])
self.target_obj[side]['euler'] = [roll, 90, 0]
pass
def state_control(self, state, side):
if state is None:
state = State.init
elif state == State.init:
state = State.get_obj_inf
elif state == State.get_obj_inf:
state = State.select_obj
elif state == State.select_obj:
if self.object_queue.empty():
if c_pose[side + '_indx'] >= 3:
state = State.finish
else:
state = State.get_obj_inf
else:
state = State.move2obj
elif state == State.move2obj:
state = State.check_pose
elif state == State.check_pose:
state = State.pick
elif state == State.pick:
if side == 'left':
is_grip = self.dual_arm.left_arm.suction.is_grip
else:
is_grip = self.dual_arm.right_arm.suction.is_grip
if is_grip:
state = State.place
elif self.next_level[side] == True:
self.next_level[side] = False
if c_pose[side + '_indx'] >= 3:
state = State.finish
else:
state = State.get_obj_inf
else:
if self.obj_retry[self.target_obj[side]['id']] == False:
self.retry_obj_queue[side].put(self.target_obj[side])
state = State.move2obj
elif state == State.place:
if self.next_level[side] == True:
self.next_level[side] = False
if c_pose[side + '_indx'] >= 3:
state = State.finish
else:
state = State.get_obj_inf
else:
state = State.move2obj
elif state == State.finish:
state = None
return state
def strategy(self, state, side):
cmd = Command()
cmd_queue = queue.Queue()
if state == State.init:
cmd['cmd'] = 'jointMove'
cmd['jpos'] = [0, 0, -1.2, 0, 1.87, 0, -0.87, 0]
cmd['state'] = State.init
cmd['speed'] = 40
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, False, cmd_queue)
elif state == State.get_obj_inf:
cmd['suc_cmd'] = 'Off'
cmd['cmd'], cmd['mode'] = 'ikMove', 'p2p'
cmd['pos'], cmd['euler'], cmd['phi'] = c_pose[side][c_pose[
side + '_indx']][0], c_pose[side][c_pose[side + '_indx']][1], 0
cmd_queue.put(copy.deepcopy(cmd))
cmd['suc_cmd'] = 0
cmd['cmd'] = 'occupied'
cmd['state'] = State.get_obj_inf
cmd_queue.put(copy.deepcopy(cmd))
side = self.dual_arm.send_cmd(side, False, cmd_queue)
if side != 'fail':
c_pose[side + '_indx'] += 1
else:
print('fuckfailfuckfailfuckfail')
elif state == State.select_obj:
print('oooooooooooooo')
self.get_obj_inf(side)
self.arrange_obj(side)
cmd['cmd'], cmd['state'] = None, State.select_obj
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, True, cmd_queue)
elif state == State.move2obj:
print('fuckmove2obj ++++++++++ ', side)
obj = None
chosed = False
if self.retry_obj_queue[side].empty(
) and self.target_obj_queue[side].empty():
if self.object_queue.empty():
self.next_level[side] = True
print('fuck10')
return
for _ in range(self.object_queue.qsize()):
obj = self.object_queue.get()
if self.obj_done[obj['id']] == False:
if side == 'left' and obj['pos'][1] < -0.02:
self.object_queue.put(obj)
continue
if side == 'right' and obj['pos'][1] > 0.02:
self.object_queue.put(obj)
continue
self.obj_done[obj['id']] = True
chosed = True
print('fuck1')
break
if chosed is False:
self.next_level[side] = True
print('fuck2')
return
print('fuck11')
elif self.target_obj_queue[side].empty():
obj = self.retry_obj_queue[side].get()
if self.obj_retry[obj['id']] == False:
self.obj_retry[obj['id']] = True
print('fuck3')
else:
print('fuck4')
return
else:
obj = self.target_obj_queue[side].get()
if self.obj_done[obj['id']] == False:
self.obj_retry[obj['id']] = True
print('fuck5')
else:
print('fuck6')
return
pos = copy.deepcopy(obj['pos'])
pos[1] += 0.032
pos[2] += 0.065
cmd['suc_cmd'] = 'Off'
cmd['cmd'], cmd['mode'], cmd[
'state'] = 'ikMove', 'p2p', State.move2obj
cmd['pos'], cmd['euler'], cmd['phi'] = [0.4, pos[1],
pos[2]], [0, 90, 0], 0
cmd_queue.put(copy.deepcopy(cmd))
cmd['cmd'] = 'occupied'
cmd_queue.put(copy.deepcopy(cmd))
side = self.dual_arm.send_cmd(side, False, cmd_queue)
if side == 'fail':
self.object_queue.put(obj)
self.obj_done[obj['id']] = False
print('fffffffffffuuuuuuuuuuccccccccccckkkkkkkkkkk')
else:
self.target_obj_queue[side].put(obj)
print('side = ', side, 'id = ', obj['id'])
elif state == State.check_pose:
self.check_pose(side)
cmd['cmd'], cmd['state'] = 'occupied', State.check_pose
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, True, cmd_queue)
elif state == State.pick:
obj = copy.deepcopy(self.target_obj[side])
if obj['vector'][2] > 0.7:
obj['pos'][0] -= 0.02
# obj['pos'][2] += 0.05
cmd['state'] = State.pick
cmd['cmd'], cmd['mode'] = 'fromtNoaTarget', 'line'
cmd['pos'], cmd['euler'], cmd['phi'] = obj['pos'], obj['euler'], 0
cmd['suc_cmd'], cmd['noa'] = obj['sucang'], [0, 0, -0.03]
cmd_queue.put(copy.deepcopy(cmd))
cmd['cmd'], cmd['mode'], cmd['noa'] = 'grasping', 'line', [
0, 0, 0.05
]
cmd['suc_cmd'], cmd['speed'] = 'On', 15
if obj['vector'][2] < 0.2:
cmd['speed'] = 30
cmd_queue.put(copy.deepcopy(cmd))
cmd['cmd'], cmd['mode'], = 'relativePos', 'line'
cmd['speed'], cmd['suc_cmd'] = 40, 'calibration'
cmd['pos'] = [0, 0, 0.03]
cmd_queue.put(copy.deepcopy(cmd))
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
cmd['pos'], cmd['euler'], cmd['phi'] = [
0.45, obj['pos'][1], obj['pos'][2] + 0.08
], obj['euler'], 0
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, True, cmd_queue)
elif state == State.place:
cmd['state'] = State.place
cmd['cmd'] = 'jointMove'
cmd['jpos'] = [0, 0, -1.5, 0, 2.07, 0, -0.57, 0]
cmd_queue.put(copy.deepcopy(cmd))
pose = self.place_pose_queue.get()
pos, euler = pose[0], pose[1]
if side == 'left':
pos[1] += 0.12
else:
pos[1] -= 0.12
cmd['cmd'], cmd['mode'] = 'fromtNoaTarget', 'line'
cmd['pos'], cmd['euler'], cmd['phi'] = pos, euler, 0
cmd['suc_cmd'], cmd['noa'] = 0, [0, 0, -0.2]
cmd_queue.put(copy.deepcopy(cmd))
cmd['cmd'], cmd['mode'], cmd['noa'] = 'noaMove', 'line', [
0, 0, 0.2
]
cmd_queue.put(copy.deepcopy(cmd))
cmd['cmd'], cmd['mode'], cmd['noa'] = 'noaMove', 'line', [
0, 0, -0.2
]
cmd['suc_cmd'] = 'Off'
cmd_queue.put(copy.deepcopy(cmd))
cmd['cmd'] = 'jointMove'
cmd['jpos'] = [0, 0, -1.8, 0, 2.57, 0, -0.87, 0]
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, True, cmd_queue)
elif state == State.finish:
cmd['suc_cmd'] = 'Off'
cmd['cmd'] = 'jointMove'
cmd['jpos'] = [0, 0, -1, 0, 1.57, 0, -0.57, 0]
cmd['state'] = State.finish
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, False, cmd_queue)
return side
def process(self):
rate = rospy.Rate(10)
rospy.on_shutdown(self.dual_arm.shutdown)
while True:
l_status = self.dual_arm.left_arm.status
if l_status == Status.idle or l_status == Status.occupied:
l_state = self.state_control(self.dual_arm.left_arm.state,
'left')
self.strategy(l_state, 'left')
rate.sleep()
r_status = self.dual_arm.right_arm.status
if r_status == Status.idle or r_status == Status.occupied:
r_state = self.state_control(self.dual_arm.right_arm.state,
'right')
self.strategy(r_state, 'right')
rate.sleep()
if l_state is None and r_state is None:
if l_status == Status.idle and r_status == Status.idle:
return
class MerchandiseTask():
def __init__(self, name_arm_ctrl, en_sim_arm_ctrl):
# arm control
self.name_arm_ctrl = name_arm_ctrl
self.en_sim_arm_ctrl = en_sim_arm_ctrl
self.dual_arm = DualArmTask(self.name_arm_ctrl, self.en_sim_arm_ctrl)
# state machine
self.state = State.init
# object information
self.camera = GetObjInfo()
self.curr_merchandise_list = []
# self.place_pose_queue = queue.Queue()
# self.object_queue = queue.Queue() #object detected from both camera
self.expired_queue = queue.Queue() #no reorient
self.old_queue = queue.Queue() #need reorient
self.new_queue = queue.Queue() #no reorient
#current level
self.expired_done = False #this_level_expired_done
self.old_done = False #this_level_old_done
self.new_done = False #this_level_new_done
self.left_tar_obj = queue.Queue()
self.right_tar_obj = queue.Queue()
self.target_obj_queue = {
'left': self.left_tar_obj,
'right': self.right_tar_obj
}
self.target_obj = {'left': None, 'right': None}
# self.left_retry_obj = queue.Queue()
# self.right_retry_obj = queue.Queue()
# self.retry_obj_queue = {'left': self.left_retry_obj, 'right': self.right_retry_obj}
# self.obj_done = np.zeros((100), dtype=bool)
# self.obj_retry = np.zeros((100), dtype=bool)
# self.init()
# def init(self):
# for pose in place_pose:
# self.place_pose_queue.put(pose)
def get_obj_info(self, arm_side):
fb = self.dual_arm.get_feedback(
arm_side
) # position(x,y,z), orientation(x,y,z,w), euler, orientation, phi
ids, base_H_mrks, names, exps, side_ids = self.camera.new_get_obj_info(
arm_side, fb.orientation)
self.curr_merchandise_list = self.camera.get_merchandise_log()
self.camera.print_merchandise_log(self.curr_merchandise_list)
if ids is None:
return
for id, base_H_mrk, name, exp, side_id in zip(ids, base_H_mrks, names,
exps, side_ids):
num = int(id / 10) - 1
self.curr_merchandise_list[num]['id'] = id
self.curr_merchandise_list[num]['side_id'] = side_id
self.curr_merchandise_list[num]['expired'] = exp
self.curr_merchandise_list[num]['pos'] = base_H_mrk[0:3, 3]
self.curr_merchandise_list[num]['vector'] = base_H_mrk[
0:3,
2] #aruco_z_axis, rotation (coordinate_axis: aruco z axis)
self.curr_merchandise_list[num][
'sucang'], roll = self.dual_arm.suc2vector(
base_H_mrk[0:3, 2],
[0, 1.57, 0]) #TODO #suck ang(0-90), roll (7 axi)
self.curr_merchandise_list[num]['euler'] = [roll, 90, 0]
# self.camera.print_merchandise_log(self.curr_merchandise_list)
#Check Arm approach from [TOP] or [DOWN]: aruco_z_axis's [z value]
if self.curr_merchandise_list[num]['vector'][
2] >= -0.2: #TODO aruco_z_axis's [z value]>=, >???
#Arm approach from top => GOOD to go!!!
# self.object_queue.put(self.curr_merchandise_list[num])
if self.curr_merchandise_list[num]['expired'] == 'expired':
#TODO: check current shelf level
self.expired_queue.put(self.curr_merchandise_list[num])
elif self.curr_merchandise_list[num]['expired'] == 'old':
#TODO: check current shelf level
self.old_queue.put(self.curr_merchandise_list[num])
elif self.curr_merchandise_list[num]['expired'] == 'new':
#TODO: check current shelf level
self.new_queue.put(self.curr_merchandise_list[num])
else: #ERROR
print('eerrrror')
pass
print('Good!!! object put in queue; name: {}, id: {}'.format(
self.curr_merchandise_list[num]['name'],
self.curr_merchandise_list[num]['id']))
else:
print(
'ERROR!!! Arm approach from downside => BAD, aruco_z_axis: {}'
.format(self.curr_merchandise_list[num]['vector']))
def check_closer_pose(self, arm_side):
self.target_obj[arm_side] = self.target_obj_queue[arm_side].get()
fb = self.dual_arm.get_feedback(arm_side)
ids, base_H_mrks, _, _, _ = self.camera.new_get_obj_info(
arm_side, fb.orientation)
if ids is None:
return
for id, base_H_mrk, in zip(ids, base_H_mrks):
if id == self.target_obj[arm_side]['id']:
self.target_obj[arm_side]['pos'] = base_H_mrk[0:3, 3]
if base_H_mrk[
2,
2] > -0.1: #FIXME: value= ?? #TODO: i dont know z value > -0.1????
self.target_obj[arm_side][
'sucang'], roll = self.dual_arm.suc2vector(
base_H_mrk[0:3, 2], [0, 1.57, 0])
self.target_obj[arm_side]['euler'] = [roll, 90, 0]
pass
def state_control(self, arm_state, arm_side):
print('\nCURRENT: arm_state= {}, arm_side = {}'.format(
arm_state, arm_side))
if arm_state is None:
arm_state = State.init
elif arm_state == State.init:
arm_state = State.open_drawer #State.move_cam2shelf#
elif arm_state == State.open_drawer:
arm_state = State.init #move_cam2shelf
elif arm_state == State.move_cam2shelf:
arm_state = State.detect_obj
elif arm_state == State.move_cam2box:
arm_state = State.detect_obj
elif arm_state == State.detect_obj:
#TODO: check if it works
if self.expired_queue.empty() & self.old_queue.empty(
) & self.new_queue.empty():
if cam_pose[arm_side + '_indx'] <= tot_shelf_level:
if (self.expired_done == True & self.old_done == True):
arm_state = State.move_cam2shelf #move_cam2 to shelf #take pic
else:
arm_state = State.move_cam2box #move_cam2 stock box #take pic
else:
arm_state = State.finish
else: #have obj in queue
arm_state = State.move2obj
elif arm_state == State.move2obj:
arm_state = State.pick #State.check_closer_pose
elif arm_state == State.check_closer_pose:
arm_state = State.pick
elif arm_state == State.pick:
if arm_side == 'left':
is_grip = self.dual_arm.left_arm.suction.is_grip
else:
is_grip = self.dual_arm.right_arm.suction.is_grip
if is_grip:
if (self.target_obj[arm_side]['expired'] == 'expired'
): #dispose
arm_state = State.dispose
elif (self.target_obj[arm_side]['expired'] == 'old'
): #reorient
arm_state = State.reorient
elif (self.target_obj[arm_side]['expired'] == 'new'): #stock
arm_state = State.stock
else:
arm_state = State.place
elif (self.expired_done & self.old_done
& self.new_done) == True: #if current level done
self.expired_done = False
self.old_done = False
self.new_done = False
if cam_pose[arm_side + '_indx'] >= tot_shelf_level:
arm_state = State.close_drawer #State.finish
else:
arm_state = State.move_cam2shelf #move to next level shelf to take picture
else:
# if self.obj_retry[self.target_obj[arm_side]['id']] == False:
# self.retry_obj_queue[arm_side].put(self.target_obj[arm_side])
# arm_state = State.move2obj
arm_state = State.finish #tmp
elif arm_state == State.dispose:
arm_state = State.move2obj
elif arm_state == State.reorient:
arm_state = State.move2obj
elif arm_state == State.stock:
arm_state = State.move2obj
elif arm_state == State.place:
if self.next_level[arm_side] == True:
self.next_level[arm_side] = False
if cam_pose[arm_side + '_indx'] >= tot_shelf_level:
arm_state = State.close_drawer #State.finish
else:
arm_state = State.move_cam2shelf #move to next level shelf to take picture
else:
arm_state = State.move2obj
# elif arm_state == State.organize:
# arm_state = State.close_drawer
elif arm_state == State.close_drawer:
if drawer_pose[arm_side + '_indx_open'] < tot_shelf_level:
arm_state = State.init #State.open_drawer
else:
arm_state = State.finish
elif arm_state == State.finish:
arm_state = None
print('SWITCHED to: arm_state= {}, arm_side = {}'.format(
arm_state, arm_side))
return arm_state
def strategy(self, arm_state, arm_side):
print("--->execute strategy: ", arm_state)
cmd = Command()
cmd_queue = queue.Queue()
if arm_state == State.init:
print(' ++++++++++ init ++++++++++ ', arm_side)
cmd['cmd'] = 'jointMove'
cmd['jpos'] = [0, 0, -1.2, 0, 1.87, 0, -0.87,
0] #[0, 0, 0, 0, 0, 0, 0, 0]
cmd['state'] = State.init
cmd['speed'] = 100
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(arm_side, False, cmd_queue)
print('left_arm.status:', self.dual_arm.left_arm.status)
print('right_arm.status:', self.dual_arm.right_arm.status)
elif arm_state == State.open_drawer:
print(' ++++++++++ open_drawer ++++++++++ ', arm_side)
# print('drawer_pose: \n\tarm_side = {}; \n\tpos, euler, phi = {}, {}, {}'.format(
# arm_side+'_indx_open',
# drawer_pose[arm_side][drawer_pose[arm_side+'_indx_open']][0],
# drawer_pose[arm_side][drawer_pose[arm_side+'_indx_open']][1],
# 0))
# #TODO: can use both hands together????????
# draw_pose = ObjInfo()
# draw_pose['pos'] = drawer_pose[arm_side][drawer_pose[arm_side+'_indx_open']][0]
# #safe prepare pose
# cmd['state'] = State.open_drawer
# cmd['cmd'], cmd['mode'] = 'ikMove', 'p2p'
# cmd['pos'], cmd['euler'], cmd['phi'] = [draw_pose['pos'][0]-0.3, draw_pose['pos'][1], draw_pose['pos'][2]], \
# drawer_pose[arm_side][drawer_pose[arm_side+'_indx_open']][1], 0
# cmd_queue.put(copy.deepcopy(cmd))
# #suck
# cmd['suc_cmd'] = 'Off'
# cmd['cmd'], cmd['mode'] = 'ikMove', 'p2p'
# cmd['pos'], cmd['euler'], cmd['phi'] = [draw_pose['pos'][0], draw_pose['pos'][1], draw_pose['pos'][2]], \
# drawer_pose[arm_side][drawer_pose[arm_side+'_indx_open']][1], 0
# cmd_queue.put(copy.deepcopy(cmd))
# #pull drawer
# cmd['suc_cmd'] = 0
# cmd['cmd'], cmd['mode'] = 'ikMove', 'p2p'
# cmd['pos'], cmd['euler'], cmd['phi'] = [draw_pose['pos'][0]-0.2, draw_pose['pos'][1], draw_pose['pos'][2]], \
# drawer_pose[arm_side][drawer_pose[arm_side+'_indx_open']][1], 0
# # cmd['cmd'] = 'occupied'
# cmd_queue.put(copy.deepcopy(cmd))
# self.dual_arm.send_cmd(arm_side, True, cmd_queue)
print('drawer_pose: \n\tarm_side = {}; \n\tjoint = {}'.format(
arm_side + '_indx_open',
drawer_pose[arm_side][drawer_pose[arm_side + '_indx_open']]))
cmd['cmd'] = 'jointMove'
cmd['jpos'] = drawer_pose[arm_side][drawer_pose[arm_side +
'_indx_open']]
cmd['state'] = State.open_drawer
cmd['speed'] = 100
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(arm_side, False, cmd_queue)
if arm_side != 'fail':
drawer_pose[
arm_side +
'_indx_open'] += 1 #TODO: 1 shelf level only take picture one time!!!!! NO!!!!
print('{} arm move move to drawer open pose SUCCEED'.format(
arm_side))
else:
print('{} arm move move to drawer open pose FAILED'.format(
arm_side))
print('next shelf level = {}'.format(drawer_pose[arm_side +
'_indx_open']))
print('left_arm.status:', self.dual_arm.left_arm.status)
print('right_arm.status:', self.dual_arm.right_arm.status)
elif arm_state == State.move_cam2shelf:
print(' ++++++++++ move_cam2shelf ++++++++++ ', arm_side)
print(
'camera_pose: \n\tarm_side = {}; \n\tpos, euler, phi = {}, {}, {}'
.format(arm_side + '_indx',
cam_pose[arm_side][cam_pose[arm_side + '_indx']][0],
cam_pose[arm_side][cam_pose[arm_side + '_indx']][1],
0))
cmd['state'] = State.move_cam2shelf
cmd['cmd'], cmd['mode'] = 'ikMove', 'p2p'
cmd['pos'], cmd['euler'], cmd['phi'] = cam_pose[arm_side][cam_pose[
arm_side +
'_indx']][0], cam_pose[arm_side][cam_pose[arm_side +
'_indx']][1], 0
cmd_queue.put(copy.deepcopy(cmd))
# cmd['cmd'], cmd['state'] = 'occupied', State.move_cam2shelf
cmd['cmd'] = 'occupied'
cmd_queue.put(copy.deepcopy(cmd))
arm_side = self.dual_arm.send_cmd(arm_side, True, cmd_queue)
if arm_side != 'fail':
cam_pose[
arm_side +
'_indx'] += 1 #TODO: 1 shelf level only take picture one time!!!!! NO!!!!
print('{} arm move move to take picture pose SUCCEED'.format(
arm_side))
else:
print('{} arm move move to take picture pose FAILED'.format(
arm_side))
print('left_arm.status:', self.dual_arm.left_arm.status)
print('right_arm.status:', self.dual_arm.right_arm.status)
elif arm_state == State.move_cam2box:
print(' ++++++++++ move_cam2box ++++++++++ ', arm_side)
print(
'box_pose: \n\tarm_side = {}; \n\tpos, euler, phi = {}, {}, {}'
.format(arm_side + '_indx', box_pose[arm_side][0][0],
box_pose[arm_side][0][1], 0))
cmd['state'] = State.move_cam2box
cmd['cmd'], cmd['mode'] = 'ikMove', 'p2p'
cmd['pos'], cmd['euler'], cmd['phi'] = box_pose[arm_side][0][
0], box_pose[arm_side][0][1], 0
cmd_queue.put(copy.deepcopy(cmd))
# cmd['cmd'], cmd['state'] = 'occupied', State.move_cam2box
cmd['cmd'] = 'occupied'
cmd_queue.put(copy.deepcopy(cmd))
arm_side = self.dual_arm.send_cmd(arm_side, True, cmd_queue)
if arm_side != 'fail':
cam_pose[
arm_side +
'_indx'] += 1 #TODO: 1 shelf level only take picture one time!!!!! NO!!!!
print(
'{} arm move move to take picture box pose SUCCEED'.format(
arm_side))
else:
print(
'{} arm move move to take picture box pose FAILED'.format(
arm_side))
print('left_arm.status:', self.dual_arm.left_arm.status)
print('right_arm.status:', self.dual_arm.right_arm.status)
elif arm_state == State.detect_obj:
print(' ++++++++++ detect_obj ++++++++++ ', arm_side)
self.get_obj_info(arm_side)
print('total expired_queue', self.expired_queue.qsize())
print('total old_queue', self.old_queue.qsize())
print('total new_queue', self.new_queue.qsize())
cmd['cmd'] = None
cmd['state'] = State.detect_obj
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(arm_side, True, cmd_queue)
print('left_arm.status:', self.dual_arm.left_arm.status)
print('right_arm.status:', self.dual_arm.right_arm.status)
elif arm_state == State.move2obj:
print(' ++++++++++ move2obj ++++++++++ ', arm_side)
# expired(disposal) -> old(reorient) -> new(stock)
if self.expired_queue.qsize() > 0:
obj = self.expired_queue.get()
#dispose State.dispose
elif self.old_queue.qsize() > 0:
obj = self.old_queue.get()
#reorient State.dispose
elif self.new_queue.qsize() > 0:
obj = self.new_queue.get()
#stock State.stock
else:
obj = ObjInfo()
#TODO:check object_queue if fail grasp, do what??? target_obj_queue, obj_done
print(obj['name'], obj['id'], obj['side_id'], obj['expired'])
#TODO: check if left/right out of reach
obj_putback = False
if (arm_side == 'left' and obj['pos'][1] < -0.02) or (
arm_side == 'right'
and obj['pos'][1] > 0.02): #y<-0.02 (2cm) too
obj_putback = True
else:
cmd['state'] = State.move2obj
cmd['suc_cmd'] = 'Off'
cmd['cmd'], cmd['mode'] = 'ikMove', 'p2p'
pos = copy.deepcopy(
obj['pos']) #(x, y, z): base_H_mrks[0:3, 3]
shift = 0.05 #FIXME: how far should the camera be???
cmd['pos'], cmd['euler'], cmd['phi'] = [
pos[0] + shift, pos[1] + shift, pos[2] + shift
], [0, 90, 0], 0
cmd_queue.put(copy.deepcopy(cmd))
# cmd['cmd'], cmd['state'] = 'occupied', State.move2obj
cmd['cmd'] = 'occupied'
cmd_queue.put(copy.deepcopy(cmd))
arm_side = self.dual_arm.send_cmd(arm_side, True, cmd_queue)
if (obj_putback == True) or (
arm_side == 'fail'
): #TODO: failed because of what???? cannot reach???
if obj['expired'] == 'expired':
self.expired_queue.put(
obj) #bcus failed so put the obj back to queue
elif obj['expired'] == 'old':
self.old_queue.put(obj)
elif obj['expired'] == 'new':
self.new_queue.put(obj)
# self.obj_done[obj['id']] = False
print(
'move2obj FAILED!!!!!!! arm_side = {}, name = {}, id = {}'.
format(arm_side, obj['name'], obj['id']))
else:
self.target_obj_queue[arm_side].put(obj)
print('move2obj OK! arm_side = {}, name = {}, id = {}'.format(
arm_side, obj['name'], obj['id']))
print('left_arm.status:', self.dual_arm.left_arm.status)
print('right_arm.status:', self.dual_arm.right_arm.status)
elif arm_state == State.check_closer_pose:
print(' ++++++++++ check_closer_pose ++++++++++ ', arm_side)
self.check_closer_pose(arm_side)
cmd['cmd'], cmd['state'] = 'occupied', State.check_closer_pose
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(arm_side, True, cmd_queue)
print('left_arm.status:', self.dual_arm.left_arm.status)
print('right_arm.status:', self.dual_arm.right_arm.status)
elif arm_state == State.pick:
print(' ++++++++++ pick ++++++++++ ', arm_side)
#TODO: check ori code, i don't know how to rewrite
#======SKIP check_closer_pose FOR VIRTUAL TESTING=======#
#======COMMENT OUT if check_closer_pose IS USED=========#
self.target_obj[arm_side] = self.target_obj_queue[arm_side].get()
#======SKIP check_closer_pose FOR VIRTUAL TESTING=======#
obj = copy.deepcopy(self.target_obj[arm_side])
pos = copy.deepcopy(obj['pos'])
mrk_z_axis = copy.deepcopy(obj['vector']) #already normalized
#away_scale: approach?/away? mrk along z axis
away_scale = 0.150 #(0.050 m = 50mm = 5cm)
pos_shifted_approach = pos + away_scale * mrk_z_axis
pos_shifted_leave = pos - away_scale * mrk_z_axis # + [0.0, 0.0, 0.05]
print('pos', pos)
print('pos_shifted_approach', pos_shifted_approach)
print('pos_shifted_leave', pos_shifted_leave)
#approach obj + suc off
cmd['state'] = State.pick
cmd['suc_cmd'] = 'Off'
cmd['cmd'], cmd['mode'] = 'ikMove', 'line' #p2p, line
cmd['pos'], cmd['euler'], cmd['phi'] = pos_shifted_approach, obj[
'euler'], 0
cmd_queue.put(copy.deepcopy(cmd))
#attach + suc on
cmd['suc_cmd'] = 'On'
cmd['cmd'], cmd['mode'] = 'ikMove', 'line' #p2p, line
cmd['pos'], cmd['euler'], cmd['phi'] = pos, obj['euler'], 0
cmd_queue.put(copy.deepcopy(cmd))
#attach + suc on + move up back off
cmd['suc_cmd'] = 'On'
cmd['cmd'], cmd['mode'] = 'ikMove', 'line' #p2p, line
cmd['pos'], cmd['euler'], cmd['phi'] = pos_shifted_leave, obj[
'euler'], 0
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(arm_side, True, cmd_queue)
print('left_arm.status:', self.dual_arm.left_arm.status)
print('right_arm.status:', self.dual_arm.right_arm.status)
elif arm_state == State.dispose:
print(' ++++++++++ dispose ++++++++++ ', arm_side)
#TODO: check if left/right out of reach
#disposal_box_intermediate_pose (left/right arm)
cmd['cmd'] = 'jointMove'
cmd['jpos'] = [0, 0, -1.2, 0, 1.87, 0, -0.87, 0]
cmd['speed'] = 100
cmd_queue.put(copy.deepcopy(cmd))
#put disposed product into disposal box
#use obj name to get dispose position
obj_name_dispose = self.target_obj[arm_side]['name']
cmd['cmd'], cmd['mode'] = 'ikMove', 'line' #p2p, line
cmd['pos'], cmd['euler'], cmd['phi'] = dispose_pose[
obj_name_dispose]
cmd['state'] = State.dispose
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(arm_side, True, cmd_queue)
print('left_arm.status:', self.dual_arm.left_arm.status)
print('right_arm.status:', self.dual_arm.right_arm.status)
print(obj_name_dispose,
'disposed successfully!!!!!!!!!!!!!!!!!!!!!!!!!!')
elif arm_state == State.reorient:
print(' ++++++++++ reorient ++++++++++ ', arm_side)
#TODO: check if left/right out of reach
obj_name_reorient = self.target_obj[arm_side]['name']
reorient_method = self.target_obj[arm_side]['side_id']
#write the related transformation of the product to the correct reorientation
cmd['state'] = State.reorient
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(arm_side, True, cmd_queue)
print('left_arm.status:', self.dual_arm.left_arm.status)
print('right_arm.status:', self.dual_arm.right_arm.status)
print(obj_name_reorient,
'reoriented successfully!!!!!!!!!!!!!!!!!!!!!!!!!!')
elif arm_state == State.stock:
print(' ++++++++++ stock ++++++++++ ', arm_side)
#TODO: check if left/right out of reach
obj_name_stock = self.target_obj[arm_side]['name']
#current shelf_intermediate_pose (left/right arm)
cmd['cmd'] = 'jointMove'
cmd['jpos'] = [0, 0, -1.2, 0, 1.87, 0, -0.87, 0]
cmd['speed'] = 100
cmd_queue.put(copy.deepcopy(cmd))
#use obj name to get dispose position
#TODO: stock the product behind reoriented product!!!!
new_pose = []
if (obj_name_stock == 'plum_riceball'):
#stock 2 times
new_pose = shelf_pose[obj_name_stock][shelf_pose['plum_cnt']]
shelf_pose['plum_cnt'] += 1
elif (obj_name_stock == 'salmon_riceball'):
#stock 2 times
new_pose = shelf_pose[obj_name_stock][shelf_pose['salmon_cnt']]
shelf_pose['salmon_cnt'] += 1
elif (obj_name_stock == 'lunchbox'):
#put UNDER the reoriented obj
new_pose = shelf_pose[obj_name_stock]
else:
#just put behind the reoriented obj
new_pose = shelf_pose[obj_name_stock]
cmd['cmd'], cmd['mode'] = 'ikMove', 'line' #p2p, line
cmd['pos'], cmd['euler'], cmd['phi'] = new_pose
cmd['state'] = State.stock
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(arm_side, True, cmd_queue)
print('left_arm.status:', self.dual_arm.left_arm.status)
print('right_arm.status:', self.dual_arm.right_arm.status)
print(obj_name_stock,
'stocked successfully!!!!!!!!!!!!!!!!!!!!!!!!!!')
elif arm_state == State.place:
print(' ++++++++++ place ++++++++++ ', arm_side)
cmd['state'] = State.place
cmd['cmd'] = 'jointMove'
cmd['jpos'] = [0, 0, -1.5, 0, 2.07, 0, -0.57, 0]
cmd_queue.put(copy.deepcopy(cmd))
pose = self.place_pose_queue.get()
pos, euler = pose[0], pose[1]
if arm_side == 'left':
pos[1] += 0.12
else:
pos[1] -= 0.12
cmd['cmd'], cmd['mode'] = 'fromtNoaTarget', 'line'
cmd['pos'], cmd['euler'], cmd['phi'] = pos, euler, 0
cmd['suc_cmd'], cmd['noa'] = 0, [0, 0, -0.2]
cmd_queue.put(copy.deepcopy(cmd))
cmd['cmd'], cmd['mode'], cmd['noa'] = 'noaMove', 'line', [
0, 0, 0.2
]
cmd_queue.put(copy.deepcopy(cmd))
cmd['cmd'], cmd['mode'], cmd['noa'] = 'noaMove', 'line', [
0, 0, -0.2
]
cmd['suc_cmd'] = 'Off'
cmd_queue.put(copy.deepcopy(cmd))
cmd['cmd'] = 'jointMove'
cmd['jpos'] = [0, 0, -1.8, 0, 2.57, 0, -0.87, 0]
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(arm_side, True, cmd_queue)
print('left_arm.status:', self.dual_arm.left_arm.status)
print('right_arm.status:', self.dual_arm.right_arm.status)
elif arm_state == State.organize:
print(' ++++++++++ organize ++++++++++ ', arm_side)
#TODO
cmd['state'] = State.organize
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(arm_side, True, cmd_queue)
print('left_arm.status:', self.dual_arm.left_arm.status)
print('right_arm.status:', self.dual_arm.right_arm.status)
elif arm_state == State.close_drawer:
print(' ++++++++++ close_drawer ++++++++++ ', arm_side)
print(
'drawer_pose: \n\tarm_side = {}; \n\tpos, euler, phi = {}, {}, {}'
.format(
arm_side + '_indx_close',
drawer_pose[arm_side][drawer_pose[arm_side +
'_indx_close']][0],
drawer_pose[arm_side][drawer_pose[arm_side +
'_indx_close']][1], 0))
#TODO: can use both hands together????????
draw_pose = ObjInfo()
draw_pose['pos'] = drawer_pose[arm_side][drawer_pose[
arm_side + '_indx_close']][0]
# safe prepare pose
cmd['cmd'], cmd['mode'] = 'ikMove', 'p2p'
cmd['pos'], cmd['euler'], cmd['phi'] = [draw_pose['pos'][0]-0.3, draw_pose['pos'][1], draw_pose['pos'][2]], \
drawer_pose[arm_side][drawer_pose[arm_side+'_indx_close']][1], 0
cmd_queue.put(copy.deepcopy(cmd))
# suck
cmd['cmd'], cmd['mode'] = 'ikMove', 'p2p'
cmd['pos'], cmd['euler'], cmd['phi'] = [draw_pose['pos'][0]-0.2, draw_pose['pos'][1], draw_pose['pos'][2]], \
drawer_pose[arm_side][drawer_pose[arm_side+'_indx_close']][1], 0
cmd['suc_cmd'] = 0
cmd_queue.put(copy.deepcopy(cmd))
# push drawer back
cmd['cmd'], cmd['mode'] = 'ikMove', 'p2p'
cmd['pos'], cmd['euler'], cmd['phi'] = [draw_pose['pos'][0], draw_pose['pos'][1], draw_pose['pos'][2]], \
drawer_pose[arm_side][drawer_pose[arm_side+'_indx_close']][1], 0
cmd['suc_cmd'] = 'Off' #TODO: release sucker when pose reached? or release sucker while moving??
# cmd['cmd'] = 'occupied'
cmd['state'] = State.close_drawer
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(arm_side, True, cmd_queue)
if arm_side != 'fail':
drawer_pose[
arm_side +
'_indx_close'] += 1 #TODO: 1 shelf level only take picture one time!!!!! NO!!!!
print('{} arm move move to drawer close pose SUCCEED'.format(
arm_side))
else:
print('{} arm move move to drawer close pose FAILED'.format(
arm_side))
print('next shelf level = {}'.format(drawer_pose[arm_side +
'_indx_close']))
print('left_arm.status:', self.dual_arm.left_arm.status)
print('right_arm.status:', self.dual_arm.right_arm.status)
elif arm_state == State.finish:
print(' ++++++++++ finish ++++++++++ ', arm_side)
cmd['suc_cmd'] = 'Off'
cmd['cmd'] = 'jointMove'
cmd['jpos'] = [0, 0, -1, 0, 1.57, 0, -0.57, 0]
cmd['state'] = State.finish
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(arm_side, False, cmd_queue)
print('left_arm.status:', self.dual_arm.left_arm.status)
print('right_arm.status:', self.dual_arm.right_arm.status)
return arm_side
def process(self):
#assign task to LEFT of RIGHT arm
# (1) check left/right arm status
# (2) state_control: switch arm state
# (3) strategy: perform arm actions
rate = rospy.Rate(10)
rospy.on_shutdown(self.dual_arm.shutdown)
while True:
l_status = self.dual_arm.left_arm.status
if l_status == Status.idle or l_status == Status.occupied:
l_state = self.state_control(self.dual_arm.left_arm.state,
'left')
self.strategy(l_state, 'left')
rate.sleep()
r_status = self.dual_arm.right_arm.status
if r_status == Status.idle or r_status == Status.occupied:
r_state = self.state_control(self.dual_arm.right_arm.state,
'right')
self.strategy(r_state, 'right')
rate.sleep()
if l_state is None and r_state is None:
if l_status == Status.idle and r_status == Status.idle:
return