import cv2 import numpy as np from FLSpegtransfer.vision.ZividCapture import ZividCapture from FLSpegtransfer.vision.PegboardCalibration import PegboardCalibration zivid = ZividCapture(which_camera='inclined') zivid.start() peg = PegboardCalibration() # define region of interest color, depth, point = zivid.capture_3Dimage(color='BGR') ycr, hcr, xcr, wcr = peg.define_boundary(color) dx = 200 dy = 200 zivid.ycr = ycr - dy zivid.hcr = hcr + 2 * dy zivid.xcr = xcr - dx zivid.wcr = wcr + 2 * dx # pegboard registration if necessary peg.registration_pegboard(color, point)
class FLSPegTransfer:
def __init__(self, use_simulation=True, which_camera='inclined'):
self.use_simulation = use_simulation
# load transform
self.Trc = np.load(root + 'calibration_files/Trc_' + which_camera + '_PSM1.npy') # robot to camera
self.Tpc = np.load(root + 'calibration_files/Tpc_' + which_camera + '.npy') # pegboard to camera
# import modules
if use_simulation:
pass
else:
self.zivid = ZividCapture(which_camera=which_camera)
self.zivid.start()
self.block = BlockDetection3D(self.Tpc)
self.gp = GraspingPose3D()
self.vd = VisualizeDetection()
self.pegboard = PegboardCalibration()
self.dvrk_motion = dvrkPegTransferMotionSingleArmOptimized()
self.dvrk_model = dvrkKinematics()
# action ordering
self.action_list = np.array([[1, 7], [0, 6], [3, 8], [2, 9], [5, 11], [4, 10], # left to right
[7, 1], [6, 0], [8, 3], [9, 2], [11, 5], [10, 4]]) # right to left
self.action_order = 0
# data members
self.color = []
self.point = []
self.state = ['initialize']
self.main()
def transform_task2robot(self, point, delta=False, inverse=False):
point = np.array(point)
if inverse == False:
Tcp = np.linalg.inv(self.block.Tpc) # (mm)
Tcp[:3, -1] = Tcp[:3, -1] * 0.001 # (m)
Trp = self.Trc.dot(Tcp)
Rrp = Trp[:3, :3]
trp = Trp[:3, -1]
if delta == False:
transformed = Rrp.dot(point * 0.001) + trp
else:
transformed = Rrp.dot(point * 0.001)
else:
Tcr = np.linalg.inv(self.Trc) # (m)
Tcr[:3,-1] = Tcr[:3,-1] * 1000 # (mm)
Tpr = self.block.Tpc.dot(Tcr)
Rpr = Tpr[:3, :3]
tpr = Tpr[:3, -1]
if delta == False:
transformed = Rpr.dot(point * 1000) + tpr
else:
transformed = Rpr.dot(point * 1000)
return transformed
@classmethod
def transform(cls, points, T):
R = T[:3, :3]
t = T[:3, -1]
return R.dot(points.T).T + t.T
def move_blocks(self):
# [n, ang, x,y,z, seen]
gp_pick = self.gp.pose_grasping
gp_place = self.gp.pose_placing
gp_pick_robot = self.transform_task2robot(gp_pick[1:]) # [x,y,z]
gp_place_robot = self.transform_task2robot(gp_place[1:])
if self.action_order==0 or self.action_order==6:
print("go and pick")
self.dvrk_motion.go_pick(pos_pick=gp_pick_robot, rot_pick=gp_pick[0])
else:
print("return to peg")
self.dvrk_motion.return_to_peg(pos_pick=gp_pick_robot, rot_pick=gp_pick[0])
print ("transfer block")
self.dvrk_motion.transfer_block(pos_pick=gp_pick_robot, rot_pick=gp_pick[0],
pos_place=gp_place_robot, rot_place=gp_place[0])
# visual servoing prior to drop
# delta, seen = self.place_servoing()
# if seen == True: # if there is a block,
# delta_robot = self.transform_task2robot(delta, delta=True)
# delta_robot = np.array([0.0, 0.0, 0.0])
# self.dvrk_motion.drop_block(pos=gp_place_robot + delta_robot, rot=gp_place[0])
def place_servoing(self):
# get current position
self.update_images()
pose_blk, pnt_blk, pnt_mask = self.block.find_block_servo(self.color, self.point)
if len(pnt_blk) < 400:
delta = []
seen = False
else:
# self.vd.plot3d(pnt_blocks=pnt_blk, pnt_masks=pnt_mask, pnt_pegs=self.block.pnt_pegs)
_, T = pose_blk
nb_place = self.action_list[self.action_order][1]
p_peg = self.block.pnt_pegs[nb_place] + np.array([0.0, 0.0, -5]) # sub-mm above peg
p_fid = [0, 0, 15]
p_fid = T[:3, :3].dot(p_fid) + T[:3,-1] # w.r.t task coordinate
delta = p_peg - p_fid
seen = True
return delta, seen
def update_images(self):
if self.use_simulation:
if self.action_order <= 5:
self.color = np.load('record/peg_transfer_kit_capture/img_color_inclined.npy')
self.point = np.load('record/peg_transfer_kit_capture/img_point_inclined.npy')
else:
self.color = np.load('record/peg_transfer_kit_capture/img_color_inclined_rhp.npy')
self.point = np.load('record/peg_transfer_kit_capture/img_point_inclined_rhp.npy')
else:
self.color, _, self.point = self.zivid.capture_3Dimage(color='BGR')
def main(self):
while True:
if self.state[0] == 'initialize':
print('')
print('* State:', self.state[0])
# define ROI
self.dvrk_motion.move_origin()
# random move when NN model uses history
# self.dvrk_motion.move_random()
self.update_images()
# find pegs
self.block.find_pegs(img_color=self.color, img_point=self.point)
self.state.insert(0, 'update_image')
elif self.state[0] == 'update_image':
print('')
print('* State:', self.state[0])
self.update_images()
print ('image updated.')
self.state.insert(0, 'plan_action')
elif self.state[0] == 'plan_action':
print('')
print('* State:', self.state[0])
if self.action_order == len(self.action_list):
self.state.insert(0, 'exit')
else:
self.state.insert(0, 'find_block')
print("action pair #", self.action_list[self.action_order])
elif self.state[0] == 'find_block':
print('')
print('* State:', self.state[0])
nb_pick = self.action_list[self.action_order][0]
nb_place = self.action_list[self.action_order][1]
pose_blk_pick, pnt_blk_pick, pnt_mask_pick = self.block.find_block(
block_number=nb_pick, img_color=self.color, img_point=self.point)
pose_blk_place, pnt_blk_place, pnt_mask_place = self.block.find_block(
block_number=nb_place, img_color=self.color, img_point=self.point)
# check if there is block to move
if pose_blk_pick != [] and pose_blk_place == []:
print('A block to move was detected.')
# find grasping & placing pose
self.gp.find_grasping_pose(pose_blk=pose_blk_pick, peg_point=self.block.pnt_pegs[nb_pick])
self.gp.find_placing_pose(peg_point=self.block.pnt_pegs[nb_place])
# visualize
# pnt_grasping = np.array(self.gp.pose_grasping)[1:]
# self.vd.plot3d(pnt_blk_pick, pnt_mask_pick, self.block.pnt_pegs, [pnt_grasping])
self.state.insert(0, 'move_block')
else:
print('No block to move was detected. Skip this order.')
self.action_order += 1
self.state.insert(0, 'plan_action')
elif self.state[0] == 'move_block':
print('')
print('* State:', self.state[0])
self.move_blocks()
if self.action_order == 5:
self.dvrk_motion.move_origin()
self.action_order += 1
self.state.insert(0, 'update_image')
elif self.state[0] == 'exit':
print("task completed.")
gp_place = self.gp.pose_placing
gp_place_robot = self.transform_task2robot(gp_place[1:])
self.dvrk_motion.return_to_origin(pos_place=gp_place_robot, rot_place=gp_place_robot[0])
# self.dvrk_motion.move_origin()
exit()
class FLSPegTransfer:
def __init__(self, use_simulation=True, which_camera='inclined'):
self.use_simulation = use_simulation
# load transform
self.Trc1 = np.load(root + 'calibration_files/Trc_' + which_camera +
'_PSM1.npy') # robot to camera
self.Trc2 = np.load(root + 'calibration_files/Trc_' + which_camera +
'_PSM2.npy') # robot to camera
self.Tpc = np.load(root + 'calibration_files/Tpc_' + which_camera +
'.npy') # pegboard to camera
# import modules
if use_simulation:
pass
else:
self.zivid = ZividCapture(which_camera=which_camera)
self.zivid.start()
self.block = BlockDetection3D(self.Tpc)
self.gp = {
'PSM1': GraspingPose3D(which_arm='PSM1'),
'PSM2': GraspingPose3D(which_arm='PSM2')
}
self.vd = VisualizeDetection()
self.pegboard = PegboardCalibration()
self.dvrk_motion = dvrkPegTransferMotionDualArm()
self.dvrk_model = dvrkKinematics()
# action ordering
self.action_list = np.array([
[[0, 1], [7, 8]],
[[2, 3], [6, 11]],
[[4, 5], [9, 10]], # left to right
[[7, 8], [0, 1]],
[[6, 11], [2, 3]],
[[9, 10], [4, 5]]
]) # right to left
self.action_order = 0
# data members
self.color = []
self.point = []
self.state = ['initialize']
self.main()
def transform_task2robot(self,
point,
delta=False,
inverse=False,
which_arm='PSM1'):
point = np.array(point)
if inverse == False:
Tcp = np.linalg.inv(self.Tpc) # (mm)
Tcp[:3, -1] = Tcp[:3, -1] * 0.001 # (m)
if which_arm == 'PSM1' or which_arm == 0:
Trp = self.Trc1.dot(Tcp)
elif which_arm == 'PSM2' or which_arm == 1:
Trp = self.Trc2.dot(Tcp)
else:
raise ValueError
Rrp = Trp[:3, :3]
trp = Trp[:3, -1]
if delta == False:
transformed = Rrp.dot(point * 0.001) + trp
else:
transformed = Rrp.dot(point * 0.001)
else:
if which_arm == 'PSM1':
Tcr = np.linalg.inv(self.Trc1) # (m)
elif which_arm == 'PSM2':
Tcr = np.linalg.inv(self.Trc2) # (m)
else:
raise ValueError
Tcr[:3, -1] = Tcr[:3, -1] * 1000 # (mm)
Tpr = self.Tpc.dot(Tcr)
Rpr = Tpr[:3, :3]
tpr = Tpr[:3, -1]
if delta == False:
transformed = Rpr.dot(point * 1000) + tpr
else:
transformed = Rpr.dot(point * 1000)
return transformed
@classmethod
def transform(cls, points, T):
R = T[:3, :3]
t = T[:3, -1]
return R.dot(points.T).T + t.T
def move_blocks(self):
# [n, ang, x,y,z, seen]
gp_pick1 = self.gp['PSM1'].pose_grasping
gp_place1 = self.gp['PSM1'].pose_placing
gp_pick2 = self.gp['PSM2'].pose_grasping
gp_place2 = self.gp['PSM2'].pose_placing
# pick-up motion
gp_pick_robot1 = self.transform_task2robot(gp_pick1[1:],
which_arm='PSM1') # [x,y,z]
gp_pick_robot2 = self.transform_task2robot(gp_pick2[1:],
which_arm='PSM2')
print(gp_pick_robot1, gp_pick_robot2, gp_pick1[0], gp_pick2[0])
print("pick_above_block")
self.dvrk_motion.move_above_block(pos1=gp_pick_robot1,
rot1=gp_pick1[0],
pos2=gp_pick_robot2,
rot2=gp_pick2[0])
print("pick_block")
self.dvrk_motion.pick_block(pos1=gp_pick_robot1,
rot1=gp_pick1[0],
pos2=gp_pick_robot2,
rot2=gp_pick2[0])
# place motion
gp_place_robot1 = self.transform_task2robot(gp_place1[1:],
which_arm='PSM1')
gp_place_robot2 = self.transform_task2robot(gp_place2[1:],
which_arm='PSM2')
print(gp_place_robot1, gp_place_robot2, gp_place1[0], gp_place2[0])
print("place_above_block")
self.dvrk_motion.move_above_block(pos1=gp_place_robot1,
rot1=gp_place1[0],
pos2=gp_place_robot2,
rot2=gp_place2[0])
# visual servoing prior to drop
# delta, seen = self.place_servoing()
# if seen == True: # if there is a block,
# delta_robot = self.transform_task2robot(delta, delta=True)
self.dvrk_motion.drop_block(pos1=gp_place_robot1,
rot1=gp_place1[0],
pos2=gp_place_robot2,
rot2=gp_place2[0])
def place_servoing(self):
# get current position
self.update_images()
pose_blk, pnt_blk, pnt_mask = self.block.find_block_servo(
self.color, self.point)
if len(pnt_blk) < 400:
delta = []
seen = False
else:
# self.vd.plot3d(pnt_blocks=pnt_blk, pnt_masks=pnt_mask, pnt_pegs=self.block.pnt_pegs)
_, T = pose_blk
nb_place = self.action_list[self.action_order][1]
p_peg = self.block.pnt_pegs[nb_place] + np.array(
[0.0, 0.0, -5]) # sub-mm above peg
p_fid = [0, 0, 15]
p_fid = T[:3, :3].dot(p_fid) + T[:3, -1] # w.r.t task coordinate
delta = p_peg - p_fid
seen = True
return delta, seen
def update_images(self):
if self.use_simulation:
if self.action_order <= 2:
self.color = np.load(
'record/peg_transfer_kit_capture/img_color_inclined.npy')
self.point = np.load(
'record/peg_transfer_kit_capture/img_point_inclined.npy')
else:
self.color = np.load(
'record/peg_transfer_kit_capture/img_color_inclined_rhp.npy'
)
self.point = np.load(
'record/peg_transfer_kit_capture/img_point_inclined_rhp.npy'
)
else:
self.color, _, self.point = self.zivid.capture_3Dimage(color='BGR')
def main(self):
while True:
if self.state[0] == 'initialize':
print('')
print('* State:', self.state[0])
# define ROI
self.dvrk_motion.move_origin()
# random move when NN model uses history
# self.dvrk_motion.move_random()
self.update_images()
# find pegs
self.block.find_pegs(img_color=self.color,
img_point=self.point)
self.state.insert(0, 'update_image')
elif self.state[0] == 'update_image':
print('')
print('* State:', self.state[0])
self.update_images()
print('image updated.')
self.state.insert(0, 'plan_action')
elif self.state[0] == 'plan_action':
print('')
print('* State:', self.state[0])
if self.action_order == len(self.action_list):
self.state.insert(0, 'exit')
else:
self.state.insert(0, 'find_block')
print("action pair #", self.action_list[self.action_order])
elif self.state[0] == 'find_block':
print('')
print('* State:', self.state[0])
nb_pick1 = self.action_list[self.action_order][0][1] # PSM1
nb_pick2 = self.action_list[self.action_order][0][0] # PSM2
nb_place1 = self.action_list[self.action_order][1][1] # PSM1
nb_place2 = self.action_list[self.action_order][1][0] # PSM2
pose_blk_pick1, pnt_blk_pick1, pnt_mask_pick1 = self.block.find_block(
block_number=nb_pick1,
img_color=self.color,
img_point=self.point)
pose_blk_place1, pnt_blk_place1, pnt_mask_place1 = self.block.find_block(
block_number=nb_place1,
img_color=self.color,
img_point=self.point)
pose_blk_pick2, pnt_blk_pick2, pnt_mask_pick2 = self.block.find_block(
block_number=nb_pick2,
img_color=self.color,
img_point=self.point)
pose_blk_place2, pnt_blk_place2, pnt_mask_place2 = self.block.find_block(
block_number=nb_place2,
img_color=self.color,
img_point=self.point)
# check if there is block to move
if pose_blk_pick1 != [] and pose_blk_place1 == []\
and pose_blk_pick2 != [] and pose_blk_place2 == []:
print('A block to move was detected.')
# find grasping & placing pose
self.gp['PSM1'].find_grasping_pose(
pose_blk=pose_blk_pick1,
peg_point=self.block.pnt_pegs[nb_pick1])
self.gp['PSM1'].find_placing_pose(
peg_point=self.block.pnt_pegs[nb_place1])
self.gp['PSM2'].find_grasping_pose(
pose_blk=pose_blk_pick2,
peg_point=self.block.pnt_pegs[nb_pick2])
self.gp['PSM2'].find_placing_pose(
peg_point=self.block.pnt_pegs[nb_place2])
# visualize
# pnt_grasping = np.array(self.gp.pose_grasping)[1:]
# self.vd.plot3d(pnt_blk_pick, pnt_mask_pick, self.block.pnt_pegs, [pnt_grasping])
self.state.insert(0, 'move_block')
else:
print('No block to move was detected. Skip this order.')
self.action_order += 1
self.state.insert(0, 'plan_action')
elif self.state[0] == 'move_block':
print('')
print('* State:', self.state[0])
self.move_blocks()
self.action_order += 1
if self.action_order == 3:
self.dvrk_motion.move_origin()
self.state.insert(0, 'update_image')
elif self.state[0] == 'exit':
print("task completed.")
self.dvrk_motion.move_origin()
exit()
class FLSPegTransferDualArm:
def __init__(self, use_simulation=True, use_controller=True, use_optimization=True, optimizer='cubic', which_camera='inclined'):
self.use_simulation = use_simulation
# load transform
self.Trc1 = np.load(root + 'calibration_files/Trc_' + which_camera + '_PSM1.npy') # robot to camera
# self.Trc1[:3, -1] += np.array([0.002, 0.00, 0.00])
self.Trc2 = np.load(root + 'calibration_files/Trc_' + which_camera + '_PSM2.npy') # robot to camera
self.Trc2[:3, -1] += np.array([-0.001, 0.00, 0.00])
self.Tpc = np.load(root + 'calibration_files/Tpc_' + which_camera + '.npy') # pegboard to camera
# import modules
if self.use_simulation:
pass
else:
self.zivid = ZividCapture(which_camera=which_camera)
self.zivid.start()
self.block = BlockDetection3D(self.Tpc)
self.gp = {'PSM1': GraspingPose3D(dist_pps=5, dist_gps=5, which_arm='PSM1'),
'PSM2': GraspingPose3D(dist_pps=5, dist_gps=5, which_arm='PSM2')}
self.vd = VisualizeDetection()
self.pegboard = PegboardCalibration()
self.dvrk_motion\
= dvrkPegTransferMotionDualArm(use_controller=use_controller, use_optimization=use_optimization, optimizer=optimizer)
# action ordering
self.action_list = np.array([[[0, 1], [7, 8]],
[[2, 3], [6, 11]],
[[4, 5], [9, 10]], # left to right
[[7, 8], [0, 1]],
[[6, 11], [2, 3]],
[[9, 10], [4, 5]]]) # right to left
self.action_order = 0
# data members
self.color = []
self.point = []
self.state = ['initialize']
self.main()
def transform_task2robot(self, point, delta=False, inverse=False, which_arm='PSM1'):
point = np.array(point)
if inverse == False:
Tcp = np.linalg.inv(self.Tpc)
if which_arm=='PSM1' or which_arm==0:
Trp = self.Trc1.dot(Tcp)
elif which_arm=='PSM2' or which_arm==1:
Trp = self.Trc2.dot(Tcp)
else:
raise ValueError
Rrp = Trp[:3, :3]
trp = Trp[:3, -1]
if delta == False:
transformed = Rrp.dot(point) + trp
else:
transformed = Rrp.dot(point)
else:
if which_arm=='PSM1':
Tcr = np.linalg.inv(self.Trc1) # (m)
elif which_arm=='PSM2':
Tcr = np.linalg.inv(self.Trc2) # (m)
else:
raise ValueError
Tpr = self.Tpc.dot(Tcr)
Rpr = Tpr[:3, :3]
tpr = Tpr[:3, -1]
if delta == False:
transformed = Rpr.dot(point) + tpr
else:
transformed = Rpr.dot(point)
return transformed
@classmethod
def transform(cls, points, T):
R = T[:3, :3]
t = T[:3, -1]
return R.dot(points.T).T + t.T
def move_blocks(self):
# [n, ang, x,y,z, seen]
gp_pick1 = self.gp['PSM1'].pose_grasping
gp_place1 = self.gp['PSM1'].pose_placing
gp_pick2 = self.gp['PSM2'].pose_grasping
gp_place2 = self.gp['PSM2'].pose_placing
gp_place1 = np.array(gp_place1)
gp_place2 = np.array(gp_place2)
# pick-up and place motion
gp_pick_robot1 = self.transform_task2robot(gp_pick1[1:]*0.001, which_arm='PSM1') # [x,y,z]
gp_pick_robot2 = self.transform_task2robot(gp_pick2[1:]*0.001, which_arm='PSM2')
gp_place_robot1 = self.transform_task2robot(gp_place1[1:]*0.001, which_arm='PSM1')
gp_place_robot2 = self.transform_task2robot(gp_place2[1:]*0.001, which_arm='PSM2')
if self.action_order == 0 or self.action_order == 3:
self.dvrk_motion.go_pick(pos_pick1=gp_pick_robot1, rot_pick1=gp_pick1[0], pos_pick2=gp_pick_robot2, rot_pick2=gp_pick2[0])
else:
self.dvrk_motion.return_to_peg(pos_pick1=gp_pick_robot1, rot_pick1=gp_pick1[0], pos_pick2=gp_pick_robot2, rot_pick2=gp_pick2[0])
self.dvrk_motion.transfer_block(pos_pick1=gp_pick_robot1, rot_pick1=gp_pick1[0], pos_place1=gp_place_robot1, rot_place1=gp_place1[0],
pos_pick2=gp_pick_robot2, rot_pick2=gp_pick2[0], pos_place2=gp_place_robot2, rot_place2=gp_place2[0])
# visual correction prior to drop
delta1, seen1, delta2, seen2 = self.place_servoing()
delta_rob1 = self.transform_task2robot(delta1, delta=True)
delta_rob1[2] = 0.0 # temporary
delta_rob2 = self.transform_task2robot(delta2, delta=True)
delta_rob2[2] = 0.0 # temporary
self.dvrk_motion.servoing_block(pos_place1=gp_place_robot1+delta_rob1, rot_place1=gp_place1[0], pos_place2=gp_place_robot2+delta_rob2, rot_place2=gp_place2[0])
def place_servoing(self):
# get current position
self.update_images()
nb_place1 = self.action_list[self.action_order][1][1] # PSM1
nb_place2 = self.action_list[self.action_order][1][0] # PSM2
pose_blk1, pnt_blk1, pnt_mask1 = self.block.find_block_servo(self.color, self.point, self.block.pnt_pegs[nb_place1])
pose_blk2, pnt_blk2, pnt_mask2 = self.block.find_block_servo(self.color, self.point, self.block.pnt_pegs[nb_place2])
if len(pnt_blk1) < 400:
delta1 = [0.0, 0.0, 0.0]
seen1 = False
else:
# self.vd.plot3d(pnt_blocks=[pnt_blk1], pnt_masks=[pnt_mask1], pnt_pegs=self.block.pnt_pegs)
_, T1 = pose_blk1
p_peg1 = self.block.pnt_pegs[nb_place1] + np.array([0.0, 0.0, -5]) # sub-mm above peg
p_fid1 = [0, 0, 15]
p_fid1 = T1[:3, :3].dot(p_fid1) + T1[:3, -1] # w.r.t task coordinate
delta1 = (p_peg1 - p_fid1) * 0.001 # unit to (m)
seen1 = True
if len(pnt_blk2) < 400:
delta2 = [0.0, 0.0, 0.0]
seen2 = False
else:
# self.vd.plot3d(pnt_blocks=[pnt_blk2], pnt_masks=[pnt_mask2], pnt_pegs=self.block.pnt_pegs)
_, T2 = pose_blk2
p_peg2 = self.block.pnt_pegs[nb_place2] + np.array([0.0, 0.0, -5]) # sub-mm above peg
p_fid2 = [0, 0, 15]
p_fid2 = T2[:3, :3].dot(p_fid2) + T2[:3, -1] # w.r.t task coordinate
delta2 = (p_peg2 - p_fid2) * 0.001 # unit to (m)
seen2 = True
return delta1, seen1, delta2, seen2
def update_images(self):
if self.use_simulation:
import cv2
if self.action_order <= 2:
self.color = np.load('record/peg_transfer_kit_capture/img_color_inclined.npy')
self.point = np.load('record/peg_transfer_kit_capture/img_point_inclined.npy')
cv2.imwrite("img_inclined_detection.png", self.color)
else:
self.color = np.load('record/peg_transfer_kit_capture/img_color_inclined_rhp.npy')
self.point = np.load('record/peg_transfer_kit_capture/img_point_inclined_rhp.npy')
else:
self.color, _, self.point = self.zivid.capture_3Dimage(color='BGR')
def main(self):
while True:
if self.state[0] == 'initialize':
print('')
print('* State:', self.state[0])
# define ROI
self.dvrk_motion.move_origin()
self.update_images()
# find pegs
self.block.find_pegs(img_color=self.color, img_point=self.point)
self.state.insert(0, 'update_image')
elif self.state[0] == 'update_image':
print('')
print('* State:', self.state[0])
self.update_images()
print ('image updated.')
self.state.insert(0, 'plan_action')
elif self.state[0] == 'plan_action':
print('')
print('* State:', self.state[0])
if self.action_order == len(self.action_list):
self.state.insert(0, 'exit')
else:
self.state.insert(0, 'find_block')
print("action pair #", self.action_list[self.action_order])
elif self.state[0] == 'find_block':
print('')
print('* State:', self.state[0])
nb_pick1 = self.action_list[self.action_order][0][1] # PSM1
nb_pick2 = self.action_list[self.action_order][0][0] # PSM2
nb_place1 = self.action_list[self.action_order][1][1] # PSM1
nb_place2 = self.action_list[self.action_order][1][0] # PSM2
pose_blk_pick1, pnt_blk_pick1, pnt_mask_pick1 = self.block.find_block(
block_number=nb_pick1, img_color=self.color, img_point=self.point)
pose_blk_pick2, pnt_blk_pick2, pnt_mask_pick2 = self.block.find_block(
block_number=nb_pick2, img_color=self.color, img_point=self.point)
# pose_blk_place1, pnt_blk_place1, pnt_mask_place1 = self.block.find_block(
# block_number=nb_place1, img_color=self.color, img_point=self.point)
# pose_blk_place2, pnt_blk_place2, pnt_mask_place2 = self.block.find_block(
# block_number=nb_place2, img_color=self.color, img_point=self.point)
# check if there is block to move
if pose_blk_pick1 != [] and pose_blk_pick2 != []:
print('A block to move was detected.')
# find grasping & placing pose
self.gp['PSM1'].find_grasping_pose(pose_blk=pose_blk_pick1, peg_point=self.block.pnt_pegs[nb_pick1])
self.gp['PSM1'].find_placing_pose(peg_point=self.block.pnt_pegs[nb_place1])
self.gp['PSM2'].find_grasping_pose(pose_blk=pose_blk_pick2, peg_point=self.block.pnt_pegs[nb_pick2])
self.gp['PSM2'].find_placing_pose(peg_point=self.block.pnt_pegs[nb_place2])
# visualize
# pnt_grasping = np.array(self.gp['PSM1'].pose_grasping)[1:]
# self.vd.plot3d([pnt_blk_pick1], [pnt_mask_pick1], self.block.pnt_pegs, [pnt_grasping])
# pnt_grasping = np.array(self.gp['PSM2'].pose_grasping)[1:]
# self.vd.plot3d([pnt_blk_pick2], [pnt_mask_pick2], self.block.pnt_pegs, [pnt_grasping])
# visualize all blocks and all grasping points
# self.block.find_block_all(img_color=self.color, img_point=self.point)
# self.gp['PSM1'].find_grasping_pose_all(pose_blks=self.block.pose_blks, peg_points=self.block.pnt_pegs)
# self.gp['PSM2'].find_grasping_pose_all(pose_blks=self.block.pose_blks, peg_points=self.block.pnt_pegs)
# pnt_graspings1 = np.array(self.gp['PSM1'].pose_grasping)[:6, 2:5]
# pnt_graspings2 = np.array(self.gp['PSM2'].pose_grasping)[:6, 2:5]
# pnt_graspings = np.concatenate((pnt_graspings1, pnt_graspings2), axis=0)
# self.vd.plot3d(self.block.pnt_blks, self.block.pnt_masks, self.block.pnt_pegs, pnt_graspings1, pnt_graspings2)
self.state.insert(0, 'move_block')
else:
print('No block to move was detected. Skip this order.')
self.action_order += 1
self.state.insert(0, 'plan_action')
elif self.state[0] == 'move_block':
print('')
print('* State:', self.state[0])
self.move_blocks()
self.action_order += 1
if self.action_order == 3:
self.dvrk_motion.move_origin()
self.state.insert(0, 'update_image')
elif self.state[0] == 'exit':
print("task completed.")
self.dvrk_motion.move_origin()
exit()
class FLSPegTransfer():
def __init__(self, use_simulation=True, which_camera='inclined'):
self.use_simulation = use_simulation
# load transform
self.Trc1 = np.load(root + 'calibration_files/Trc_' + which_camera +
'_PSM1.npy') # robot to camera
self.Trc2 = np.load(root + 'calibration_files/Trc_' + which_camera +
'_PSM2.npy') # robot to camera
self.Tpc = np.load(root + 'calibration_files/Tpc_' + which_camera +
'.npy') # pegboard to camera
# import modules
if use_simulation:
pass
else:
self.zivid = ZividCapture(which_camera=which_camera)
self.zivid.start()
self.block = BlockDetection3D(self.Tpc)
# self.gp = [GraspingPose3D(which_arm='PSM1'), GraspingPose3D(which_arm='PSM2')]
self.gp = {
'PSM1': GraspingPose3D(which_arm='PSM1'),
'PSM2': GraspingPose3D(which_arm='PSM2')
}
self.vd = VisualizeDetection()
self.pegboard = PegboardCalibration()
self.dvrk_motion = dvrkPegTransferMotionHandOver()
self.dvrk_model = dvrkKinematics()
# action ordering (pick, place) pair
self.action_list = [
['PSM2', 1, 'PSM1', 7], # left to right
['PSM2', 0, 'PSM1', 6],
['PSM2', 3, 'PSM1', 8],
['PSM2', 2, 'PSM1', 9],
['PSM2', 5, 'PSM1', 11],
['PSM2', 4, 'PSM1', 10],
['PSM1', 7, 'PSM2', 1], # right to left
['PSM1', 6, 'PSM2', 0],
['PSM1', 8, 'PSM2', 3],
['PSM1', 9, 'PSM2', 2],
['PSM1', 11, 'PSM2', 5],
['PSM1', 10, 'PSM2', 4]
]
self.action_order = [-1, -1] # [pick action, place action]
# data members
self.color = []
self.point = []
self.state = [] # [pick state, place state]
self.pnt_handover = []
# event
self.event_handover = threading.Event()
self.event_waiting = threading.Event()
# parallelize
self.initialize()
# self.thread1 = threading.Thread(target=self.PSM1)
self.thread2 = threading.Thread(target=self.run_pick)
# self.thread1.start()
self.thread2.start()
self.run_place()
def initialize(self):
self.dvrk_motion.move_origin()
# self.dvrk_motion.move_random(which_arm='PSM1') # random move when NN model uses history
self.update_images()
self.block.find_pegs(img_color=self.color, img_point=self.point)
self.pnt_handover = (self.block.pnt_pegs[3] + self.block.pnt_pegs[6] +
self.block.pnt_pegs[9]) / 3
self.state = ['plan_action', 'plan_action']
def transform_task2robot(self,
point,
delta=False,
inverse=False,
which_arm='PSM1'):
point = np.array(point)
if inverse == False:
Tcp = np.linalg.inv(self.Tpc) # (mm)
Tcp[:3, -1] = Tcp[:3, -1] * 0.001 # (m)
if which_arm == 'PSM1' or which_arm == 0:
Trp = self.Trc1.dot(Tcp)
elif which_arm == 'PSM2' or which_arm == 1:
Trp = self.Trc2.dot(Tcp)
else:
raise ValueError
Rrp = Trp[:3, :3]
trp = Trp[:3, -1]
if delta == False:
transformed = Rrp.dot(point * 0.001) + trp
else:
transformed = Rrp.dot(point * 0.001)
else:
if which_arm == 'PSM1':
Tcr = np.linalg.inv(self.Trc1) # (m)
elif which_arm == 'PSM2':
Tcr = np.linalg.inv(self.Trc2) # (m)
else:
raise ValueError
Tcr[:3, -1] = Tcr[:3, -1] * 1000 # (mm)
Tpr = self.Tpc.dot(Tcr)
Rpr = Tpr[:3, :3]
tpr = Tpr[:3, -1]
if delta == False:
transformed = Rpr.dot(point * 1000) + tpr
else:
transformed = Rpr.dot(point * 1000)
return transformed
def place_servoing(self, nb_place):
# get current position
self.update_images()
(_, T), pnt_blk, pnt_mask = self.block.find_block_servo(
self.color, self.point)
if len(pnt_blk) < 400:
delta = []
seen = False
else:
# self.vd.plot3d(pnt_blocks=pnt_blk, pnt_masks=pnt_mask, pnt_pegs=self.block.pnt_pegs)
p_peg = self.block.pnt_pegs[nb_place] + np.array(
[0.0, 0.0, -5]) # sub-mm above peg
p_fid = [0, 0, 15]
p_fid = T[:3, :3].dot(p_fid) + T[:3, -1] # w.r.t task coordinate
delta = p_peg - p_fid
seen = True
return delta, seen
def update_images(self):
if self.use_simulation:
if self.action_order[0] <= 6 and self.action_order[1] <= 5:
self.color = np.load(
'record/peg_transfer_kit_capture/img_color_inclined_.npy')
self.point = np.load(
'record/peg_transfer_kit_capture/img_point_inclined_.npy')
# self.color = np.load('record/peg_transfer_kit_capture/img_color_inclined_rhp.npy')
# self.point = np.load('record/peg_transfer_kit_capture/img_point_inclined_rhp.npy')
else:
self.color = np.load(
'record/peg_transfer_kit_capture/img_color_inclined_.npy')
self.point = np.load(
'record/peg_transfer_kit_capture/img_point_inclined_.npy')
# self.color = np.load('record/peg_transfer_kit_capture/img_color_inclined_rhp.npy')
# self.point = np.load('record/peg_transfer_kit_capture/img_point_inclined_rhp.npy')
else:
self.color, _, self.point = self.zivid.capture_3Dimage(color='BGR')
def run_pick(self):
while True:
time.sleep(0.2)
if self.state[0] == 'plan_action' or self.state[0] == 'no_block':
if self.action_order[0] == len(self.action_list) - 1:
print("Pick Task Completed!")
exit()
else:
if self.state[0] == 'no_block':
print(
"No block detected for pick-up, Skip this order.")
self.action_order[1] += 1
arm_pick = self.action_list[self.action_order[0]][0]
arm_pick_next = self.action_list[self.action_order[0] +
1][0]
print('')
print("(" + arm_pick + ")", " Pick order: ",
self.action_order[0], "th completed")
if arm_pick != arm_pick_next:
self.event_waiting.clear()
self.event_waiting.wait()
self.action_order[0] += 1
self.state[0] = 'find_block'
elif self.state[0] == 'wait':
if self.state[1] == 'picked_hand_over':
self.state[0] = 'hand_off'
self.state[1] = 'wait'
elif self.state[0] == 'find_block':
# find block
self.update_images()
nb_pick = self.action_list[self.action_order[0]][1]
arm_pick = self.action_list[self.action_order[0]][0]
pose_blk_pick, pnt_blk_pick, pnt_mask_pick = self.block.find_block(
block_number=nb_pick,
img_color=self.color,
img_point=self.point)
# check if there is block to move
if pose_blk_pick != []:
# find grasping & placing pose
self.gp[arm_pick].find_grasping_pose(
pose_blk=pose_blk_pick,
peg_point=self.block.pnt_pegs[nb_pick])
self.gp[arm_pick].find_placing_pose(
peg_point=self.pnt_handover)
self.state[0] = 'move_block'
else: # no block detected
self.state[0] = 'no_block'
elif self.state[0] == 'move_block':
# print(' ')
# print("action order ", self.action_order)
# print("action pair #", self.action_list[self.action_order[0]])
# print(' ')
arm_pick = self.action_list[self.action_order[0]][0]
gp_pick = self.gp[
arm_pick].pose_grasping # [n, ang, x,y,z, seen]
gp_place = self.gp[
arm_pick].pose_placing # [n, ang, x,y,z, seen]
gp_pick_robot = self.transform_task2robot(
gp_pick[1:], which_arm=arm_pick) # [x,y,z]
gp_place_robot = self.transform_task2robot(
gp_place[1:], which_arm=arm_pick) # [x,y,z]
# pick up block and move to center
self.dvrk_motion.move_upright(pos=[
gp_pick_robot[0], gp_pick_robot[1],
self.dvrk_motion.height_ready
],
rot=gp_pick[0],
jaw='close',
which_arm=arm_pick)
self.dvrk_motion.pick_block(pos=gp_pick_robot,
rot=gp_pick[0],
which_arm=arm_pick)
self.dvrk_motion.move_upright(pos=[
gp_pick_robot[0], gp_pick_robot[1],
self.dvrk_motion.height_ready
],
rot=gp_pick[0],
jaw='close',
which_arm=arm_pick,
interpolate=True)
self.dvrk_motion.move_upright(pos=[
gp_place_robot[0], gp_place_robot[1],
self.dvrk_motion.height_ready
],
rot=gp_place[0],
jaw='close',
which_arm=arm_pick)
self.state[0] = 'ready_hand_over'
elif self.state[0] == 'hand_off': # escape from hand-over
arm_pick = self.action_list[self.action_order[0]][0]
gp_place = self.gp[
arm_pick].pose_placing # [n, ang, x,y,z, seen]
gp_place_robot = self.transform_task2robot(
gp_place[1:], which_arm=arm_pick) # [x,y,z]
self.dvrk_motion.move_jaw(jaw='open', which_arm=arm_pick)
self.dvrk_motion.move_upright(pos=[
gp_place_robot[0], gp_place_robot[1],
self.dvrk_motion.height_ready +
self.dvrk_motion.offset_handover
],
rot=gp_place[0],
jaw='open',
which_arm=arm_pick)
self.state[1] = 'place_block'
self.state[0] = 'plan_action'
def run_place(self):
while True:
time.sleep(0.1)
if self.state[1] == 'plan_action' or self.state[1] == 'no_block':
if self.action_order[1] == len(self.action_list) - 1:
self.dvrk_motion.move_origin()
print("Place Task Completed!")
exit()
else:
if self.state[1] == 'no_block':
print(
"No block detected for hand-over, Skip this order."
)
self.event_handover.set()
arm_place = self.action_list[self.action_order[1]][2]
arm_place_next = self.action_list[self.action_order[1] +
1][2]
print('')
print("(" + arm_place + ")", " Place order: ",
self.action_order[1], "th completed")
if arm_place != arm_place_next:
self.dvrk_motion.move_origin()
self.event_waiting.set()
self.action_order[1] = self.action_order[0]
self.action_order[1] += 1
self.state[1] = 'wait'
elif self.state[1] == 'wait':
if self.state[0] == 'ready_hand_over':
self.state[1] = 'find_block'
self.state[0] = 'wait'
elif self.state[0] == 'hand_off':
self.state[1] = 'place_block'
elif self.state[1] == 'find_block':
# find block
if self.use_simulation:
self.color = np.load(
'record/peg_transfer_kit_capture/img_color_inclined_ho.npy'
)
self.point = np.load(
'record/peg_transfer_kit_capture/img_point_inclined_ho.npy'
)
# self.color = np.load('record/peg_transfer_kit_capture/img_color_inclined.npy')
# self.point = np.load('record/peg_transfer_kit_capture/img_point_inclined.npy')
else:
self.update_images()
nb_place = self.action_list[self.action_order[1]][3]
arm_place = self.action_list[self.action_order[1]][2]
pose_blk_pick, pnt_blk_pick, pnt_mask_pick = self.block.find_block_servo(
img_color=self.color, img_point=self.point)
# pose_blk_place, pnt_blk_place, pnt_mask_place = self.block.find_block(
# block_number=nb_place, img_color=self.color, img_point=self.point)
# check if there is block to move
if pose_blk_pick != []:
# find grasping & placing pose
self.gp[arm_place].find_grasping_pose_handover(
pose_blk=pose_blk_pick)
self.gp[arm_place].find_placing_pose(
peg_point=self.block.pnt_pegs[nb_place])
# visualize
# pnt_grasping = np.array(self.gp[0].pose_grasping)[1:]
# self.vd.plot3d(pnt_blk_pick, pnt_mask_pick, self.block.pnt_pegs, [pnt_grasping])
self.state[1] = 'move_block'
else:
self.state[1] = 'no_block'
elif self.state[1] == 'move_block':
# [n, ang, x,y,z, seen]
arm_place = self.action_list[self.action_order[1]][2]
gp_pick = self.gp[arm_place].pose_grasping
gp_pick_robot = self.transform_task2robot(
gp_pick[1:], which_arm=arm_place) # [x,y,z]
# pick (hand-over)
self.dvrk_motion.move_upright(pos=[
gp_pick_robot[0], gp_pick_robot[1],
gp_pick_robot[2] + self.dvrk_motion.offset_handover
],
rot=gp_pick[0],
jaw='open',
which_arm=arm_place)
self.dvrk_motion.pick_block(pos=gp_pick_robot,
rot=gp_pick[0],
which_arm=arm_place)
self.state[1] = 'picked_hand_over'
elif self.state[1] == 'place_block':
# place
nb_place = self.action_list[self.action_order[1]][3]
arm_place = self.action_list[self.action_order[1]][2]
gp_place = self.gp[arm_place].pose_placing
gp_place_robot = self.transform_task2robot(
gp_place[1:], which_arm=arm_place) # [x,y,z]
pnt_handover_robot = self.transform_task2robot(
self.pnt_handover, which_arm=arm_place) # [x,y,z]
self.dvrk_motion.move_upright(pos=[
gp_place_robot[0], gp_place_robot[1],
self.dvrk_motion.height_ready
],
rot=gp_place[0],
jaw='close',
which_arm=arm_place)
# visual servoing prior to drop
delta, seen = self.place_servoing(nb_place)
if seen == True: # if there is a block,
delta_robot = self.transform_task2robot(delta, delta=True)
else:
delta_robot = [0.0, 0.0, 0.0]
self.dvrk_motion.drop_block(pos=gp_place_robot + delta_robot,
rot=gp_place[0],
which_arm=arm_place)
if arm_place == 'PSM1': # at the first half
self.dvrk_motion.move_upright(pos=[
pnt_handover_robot[0] - 0.02, pnt_handover_robot[1],
self.dvrk_motion.height_ready_handover
],
rot=0.0,
jaw='close',
which_arm=arm_place)
else: # at the last half
self.dvrk_motion.move_upright(pos=[
pnt_handover_robot[0] + 0.02, pnt_handover_robot[1],
self.dvrk_motion.height_ready_handover
],
rot=0.0,
jaw='close',
which_arm=arm_place)
self.state[1] = 'plan_action'
class FLSPegTransferHandover:
def __init__(self,
use_simulation=True,
use_controller=True,
use_optimization=True,
optimizer='cubic',
which_camera='inclined'):
self.use_simulation = use_simulation
# load transform
self.Trc1 = np.load(root + 'calibration_files/Trc_' + which_camera +
'_PSM1.npy') # robot to camera
self.Trc2 = np.load(root + 'calibration_files/Trc_' + which_camera +
'_PSM2.npy') # robot to camera
self.Trc2[:3, -1] += np.array([-0.001, 0.00, 0.00])
self.Tpc = np.load(root + 'calibration_files/Tpc_' + which_camera +
'.npy') # pegboard to camera
# import modules
if use_simulation:
pass
else:
self.zivid = ZividCapture(which_camera=which_camera)
self.zivid.start()
self.block = BlockDetection3D(self.Tpc)
self.gp = {
'PSM1': GraspingPose3D(dist_pps=5, dist_gps=5, which_arm='PSM1'),
'PSM2': GraspingPose3D(dist_pps=5, dist_gps=5, which_arm='PSM2')
}
self.vd = VisualizeDetection()
self.pegboard = PegboardCalibration()
self.dvrk_motion\
= dvrkPegTransferMotionHandOver(use_controller=use_controller, use_optimization=use_optimization, optimizer=optimizer)
self.dvrk_model = dvrkKinematics()
# action ordering (pick, place) pair
self.action_list = [
['PSM2', 1, 'PSM1', 7], # left to right
['PSM2', 0, 'PSM1', 6],
['PSM2', 3, 'PSM1', 8],
['PSM2', 2, 'PSM1', 9],
['PSM2', 5, 'PSM1', 11],
['PSM2', 4, 'PSM1', 10],
['PSM1', 7, 'PSM2', 1], # right to left
['PSM1', 6, 'PSM2', 0],
['PSM1', 8, 'PSM2', 3],
['PSM1', 9, 'PSM2', 2],
['PSM1', 11, 'PSM2', 5],
['PSM1', 10, 'PSM2', 4]
]
self.action_order = 0
# data members
self.color = []
self.point = []
self.state = ['initialize', 'initialize'] # [pick state, place state]
# event
self.event_handover = threading.Event()
self.event_waiting = threading.Event()
self.event_blockready = threading.Event()
# parallelize
self.initialize()
self.thread1 = threading.Thread(target=self.run_pick)
self.thread2 = threading.Thread(target=self.run_place)
self.thread1.start()
self.thread2.start()
# self.run_place()
def initialize(self):
self.dvrk_motion.move_origin()
# self.dvrk_motion.move_random(which_arm='PSM1') # random move when NN model uses history
self.update_images()
self.block.find_pegs(img_color=self.color, img_point=self.point)
self.pnt_handover = (self.block.pnt_pegs[3] + self.block.pnt_pegs[6] +
self.block.pnt_pegs[9]) / 3 + np.array(
[0.0, -10.0, 0.0])
self.state = ['plan_action', 'plan_action']
def transform_task2robot(self,
point,
delta=False,
inverse=False,
which_arm='PSM1'):
point = np.array(point)
if inverse == False:
Tcp = np.linalg.inv(self.Tpc)
if which_arm == 'PSM1' or which_arm == 0:
Trp = self.Trc1.dot(Tcp)
elif which_arm == 'PSM2' or which_arm == 1:
Trp = self.Trc2.dot(Tcp)
else:
raise ValueError
Rrp = Trp[:3, :3]
trp = Trp[:3, -1]
if delta == False:
transformed = Rrp.dot(point) + trp
else:
transformed = Rrp.dot(point)
else:
if which_arm == 'PSM1':
Tcr = np.linalg.inv(self.Trc1)
elif which_arm == 'PSM2':
Tcr = np.linalg.inv(self.Trc2)
else:
raise ValueError
Tpr = self.Tpc.dot(Tcr)
Rpr = Tpr[:3, :3]
tpr = Tpr[:3, -1]
if delta == False:
transformed = Rpr.dot(point) + tpr
else:
transformed = Rpr.dot(point)
return transformed
def place_servoing(self, nb_place_corr):
# get current position
self.update_images()
pose_blk, pnt_blk, pnt_mask = self.block.find_block_servo(
self.color, self.point, self.block.pnt_pegs[nb_place_corr])
if len(pnt_blk) < 400:
delta = [0.0, 0.0, 0.0]
seen = False
else:
# self.vd.plot3d(pnt_blocks=[pnt_blk], pnt_masks=[pnt_mask], pnt_pegs=self.block.pnt_pegs)
_, T = pose_blk
p_peg = self.block.pnt_pegs[nb_place_corr] + np.array(
[0.0, 0.0, -5]) # sub-mm above peg
p_fid = [0, 0, 15]
p_fid = T[:3, :3].dot(p_fid) + T[:3, -1] # w.r.t task coordinate
delta = (p_peg - p_fid) * 0.001 # unit to (m)
seen = True
return delta, seen
def update_images(self):
if self.use_simulation:
if self.action_order <= 5:
self.color = np.load(
'record/peg_transfer_kit_capture/img_color_inclined_.npy')
self.point = np.load(
'record/peg_transfer_kit_capture/img_point_inclined_.npy')
# self.color = np.load('record/peg_transfer_kit_capture/img_color_inclined_rhp.npy')
# self.point = np.load('record/peg_transfer_kit_capture/img_point_inclined_rhp.npy')
else:
self.color = np.load(
'record/peg_transfer_kit_capture/img_color_inclined_.npy')
self.point = np.load(
'record/peg_transfer_kit_capture/img_point_inclined_.npy')
# self.color = np.load('record/peg_transfer_kit_capture/img_color_inclined_rhp.npy')
# self.point = np.load('record/peg_transfer_kit_capture/img_point_inclined_rhp.npy')
else:
self.color, _, self.point = self.zivid.capture_3Dimage(color='BGR')
def run_pick(self):
while True:
if self.state[0] == 'plan_action':
if self.action_order == len(self.action_list):
print("Pick Task Completed!")
exit()
elif self.action_order == 6:
self.event_blockready.set()
self.event_blockready.clear()
self.event_waiting.clear()
self.event_waiting.wait()
print('')
print("Action order: ", self.action_order, "started")
self.state[0] = 'find_block'
elif self.state[0] == 'find_block':
# find block
self.update_images()
nb_pick = self.action_list[self.action_order][1]
arm_pick = self.action_list[self.action_order][0]
pose_blk_pick, pnt_blk_pick, pnt_mask_pick = self.block.find_block(
block_number=nb_pick,
img_color=self.color,
img_point=self.point)
# check if there is block to move
if pose_blk_pick != []:
# find grasping & placing pose
self.gp[arm_pick].find_grasping_pose(
pose_blk=pose_blk_pick,
peg_point=self.block.pnt_pegs[nb_pick])
self.gp[arm_pick].find_placing_pose(
peg_point=self.pnt_handover)
# # visualize
# pnt_grasping = np.array(self.gp[arm_pick].pose_grasping)[1:]
# self.vd.plot3d([pnt_blk_pick], [pnt_mask_pick], self.block.pnt_pegs, [pnt_grasping])
self.state[0] = 'move_block'
else: # no block detected
print("No block detected for pick-up, Skip this order.")
self.action_order += 1
self.state[0] = 'plan_action'
elif self.state[0] == 'move_block':
# print(' ')
# print("action order ", self.action_order)
# print("action pair #", self.action_list[self.action_order[0]])
# print(' ')
arm_pick = self.action_list[self.action_order][0]
gp_pick = self.gp[
arm_pick].pose_grasping # [n, ang, x,y,z, seen]
gp_place = self.gp[
arm_pick].pose_placing # [n, ang, x,y,z, seen]
gp_place = np.array(gp_place)
gp_pick_robot = self.transform_task2robot(
gp_pick[1:] * 0.001, which_arm=arm_pick) # [x,y,z]
gp_place_robot = self.transform_task2robot(
gp_place[1:] * 0.001, which_arm=arm_pick) # [x,y,z]
# pick up block and move to center
self.dvrk_motion.go_pick(pos_pick=gp_pick_robot,
rot_pick=gp_pick[0],
which_arm=arm_pick)
self.dvrk_motion.go_handover(pos_pick=gp_pick_robot,
rot_pick=gp_pick[0],
pos_place=gp_place_robot,
rot_place=gp_place[0],
which_arm=arm_pick)
self.event_blockready.set()
self.event_handover.wait(
) # until block is grasped by other gripper
self.event_handover.clear()
# escape from hand-over
self.dvrk_motion.move_jaw(jaw='open', which_arm=arm_pick)
self.dvrk_motion.move_upright(pos=[
gp_place_robot[0], gp_place_robot[1],
self.dvrk_motion.height_handover +
self.dvrk_motion.offset_handover
],
rot=gp_place[0],
jaw='open',
which_arm=arm_pick)
self.event_handover.set()
self.action_order += 1
self.state[0] = 'plan_action'
def run_place(self):
while True:
if self.state[1] == 'plan_action':
if self.action_order == len(self.action_list):
self.dvrk_motion.move_origin()
print("Place Task Completed!")
exit()
elif self.action_order == 6:
self.dvrk_motion.move_origin()
self.event_waiting.set()
self.state[1] = 'find_block'
elif self.state[1] == 'find_block':
arm_place = self.action_list[self.action_order][2]
self.dvrk_motion.handover_ready(which_arm=arm_place)
self.event_blockready.wait(
) # until block is ready to hand-over
self.event_blockready.clear()
if self.use_simulation:
self.color = np.load(
'record/peg_transfer_kit_capture/img_color_inclined_ho.npy'
)
self.point = np.load(
'record/peg_transfer_kit_capture/img_point_inclined_ho.npy'
)
else:
self.update_images()
nb_place = self.action_list[self.action_order][3]
arm_place = self.action_list[self.action_order][2]
nb_place_corr = nb_place
pose_blk_pick, pnt_blk_pick, pnt_mask_pick = self.block.find_block_servo_handover(
img_color=self.color, img_point=self.point)
# check if there is block to move
if pose_blk_pick != []:
# find grasping & placing pose
self.gp[arm_place].find_grasping_pose_handover(
pose_blk=pose_blk_pick, which_arm=arm_place)
self.gp[arm_place].find_placing_pose(
peg_point=self.block.pnt_pegs[nb_place])
# visualize
# pnt_grasping = np.array(self.gp[arm_place].pose_grasping)[1:]
# np.save('pnt_blk_pick', pnt_blk_pick)
# np.save('pnt_mask_pick', pnt_mask_pick)
# np.save('block_pnt_pegs', self.block.pnt_pegs)
# np.save('pnt_grasping', pnt_grasping)
# self.vd.plot3d([pnt_blk_pick], [pnt_mask_pick], self.block.pnt_pegs, [pnt_grasping])
self.state[1] = 'move_block'
else:
print("No block detected for hand-over, Skip this order.")
self.event_handover.set()
self.state[1] = 'plan_action'
elif self.state[1] == 'move_block':
# [n, ang, x,y,z, seen]
# nb_place = self.action_list[self.action_order][3]
arm_place = self.action_list[self.action_order][2]
gp_pick = self.gp[arm_place].pose_grasping
gp_place = self.gp[arm_place].pose_placing
gp_place = np.array(gp_place)
gp_pick_robot = self.transform_task2robot(
gp_pick[1:] * 0.001, which_arm=arm_place) # [x,y,z]
gp_place_robot = self.transform_task2robot(
gp_place[1:] * 0.001, which_arm=arm_place) # [x,y,z]
# pick (hand-over)
self.dvrk_motion.pick_block(pos=gp_pick_robot,
rot=gp_pick[0],
which_arm=arm_place)
self.event_handover.set()
self.event_handover.clear()
self.event_handover.wait() # until other gripper escape
self.event_handover.clear()
# place
self.dvrk_motion.go_place(pos_place=gp_place_robot,
rot_place=gp_place[0],
which_arm=arm_place)
# visual correction
delta, seen = self.place_servoing(nb_place_corr)
delta_rob = self.transform_task2robot(delta, delta=True)
delta_rob[2] = 0.0 # temporary
print(delta_rob)
self.dvrk_motion.servoing_block(pos_place=gp_place_robot +
delta_rob,
rot_place=gp_place[0],
which_arm=arm_place)
print('')
print("Action completed")
self.state[1] = 'plan_action'
from FLSpegtransfer.vision.ZividCapture import ZividCapture
from FLSpegtransfer.motion.deprecated.dvrkMotionBridgeP import dvrkMotionBridgeP
import FLSpegtransfer.utils.CmnUtil as U
import sys
import os
for p in sys.path:
if p == '/opt/ros/kinetic/lib/python2.7/dist-packages':
sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages')
import cv2
import numpy as np
zc = ZividCapture()
zc.start()
# check images
img_color, img_depth, img_point = zc.capture_3Dimage(color='BGR')
zc.display_rgb(img_color)
zc.display_depthmap(img_point)
zc.display_pointcloud(img_point, img_color)
dvrk = dvrkMotionBridgeP()
sampling = 100
for i in range(sampling):
r1 = np.random.uniform(-40, 40)
r2 = np.random.uniform(-40, 40)
r3 = np.random.uniform(-40, 40)
rot1 = [r1, r2, r3]
q1 = U.euler_to_quaternion(rot1, unit='deg')
jaw1 = [10 * np.pi / 180.]
dvrk.set_pose(rot1=q1, jaw1=jaw1)
img_color, img_depth, img_point = zc.capture_3Dimage(img_crop=False)
class FLSPegTransfer:
def __init__(self, use_simulation=True, use_controller=True, use_optimization=True, optimizer='cubic', which_camera='inclined', which_arm='PSM1'):
self.use_simulation = use_simulation
# load transform
self.Trc = np.load(root + 'calibration_files/Trc_' + which_camera + '_'+which_arm+'.npy') # robot to camera
self.Tpc = np.load(root + 'calibration_files/Tpc_' + which_camera + '.npy') # pegboard to camera
# self.Trc_stereo = np.load(root + 'calibration_files/Trc_' + 'stereo' + '_'+which_arm+'.npy') # robot to camera
# self.Trc_stereo[:3, -1] += np.array([0.00, 0.003, 0.00])
# self.Tc1c2 = np.linalg.inv(self.Trc).dot(self.Trc_stereo) # cam1 = inclined, cam2 = stereo
# import modules
if self.use_simulation:
pass
else:
self.zivid = ZividCapture(which_camera=which_camera)
self.zivid.start()
self.block = BlockDetection3D(self.Tpc)
self.gp = GraspingPose3D(dist_gps=5, dist_pps=5, which_arm=which_arm)
self.vd = VisualizeDetection()
self.pegboard = PegboardCalibration()
self.dvrk_motion\
= dvrkPegTransferMotionSingleArm(use_controller=use_controller, use_optimization=use_optimization,
optimizer=optimizer, which_arm=which_arm)
self.zivid_utils = ZividUtils(which_camera='inclined')
# action ordering
self.action_list = np.array([[1, 7], [0, 6], [3, 8], [2, 9], [5, 11], [4, 10], # left to right
[7, 1], [6, 0], [8, 3], [9, 2], [11, 5], [10, 4]]) # right to left
self.action_order = 0
# data members
self.color = []
self.point = []
self.state = ['initialize']
self.main()
def transform_task2robot(self, point, delta=False, inverse=False):
point = np.array(point)
if inverse == False:
Tcp = np.linalg.inv(self.block.Tpc)
Trp = self.Trc.dot(Tcp)
Rrp = Trp[:3, :3]
trp = Trp[:3, -1]
if delta == False:
transformed = Rrp.dot(point) + trp
else:
transformed = Rrp.dot(point)
else:
Tcr = np.linalg.inv(self.Trc)
Tpr = self.block.Tpc.dot(Tcr)
Rpr = Tpr[:3, :3]
tpr = Tpr[:3, -1]
if delta == False:
transformed = Rpr.dot(point) + tpr
else:
transformed = Rpr.dot(point)
return transformed
@classmethod
def transform(cls, points, T):
R = T[:3, :3]
t = T[:3, -1]
return R.dot(points.T).T + t.T
def move_blocks(self):
# [n, ang, x,y,z, seen]
gp_pick = self.gp.pose_grasping
gp_place = np.array(self.gp.pose_placing)
# pick-up and place motion
gp_pick_robot = self.transform_task2robot(gp_pick[1:]*0.001) # [x,y,z]
gp_place_robot = self.transform_task2robot(gp_place[1:]*0.001)
if self.action_order == 0 or self.action_order == 6:
self.dvrk_motion.go_pick(pos_pick=gp_pick_robot, rot_pick=gp_pick[0])
else:
self.dvrk_motion.return_to_peg(pos_pick=gp_pick_robot, rot_pick=gp_pick[0])
self.dvrk_motion.transfer_block(pos_pick=gp_pick_robot, rot_pick=gp_pick[0],
pos_place=gp_place_robot, rot_place=gp_place[0])
# visual correction
delta, seen = self.place_servoing()
delta_rob = self.transform_task2robot(delta, delta=True)
delta_rob[2] = 0.0 # temporary
self.dvrk_motion.servoing_block(pos_place=gp_place_robot+delta_rob, rot_place=gp_place[0])
def place_servoing(self):
# get current position
self.update_images()
nb_place = self.action_list[self.action_order][1]
pose_blk, pnt_blk, pnt_mask = self.block.find_block_servo(self.color, self.point, self.block.pnt_pegs[nb_place])
if len(pnt_blk) < 400:
delta = [0.0, 0.0, 0.0]
seen = False
else:
# self.vd.plot3d(pnt_blocks=[pnt_blk], pnt_masks=[pnt_mask], pnt_pegs=self.block.pnt_pegs)
_, T = pose_blk
p_peg = self.block.pnt_pegs[nb_place] + np.array([0.0, 0.0, -5]) # sub-mm above peg
p_fid = [0, 0, 15]
p_fid = T[:3, :3].dot(p_fid) + T[:3,-1] # w.r.t task coordinate
delta = (p_peg - p_fid)*0.001 # unit to (m)
seen = True
return delta, seen
def update_images(self):
if self.use_simulation:
if self.action_order <= 5:
self.color = np.load('record/peg_transfer_kit_capture/img_color_inclined.npy')
self.point = np.load('record/peg_transfer_kit_capture/img_point_inclined.npy')
else:
self.color = np.load('record/peg_transfer_kit_capture/img_color_inclined_rhp.npy')
self.point = np.load('record/peg_transfer_kit_capture/img_point_inclined_rhp.npy')
else:
self.color, _, self.point = self.zivid.capture_3Dimage(color='BGR')
def main(self):
while True:
if self.state[0] == 'initialize':
print('')
print('* State:', self.state[0])
# define ROI
self.dvrk_motion.move_origin()
# random move when NN model uses history
# self.dvrk_motion.move_random()
self.update_images()
# find pegs
self.block.find_pegs(img_color=self.color, img_point=self.point)
self.state.insert(0, 'update_image')
print ("started")
time.sleep(3)
st = time.time()
elif self.state[0] == 'update_image':
print('')
print('* State:', self.state[0])
self.update_images()
print ('image updated.')
self.state.insert(0, 'plan_action')
elif self.state[0] == 'plan_action':
print('')
print('* State:', self.state[0])
if self.action_order == len(self.action_list):
self.state.insert(0, 'exit')
else:
self.state.insert(0, 'find_block')
print("action pair #", self.action_list[self.action_order])
elif self.state[0] == 'find_block':
print('')
print('* State:', self.state[0])
nb_pick = self.action_list[self.action_order][0]
nb_place = self.action_list[self.action_order][1]
pose_blk_pick, pnt_blk_pick, pnt_mask_pick = self.block.find_block(
block_number=nb_pick, img_color=self.color, img_point=self.point)
pose_blk_place, pnt_blk_place, pnt_mask_place = self.block.find_block(
block_number=nb_place, img_color=self.color, img_point=self.point)
# check if there is block to move
if pose_blk_pick != [] and pose_blk_place == []:
print('A block to move was detected.')
# find grasping & placing pose
self.gp.find_grasping_pose(pose_blk=pose_blk_pick, peg_point=self.block.pnt_pegs[nb_pick])
self.gp.find_placing_pose(peg_point=self.block.pnt_pegs[nb_place])
# visualize
# pnt_grasping = np.array(self.gp.pose_grasping)[1:]
# self.vd.plot3d([pnt_blk_pick], [pnt_mask_pick], self.block.pnt_pegs, [pnt_grasping])
# pnt_placing = np.array(self.gp.pose_placing)[1:]
# self.vd.plot3d([pnt_blk_place], [pnt_mask_pick], self.block.pnt_pegs, [pnt_placing])
# visualize all blocks and all grasping points
# self.block.find_block_all(img_color=self.color, img_point=self.point)
# self.gp.find_grasping_pose_all(pose_blks=self.block.pose_blks, peg_points=self.block.pnt_pegs)
# pnt_graspings = np.array(self.gp.pose_grasping)[:6, 2:5]
# self.vd.plot3d(self.block.pnt_blks, self.block.pnt_masks, self.block.pnt_pegs, pnt_graspings)
self.state.insert(0, 'move_block')
else:
print('No block to move was detected. Skip this order.')
self.action_order += 1
self.state.insert(0, 'plan_action')
elif self.state[0] == 'move_block':
print('')
print('* State:', self.state[0])
self.move_blocks()
if self.action_order == 5:
self.dvrk_motion.move_origin()
self.action_order += 1
self.state.insert(0, 'update_image')
elif self.state[0] == 'exit':
print("task completed.")
self.dvrk_motion.move_origin()
t_comp = time.time() - st
print (t_comp)
np.save('/home/hwangmh/t_comp', t_comp)
print(self.dvrk_motion.time_motion)
print (np.sum(self.dvrk_motion.time_motion))
print(self.dvrk_motion.time_computing)
print (np.sum(self.dvrk_motion.time_computing))
import pdb; pdb.set_trace()
exit()