from mpl_toolkits.mplot3d import axes3d
import FLSpegtransfer.utils.CmnUtil as U
from FLSpegtransfer.vision.BallDetectionRGBD import BallDetectionRGBD
root = '/home/hwangmh/pycharmprojects/FLSpegtransfer/'
plt.style.use('seaborn-whitegrid')
# plt.style.use('bmh')
plt.rc('font', size=12) # controls default text sizes
plt.rc('axes', titlesize=20) # fontsize of the axes title
plt.rc('axes', labelsize=15) # fontsize of the x and y labels
plt.rc('xtick', labelsize=10) # fontsize of the tick labels
plt.rc('ytick', labelsize=10) # fontsize of the tick labels
plt.rc('legend', fontsize=15) # legend fontsize
plt.rc('figure', titlesize=10) # fontsize of the figure title
# Trajectory check
BD = BallDetectionRGBD()
file_path = root + 'experiment/0_trajectory_extraction/'
arm_traj = np.load(file_path + 'ready_movement.npy')
wrist_traj = np.load(file_path + 'ready_movement.npy')
# arm_traj[:, 2] -= 0.010
# arm_traj = arm_traj[6255-5618:]
# wrist_traj = wrist_traj[1:]
# print(arm_traj.shape, wrist_traj.shape)
fc = 5
dt = 0.01
arm_traj = U.LPF(arm_traj, fc, dt)
wrist_traj = U.LPF(wrist_traj, fc, dt)
arm_traj = arm_traj[::
3] # down sampling by 3 for peg transfer and by 2 for random trajectory
wrist_traj = wrist_traj[::3]
# configurations
use_Trc = True
which_camera = 'inclined'
which_arm = 'PSM1'
# define objects
zivid = ZividCapture(which_camera=which_camera)
zivid.start()
dvrk = dvrkController(arm_name='/' + which_arm, comp_hysteresis=True)
# dvrk = dvrkArm('/'+which_arm)
Trc = np.load(root + 'calibration_files/Trc_' + which_camera + '_' +
which_arm + '.npy')
Tpc = np.load(root + 'calibration_files/Tpc_' + which_camera + '.npy')
bd = BallDetectionRGBD(Trc=Trc, Tpc=Tpc, which_camera=which_camera)
traj = np.load("traj.npy", allow_pickle=True)
traj_opt = np.load("traj_opt_cubic.npy", allow_pickle=True)
# # run motion (linear)
# for tt in traj:
# for qs in tt:
# dvrk.set_joint(qs[0], wait_callback=True)
# for q in qs:
# dvrk.set_joint(q, wait_callback=False)
# time.sleep(0.01)
# dvrk.set_joint(qs[-1], wait_callback=True)
#
# # run motion (optimized)
# for qs in traj_opt:
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.BallDetectionRGBD import BallDetectionRGBD
from FLSpegtransfer.utils.RANSAC.RANSAC_circle import RANSAC_circle
from sklearn.metrics import mean_squared_error
ball = BallDetectionRGBD()
ransac = RANSAC_circle()
zivid = ZividCapture()
# zivid.start()
root = '/home/hwangmh/pycharmprojects/FLSpegtransfer/'
Tpc = np.load(root + 'calibration_files/Tpc.npy')
hsv_lower_red = np.array([0 - 20, 130, 40])
hsv_upper_red = np.array([0 + 20, 255, 255])
# masking_depth = [-150, 0]
masking_depth = [500, 730]
def mask_image(img_color, img_point):
# 2D color masking
img_hsv = cv2.cvtColor(img_color, cv2.COLOR_BGR2HSV)
img_masked = cv2.inRange(img_hsv, hsv_lower_red, hsv_upper_red)
# noise filtering
kernel = np.ones((2, 2), np.uint8)