def __init__(self, use_controller, use_optimization, optimizer):
# motion library
self.use_optimization = use_optimization
self.optimizer = optimizer
if use_controller:
self.arm1 = dvrkController(arm_name='/PSM1',
comp_hysteresis=True,
stop_collision=False)
self.arm2 = dvrkController(arm_name='/PSM2',
comp_hysteresis=True,
stop_collision=False)
# self.arm2 = dvrkArm('/PSM2')
else:
self.arm1 = dvrkArm('/PSM1')
self.arm2 = dvrkArm('/PSM2')
if self.optimizer == 'cubic':
# self.motion_opt_2wp = CubicOptimizer_2wp(max_vel=dvrkVar.v_max, max_acc=dvrkVar.a_max,
# t_step=0.01, print_out=False, visualize=False)
self.motion_opt_1wp_PSM1 = CubicOptimizer_1wp(
max_vel=dvrkVar.v_max,
max_acc=dvrkVar.a_max,
t_step=0.01,
print_out=False,
visualize=False)
self.motion_opt_1wp_PSM2 = CubicOptimizer_1wp_(
max_vel=dvrkVar.v_max,
max_acc=dvrkVar.a_max,
t_step=0.01,
print_out=False,
visualize=False)
elif self.optimizer == 'qp':
self.motion_opt = PegMotionOptimizerV2b(max_vel=dvrkVar.v_max,
max_acc=dvrkVar.a_max,
t_step=0.01)
else:
raise ValueError
# Motion variables
self.jaw_opening = np.deg2rad([60, 65]) # PSM1, PSM2
self.jaw_closing = np.deg2rad([0, -10])
self.pos_org1 = [0.060, 0.0, -0.095]
self.rot_org1 = [0.0, 0.0, 0.0, 1.0]
self.jaw_org1 = self.jaw_closing[0]
self.pos_org2 = [-0.060, 0.0, -0.095]
self.rot_org2 = [0.0, 0.0, 0.0, 1.0]
self.jaw_org2 = self.jaw_closing[1]
self.offset_grasp = {
'PSM1': [-0.004, +0.003],
'PSM2': [-0.005, +0.003]
}
self.height_ready = -0.120
self.height_drop = -0.138
self.height_handover = self.height_ready + 0.01
self.height_ready_handover = self.height_handover + 0.013
self.offset_handover = 0.01 # the amount of move to hand off
self.lock = threading.Lock()
def __init__(self):
# motion library
# self.dvrk = dvrkController(comp_hysteresis=True, stop_collision=False)
self.arm1 = dvrkArm('/PSM1')
self.arm2 = dvrkArm('/PSM2')
# Motion variables
self.jaw_opening = np.deg2rad([40, 40]) # PSM1, PSM2
self.jaw_closing = np.deg2rad([0, 0])
self.pos_org1 = [0.060, 0.0, -0.095]
self.rot_org1 = [0.0, 0.0, 0.0, 1.0]
self.jaw_org1 = self.jaw_closing[0]
self.pos_org2 = [-0.060, 0.0, -0.095]
self.rot_org2 = [0.0, 0.0, 0.0, 1.0]
self.jaw_org2 = self.jaw_closing[1]
self.offset_grasp = [-0.005, +0.002]
self.height_ready = -0.120
self.height_drop = -0.133
def __init__(self, use_controller, use_optimization, optimizer, which_arm):
# motion library
self.use_optimization = use_optimization
self.optimizer = optimizer
if use_controller:
self.arm1 = dvrkController(arm_name='/'+which_arm, comp_hysteresis=True)
else:
self.arm1 = dvrkArm(arm_name='/'+which_arm)
if self.optimizer == 'cubic':
self.motion_opt_2wp = CubicOptimizer_2wp(max_vel=dvrkVar.v_max, max_acc=dvrkVar.a_max,
t_step=0.01, print_out=False, visualize=False)
self.motion_opt_1wp = CubicOptimizer_1wp(max_vel=dvrkVar.v_max, max_acc=dvrkVar.a_max,
t_step=0.01, print_out=False, visualize=False)
elif self.optimizer == 'qp':
self.motion_opt = PegMotionOptimizerV2b(max_vel=dvrkVar.v_max, max_acc=dvrkVar.a_max, t_step=0.01, minimize='a')
elif self.optimizer == 'mtsqp':
self.motion_opt = MTSQPMotionOptimizer(dim=6, H=12, t_step=0.1, max_vel=dvrkVar.v_max, max_acc=dvrkVar.a_max, objective='v')
else:
raise ValueError
# Motion variables
if which_arm=='PSM1':
self.pos_org = [0.060, 0.0, -0.095]
self.rot_org = [0.0, 0.0, 0.0, 1.0]
self.jaw_org = [0.0]
elif which_arm=='PSM2':
self.pos_org = [-0.060, 0.0, -0.095]
self.rot_org = [0.0, 0.0, 0.0, 1.0]
self.jaw_org = [0.0]
else:
raise ValueError
self.offset_grasp = [-0.004, +0.003]
self.height_ready = -0.120
self.height_drop = -0.130
self.jaw_opening = [np.deg2rad(65)]
self.jaw_opening_drop = [np.deg2rad(70)]
self.jaw_closing = [np.deg2rad(0)]
self.time_motion = []
self.time_computing = []
def __init__(self):
self.arm1 = dvrkArm('/PSM1')
self.arm2 = dvrkArm('/PSM2')
from FLSpegtransfer.motion.dvrkArm import dvrkArm
from FLSpegtransfer.path import *
import time
import numpy as np
p1 = dvrkArm('/PSM1')
joint1_record = []
try:
time_curr = 0.0
time_st = time.perf_counter()
while time_curr < 50:
time_curr = time.perf_counter() - time_st
joint1 = p1.get_current_joint(wait_callback=True)
time.sleep(0.005)
joint1_record.append(joint1)
print(joint1, time_curr)
finally:
np.save(root+"new_traj", joint1_record)
print("Data is successfully saved")
from FLSpegtransfer.motion.dvrkArm import dvrkArm
from FLSpegtransfer.vision.ZividCapture import ZividCapture
import threading
arm1 = dvrkArm('/PSM1')
arm2 = dvrkArm('/PSM2')
zivid = ZividCapture()
zivid.start()
def run():
while True:
arm1.set_pose(pos=[0.05, 0.0, -0.15], rot=[0.0, 0.0, 0.0, 1.0])
zivid.capture_3Dimage()
arm1.set_pose(pos=[-0.05, 0.0, -0.15], rot=[0.0, 0.0, 0.0, 1.0])
zivid.capture_3Dimage()
arm1.set_pose(pos=[0.05, 0.0, -0.15], rot=[0.0, 0.0, 0.0, 1.0])
zivid.capture_3Dimage()
arm1.set_pose(pos=[-0.05, 0.0, -0.15], rot=[0.0, 0.0, 0.0, 1.0])
zivid.capture_3Dimage()
arm1.set_pose(pos=[0.05, 0.0, -0.15], rot=[0.0, 0.0, 0.0, 1.0])
zivid.capture_3Dimage()
arm1.set_pose(pos=[-0.05, 0.0, -0.15], rot=[0.0, 0.0, 0.0, 1.0])
print("completed_arm1")
th = threading.Thread(target=run)
th.start()
while True:
arm2.set_pose(pos=[0.05, 0.0, -0.15], rot=[0.0, 0.0, 0.0, 1.0])
Trp = Trc.dot(Tcp)
Rrp = Trp[:3, :3]
trp = Trp[:3, -1]
transformed = Rrp.dot(point) + trp
else:
Tcr = np.linalg.inv(Trc)
Tpr = Tpc.dot(Tcr)
Rpr = Tpr[:3, :3]
tpr = Tpr[:3, -1]
transformed = Rpr.dot(point) + tpr
return transformed
which_camera = 'inclined'
which_arm = 'PSM2'
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 = BlockDetection3D(Tpc)
# dvrk = dvrkController(comp_hysteresis=False, stop_collision=False)
zivid = ZividCapture(which_camera=which_camera)
zivid.start()
while True:
# move_origin(dvrk, which_arm)
img_color, _, img_point = zivid.capture_3Dimage(color='BGR')
bd.find_pegs(img_color, img_point)
for i in range(12):
import pdb
pdb.set_trace()
pr = transform_task2robot(bd.pnt_pegs[i] * 0.001, Trc=Trc, Tpc=Tpc)
def __init__(self):
self.arm1 = dvrkArm('/PSM1')
self.arm2 = dvrkArm('/PSM2')
self.dvrk_kin = dvrkKinematics()
