import sys
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
from FLSpegtransfer.vision.ZividCapture import ZividCapture
from FLSpegtransfer.vision.BlockDetection3D import BlockDetection3D
from FLSpegtransfer.vision.PegboardCalibration import PegboardCalibration
from FLSpegtransfer.vision.VisualizeDetection import VisualizeDetection
from FLSpegtransfer.vision.GraspingPose3D import GraspingPose3D
zivid = ZividCapture()
zivid.start()
peg = PegboardCalibration()
bd = BlockDetection3D()
vd = VisualizeDetection()
gp = GraspingPose3D()
# define region of interest
color = np.load('../../record/color_new.npy')
depth = np.load('../../record/depth_new.npy')
point = np.load('../../record/point_new.npy')
# color = cv2.cvtColor(color, cv2.COLOR_RGB2BGR)
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
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()
raise ValueError
ref_waypoints = np.concatenate((q0, qw1, qw2, qf), axis=0).reshape(4, 6)
return q_pos, ref_waypoints, cal_time
# load transform
which_camera = 'inclined'
Trc = np.load(root + 'calibration_files/Trc_' + which_camera +
'_PSM1.npy') # robot to camera
Tpc = np.load(root + 'calibration_files/Tpc_' + which_camera +
'.npy') # pegboard to camera
block = BlockDetection3D(Tpc)
gp = GraspingPose3D(dist_gps=5.5, dist_pps=5.5)
vd = VisualizeDetection()
pegboard = PegboardCalibration()
dvrk_model = dvrkKinematics()
motion_opt_2wp = PegMotionOptimizer_2wp()
motion_opt_cubic = CubicOptimizer_2wp()
# action ordering
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],