class WipeTask:
def __init__(self, _name, en_sim):
self.name = _name
self.en_sim = en_sim
self.state = State.init
print("1")
self.dual_arm = DualArmTask(self.name, self.en_sim)
print("2")
# 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}
print("3")
#self.init()
def state_control(self, state, side):
print('START', state, side)
if state is None:
state = State.init
elif state == State.init:
print("8")
state = State.get_ready
elif state == State.get_ready:
state = State.apporach_obj
elif state == State.apporach_obj:
state = State.move2obj
# state = State.grap_obj
elif state == State.move2obj:
state = State.grap_obj
elif state == State.grap_obj:
if side == 'left':
is_grip = self.dual_arm.left_arm.gripper.is_grip
else:
is_grip = self.dual_arm.right_arm.gripper.is_grip
state = State.move_back
elif state == State.move_back:
if c_pose[side+'_indx'] >= 6:
state = State.finish_init
else:
state = State.move_back
elif state == State.finish_init:
state = None
print('END', state)
return state
def strategy(self, state, side):
print("strategy", state)
print("5")
cmd = Command()
cmd_queue = queue.Queue()
if state == State.init:
print("6")
cmd['cmd'] = 'jointMove'
cmd['gripper_cmd'] = 'active'
#cmd['cmd'] = 'occupied'
cmd['jpos'] = [0, 0, 0, 0, 0, 0, 0, 1]
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_ready:
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['cmd'] = 'occupied'
cmd['state'] = State.get_ready
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.apporach_obj:
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
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['cmd'] = 'occupied'
cmd['state'] = State.apporach_obj
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.move2obj:
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.move2obj
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.grap_obj:
#cmd['cmd'] = 'occupied'
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
if side == 'left':
cmd['gripper_cmd'] = 'grap_bottle'
else:
cmd['gripper_cmd'] = 'grap_rag'
cmd['state'] = State.grap_obj
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, False, cmd_queue)
if side != 'fail':
c_pose[side+'_indx'] +=1
else:
print('fuckfailfuckfailfuckfail')
elif state == State.move_back:
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['cmd'] = 'occupied'
if side == 'left':
cmd['gripper_cmd'] = 'grap_bottle'
else:
cmd['gripper_cmd'] = 'grap_rag'
cmd['state'] = State.move_back
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.finish_init:
cmd['cmd'] = 'jointMove'
cmd['gripper_cmd'] = 'grap_bottle'
cmd['jpos'] = [0, 0, 0, 0, 0, 0, 0, 0]
cmd['state'] = State.finish_init
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, False, cmd_queue)
print("7")
return side
def process(self):
rate = rospy.Rate(10)
rospy.on_shutdown(self.dual_arm.shutdown)
while True:
#print("8")
l_status = self.dual_arm.left_arm.status
# print(l_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
if l_state is None :
if l_status == Status.idle:
return
class ScratchTask:
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):
print('START', state, side)
if state is None:
state = State.init
elif state == State.init:
state = State.apporach_obj
elif state == State.apporach_obj:
state = State.move2obj
elif state == State.move2obj:
state = State.grap_obj
elif state == State.grap_obj:
if side == 'left':
is_grip = self.dual_arm.left_arm.gripper.is_grip
else:
is_grip = self.dual_arm.right_arm.gripper.is_grip
if is_grip:
state = State.move_to_clean
elif self.next_level[side] == True:
self.next_level[side] = False
if c_pose[side + '_indx'] >= 3:
state = State.finish
else:
state = State.init
state = State.move_to_clean
elif state == State.move_to_clean:
# print(c_pose[side+'_indx'])
if c_pose[side + '_indx'] >= 8:
state = State.finish
else:
if c_pose[side + '_indx'] % 2 == 1:
state = State.arrive_clean
else:
state = State.move_to_clean
elif state == State.arrive_clean:
if side == 'left':
state = State.move_to_clean
else:
state = State.scratch
elif state == State.scratch:
print(c_pose[side + '_indx'])
if c_pose[side + '_indx'] >= 11:
state = State.finish
else:
state = State.scratch
elif state == State.finish:
state = State.safety_back
elif state == State.safety_back:
state = State.finish_init
elif state == State.finish_init:
state = None
print('END', state)
return state
def strategy(self, state, side):
print("strategy", state)
cmd = Command()
cmd_queue = queue.Queue()
if state == State.init:
cmd['cmd'] = 'jointMove'
cmd['gripper_cmd'] = 'active'
cmd['jpos'] = [0, 0, 0, 0, 0, 0, 0, 0]
cmd['state'] = State.init
cmd['speed'] = 20
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, True, cmd_queue)
elif state == State.apporach_obj:
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
# if side == 'left':
# cmd['pos'], cmd['euler'], cmd['phi'] = c_pose[side][c_pose[side+'_indx']][0], c_pose[side][c_pose[side+'_indx']][1], 0
# else:
# cmd['pos'], cmd['euler'], cmd['phi'] = c_pose[side][c_pose[side+'_indx']][0], c_pose[side][c_pose[side+'_indx']][1], 0
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['cmd'] = 'occupied'
cmd['state'] = State.apporach_obj
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.move2obj:
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
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.move2obj
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.grap_obj:
#cmd['cmd'] = 'occupied'
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
cmd['pos'], cmd['euler'], cmd['phi'] = c_pose[side][c_pose[
side + '_indx']][0], c_pose[side][c_pose[side + '_indx']][1], 0
if side == 'left':
cmd['gripper_cmd'] = 'grap_scratcher'
if side == 'right':
cmd['gripper_cmd'] = 'grap_scratcher'
cmd['state'] = State.grap_obj
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, False, cmd_queue)
if side != 'fail':
c_pose[side + '_indx'] += 1
else:
print('fuckfailfuckfailfuckfail')
elif state == State.move_to_clean:
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
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['cmd'] = 'occupied'
# if side == 'left':
# cmd['gripper_cmd'] = 'grap_alcohol'
# else:
# cmd['gripper_cmd'] = 'grap_rag'
cmd['state'] = State.move_to_clean
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.arrive_clean:
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
cmd['pos'], cmd['euler'], cmd['phi'] = c_pose[side][c_pose[
side + '_indx']][0], c_pose[side][c_pose[side + '_indx']][1], 0
#print(c_pose[side][c_pose[side+'_indx']][0])
cmd_queue.put(copy.deepcopy(cmd))
#cmd['suc_cmd'] = 0
# cmd['cmd'] = 'occupied'
# if side == 'left':
# cmd['gripper_cmd'] = 'squeeze'
cmd['state'] = State.arrive_clean
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.scratch:
#cmd['cmd'] = 'occupied'
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
cmd['pos'], cmd['euler'], cmd['phi'] = c_pose[side][c_pose[
side + '_indx']][0], c_pose[side][c_pose[side + '_indx']][1], 0
cmd['state'] = State.scratch
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, False, cmd_queue)
if side != 'fail':
c_pose[side + '_indx'] += 1
else:
print('fuckfailfuckfailfuckfail')
elif state == State.finish:
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
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['cmd'] = 'occupied'
cmd['gripper_cmd'] = 'open'
cmd['state'] = State.finish
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.safety_back:
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
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['cmd'] = 'occupied'
cmd['gripper_cmd'] = 'open'
cmd['state'] = State.safety_back
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')
# cmd['cmd'] = 'jointMove'
# cmd['gripper_cmd'] = 'open'
# cmd['jpos'] = [0, 0, 0, 0, 0, 0, 0, 0]
# cmd['state'] = State.safety_back
# cmd_queue.put(copy.deepcopy(cmd))
# self.dual_arm.send_cmd(side, False, cmd_queue)
elif state == State.finish_init:
cmd['cmd'] = 'jointMove'
cmd['gripper_cmd'] = 'reset'
cmd['jpos'] = [0, 0, 0, 0, 0, 0, 0, 0]
cmd['state'] = State.finish_init
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
# if l_state is None :
# if l_status == Status.idle:
# return
if r_state is None:
if r_status == Status.idle:
return
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 StockingTask:
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.pick_left_queue = queue.Queue()
self.pick_right_queue = queue.Queue()
self.place_left_queue = queue.Queue()
self.place_right_queue = queue.Queue()
self.sucang_left_queue = queue.Queue()
self.sucang_right_queue = queue.Queue()
self.init()
self.pick_pose = {
'left': self.pick_left_queue,
'right': self.pick_right_queue
}
self.place_pose = {
'left': self.place_left_queue,
'right': self.place_right_queue
}
self.place_sucang = {
'left': self.sucang_left_queue,
'right': self.sucang_right_queue
}
def init(self):
for pose in pick_pose_left:
self.pick_left_queue.put(pose)
for pose in place_pose_left:
self.place_left_queue.put(pose)
for angle in place_sucang_left:
self.sucang_left_queue.put(angle)
def state_control(self, state, side):
print(state, state == State.pick_and_place)
if state is None:
state = State.init
elif state == State.init:
state = State.pick_and_place
elif state == State.pick_and_place:
if self.pick_pose[side].empty():
print('as1111111111111')
state = State.finish
else:
print('222222222222')
state = State.pick_and_place
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.4, 0, 1.57, 0, -0.4, 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.pick_and_place:
pick_pose = self.pick_pose[side].get()
place_pose = self.place_pose[side].get()
suc_angle = self.place_sucang[side].get()
print('=============================================')
print(side, pick_pose)
print('++++++++++++++++++++++++++++++++++++++++++++')
print(side, place_pose)
print('============================================')
cmd['state'] = State.pick_and_place
# cmd['cmd'], cmd['mode'] = 'fromtNoaTarget', 'line'
# cmd['pos'], cmd['euler'], cmd['phi'] = pick_pose[0], pick_pose[1], 0
# cmd['suc_cmd'], cmd['noa'] = 0, [0, 0, -0.25]
# cmd_queue.put(copy.deepcopy(cmd))
# cmd['cmd'], cmd['mode'] = 'fromtNoaTarget', 'line'
# cmd['pos'], cmd['euler'], cmd['phi'] = pick_pose[0], pick_pose[1], 0
# cmd['suc_cmd'], cmd['noa'] = 0, [0, 0, 0.045]
# cmd_queue.put(copy.deepcopy(cmd))
# cmd['cmd'], cmd['mode'], cmd['noa'] = 'grasping', 'line', [0, 0, 0.03]
# cmd['suc_cmd'], cmd['speed'] = 'On', 5
# cmd_queue.put(copy.deepcopy(cmd))
# cmd['cmd'], cmd['mode'], = 'relativePos', 'line'
# cmd['speed'], cmd['pos'] = 40, [0, 0, 0.33]
# cmd_queue.put(copy.deepcopy(cmd))
cmd['cmd'], cmd['suc_cmd'] = 'jointMove', 'calibration'
cmd['jpos'] = [0, 0, -1.4, 0, 1.57, 0, -0.4, 0]
cmd_queue.put(copy.deepcopy(cmd))
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
cmd['pos'] = [0.45, place_pose[0][1], place_pose[0][2] + 0.05]
cmd['euler'], cmd['phi'] = place_pose[1], 0
if place_pose[0][2] > -0.25:
cmd['mode'] = 'p2p'
cmd_queue.put(copy.deepcopy(cmd))
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
cmd['pos'], cmd['euler'], cmd['phi'] = place_pose[0], place_pose[
1], 0
cmd['suc_cmd'] = suc_angle
cmd_queue.put(copy.deepcopy(cmd))
cmd['cmd'], cmd['mode'], = 'noaMove', 'line'
cmd['noa'] = [0, 0, -0.02]
cmd['suc_cmd'] = 'Off'
cmd_queue.put(copy.deepcopy(cmd))
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
cmd['pos'] = [0.45, place_pose[0][1], place_pose[0][2] + 0.03]
cmd['euler'], cmd['phi'] = place_pose[1], 0
cmd_queue.put(copy.deepcopy(cmd))
cmd['cmd'], cmd['suc_cmd'] = 'jointMove', 'calibration'
cmd['jpos'] = [0, 0, -1.4, 0, 1.57, 0, -0.4, 0]
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, False, cmd_queue)
elif state == State.finish:
print('asdfasdfasdf')
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)
print('dddddddddddddddd')
def process(self):
rate = rospy.Rate(100)
rospy.on_shutdown(self.dual_arm.shutdown)
while True:
l_status = self.dual_arm.left_arm.status
print(self.dual_arm.left_arm.state)
if l_status == Status.idle or l_status == Status.occupied:
print('333333333333333333')
l_state = self.state_control(self.dual_arm.left_arm.state,
'left')
print(l_state)
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 == State.finish: # and r_state == State.finish:
if self.dual_arm.left_arm.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
class WipeTask:
def __init__(self, _name, en_sim):
self.name = _name
self.en_sim = en_sim
self.state = State.init
print("1")
self.dual_arm = DualArmTask(self.name, self.en_sim)
print("2")
# 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}
print("3")
#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):
print('START', state, side)
if state is None:
state = State.init
elif state == State.init:
print("8")
state = State.safety_up
elif state == State.safety_up:
state = State.apporach_obj
elif state == State.apporach_obj:
state = State.move2obj
elif state == State.move2obj:
state = State.grab_obj
elif state == State.grab_obj:
# if side == 'left':
# is_grip = self.dual_arm.left_arm.gripper.is_grip
# else:
# is_grip = self.dual_arm.right_arm.gripper.is_grip
# if is_grip:
# state = State.safety_up
# elif self.next_level[side] == True:
# self.next_level[side] = False
# if c_pose[side+'_indx'] >= 3:
# state = State.finish
# else:
# state = State.init
state = State.grabed
elif state == State.grabed:
state = State.lift_up
elif state == State.lift_up:
# print(c_pose[side+'_indx'])
state = State.apporach_place
elif state == State.apporach_place:
state = State.place_obj
elif state == State.place_obj:
# if side == 'left':
# is_grip = self.dual_arm.left_arm.gripper.is_grip
# else:
# is_grip = self.dual_arm.right_arm.gripper.is_grip
# if is_grip:
# state = State.place_obj
# elif self.next_level[side] == True:
# self.next_level[side] = False
# if c_pose[side+'_indx'] >= 3:
# state = State.finish
# else:
# state = State.init
state = State.placed
elif state == State.placed:
state = State.above_obj
elif state == State.above_obj:
state = State.safety_back
elif state == State.safety_back:
state = State.finish_init
elif state == State.finish_init:
state = None
print('END', state)
return state
def strategy(self, state, side):
print("strategy", state)
cmd = Command()
cmd_queue = queue.Queue()
if state == State.init:
cmd['cmd'] = 'jointMove'
cmd['gripper_cmd'] = 'active'
cmd['jpos'] = [0, 0, 0, 0, 0, 0, 0, 0]
cmd['state'] = State.init
cmd['speed'] = 20
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, True, cmd_queue)
elif state == State.safety_up:
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['cmd'] = 'occupied'
cmd['state'] = State.safety_up
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.apporach_obj:
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['cmd'] = 'occupied'
cmd['state'] = State.apporach_obj
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.move2obj:
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
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.move2obj
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.grab_obj:
#cmd['cmd'] = 'occupied'
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
cmd['pos'], cmd['euler'], cmd['phi'] = c_pose[side][c_pose[side+'_indx']][0], c_pose[side][c_pose[side+'_indx']][1], 0
cmd['state'] = State.grab_obj
#cmd['gripper_cmd'] = 'grap_pose_point'
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, False, cmd_queue)
if side != 'fail':
c_pose[side+'_indx'] +=1
else:
print('fuckfailfuckfailfuckfail')
elif state == State.grabed:
#cmd['cmd'] = 'occupied'
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
cmd['pos'], cmd['euler'], cmd['phi'] = c_pose[side][c_pose[side+'_indx']][0], c_pose[side][c_pose[side+'_indx']][1], 0
cmd['gripper_cmd'] = 'grap_pose_point'
cmd['state'] = State.grabed
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, False, cmd_queue)
if side != 'fail':
c_pose[side+'_indx'] +=1
else:
print('fuckfailfuckfailfuckfail')
elif state == State.lift_up:
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
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['cmd'] = 'occupied'
cmd['state'] = State.lift_up
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')
# if place point change more than two axis than > p2p
elif state == State.apporach_place:
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
cmd['pos'], cmd['euler'], cmd['phi'] = c_pose[side][c_pose[side+'_indx']][0], c_pose[side][c_pose[side+'_indx']][1], 0
#print(c_pose[side][c_pose[side+'_indx']][0])
cmd_queue.put(copy.deepcopy(cmd))
#cmd['suc_cmd'] = 0
# cmd['cmd'] = 'occupied'
cmd['state'] = State.apporach_place
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.place_obj:
#cmd['cmd'] = 'occupied'
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
cmd['pos'], cmd['euler'], cmd['phi'] = c_pose[side][c_pose[side+'_indx']][0], c_pose[side][c_pose[side+'_indx']][1], 0
cmd['state'] = State.place_obj
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, False, cmd_queue)
if side != 'fail':
c_pose[side+'_indx'] +=1
else:
print('fuckfailfuckfailfuckfail')
elif state == State.placed:
#cmd['cmd'] = 'occupied'
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
cmd['pos'], cmd['euler'], cmd['phi'] = c_pose[side][c_pose[side+'_indx']][0], c_pose[side][c_pose[side+'_indx']][1], 0
cmd['gripper_cmd'] = 'open'
cmd['state'] = State.placed
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, False, cmd_queue)
if side != 'fail':
c_pose[side+'_indx'] +=1
else:
print('fuckfailfuckfailfuckfail')
elif state == State.above_obj:
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
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['cmd'] = 'occupied'
cmd['state'] = State.above_obj
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.safety_back:
cmd['cmd'], cmd['mode'] = 'ikMove', 'line'
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['cmd'] = 'occupied'
#cmd['gripper_cmd'] = 'open'
cmd['state'] = State.safety_back
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.finish_init:
cmd['cmd'] = 'jointMove'
cmd['gripper_cmd'] = 'reset'
cmd['jpos'] = [0, 0, 0, 0, 0, 0, 0, 0]
cmd['state'] = State.finish_init
# cmd['suc_cmd'] = 'Off'
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, False, cmd_queue)
print("7")
return side
def process(self):
global point_transformed
rate = rospy.Rate(10)
rospy.on_shutdown(self.dual_arm.shutdown)
while True:
if point_transformed[2][1][0]>=0:
print("8")
l_status = self.dual_arm.left_arm.status
# print(l_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()
if l_state is None :
if l_status == Status.idle:
return
#==============================================================================
elif point_transformed[2][1][0]<0:
print("82")
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 r_state is None :
if r_status == Status.idle:
return
class CameraCalib:
def __init__(self, _name, en_sim):
self.arm_move = False
self.state = State.move
self.is_done = {'left': False, 'right': False}
self.name = _name
self.en_sim = en_sim
self.dual_arm = DualArmTask(self.name, self.en_sim)
def hand_eye_client(self, req):
rospy.wait_for_service('/camera/hand_eye_calibration')
try:
hand_eye = rospy.ServiceProxy('/camera/hand_eye_calibration',
hand_eye_calibration)
res = hand_eye(req)
return res
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
def get_feedback(self, side):
rospy.wait_for_service(side + '_arm/get_kinematics_pose')
try:
get_endpos = rospy.ServiceProxy(side + '_arm/get_kinematics_pose',
GetKinematicsPose)
res = get_endpos('arm')
return res
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
def state_control(self, state, side):
if state is None:
state = State.init
elif state == State.init:
state = State.move
elif state == State.move:
state = State.take_pic
elif state == State.take_pic:
if self.is_done[side]:
state = State.finish
else:
state = State.move
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
cmd['speed'] = 40
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, False, cmd_queue)
print('state init')
elif state == State.move:
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], c_pose[side][c_pose[side +
'_indx']][2]
cmd['state'] = state
cmd_queue.put(copy.deepcopy(cmd))
side = self.dual_arm.send_cmd(side, False, cmd_queue)
if side != 'fail':
c_pose[side + '_indx'] = c_pose[side + '_indx'] + 1
if c_pose[side + '_indx'] == len(c_pose[side]):
self.is_done[side] = True
c_pose[side + '_indx'] = 0
else:
print('fuckfailfuckfailfuckfail ', cmd)
print('state move')
elif state == State.take_pic:
print('state take_pic start')
time.sleep(0.2)
cmd['cmd'], cmd['state'] = None, state
cmd_queue.put(copy.deepcopy(cmd))
self.dual_arm.send_cmd(side, True, cmd_queue)
arm_feedback = self.get_feedback(side)
req = hand_eye_calibrationRequest()
req.is_done = self.is_done[side]
req.end_trans.translation.x = arm_feedback.group_pose.position.x
req.end_trans.translation.y = arm_feedback.group_pose.position.y
req.end_trans.translation.z = arm_feedback.group_pose.position.z
req.end_trans.rotation.x = arm_feedback.group_pose.orientation.x
req.end_trans.rotation.y = arm_feedback.group_pose.orientation.y
req.end_trans.rotation.z = arm_feedback.group_pose.orientation.z
req.end_trans.rotation.w = arm_feedback.group_pose.orientation.w
res = self.hand_eye_client(req)
if self.is_done[side]:
trans_mat = np.array(res.end2cam_trans).reshape(4, 4)
# camera_mat = np.array(res.camera_mat).reshape(4, 4)
print(
'##################################################################'
)
print(trans_mat)
print(
'##################################################################'
)
config = ConfigParser.ConfigParser()
config.optionxform = str #reference: http://docs.python.org/library/configparser.html
# curr_path = os.path.dirname(os.path.abspath(__file__))
# config.read(['img_trans_pinto.ini', curr_path])
rospack = rospkg.RosPack()
curr_path = rospack.get_path('hand_eye')
config.read(curr_path + '/config/' + side +
'_arm_img_trans.ini')
config.set("External", "Key_1_1", str(trans_mat[0][0]))
config.set("External", "Key_1_2", str(trans_mat[0][1]))
config.set("External", "Key_1_3", str(trans_mat[0][2]))
config.set("External", "Key_1_4", str(trans_mat[0][3]))
config.set("External", "Key_2_1", str(trans_mat[1][0]))
config.set("External", "Key_2_2", str(trans_mat[1][1]))
config.set("External", "Key_2_3", str(trans_mat[1][2]))
config.set("External", "Key_2_4", str(trans_mat[1][3]))
config.set("External", "Key_3_1", str(trans_mat[2][0]))
config.set("External", "Key_3_2", str(trans_mat[2][1]))
config.set("External", "Key_3_3", str(trans_mat[2][2]))
config.set("External", "Key_3_4", str(trans_mat[2][3]))
config.write(curr_path + '/config/' + side +
'_arm_img_trans.ini')
print('state take_pic end')
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)
print('state take_pic finish')
def process(self, side):
rate = rospy.Rate(10)
rospy.on_shutdown(self.dual_arm.shutdown)
l_state = None
r_state = None
l_status = None
r_status = None
while True:
if side == 'left':
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')
else:
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