def __init__(self, subj_data):
self.p2 = dvrk.psm('PSM2')
self.m2 = dvrk.mtm('MTMR')
self.m2.set_wrench_body_orientation_absolute(True)
self.c = dvrk.console()
self.robot_state = False # initialize the flag that helps with switch the robot state
filename = 'Subj' + str(subj_data[0])
if subj_data[1] == 0:
filename = filename + '_nohaptics'
elif subj_data[1] == 1:
filename = filename + '_haptics'
else:
filename = filename + '_manual'
if subj_data[2] == 0:
filename = filename + '_train'
else:
filename = filename + '_test'
if subj_data[3] == 0:
filename = filename + '_ef50'
elif subj_data[3] == 1:
filename = filename + '_ds10'
elif subj_data[3] == 2:
filename = filename + '_ef30'
else:
filename = filename + '_ds30'
self.name = filename
def __init__(self, subj_data):
self.c = dvrk.console()
filename = 'Subj' + str(subj_data[0])
if subj_data[1] == 0:
filename = filename + '_nohaptics'
elif subj_data[1] == 1:
filename = filename + '_haptics'
else:
filename = filename + '_manual'
if subj_data[2] == 0:
filename = filename + '_train'
else:
filename = filename + '_test'
if subj_data[3] == 0:
filename = filename + '_ef50'
elif subj_data[3] == 1:
filename = filename + '_ds10'
elif subj_data[3] == 2:
filename = filename + '_ef30'
else:
filename = filename + '_ds30'
self.name = filename
self.action_complete = False
def main(screen):
screen.nodelay(1)
# this sets the increment for our displacement
delta = 0.001
total_translation = 0.0
translation = PyKDL.Vector(0.0,0.0,delta)
'''define the waypoint positions for the PSMs for this load test'''
cart1 = PyKDL.Vector(0.116,-0.090,-0.083)
rot1 = PyKDL.Rotation()
rot1 = rot1.Quaternion(0.694,0.0113,0.719,-0.005)
pos1 = PyKDL.Frame(rot1,cart1)
p2 = dvrk.psm('PSM2')
c = dvrk.console()
# set our rate to 30hz
rate = rospy.Rate(30)
print 'homing to position...'
c.teleop_stop()
#p2.open_jaw()
#p2.move(pos1)
p2.close_jaw()
while True:
if screen.getch() == ord('w'):
p2.dmove(translation)
total_translation = total_translation + delta
if screen.getch() == ord('d'):
print 'displacement is: '
print total_translation
if screen.getch() == ord('q'):
print 'quitting'
break
c.teleop_start()
x = frame.p.x()
y = frame.p.y()
z = frame.p.z()
q1,q2,q3,q4 = frame.M.GetQuaternion()
output = (x,y,z,q1,q2,q3,q4)
return output
""" MTM home rough position """
''' We use this to initialize a position for the MTMR'''
MTMR_cart = PyKDL.Vector(0.055288515671, -0.0508310176185, -0.0659661913251)
MTMR_rot = PyKDL.Rotation()
MTMR_rot = MTMR_rot.Quaternion(0.750403138242, -0.0111643539824, 0.657383142871, -0.0679550644629)
MTMR_pos = PyKDL.Frame(MTMR_rot, MTMR_cart)
c = dvrk.console()
p2 = dvrk.psm('PSM2')
mtm = dvrk.mtm('MTMR')
print('initializing approximate MTMR position')
mtm.move(MTMR_pos)
c.teleop_start()
sub = rospy.Subscriber('/dvrk/footpedals/camera', Joy, trigger_callback)
trigger = False
trigger_time = 0
filename = './manipulator_homing/'
print("initialized recording")
print("step on GO pedal to record PSM and MTM position")
rate = rospy.Rate(1000)
time_init = rospy.get_time()