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)