Example #1
0
    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
Example #2
0
def init_dvrk_home():
    # init dvrk objject
    p = dvrk.psm('PSM1')
    print "Initialisation complete, will home the PSM"
    p.home()
    time.sleep(0.25)
    return p
 def __init__(self, robotName):
     self._robot_name = robotName
     self._robot = psm(self._robot_name)
     self._last_axes = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
     self._last_buttons = [0, 0]
     self.previous_mouse_buttons = [0, 0]
     rospy.Subscriber('/spacenav/joy', Joy, self.joy_callback)
Example #4
0
def main():
	rospy.init_node('controller', anonymous=True)
	robot = psm('PSM1')
	rate = rospy.Rate(100) # 10hz
	robot.home()
	position_start = robot.get_current_position()
	safe_pos = PyKDL.Frame( PyKDL.Rotation(PyKDL.Vector(0, 1, 0),
										   PyKDL.Vector(1, 0, 0),
										   PyKDL.Vector(0, 0,-1)), 
							PyKDL.Vector(-0.05,0,-0.10))
	robot.move(safe_pos)
	positions = [1,2,3,4]
	# step_val_x = 0.003 #scan in increments of 3mm
	# step_val_y = 0.006
	# dir = 1

	# for i in range(5):
	# 	for j in range(10):
	# 		robot.dmove(PyKDL.Vector(step_val_x * dir, 0, 0))
	# 	robot.dmove(PyKDL.Vector(0, step_val_y, 0))
	# 	dir *= -1

	for pos in positions:
		robot.move(safe_pos)
		robot.dmove(PyKDL.Vector(0, 0.05, -0.05))
		rate.sleep()
	while not rospy.is_shutdown():
		#print("Running")
		rate.sleep()
Example #5
0
    def __init__(self,
                 world="/world",
                 listener=None,
                 traj_step_t=0.1,
                 max_acc=1,
                 max_vel=1,
                 max_goal_diff=0.02,
                 goal_rotation_weight=0.01,
                 max_q_diff=1e-6):

        # TODO: correct these
        base_link = 'PSM1_psm_base_link'
        end_link = 'PSM1_tool_wrist_sca_ee_link_0'
        planning_group = 'manipulator'

        self.dvrk_arm = dvrk.psm('PSM1')
        self.psm_initialized = False

        rospy.Subscriber("/instructor_marker/feedback",
                         InteractiveMarkerFeedback, self.marker_cbback)
        self.last_marker_frame = PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, 0),
                                             PyKDL.Vector(0, 0, 0))
        self.last_marker_trans = (0, 0, 0)
        self.last_marker_rot = (0, 0, 0, 1)

        super(CostarPSMDriver, self).__init__(base_link,
                                              end_link,
                                              planning_group,
                                              dof=6)
 def __init__(self, robotName):
     self._robot_name = robotName
     self._robot = psm(self._robot_name)
     self._last_axes = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
     self._last_buttons = [0, 0]
     self.previous_mouse_buttons = [0, 0]
     rospy.Subscriber('/spacenav/joy', Joy, self.joy_callback)
def main():
    rospy.init_node('controller', anonymous=True)
    robot = psm('PSM1')
    rate = rospy.Rate(100)  # 10hz
    robot.home()
    position_start = robot.get_current_position()
    safe_pos = PyKDL.Frame(
        PyKDL.Rotation(PyKDL.Vector(0, 1, 0), PyKDL.Vector(1, 0, 0),
                       PyKDL.Vector(0, 0, -1)),
        PyKDL.Vector(0.05, -0.04, -0.13))
    robot.move(safe_pos)

    step_val_x = 0.02
    step_val_y = 0.005
    step_val_z = 0.0001
    dir = 1
    z_dir = 1

    for i in range(7):
        for j in range(18):
            if j > 45:
                z_dir = -1
            robot.dmove(PyKDL.Vector(0, step_val_y * dir, step_val_z * z_dir))
        robot.dmove(PyKDL.Vector(-step_val_x, 0, 0))
        dir *= -1
        z_dir = 1
    print("Scan complete")
Example #8
0
    def __init__(self, robot_name, config_file):
        print("initializing recording for", robot_name)
        print("have a flat surface below the robot")
        self.data = []
        self.tracker = False
        self.info = {}
        # Add checker for directory
        strdate = time.strftime("%Y-%m-%d_%H-%M-%S")
        self.folder = os.path.join("data", "{}_{}".format(robot_name, strdate))
        os.mkdir(self.folder)
        print("Created folder at {}".format(os.path.abspath(self.folder)))

        self.arm = dvrk.psm(robot_name)
        self.home()

        tree = ET.parse(config_file)
        root = tree.getroot()
        xpath_search_results = root.findall("./Robot/Actuator[@ActuatorID='2']"
                                            "/AnalygIn/VoltsToPosSI")
        if len(xpath_search_results) == 1:
            VoltsToPosSI = xpath_search_results[0]
        else:
            print("Error: There must be exactly Actuator 2")
            sys.exit(1)

        current_offset = float(VoltsToPosSI.get("Offset"))
        self.info["Config File"] = config_file
        self.info["Current Offset"] = current_offset
Example #9
0
    def __init__(self, cameraTransform, objPath, scale):
        s = rospy.Service('probe2D', oct_15_demo.srv.Probe2D, self.probe2D)
        # Set up subscribers
        self.forceSub = rospy.Subscriber('/force_sensor_topic',
                                         ForceSensorData, self.forceCb)
        # self.forceSub = rospy.Subscriber('/atinetft/wrench',
        #                                   WrenchStamped, self.forceCb)
        # self.forceSub = rospy.Subscriber('/dvrk/PSM2/wrench_body_current',
        #                                  WrenchStamped, self.forceCb)
        self.organPoseSub = rospy.Subscriber('registration_pose', PoseStamped,
                                             self.poseCb)
        self.roiSub = rospy.Subscriber('stiffness_roi', RegionOfInterest,
                                       self.roiCb)
        self.cameraTransform = cameraTransform
        self.organTransform = None
        self.robot = psm('PSM2')

        self.toolOffset = .02  # distance from pinching axle to center of orange nub

        rate = 1000.0
        # TODO make these values not hard coded
        self.rate = rospy.Rate(rate)  # 1000hz

        self.resolvedRatesConfig = \
        {   'velMin': 2.0/1000,
            'velMax': 30.0/1000,
            'angVelMin': 1.0/180.0*3.14,
            'angVelMax': 60.0/180.0*3.14,
            'tolPos': 0.1/1000.0, # positional tolerance
            'tolRot': 1.0/180.0*3.14, # rotational tolerance
            'velRatio': 1, # the ratio of max velocity error radius to tolarance radius, this value >1
            'rotRatio': 1,
            'dt': 1.0/rate, # this is the time step of the system. 
                            # if rate=1khz, then dt=1.0/1000. However,
                            # we don't know if the reality will be the same as desired rate
        }

        # TODO make these not hard-coded
        self.maxDepth = 0.01  # Meters
        self.maxForce = 800  # Not sure of this?
        self.safeZ = .05  # Safe height above organ in meters
        self.normalDistance = 0.005  # Meters

        self.safeSpot = PyKDL.Frame()
        self.safeSpot.p = PyKDL.Vector(0, 0.00, -0.05)
        self.safeSpot.M = rotationFromVector(PyKDL.Vector(0, 0, -.1))

        self.robot.move(self.safeSpot)
        self.resetZRot()
        self.rate.sleep()
        self.force = None
        self.baselineForce = None

        # Get obj data
        objData = makeTexturedObjData(objPath, scale)
        self.uvToWorldConverter = UVToWorldConverter(objData)

        self.roi = None
        self.stiffness_img_size = 1000  # in pixels
Example #10
0
    def __init__(self, psmName, forceTopic, bufferSize=50):

        rospy.init_node('continuous_palpation', anonymous=True)

        self.f_buffer = deque([], bufferSize)
        self.f_current = np.zeros((3))

        self.trajectory = deque([])

        # Set up subscibers
        self.poseSub = rospy.Subscriber(name='set_continuous_palpation_goal',
                                        data_class=PoseStamped,
                                        callback=self.poseCB,
                                        queue_size=1)
        self.trajSub = rospy.Subscriber(
            name='set_continuous_palpation_trajectory',
            data_class=PoseArray,
            callback=self.poseArrayCB,
            queue_size=1)
        self.forceSub = rospy.Subscriber(name=forceTopic,
                                         data_class=WrenchStamped,
                                         callback=self.forceCB,
                                         queue_size=1)

        # Set up publishers
        self.trajStatusPub = rospy.Publisher(name='trajectory_length',
                                             data_class=UInt32,
                                             queue_size=1)

        self.robot = psm(psmName)

        # TODO make these values not hard coded
        self.rate = rospy.Rate(1000)  # 1000hz
        self.forceProfile = \
        {   'period':0.5, # Seconds
            'amplitude': 0.5, # Newtons
            'fBiasMag': 0.7, # Newtons, biased force magnitude
            'magnitudeMode': 'bias', # 'bias' or 'sine', or 'sine bias'
            'controlDir':'default', # 'default' or 'surf normal'
            'defaultDir':[0.0,0.0,1.0],
            'admittanceGains':[ 100.0/1000,\
                                100.0/1000,\
                                100.0/1000] # force admittance gains, (meter/sec)/Newton
        }
        self.resolvedRatesConfig = \
        {   'velMin': 5.0/1000,
            'velMax': 50.0/1000,
            'angVelMin': 3.0/180.0*3.14,
            'angVelMax': 60.0/180.0*3.14,
            'tolPos': 0.5/1000, # positional tolerance
            'tolRot': 1.0/180*3.14, # rotational tolerance
            'velRatio': 10.0, # the ratio of max velocity error radius to tolarance radius, this value >1
            'rotRatio': 5.0,
            'dt': 1.0/1000, # this is the time step of the system. 
                            # if rate=1khz, then dt=1.0/1000. However,
                            # we don't know if the reality will be the same as desired rate
        }
        self.run()
Example #11
0
 def configure(self, robot_name):
     print rospy.get_caller_id(
     ), ' -> configuring dvrk_psm_test for ', robot_name
     self.arm = dvrk.psm(robot_name)
     self.markerArray = MarkerArray()
     self.publisher = rospy.Publisher('/visualization_marker_array',
                                      MarkerArray,
                                      queue_size=100)
     rospy.sleep(2)
Example #12
0
    def __init__(self,psm_name, totaltime_s,num_wayPoints,joints,joint_limits, Ts):
        # create the psm
	self.statSubscriber = rospy.Subscriber(psm_name+'/recState',String,self.__subCallback)
        self.recState = ''
        self.arm = dvrk.psm(psm_name)       
        self.time = totaltime_s
        self.num_wayPoints = num_wayPoints
        self.joints = joints
        self.joint_limits = joint_limits
	self.Ts = Ts
Example #13
0
def init_dvrk_mc():
    # init dvrk objject
    p = dvrk.psm('PSM1')
    # init MC objects
    moveit_commander.roscpp_initialize(sys.argv)
    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()
    group_name = "psm_arm"
    group = moveit_commander.MoveGroupCommander(group_name)
    return p,group
 def jigsaw_move_dvrk_arm(self):
     # rospy.init_node('move_dvrk_arm')
     p = dvrk.psm('PSM1')
     p.home()
     # Create a Python proxy for PSM1, name must match ros namespace
     # p.move(0.001, 0)
     tip_pos_x, tip_pos_y, tip_pos_z, frame_val = jigsaw_data_parser()
     for i in range(0, 4):
         p.move(
             PyKDL.Vector(0.5 * tip_pos_x[i], 0.5 * tip_pos_y[i],
                          0.5 * tip_pos_z[i]))
         print(tip_pos_x[i], tip_pos_y[i], tip_pos_z[i])
Example #15
0
def main(psmName):
    rospy.init_node('dvrk_registration', anonymous=True)
    robot = psm(psmName)

    frameRate = 15
    slop = 1.0 / frameRate
    cams = StereoCameras("left/image_rect",
                         "right/image_rect",
                         "left/camera_info",
                         "right/camera_info",
                         slop=slop)

    tfSync = CameraSync(
        '/stereo/left/camera_info',
        topics=['/dvrk/' + psmName + '/position_cartesian_current'],
        frames=[
            psmName + '_psm_base_link', psmName + '_tool_wrist_link',
            psmName + '_tool_wrist_caudier_link_shaft'
        ])

    camModel = StereoCameraModel()
    topicLeft = rospy.resolve_name("left/camera_info")
    msgL = rospy.wait_for_message(topicLeft, CameraInfo, 10)
    topicRight = rospy.resolve_name("right/camera_info")
    msgR = rospy.wait_for_message(topicRight, CameraInfo, 10)
    camModel.fromCameraInfo(msgL, msgR)

    # Set up GUI
    filePath = rospy.get_param('~registration_yaml')
    print(filePath)
    with open(filePath, 'r') as f:
        data = yaml.load(f)
    if any(k not in data
           for k in ['H', 'minS', 'minV', 'maxV', 'transform', 'points']):

        rospy.logfatal('dVRK Registration: ' + filePath +
                       ' empty or malformed.')
        quit()

    cv2.namedWindow(_WINDOW_NAME)

    transformOld = np.array(data['transform'])

    toolOffset = data[
        'toolOffset']  # distance from pinching axle to center of orange nub
    points = np.array(
        data['points'])  # Set of points in robots frame to register against

    # Wait for registration to start
    displayRegistration(cams, camModel, toolOffset, transformOld, tfSync)

    print('Done')
    cv2.destroyAllWindows()
Example #16
0
def main():
    rospy.init_node('controller', anonymous=True)
    robot = psm('PSM1')
    rate = rospy.Rate(100)  # 10hz
    robot.home()
    position_start = robot.get_current_position()
    safe_pos = PyKDL.Frame(
        PyKDL.Rotation(PyKDL.Vector(0, 1, 0), PyKDL.Vector(1, 0, 0),
                       PyKDL.Vector(0, 0, -1)), PyKDL.Vector(0, 0, -0.1135))
    robot.move(safe_pos)
    positions = [1, 2, 3, 4]
    for pos in positions:
        robot.move(safe_pos)
        robot.dmove(PyKDL.Vector(0, 0.05, -0.05))
        rate.sleep()
Example #17
0
    def dvrk_data_write_to_file_single_arm(self):
        rospy.init_node('read_write_data_dvrk')
        rate = rospy.Rate(self.loop_rate)
        # Create a Python proxy for PSM1, name must match ros namespace
        p = dvrk.psm('PSM1')
        # p.move_joint_one(0.05, 0)
        # Location of file storage
        # data_file_dir = "/home/aimlabx/PycharmProjects/dvrk_automated_suturing/data/dvrk_joint_data.csv"
        csv = open(self.file_dir, "a")
        # Initialization of rpy and pos
        current_rpy = np.zeros(3)
        current_pos = np.zeros(3)
        # For writing the heading to the csv file
        if self.file_heading_exits == 0:
            p.home()
            column_title = "Rot R, Rot P, Rot Y, Pos X, Pos Y, Pos Z \n"
            csv.write(column_title)
            self.file_heading_exits = 1
        # While loop to read and write data continuously
        while not rospy.is_shutdown():
            # Read joint positions and angles of dvrk arm
            current_pose = p.get_current_position()
            current_rpy = current_pose.M.GetRPY()
            current_pos = current_pose.p
            # print("\nThe current joint position is ", current_pos, "\n")
            # print("\nThe current joint position is ", current_vel, "\n")

            # Initialize the row data with joint 0 position
            row_data = str(current_rpy[0])
            # Write the data to file
            # For writing each end effector angle value
            for i in range(1, 3):
                row_data += "," + str(current_rpy[i])
                # print("\n I value is ", i, "\n")
                if i == 2:
                    # For writing each end effector position value
                    row_data += "," + str(current_pos[0])
                    print "Row data is ", row_data
                    for j in range(1, 3):
                        row_data += "," + str(current_pos[j])
                        # New set of data starts in a new line
                        if j == 2:
                            row_data = row_data + "\n"
            # Finally write the data in row_data to the csv file
            csv.write(row_data)
            # Decides at what rate the data is written to the csv file
            rate.sleep()
Example #18
0
 def __init__(self,
              cam,
              camTransform,
              dvrkName,
              forceTopic,
              draw="bar",
              masterWidget=None,
              parent=None):
     super(ForceOverlayWidget, self).__init__(cam, parent=parent)
     self.masterWidget = masterWidget
     if self.masterWidget == None:
         self.robot = psm(dvrkName)
     else:
         self.robot = self.masterWidget.robot
     self.cameraTransform = camTransform
     self.drawType = draw
     rospy.Subscriber(forceTopic, WrenchStamped, self.forceCB)
Example #19
0
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()
Example #20
0
    def __init__(self, startp, endp):

        self.organPoseSub = rospy.Subscriber('registration_pose', PoseStamped,
                                             self.poseCb)
        self.organTransform = None
        self.robot = psm('PSM2')

        self.toolOffset = .02  # distance from pinching axle to center of orange nub

        rate = 1000.0
        # TODO make these values not hard coded
        self.rate = rospy.Rate(rate)  # 1000hz

        self.resolvedRatesConfig = \
        {   'velMin': 2.0/1000,
            'velMax': 30.0/1000,
            'angVelMin': 1.0/180.0*3.14,
            'angVelMax': 60.0/180.0*3.14,
            'tolPos': 0.1/1000.0, # positional tolerance
            'tolRot': 1.0/180.0*3.14, # rotational tolerance
            'velRatio': 1, # the ratio of max velocity error radius to tolarance radius, this value >1
            'rotRatio': 1,
            'dt': 1.0/rate, # this is the time step of the system. 
                            # if rate=1khz, then dt=1.0/1000. However,
                            # we don't know if the reality will be the same as desired rate
        }

        # TODO make these not hard-coded
        self.maxDepth = 0.01  # Meters
        self.maxForce = 800  # Not sure of this?
        self.safeZ = .05  # Safe height above organ in meters
        self.normalDistance = 0.005  # Meters

        self.safeSpot = PyKDL.Frame()
        self.safeSpot.p = startp
        self.safeSpot.M = fingertipConstraint(startp)

        self.scanEndSpot = PyKDL.Frame()
        self.scanEndSpot.p = endp
        self.scanEndSpot.M = fingertipConstraint(endp)

        self.robot.move(self.safeSpot)
        self.resetZRot()
        self.rate.sleep()
        self.robot.move(self.scanEndSpot)
Example #21
0
    def dvrk_data_write_to_file_two_arms(self):
        # rospy.init_node('read_write_data_dvrk')
        rate = rospy.Rate(self.loop_rate)
        # Create a Python proxy for PSM1, name must match ros namespace
        p = dvrk.psm('PSM1')
        # p.move_joint_one(0.05, 0)
        # Location of file storage
        # data_file_dir = "/home/aimlabx/PycharmProjects/dvrk_automated_suturing/data/dvrk_joint_data.csv"
        csv = open(self.file_dir, "a")
        # For writing the heading to the csv file
        if self.file_heading_exits == 0:
            p.home()
            column_title = "Pos Joint 1, Pos Joint 2, Pos Joint 3, Pos Joint 4, Pos Joint 5, Pos Joint 6, " \
                           "Vel Joint 1, Vel Joint 2, Vel Joint 3, Vel Joint 4, Vel Joint 5, Vel Joint 6 \n"
            csv.write(column_title)
            self.file_heading_exits = 1
        # While loop to read and write data continuously
        while not rospy.is_shutdown():
            # Read joint positions of dvrk arm
            current_pos = p.get_current_joint_position()
            # Read joint velocities of dvrk arm
            current_vel = p.get_current_joint_velocity()
            # print("\nThe current joint position is ", current_pos, "\n")
            # print("\nThe current joint position is ", current_vel, "\n")

            # Initialize the row data with joint 0 position
            row_data = str(current_pos[0])
            # Write the data to file
            # For writing each joint position value
            for i in range(1, 6):
                row_data += "," + str(current_pos[i])
                # print("\n I value is ", i, "\n")
                if i == 5:
                    # For writing each joint velocity value
                    row_data += "," + str(current_vel[0])
                    for j in range(1, 6):
                        row_data += "," + str(current_vel[j])
                        # New set of data starts in a new line
                        if j == 5:
                            row_data = row_data + "\n"
            # Finally write the data in row_data to the csv file
            csv.write(row_data)
            # Decides at what rate the data is written to the csv file
            rate.sleep()
def main():
	p=dvrk.psm('PSM1')

	rate = rospy.Rate(250) 
	p.home()
	
	#Home robot to zero 
	p.dmove_joint(np.array([0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0]))
	
	
	#parameters of the identification task 
	rospack = rospkg.RosPack()
	
	fileloc = rospack.get_path(dvrk_si) + '/test/6dof_31par_test1'
	filename = fileloc +'Test.csv'
	q= genfromtxt(filename, delimiter=',')
	qt= q.transpose()

 	q_output=[None]*len(q[1][:])
Example #23
0
    def __init__(self, names):
        self.name = names
        self.pos = np.zeros(3)
        self.rot = np.zeros(4)
        self.arm_rot = np.zeros(4)
        self.marker_data_pos = np.zeros((1, 3))
        self.marker_data_rot = np.zeros((1, 4))

        print(self.name)
        if self.name[0:3] == 'ECM':
            self.interface = dvrk.ecm(self.name[0])
        else:
            self.interface = dvrk.psm(self.name[0])

        rospy.Subscriber(
            '/vrpn_client_node/RigidBody' + str(self.name[1]) + '/pose',
            gm.PoseStamped, self.handle_rotation)
        rospy.Subscriber(
            '/vrpn_client_node/RigidBody' + str(self.name[2]) + '/pose',
            gm.PoseStamped, self.handle_rcm)
Example #24
0
    def __init__(self,
                 name="psm1",
                 world="/world",
                 listener=None,
                 traj_step_t=0.1,
                 max_acc=1,
                 max_vel=1,
                 max_goal_diff=0.02,
                 goal_rotation_weight=0.01,
                 max_q_diff=1e-6):

        # TODO: correct these
        base_link = 'PSM1_psm_base_link'
        end_link = 'PSM1_tool_tip_link'
        planning_group = 'manipulator'

        self.dvrk_arm = dvrk.psm('PSM1')

        super(CostarPSMDriver, self).__init__(base_link,
                                              end_link,
                                              planning_group,
                                              dof=6)
Example #25
0
    def configure(self, robot_name, config_file, expected_interval):
        self.expected_interval = expected_interval
        self.config_file = config_file
        # check that the config file is good
        if not os.path.exists(self.config_file):
            sys.exit("Config file \"{%s}\" not found".format(self.config_file))

        # XML parsing, find current offset
        self.tree = ET.parse(config_file)
        root = self.tree.getroot()

        # find Robot in config file and make sure name matches
        xpath_search_results = root.findall("./Robot")
        if len(xpath_search_results) == 1:
            xmlRobot = xpath_search_results[0]
        else:
            sys.exit("Can't find \"Robot\" in configuration file {:s}".format(
                self.config_file))

        if xmlRobot.get("Name") == robot_name:
            serial_number = xmlRobot.get("SN")
            print(
                "Successfully found robot \"{:s}\", serial number {:s} in XML file"
                .format(robot_name, serial_number))
            robotFound = True
        else:
            sys.exit(
                "Found robot \"{:s}\" while looking for \"{:s}\", make sure you're using the correct configuration file!"
                .format(xmlRobot.get("Name"), robot_name))

        # now find the offset for joint 2, we assume there's only one result
        xpath_search_results = root.findall(
            "./Robot/Actuator[@ActuatorID='2']/AnalogIn/VoltsToPosSI")
        self.xmlPot = xpath_search_results[0]
        print("Potentiometer offset for joint 2 is currently: {:s}".format(
            self.xmlPot.get("Offset")))

        self.arm = dvrk.psm(arm_name=robot_name,
                            expected_interval=expected_interval)
Example #26
0
    def __init__(self,
            world="/world",
            listener=None,
            traj_step_t=0.1,
            max_acc=1,
            max_vel=1,
            max_goal_diff = 0.02,
            goal_rotation_weight = 0.01,
            max_q_diff = 1e-6):

        # TODO: correct these
        base_link = 'PSM1_psm_base_link'
        end_link = 'PSM1_tool_wrist_sca_ee_link_0'
        planning_group = 'manipulator'

        self.dvrk_arm = dvrk.psm('PSM1')
        self.psm_initialized = False

        rospy.Subscriber("/instructor_marker/feedback", InteractiveMarkerFeedback, self.marker_cbback)
        self.last_marker_frame = PyKDL.Frame(PyKDL.Rotation.RPY(0,0,0),PyKDL.Vector(0,0,0))
        self.last_marker_trans = (0,0,0)
        self.last_marker_rot = (0,0,0,1)

        super(CostarPSMDriver, self).__init__(base_link,end_link,planning_group, dof=6)
    def __init__(self, stlScale = 0.001, parent = None):
        QtWidgets.QMainWindow.__init__(self, parent)
 
        self.frame = QtWidgets.QFrame()
 
        self.vl = QtWidgets.QVBoxLayout()
        self.vtkWidget = QVTKRenderWindowInteractor(self.frame)
        self.vl.addWidget(self.vtkWidget)
 
        self.ren = vtk.vtkRenderer()
        self.vtkWidget.GetRenderWindow().AddRenderer(self.ren)
        self.iren = self.vtkWidget.GetRenderWindow().GetInteractor()
 
        # Create sphere
        source = vtk.vtkSphereSource()
        source.SetCenter(0, 0, 0)
        source.SetRadius(0.003)
        sphereMapper = vtk.vtkPolyDataMapper()
        sphereMapper.SetInputConnection(source.GetOutputPort())
        self.sphereActor = vtk.vtkActor()
        self.sphereActor.SetMapper(sphereMapper)
        self.sphereActor.GetProperty().SetColor(1, 0, 0)
        self.ren.AddActor(self.sphereActor)
 
        # Read in STL
        reader = vtk.vtkSTLReader()
        reader.SetFileName('/home/biomed/october_15_ws/src/dvrk_vision/defaults/femur.stl')
        scaler = vtk.vtkTransformFilter()
        if vtk.VTK_MAJOR_VERSION <= 5:
            scaler.SetInputConnection(reader.GetOutput())
        else:
            scaler.SetInputConnection(reader.GetOutputPort())
        scaleTransform = vtk.vtkTransform()
        scaleTransform.Identity()
        scaleTransform.Scale(stlScale, stlScale, stlScale)
        scaler.SetTransform(scaleTransform)
        # Create a mapper
        mapper = vtk.vtkPolyDataMapper()
        if vtk.VTK_MAJOR_VERSION <= 5:
            mapper.SetInput(scaler.GetOutput())
        else:
            mapper.SetInputConnection(scaler.GetOutputPort())
        # Create an actor
        self.actor = vtk.vtkActor()
        self.actor.SetMapper(mapper)

        # Read in camera registration
        scriptDirectory = os.path.dirname(os.path.abspath(__file__))
        filePath = os.path.join(scriptDirectory, '..', '..', 'defaults', 
                                'registration_params.yaml')
        with open(filePath, 'r') as f:
            data = yaml.load(f)

        self.camTransform = np.array(data['transform'])
 
        # Add point cloud
        self.pointCloud = VtkPointCloud()
        tf = np.linalg.inv(self.camTransform)
        transform = vtk.vtkTransform()
        transform.SetMatrix(tf.ravel())
        self.pointCloud.vtkActor.SetPosition(transform.GetPosition())
        self.pointCloud.vtkActor.SetOrientation(transform.GetOrientation())
        self.ren.AddActor(self.pointCloud.vtkActor)
 
        self.ren.AddActor(self.actor)

        poseSub = rospy.Subscriber("/stereo/registration_pose", PoseStamped, self.poseCallback)
 
        self.ren.ResetCamera()
 
        self.frame.setLayout(self.vl)
        self.setCentralWidget(self.frame)

        self.vtkThread = QThread()
        
        self.actorMat = np.eye(4)

        self.thread = QThread()
        robot = psm('PSM2')
        self.worker = Worker(robot)
        self.worker.intReady.connect(self.onIntReady)
        self.worker.moveToThread(self.thread)
        self.worker.finished.connect(self.thread.quit)
        self.thread.started.connect(self.worker.procCounter)
        self.thread.start()

        self.started = False
 
        self.show()
        self.iren.Initialize()

def pykdl_frame2str(frame):
    (rx, ry, rz, rw) = frame.M.GetQuaternion()


    message1 = "Position (x,y,z): {: 0.8f}, {: 0.8f}, {: 0.8f}  Orientation(rx,ry,rz,rw): {: 0.8f}, {: 0.8f}, {: 0.8f}, {: 0.8f}"\
          .format(frame.p.x(), frame.p.y(), frame.p.z(), rx,ry,rz,rw)
    return message1


if __name__ == "__main__":

    sleep_time = 1

    psm3 = dvrk.psm('PSM3')
    time.sleep(0.3)
    psm3.home()

    current_pose = psm3.get_current_position()
    initial_orientation = current_pose.M
    print("Psm3 current position")
    print(pykdl_frame2str(current_pose))

    answer = raw_input(
        "position the psm3 in a safe place and write 'Y' to continue. ")
    if answer != 'Y':
        print("Exiting the program")
        exit(0)
    else:
        print("Start moving arm 5 times")
 def configure(self, robot_name):
     # configuration
     print(rospy.get_caller_id(), ' -> configuring dvrk_arm_test for ', robot_name)
     self.arm = dvrk.psm(robot_name)
     self.arm.close_jaw()
Example #30
0
psm3.pos()
psm3.master()

#Getting the actual transformation matrix instead of the quaternion, or an error message
transform_obj.trasformation(nodo_obj.left_tf)

T_c_p0 = np.dot(
    psm3.T_c_l,
    np.dot(transform_obj.T_l_o,
           np.dot(psm3.T_o_pb,
                  psm3.T_pb_p0)))  #transformation from fulcrum to camera frame
T_p0_c = np.linalg.inv(T_c_p0)  #transformation from camera frame to fulcrum
#T_o_p0 = np.dot(psm3.T_o_pb, psm3.T_pb_p0) # I do not need it at the moment, it is the transform from the fulcrum to the point o on the checkerboard
#T_c_p0= np.dot(self.T_c_l*self.T_o_pb) #I do not need it

psm3 = dvrk.psm(psm3.psmName)  #this is a dvrk API class from psm.py
#psm1= dvrk.psm('PSM1')  # this is the first arm
#psm2= dvrk.psm('PSM2')  # this is the second arm

#############modifica per non rompere il robot un altra volta.
sleep(3)  #to see if a can publish the base frame now

#psm3.set_joint_velocity_ratio(0.2)
#psm3.set_joint_acceleration_ratio(0.2)

#############

psm3.set_base_frame(
    T_c_p0
)  #this is the function I've Implemented to publish the base frame, but before you have to run the broadcaster in broadcaster_monel ( not sure, cannot verify it with the robot disconnected)
# plt.plot(q[:,0])
# plt.show()
print (q.shape)

a = q

print("data shape: {}".format(a.shape))

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
Example #32
0
 def configure(self, robot_name, expected_interval):
     print_id('configuring dvrk_psm_test for %s' % robot_name)
     self.expected_interval = expected_interval
     self.arm = dvrk.psm(arm_name=robot_name,
                         expected_interval=expected_interval)
Example #33
0
 def configure(self, robot_name):
     print(rospy.get_caller_id(), ' -> configuring dvrk_psm_test for ', robot_name)
     self.arm = dvrk.psm(robot_name)
    def run(self, calibrate, filename):
        nb_joint_positions = 20 # number of positions between limits
        nb_samples_per_position = 500 # number of values collected at each position
        total_samples = nb_joint_positions * nb_samples_per_position
        samples_so_far = 0

        sleep_time_after_motion = 0.5 # time after motion from position to position to allow potentiometers to stabilize
        sleep_time_between_samples = 0.01 # time between two samples read (potentiometers)

        encoders = []
        potentiometers = []
        range_of_motion_joint = []

        average_encoder = [] # all encoder values collected at a sample position used for averaging the values of that position
        average_potentiometer = [] # all potentiometer values collected at a sample position used for averaging the values of that position
        d2r = math.pi / 180.0 # degrees to radians
        r2d = 180.0 / math.pi # radians to degrees

        slopes = []
        offsets = []
        average_offsets = []

        # Looking in XML assuming following tree structure
        # config > Robot> Actuator > AnalogIn > VoltsToPosSI > Scale = ____   or   Offset = ____
        xmlVoltsToPosSI = {}

        tree = ET.parse(filename)
        root = tree.getroot()
        robotFound = False
        stuffInRoot = root.getchildren()
        for index in range(len(stuffInRoot)):
            if stuffInRoot[index].tag == "Robot":
                currentRobot = stuffInRoot[index]
                if currentRobot.attrib["Name"] == self._robot_name:
                    self._serial_number = currentRobot.attrib["SN"]
                    xmlRobot = currentRobot
                    print("Succesfully found robot \"", currentRobot.attrib["Name"], "\", Serial number: ", self._serial_number, " in XML file")
                    robotFound = True
                else:
                    print("Found robot \"", currentRobot.attrib["Name"], "\", while looking for \"", self._robot_name, "\"")

        if robotFound == False:
            sys.exit("Robot tree could not be found in xml file")

        # look for all VoltsToPosSI
        stuffInRobot = xmlRobot.getchildren()
        for index in range(len(stuffInRobot)):
            child = stuffInRobot[index]
            if child.tag == "Actuator":
                actuatorId = int(child.attrib["ActuatorID"])
                stuffInActuator = child.getchildren()
                for subIndex in range(len(stuffInActuator)):
                    subChild = stuffInActuator[subIndex]
                    if subChild.tag == "AnalogIn":
                        stuffInAnalogIn = subChild.getchildren()
                        for subSubIndex in range(len(stuffInAnalogIn)):
                            subSubChild = stuffInAnalogIn[subSubIndex]
                            if subSubChild.tag == "VoltsToPosSI":
                                xmlVoltsToPosSI[actuatorId] = subSubChild

        # set joint limits and number of axis based on arm type, using robot name
        if ("").join(list(currentRobot.attrib["Name"])[:-1]) == "PSM": #checks to see if the robot being tested is a PSM
            arm_type = "PSM"
            lower_joint_limits = [-60.0 * d2r, -30.0 * d2r, 0.005, -170.0 * d2r, -170.0 * d2r, -170.0 * d2r, -170.0 * d2r]
            upper_joint_limits = [ 60.0 * d2r,  30.0 * d2r, 0.235,  170.0 * d2r,  170.0 * d2r,  170.0 * d2r,  170.0 * d2r]
            nb_axis = 7 #number of joints being tested
        elif currentRobot.attrib["Name"] == "MTML":
            arm_type = "MTM"
            lower_joint_limits = [-15.0 * d2r, -10.0 * d2r, -10.0 * d2r, -180.0 * d2r, -80.0 * d2r, -40.0 * d2r, -100.0 * d2r]
            upper_joint_limits = [ 35.0 * d2r,  20.0 * d2r,  10.0 * d2r,   80.0 * d2r, 160.0 * d2r,  40.0 * d2r,  100.0 * d2r]
            nb_axis = 7
        elif currentRobot.attrib["Name"] == "MTMR":
            arm_type = "MTM"
            lower_joint_limits = [-30.0 * d2r, -10.0 * d2r, -10.0 * d2r,  -80.0 * d2r, -80.0 * d2r, -40.0 * d2r, -100.0 * d2r]
            upper_joint_limits = [ 15.0 * d2r,  20.0 * d2r,  10.0 * d2r,  180.0 * d2r, 160.0 * d2r,  40.0 * d2r,  100.0 * d2r]
            nb_axis = 7
        elif currentRobot.attrib["Name"] == "ECM":
            arm_type = "ECM"
            lower_joint_limits = [-60.0 * d2r, -40.0 * d2r,  0.005, -80.0 * d2r]
            upper_joint_limits = [ 60.0 * d2r,  40.0 * d2r,  0.230,  80.0 * d2r]
            nb_axis = 4


        if arm_type == "PSM":
            this_arm = dvrk.psm(self._robot_name)
        else:
            this_arm = dvrk.arm(self._robot_name)


        # resize all arrays
        for axis in range(nb_axis):
            encoders.append([])
            offsets.append([])
            potentiometers.append([])
            average_encoder.append([])
            average_offsets.append([])
            average_potentiometer.append([])
            range_of_motion_joint.append(math.fabs(upper_joint_limits[axis] - lower_joint_limits[axis]))

        # Check that everything is working
        time.sleep(2.0) # to make sure some data has arrived
        if not self._data_received:
            print("It seems the console for ", self._robot_name, " is not started or is not publishing the IO topics")
            print("Make sure you use \"rosrun dvrk_robot dvrk_console_json\" with the -i option")
            sys.exit("Start the dvrk_console_json with the proper options first")

        print("The serial number found in the XML file is: ", self._serial_number)
        print("Make sure the dvrk_console_json is using the same configuration file.  Serial number can be found in GUI tab \"IO\".")
        ok = input("Press `c` and [enter] to continue\n")
        if ok != "c":
            sys.exit("Quitting")

        ######## scale calibration
        now = datetime.datetime.now()
        now_string = now.strftime("%Y-%m-%d-%H:%M")

        if calibrate == "scales":

            print("Calibrating scales using encoders as reference")

            # write all values to csv file
            csv_file_name = 'pot_calib_scales_' + self._robot_name + '-' + self._serial_number + '-' + now_string + '.csv'
            print("Values will be saved in: ", csv_file_name)
            f = open(csv_file_name, 'wb')
            writer = csv.writer(f)
            header = []
            for axis in range(nb_axis):
                header.append("potentiometer" + str(axis))
            for axis in range(nb_axis):
                header.append("encoder" + str(axis))
            writer.writerow(header)

            # messages
            input("To start with some initial values, you first need to \"home\" the robot.  When homed, press [enter]\n")

            if arm_type == "PSM":
                input("Since you are calibrating a PSM, make sure there is no tool inserted.  Please remove tool or calibration plate if any and press [enter]\n")
            if arm_type == "ECM":
                input("Since you are calibrating an ECM, remove the endoscope and press [enter]\n")
            input("The robot will make LARGE MOVEMENTS, please hit [enter] to continue once it is safe to proceed\n")

            for position in range(nb_joint_positions):
                # create joint goal
                joint_goal = []
                for axis in range(nb_axis):
                    joint_goal.append(lower_joint_limits[axis] + position * (range_of_motion_joint[axis] / nb_joint_positions))
                    average_encoder[axis] = []
                    average_potentiometer[axis] = []

                # move and sleep
                if arm_type == "PSM":
                    this_arm.move_jaw(joint_goal[6], blocking = False)
                    this_arm.move_joint(numpy.array(joint_goal[0:6]))
                else:
                    this_arm.move_joint(numpy.array(joint_goal))
                time.sleep(sleep_time_after_motion)

                # collect nb_samples_per_position at current position to compute average
                for sample in range(nb_samples_per_position):
                    for axis in range(nb_axis):
                        average_potentiometer[axis].append(self._last_potentiometers[axis])
                        average_encoder[axis].append(self._last_joints[axis])
                    # log data
                    writer.writerow(self._last_potentiometers + self._last_joints)
                    time.sleep(sleep_time_between_samples)
                    samples_so_far = samples_so_far + 1
                    sys.stdout.write('\rProgress %02.1f%%' % (float(samples_so_far) / float(total_samples) * 100.0))
                    sys.stdout.flush()

                # compute averages
                for axis in range(nb_axis):
                    potentiometers[axis].append(math.fsum(average_potentiometer[axis]) / nb_samples_per_position)
                    encoders[axis].append(math.fsum(average_encoder[axis]) / nb_samples_per_position)


            # at the end, return to home position
            if arm_type == "PSM":
                this_arm.move_jaw(0.0, blocking = False)
                this_arm.move_joint(numpy.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]))
            elif arm_type == "MTM":
                this_arm.move_joint(numpy.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]))
            elif arm_type == "ECM":
                this_arm.move_joint(numpy.array([0.0, 0.0, 0.0, 0.0]))

            # close file
            f.close()


        ######## offset calibration
        if calibrate == "offsets":

            print("Calibrating offsets")

            # write all values to csv file
            csv_file_name = 'pot_calib_offsets_' + self._robot_name + '-' + self._serial_number + '-' + now_string + '.csv'
            print("Values will be saved in: ", csv_file_name)
            f = open(csv_file_name, 'wb')
            writer = csv.writer(f)
            header = []
            for axis in range(nb_axis):
                header.append("potentiometer" + str(axis))
            writer.writerow(header)

            # messages
            print("Please home AND power off the robot first.  Then hold/clamp your arm in zero position.")
            if arm_type == "PSM":
                print("For a PSM, you need to hold at least the last 4 joints in zero position.  If you don't have a way to constrain the first 3 joints, you can still just calibrate the last 4.  This program will ask you later if you want to save all PSM joint offsets");
            input("Press [enter] to continue\n")
            nb_samples = 10 * nb_samples_per_position
            for sample in range(nb_samples):
                for axis in range(nb_axis):
                    average_offsets[axis].append(self._last_potentiometers[axis] * r2d)
                writer.writerow(self._last_potentiometers)
                time.sleep(sleep_time_between_samples)
                sys.stdout.write('\rProgress %02.1f%%' % (float(sample) / float(nb_samples) * 100.0))
                sys.stdout.flush()
            for axis in range(nb_axis):
                offsets[axis] = (math.fsum(average_offsets[axis]) / (nb_samples) )

        print("")


        if calibrate == "scales":
            print("index | old scale  | new scale  | correction")
            for index in range(nb_axis):
                # find existing values
                oldScale = float(xmlVoltsToPosSI[index].attrib["Scale"])
                # compute new values
                correction = slope(encoders[index], potentiometers[index])
                newScale = oldScale / correction

                # display
                print(" %d    | % 04.6f | % 04.6f | % 04.6f" % (index, oldScale, newScale, correction))
                # replace values
                xmlVoltsToPosSI[index].attrib["Scale"] = str(newScale)

        if calibrate == "offsets":
            newOffsets = []
            print("index | old offset  | new offset | correction")
            for index in range(nb_axis):
                # find existing values
                oldOffset = float(xmlVoltsToPosSI[index].attrib["Offset"])
                # compute new values
                newOffsets.append(oldOffset - offsets[index])

                # display
                print(" %d    | % 04.6f | % 04.6f | % 04.6f " % (index, oldOffset, newOffsets[index], offsets[index]))

            if arm_type == "PSM":
                all = input("Do you want to save all joint offsets or just the last 4, press 'a' followed by [enter] to save all\n");
                if all == "a":
                    print("This program will save ALL new PSM offsets")
                    for axis in range(nb_axis):
                        # replace values
                        xmlVoltsToPosSI[axis].attrib["Offset"] = str(newOffsets[axis])
                else:
                    print("This program will only save the last 4 PSM offsets")
                    for axis in range(3, nb_axis):
                        # replace values
                        xmlVoltsToPosSI[axis].attrib["Offset"] = str(newOffsets[axis])
            else:
                for axis in range(nb_axis):
                    # replace values
                    xmlVoltsToPosSI[axis].attrib["Offset"] = str(newOffsets[axis])

        save = input("To save this in new file press 'y' followed by [enter]\n")
        if save == "y":
            tree.write(filename + "-new")
            print('Results saved in', filename + '-new. Restart your dVRK application with the new file!')
            print('To copy the new file over the existing one: cp', filename + '-new', filename)
qt = q.transpose()

times = 1
speedscale=0.5
scale = 1

if times ==1: 
	a = q;
else:	
	a = np.zeros((len(q[:][1]),len(q[1][:])*times))

	#Make twice the trajectory
	for i in range(len(qt[1][:])):
		a[i][:]=np.append(q[i][:],q[i][:])

p=dvrk.psm('PSM2')
r=rospy.Rate(200*speedscale)
p.home()

period_data = len(a[0][:])
data_cycle = len(a)


states = np.zeros((len(a)*len(a[1][:]),3*3))

j = 0
while j<data_cycle and not rospy.is_shutdown():

	p.move_joint_some(np.array([0, 0, scale*a[j][0], 0, 0, 0]),np.array([0,1,2,3,4,5]))

	#p.move_joint_some(np.array([scale*a[0][0], scale*a[1][0], scale*a[2][0]]),np.array([0,1,2]))
qt = q.transpose()

times = 1
speedscale=0.5
scale = 1

if times ==1: 
	a = q;
else:	
	a = np.zeros((len(q[:][1]),len(q[1][:])*times))

	#Make twice the trajectory
	for i in range(len(qt[1][:])):
		a[i][:]=np.append(q[i][:],q[i][:])

p=dvrk.psm('PSM1')
r=rospy.Rate(200*speedscale)
p.home()

period_data = len(a)
data_cycle = len(a)


states = np.zeros((len(a),3*3))

j = 0
while j<1 and not rospy.is_shutdown():

	p.move_joint_some(np.array([0, 0, scale*a[0], 0, 0, 0]),np.array([0,1,2,3,4,5]))

	#p.move_joint_some(np.array([scale*a[0][0], scale*a[1][0], scale*a[2][0]]),np.array([0,1,2]))