def __init__(self, ): self.arm_kinematic = self.ArmKinematic('PSM3') #python api self.api_psm3_arm = dvrk.arm('PSM3') #tf self.tf_world_psm3b = None #subscribers self.psm3_cartesian_subs = rospy.Subscriber( "/dvrk/PSM3/position_cartesian_current", PoseStamped, self.psm3_cartesian_callback) self.psm1_suj_subs = rospy.Subscriber("/dvrk/PSM1/io/suj_clutch", Joy, self.setup_button_psm1_callback) #tf self.tf_world_psm3b_subs = rospy.Subscriber( "/pu_dvrk_tf/tf_world_psm3b", PoseStamped, self.tf_world_psm3b_callback) #psm offset self.fix_orientation = PyKDL.Rotation.Quaternion( 0.20611444, -0.10502740, 0.60974223, 0.75809003) #Upload homography between pixels and robot x,y self.homography = None with open("./vision_modules/misc/homography_data/homography.json", "r") as f1: self.homography = np.array(json.load(f1)) print("Retrieved homography") print(self.homography)
def __init__(self, robotName): self._robot_name = robotName self._robot = arm(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 __init__(self, ): #python api self.api_psm3_arm = dvrk.arm('PSM3') self.dvrk_controller = dvrk_tf_module(userId="test", dst_path='./videos', rig_name="pu_dvrk_cam") #tf self.tf_world_psm3b = PyKDL.Frame() #subscribers self.psm1_suj_subs = rospy.Subscriber("/dvrk/PSM1/io/suj_clutch", Joy, self.setup_button_psm1_callback) #tf self.tf_world_psm3b_subs = rospy.Subscriber( "/pu_dvrk_tf/tf_world_psm3b", PoseStamped, self.tf_world_psm3b_callback) self.ar_orientation = PyKDL.Rotation.Quaternion( 0.68175651, -0.27654879, 0.56096521, 0.37953506) self.dvrk_controller.ar_orientation = self.ar_orientation self.fix_orientation = PyKDL.Rotation.Quaternion( 0.20611444, -0.10502740, 0.60974223, 0.75809003) self.base = Vector(*[0.23937060, -0.09208578, 0.05523316]) self.position1 = Vector(*[0.27518547, -0.18335637, 0.06959790]) self.position2 = Vector(*[0.23203641, -0.18565913, 0.06422155]) self.position3 = Vector(*[0.26621665, -0.11688039, 0.07422218]) self.position4 = Vector(*[0.21614252, -0.10163200, 0.07084345])
def __init__(self, ): self.arm_kinematic = self.ArmKinematic('PSM3') #python api self.api_psm3_arm = dvrk.arm('PSM3') #tf self.tf_world_psm3b = None #subscribers self.psm3_cartesian_subs = rospy.Subscriber( "/dvrk/PSM3/position_cartesian_current", PoseStamped, self.psm3_cartesian_callback) self.psm1_suj_subs = rospy.Subscriber("/dvrk/PSM1/io/suj_clutch", Joy, self.setup_button_psm1_callback) #tf self.tf_world_psm3b_subs = rospy.Subscriber( "/pu_dvrk_tf/tf_world_psm3b", PoseStamped, self.tf_world_psm3b_callback) #psm offset self.offset = PyKDL.Vector(-0.009, -0.021, +0.069) self.fix_orientation = PyKDL.Rotation.Quaternion( 0.20611444, -0.10502740, 0.60974223, 0.75809003)
def __init__(self, arm_name, camera_model, tf_listener): rospy.loginfo('Configuring motion controller for ' + arm_name) self.arm = dvrk.arm(arm_name) self.arm_name = arm_name self.arm_base = arm_name + "_base" self.camera_model = camera_model self.tf_listener = tf_listener self.position_threshold = 1e-5 self.joint_threshold = 3 * math.pi / 180
def __init__(self, ): self.api_arm = dvrk.arm('MTML') #tf self.tf_mtmlb_mtml = None #subscribers self.tf_subs = rospy.Subscriber( "/dvrk/MTML/position_cartesian_current", PoseStamped, self.tf_callback)
def main(): sleep_time = 1 psm3_arm = dvrk.arm('PSM3') psm3_arm.home() # #Set up the velocity and acceleration ratio of the psm3 # ratio = 0.01 # velocity_ratio_pub = rospy.Publisher("/dvrk/PSM3/set_joint_velocity_ratio",Float64,latch=True, queue_size=1) # acceleration_ratio_pub = rospy.Publisher("/dvrk/PSM3/set_joint_acceleration_ratio",Float64,latch=True, queue_size=1) # velocity_ratio_pub.publish(Float64(ratio)) # acceleration_ratio_pub.publish(Float64(ratio)) answer = raw_input( "Make sure the PSM3 will not crash with the programmed movements and write 'Y' to continue. " ) if answer != 'Y': print("Exiting the program") exit(0) for i in range(2): square_trajectory = [[[0.21134809, -0.12155905, 0.10276267], [0.26790356, -0.12201603, 0.10604522]], [[0.26790356, -0.12201603, 0.10604522], [0.21134809, -0.12155905, 0.10276267]], [[0.21134809, -0.12155905, 0.10276267], [0.27185202, -0.14776761, 0.09245916]], [[0.27185202, -0.14776761, 0.09245916], [0.21329086, -0.14354401, 0.08516887]], [[0.21329086, -0.14354401, 0.08516887], [0.21134809, -0.12155905, 0.10276267]]] for init_pt, end_pt in square_trajectory: init_pt = np.array(init_pt) end_pt = np.array(end_pt) trajectory = generate_trajectory(init_pt, end_pt, n=2) for idx, c in enumerate(range(trajectory.shape[1])): next_position = PyKDL.Vector( trajectory[0, c], trajectory[1, c], trajectory[2, c], ) print("position {:}".format(idx), next_position) psm3_arm.move(PyKDL.Vector(next_position))
def __init__(self, ): self.arm_kinematic = self.ArmKinematic('PSM3') #python api self.api_psm3_arm = dvrk.arm('PSM3') #tf self.tf_world_psm3b = PyKDL.Frame() #subscribers self.psm3_cartesian_subs = rospy.Subscriber( "/dvrk/PSM3/position_cartesian_current", PoseStamped, self.psm3_cartesian_callback) self.psm1_suj_subs = rospy.Subscriber("/dvrk/PSM1/io/suj_clutch", Joy, self.setup_button_psm1_callback) #tf self.tf_world_psm3b_subs = rospy.Subscriber( "/pu_dvrk_tf/tf_world_psm3b", PoseStamped, self.tf_world_psm3b_callback)
def __init__(self, ): self.arm_kinematic = self.ArmKinematic('PSM3') #python api self.api_psm3_arm = dvrk.arm('PSM3') #subscribers self.psm3_cartesian_subs = rospy.Subscriber( "/dvrk/PSM3/position_cartesian_current", PoseStamped, self.psm3_cartesian_callback) self.psm1_suj_subs = rospy.Subscriber("/dvrk/PSM1/io/suj_clutch", Joy, self.setup_button_psm1_callback) self.tf_world_psm3b_pub = rospy.Publisher("/pu_dvrk_tf/tf_world_psm3b", PoseStamped, queue_size=5) #publisher self.multi_arr_pub = rospy.Publisher( "/pu_dvrk_tf/centroid_coordinates", Int32MultiArray, queue_size=5)
def configure(self, robot_name): print(rospy.get_caller_id(), ' -> configuring dvrk_arm_test for ', robot_name) self.arm = dvrk.arm(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) d2r = math.pi / 180.0 # degrees to radians r2d = 180.0 / math.pi # radians to degrees # 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) # 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 = raw_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") # messages # raw_input("To start with some initial values, you first need to \"home\" the robot. When homed, press [enter]\n") # if arm_type == "PSM": # raw_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": # raw_input("Since you are calibrating an ECM, remove the endoscope and press [enter]\n") # raw_input("The robot will make LARGE MOVEMENTS, please hit [enter] to continue once it is safe to proceed\n") # move to the home position from the start # this_arm.home() 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])) time.sleep(2.0) print(this_arm.get_current_position()) print(PyKDL.Vector(0.0, 0.05, 0.0)) this_arm.move(PyKDL.Vector(0.0, 0.0, -0.05)) print(this_arm.get_current_position()) print(this_arm.get_desired_position()) # if arm_type == "PSM": # this_arm.move_jaw(30.0*d2r, blocking = False) # this_arm.move_joint(numpy.array([30.0*d2r, 15.0*d2r, 0.2, 30.0*d2r, 30.0*d2r, 30.0*d2r])) # 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])) # time.sleep(1.0) # # at the end, return to home position # if arm_type == "PSM": # this_arm.move_jaw(0.0*d2r, blocking = False) # this_arm.move_joint(numpy.array([0.0*d2r, 0.0*d2r, 0.0, 0.0*d2r, 0.0*d2r, 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])) ## Main ## # if __name__ == '__main__': # if (len(sys.argv) != 4): # print sys.argv[0] + ' requires three arguments, i.e. "scales"/"offsets", name of dVRK arm and file name. Always start with scales calibration.' # else: # if (sys.argv[1] == 'offsets') or (sys.argv[1] == 'scales'): # calibration = potentiometer_calibration(sys.argv[2]) # calibration.run(sys.argv[1], sys.argv[3]) # else: # print sys.argv[0] + ', first argument must be either scales or offsets. You must start with the scale calibration'
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 = {} # Make sure file exists if not os.path.exists(filename): sys.exit("Config file \"%s\" not found" % filename) tree = ET.parse(filename) root = 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") if xmlRobot.get("Name") == self._robot_name: self._serial_number = xmlRobot.get("SN") print( "Successfully found robot \"%s\", serial number %s in XML file" % (self._robot_name, self._serial_number)) robotFound = True else: sys.exit( "Found robot \"%s\" while looking for \"%s\", make sure you're using the correct configuration file!" % (xmlRobot.get("Name"), self._robot_name)) # Look for all actuators/VoltsToPosSI xpath_search_results = root.findall("./Robot/Actuator") for actuator in xpath_search_results: actuatorId = int(actuator.get("ActuatorID")) voltsToPosSI = actuator.find("./AnalogIn/VoltsToPosSI") xmlVoltsToPosSI[actuatorId] = voltsToPosSI # set joint limits and number of axis based on arm type, using robot name if ("").join( list(self._robot_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 elif self._robot_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 self._robot_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 self._robot_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 # Create the dVRK python ROS client 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 %s is not started or is not publishing the IO topics" % self._robot_name) 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: %s" % 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: %s" % csv_file_name) f = open(csv_file_name, 'w') 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 this_arm.move_jp(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" or arm_type == "MTM": this_arm.move_jp( numpy.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])) elif arm_type == "ECM": this_arm.move_jp(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, 'w') 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 = 2 * 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].get("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].set("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].get("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].set("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].set("Offset", str(newOffsets[axis])) else: for axis in range(nb_axis): # replace values xmlVoltsToPosSI[axis].set("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 %s-new. Restart your dVRK application with the new file!' % filename) print('To copy the new file over the existing one: cp %s-new %s' % (filename, filename))
def __init__(self, activate_ar=False, activate_debug_visuals=False): #python api -- This code initializes the ros node self.api_psm3_arm = dvrk.arm('PSM3') #visual controller self.dvrk_controller = dvrk_tf_module( userId="test", dst_path='./videos', rig_name="pu_dvrk_cam", activate_ar=activate_ar, activate_debug_visuals=activate_debug_visuals) #Ar config self.ar_orientation = PyKDL.Rotation.Quaternion( 0.68175651, -0.27654879, 0.56096521, 0.37953506) self.dvrk_controller.ar_orientation = self.ar_orientation # self.arm_kinematic = self.ArmKinematic('PSM3') #tf self.tf_world_psm3b = None #frames self.bridge = CvBridge() self.right_frame = None ############### ##subscribers## ############### self.psm3_cartesian_subs = rospy.Subscriber( "/dvrk/PSM3/position_cartesian_current", PoseStamped, self.psm3_cartesian_callback) self.psm1_suj_subs = rospy.Subscriber("/dvrk/PSM1/io/suj_clutch", Joy, self.setup_button_psm1_callback) self.camera_subs = rospy.Subscriber("/pu_dvrk_cam/right/inverted", Image, self.camera_callback) #tf self.tf_world_psm3b_subs = rospy.Subscriber( "/pu_dvrk_tf/tf_world_psm3b", PoseStamped, self.tf_world_psm3b_callback) ############## ##Publishers## ############## self.multi_arr_pub = rospy.Publisher( "/pu_dvrk_tf/centroid_coordinates", Int32MultiArray, queue_size=5) #psm offset self.fix_orientation = PyKDL.Rotation.Quaternion( 0.35131810, -0.12377625, 0.53801977, 0.75616781) #Upload homography between pixels and robot x,y self.homography = None with open("./vision_modules/misc/homography_data/homography.json", "r") as f1: self.homography = np.array(json.load(f1)) print("Retrieved homography") print(self.homography)
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)
#!/usr/bin/env python import dvrk import numpy as np import time #DVRK interface does not allow to instantiate multiple arms at the same time. # Move PSM1 to the initial position mtml = dvrk.arm('MTML') mtml.home() mtml_joint_pos = [ 0.16094356, 0.18617774, -0.13542208, -1.70923487, 1.67276075, -0.0769766,-1.01845961] print('mtml current') print(mtml.get_current_joint_position()) print('mtml next') print(mtml_joint_pos) mtml.move_joint(np.array([mtml_joint_pos]))
'--arm', type=str, required=True, choices=['ECM', 'MTML', 'MTMR', 'PSM1', 'PSM2', 'PSM3'], help= 'arm name corresponding to ROS topics without namespace. Use __ns:= to specify the namespace' ) parser.add_argument( '-i', '--interval', type=float, default=0.01, help='expected interval in seconds between messages sent by the device') args = parser.parse_args(argv[1:]) # skip argv[0], script name arm = dvrk.arm(arm_name=args.arm, expected_interval=args.interval) print('starting move_jp') # get current position initial_joint_position = numpy.copy(arm.setpoint_jp()) amplitude = math.radians(10.0) goal = numpy.copy(initial_joint_position) print('--> Testing the trajectory with wait()') start_time = time.time() # first motion goal[0] = initial_joint_position[0] + amplitude arm.move_jp(goal).wait()
def configure(self, robot_name, expected_interval): print_id('configuring dvrk_arm_test for %s' % robot_name) self.expected_interval = expected_interval self.arm = dvrk.arm(arm_name = robot_name, expected_interval = expected_interval)
#!/usr/bin/env python import dvrk import numpy as np import time #DVRK interface does not allow to instantiate multiple arms at the same time. # Move PSM1 to the initial position mtmr = dvrk.arm('MTMR') mtmr.home() mtmr_joint_pos = [ -0.14427202, 0.18954733, -0.15922543, 1.67041871, 1.68164266, 0.03256702, 0.95332556 ] print('mtmr current') print(mtmr.get_current_joint_position()) print('mtmr next') print(mtmr_joint_pos) mtmr.move_joint(np.array([mtmr_joint_pos]))