def configure(self, robot_name, expected_interval):
print_id('configuring dvrk_mtm_test for %s' % robot_name)
self.expected_interval = expected_interval
self.arm = dvrk.mtm(arm_name=robot_name,
expected_interval=expected_interval)
self.coag_event = threading.Event()
rospy.Subscriber('footpedals/coag', Joy, self.coag_event_cb)
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 configure(self, robot_name):
print(rospy.get_caller_id(), ' -> configuring dvrk_arm_test for ', robot_name)
self.arm = dvrk.mtm(robot_name)
self.coag_event = threading.Event()
rospy.Subscriber('/dvrk/footpedals/coag',
Joy, self.coag_event_cb)
self.set_gains_pub = rospy.Publisher(self.arm._arm__full_ros_namespace + '/set_cartesian_impedance_gains',
prmCartesianImpedanceGains, latch = True, queue_size = 1)
def configure(self, robot_name):
print(rospy.get_caller_id(), ' -> configuring dvrk_arm_test for ', robot_name)
self.arm = dvrk.mtm(robot_name)
self.coag_event = threading.Event()
rospy.Subscriber('/dvrk/footpedals/coag',
Joy, self.coag_event_cb)
self.set_gains_pub = rospy.Publisher(self.arm._arm__full_ros_namespace + '/set_cartesian_impedance_gains',
prmCartesianImpedanceGains, latch = True, queue_size = 1)
def configure(self, robot_name, expected_interval):
print_id('configuring dvrk_mtm_cartesian_impedance for %s' %
robot_name)
self.expected_interval = expected_interval
self.arm = dvrk.mtm(arm_name=robot_name,
expected_interval=expected_interval)
self.coag_event = threading.Event()
rospy.Subscriber('footpedals/coag', Joy, self.coag_event_cb)
self.set_gains_pub = rospy.Publisher(
self.arm._arm__full_ros_namespace +
'/set_cartesian_impedance_gains',
prmCartesianImpedanceGains,
latch=True,
queue_size=1)
def __init__(self, arm_name):
self._arm_name = arm_name
mtml = mtm('MTML')
mtmr = mtm('MTMR')
self._base_offset = Frame()
self._tip_offset = Frame()
self._adjusted_pose = Frame()
self._adjusted_pose_pre = None
self._scale = 1000
if arm_name == 'MTMR':
self._base_offset = Frame(Rotation.RPY(3.0397, -1.0422, -1.477115),
Vector(-0.182, -0.016, -0.262))
self._tip_offset = Frame(Rotation.RPY(-1.57079, 0 , -1.57079),
Vector(0,0,0))
elif arm_name == 'MTML':
self._base_offset = Frame(Rotation.RPY(3.0397, -1.0422, -1.477115),
Vector(0.182, -0.016, -0.262))
self._tip_offset = Frame(Rotation.RPY(-1.57079, 0 , -1.57079),
Vector(0, 0, 0))
else:
assert (arm_name == 'MTML' and arm_name == 'MTMR')
mtml.home()
mtmr.home()
mtml.set_wrench_body_orientation_absolute(True)
mtmr.set_wrench_body_orientation_absolute(True)
mtml.set_wrench_body_force((0, 0, 0))
mtmr.set_wrench_body_force((0, 0, 0))
self._pose = PoseStamped()
self._frame = Frame()
self._pose_sub = rospy.Subscriber('/dvrk/' + arm_name + '/position_cartesian_current', PoseStamped, self.pose_cb)
def configure(self, robot_name):
print(rospy.get_caller_id(), ' -> configuring dvrk_arm_test for ', robot_name)
self.arm = dvrk.mtm(robot_name)
self.coag_event = threading.Event()
rospy.Subscriber('/dvrk/footpedals/coag',
Joy, self.coag_event_cb)
print(dof) b = copy.deepcopy(a) is_psm = robotname[0:3] == 'PSM' if is_psm: p = dvrk.psm(robotname) for i in range(a.shape[0]): b[i, 4] = 1.0186 * a[i, 4] b[i, 5] = -0.8306 * a[i, 4] + 0.6089 * a[i, 5] + 0.6089 * a[i, 6] b[i, 6] = -1.2177 * a[i, 5] +1.2177 * a[i, 6] elif robotname[0:3] == 'MTM': p = dvrk.mtm(robotname) for i in range(a.shape[0]): b[i, 2] = -a[i, 1] + a[i, 2] b[i, 3] = 0.6697 * a[i, 1] - 0.6697 * a[i, 2] + a[i, 3] a = b * scales p.home() rospy.sleep(3) # Home to start of trajectory based on CSV if is_psm and dof == 7:
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()
while not rospy.is_shutdown():
motor2dvrk_psm = np.array([[1.0186, 0, 0], [-.8306, .6089, .6089],
[0, -1.2177, 1.2177]])
motor2dvrk_mtm = np.array([[1.0, 0, 0], [-1.0, 1.0, 0], [0.6697, -0.6697,
1.0]])
# wait for a short period of time before recording data
sampling_rate = 200
speed = 0.4
model_folder = 'data/' + model_name + '/model/'
robot_model = load_data(model_folder, model_name)
trajectory_folder = 'data/' + model_name + '/optimal_trajectory/'
p = dvrk.mtm(robot_name)
p.home()
rospy.sleep(3)
jonits_array = np.array([d for d in range(7)])
q_start = np.deg2rad(-85)
q_end = np.deg2rad(195)
q_trajectory = np.linspace(q_start,
q_end,
num=int(sampling_rate * (q_end - q_start) / speed))
states = np.zeros((q_trajectory.shape[0] * 2, 3))
q = np.array([0., np.deg2rad(2.5), np.deg2rad(2.5), q_start, 0., 0., 0.])
pub_topic = '/dvrk/' + MTM_ARM + '/set_effort_joint'
sub_topic = '/dvrk/' + MTM_ARM + '/state_joint_current'
use_net = 'SinNet'
D = 5
device = 'cpu'
model = get_model('MTM', use_net, D, device=device)
model, input_scaler, output_scaler = load_model('.','test_Controller',model)
model = model.to('cpu')
pub = rospy.Publisher(pub_topic, JointState, queue_size=15)
rospy.init_node(MTM_ARM + 'controller', anonymous=True)
rate = rospy.Rate(10) # 10hz
mtm_arm = dvrk.mtm(MTM_ARM)
count = 0
def callback(data):
global use_net
global D
global input_scaler
global output_scaler
global pub
global count
global model
start = time.clock()
MTMR_rot = MTMR_rot.Quaternion(0.7232, 0.0306, 0.6875, 0.05687)
MTMR_pos = PyKDL.Frame(MTMR_rot, MTMR_cart)
""" PSM home position """
PSM_cart = PyKDL.Vector(0.1429, -0.06655, -0.09587)
PSM_rot = PyKDL.Rotation()
PSM_rot = PSM_rot.Quaternion(0.652, -0.113558, 0.7487, 0.01616)
PSM_pos = PyKDL.Frame(PSM_rot, PSM_cart)
# Declare a list that holds our reference forces
training_forces = [1, 1.5, 2, 2.75, 4]
test_forces_ds = np.array(
[1, 1, 1, 1.5, 1.5, 1.5, 2, 2, 2, 2.75, 2.75, 2.75, 4, 4, 4])
# declare our PSM and MTMs
p2 = dvrk.psm('PSM2')
m2 = dvrk.mtm('MTMR')
c = dvrk.console()
# set our rate to 30hz
rate = rospy.Rate(30)
# create the subscriber to check the footpedal
sub = rospy.Subscriber('/dvrk/footpedals/camera', Joy, trigger_callback)
force_sub = rospy.Subscriber('/force_sensor', Wrench, haptic_feedback)
# initialize a flag to denote end of test trials
trigger = False # create a variable to store the trigger boolean
flag_next = False # create a flag variable for the trigger
exiter = False
filename = raw_input("Please key in subject name: ")
def configure(self, robot_name):
print(rospy.get_caller_id(), ' -> configuring dvrk_arm_test for ', robot_name)
self.arm = dvrk.mtm(robot_name)
self.coag_event = threading.Event()
rospy.Subscriber('/dvrk/footpedals/coag',
Joy, self.coag_event_cb)
