sr_gui_controller_tuner: sr_controller

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2011 Shadow Robot Company Ltd.
00004 #
00005 # This program is free software: you can redistribute it and/or modify it
00006 # under the terms of the GNU General Public License as published by the Free
00007 # Software Foundation, either version 2 of the License, or (at your option)
00008 # any later version.
00009 #
00010 # This program is distributed in the hope that it will be useful, but WITHOUT
00011 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00012 # FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
00013 # more details.
00014 #
00015 # You should have received a copy of the GNU General Public License along
00016 # with this program.  If not, see <http://www.gnu.org/licenses/>.
00017 #
00018 
00019 import rospy
00020 import re
00021 
00022 from xml.etree import ElementTree as ET
00023 from controller_manager_msgs.srv import ListControllers
00024 
00025 from sr_robot_msgs.srv import ForceController, SetEffortControllerGains, SetMixedPositionVelocityPidGains, SetPidGains
00026 from sr_gui_controller_tuner.pid_loader_and_saver import PidLoader, PidSaver
00027 
00028 
00029 class CtrlSettings(object):
00030 
00031     """
00032     Parses xml file and reads controller settings
00033     Creates lists for headers, fingers, motors
00034     """
00035 
00036     def __init__(self, xml_path, controller_type, joint_prefix):
00037         self.headers = []
00038 
00039         # open and parses the xml config file
00040         xml_file = open(xml_path)
00041         xml_tree = ET.parse(xml_file)
00042         xml_file.close()
00043 
00044         # read the settings from the xml file
00045         ctrl_tree = None
00046         for ctrl in xml_tree.findall("controller"):
00047             ctrl_name = ctrl.attrib['name']
00048             if ctrl_name == controller_type:
00049                 ctrl_tree = ctrl
00050                 break
00051 
00052         if ctrl_tree is None:
00053             rospy.logerr(
00054                 "Couldn't find the settings for the controller " + controller_type)
00055 
00056         # read the headers settings
00057         xml_headers = ctrl_tree.find("headers")
00058         for header in xml_headers.findall("item"):
00059             self.headers.append(header.attrib)
00060 
00061         self.nb_columns = len(self.headers)
00062 
00063         self.hand_item = ["Hand"]
00064         self.hand_item.extend((self.nb_columns - 1) * [""])
00065 
00066         # read the fingers and the motors from the xml file
00067         self.fingers = []
00068         self.motors = []
00069         all_fingers = xml_tree.find("fingers")
00070         for finger in all_fingers.findall("finger"):
00071             finger_row = [finger.attrib['name']]
00072             finger_row.extend((self.nb_columns - 1) * [""])
00073             self.fingers.append(finger_row)
00074 
00075             motors_for_finger = []
00076             for motor in finger.findall("motor"):
00077                 motor_row = ["", joint_prefix + motor.attrib['name']]
00078                 motor_row.extend((self.nb_columns - 1) * [""])
00079                 motors_for_finger.append(motor_row)
00080 
00081             self.motors.append(motors_for_finger)
00082 
00083 
00084 class SrControllerTunerApp(object):
00085 
00086     """
00087     Handles loading, saving and setting of controller settings
00088     """
00089     CONTROLLER_MANAGER_DETECTION_TIMEOUT = 3.0
00090 
00091     def __init__(self, xml_path):
00092         self.xml_path = xml_path
00093         self.all_controller_types = ["Motor Force", "Position", "Velocity",
00094                                      "Mixed Position/Velocity", "Effort", "Muscle Position"]
00095         self.pid_loader = PidLoader()
00096 
00097         self.edit_only_mode = False
00098         self.control_mode = "FORCE"
00099         # global prefix
00100 
00101         # prefix used in shadow controllers
00102         self.controller_prefix = "sh_"
00103 
00104         # both prefix and selected_prefix are stored
00105         # to handle case when gui is started in a namespace, then prefix
00106         # must be set to empty but selected prefix is still necessary
00107         # to select which joint_prefix to choose
00108         self.selected_prefix = ""
00109         self.prefix = ""
00110         self.joint_prefix = ""
00111         # store if one or two loops are running
00112         self.single_loop = False
00113 
00114         self.namespace = rospy.get_namespace()
00115 
00116     def check_prefix(self):
00117         """
00118         Get the prefix (hand and joint)
00119         Check if it matches selected prefix
00120         """
00121 
00122         hand_ids = []
00123         hand_joint_prefixes = []
00124         # unless incompatible, use the selected prefix by default
00125         self.prefix = self.selected_prefix
00126         prefix = self.selected_prefix.rstrip("/")
00127         # mapping is always in global ns
00128         if rospy.has_param("/hand/mapping"):
00129             hand_mapping = rospy.get_param("/hand/mapping")
00130             for _, value in hand_mapping.items():
00131                 # if prefix matches the mapping, add this hand (empty prefix
00132                 # means both hands)
00133                 if value.startswith(prefix):
00134                     hand_ids.append(value)
00135         if len(hand_ids) == 0:
00136             # no matching hand id to selected prefix, check for namespace
00137             if prefix in self.namespace:
00138                 rospy.loginfo("Using namespace, no prefix")
00139                 # in this case no prefix is needed
00140                 self.prefix = ""
00141             else:
00142                 rospy.logwarn("Prefix not matching namespace")
00143                 return False
00144 
00145         if len(hand_ids) > 1:
00146             # the plugin cannot handle more than one hand
00147             rospy.logwarn(
00148                 "More than one hand found with prefix :" + prefix + " !\n Not loading controllers")
00149             return False
00150 
00151         # joint_prefix always in global ns
00152         if rospy.has_param("/hand/joint_prefix"):
00153             hand_joint_prefix_mapping = rospy.get_param("/hand/joint_prefix")
00154             for _, value in hand_joint_prefix_mapping.items():
00155                 # if prefix matches the mapping, add this joint prefix
00156                 if prefix in value:
00157                     hand_joint_prefixes.append(value)
00158 
00159         if len(hand_joint_prefixes) > 1:
00160             # the plugin cannot handle more than one hand
00161             rospy.logwarn(
00162                 "More than one hand found with prefix :" + prefix + " !\n Not loading controllers")
00163             return False
00164 
00165         if len(hand_joint_prefixes) == 0:
00166             rospy.logwarn("No hand found with prefix :" + prefix)
00167             self.joint_prefix = ""
00168         else:
00169             self.joint_prefix = hand_joint_prefixes[0]
00170 
00171         rospy.loginfo("using joint_prefix " + self.joint_prefix)
00172         return True
00173 
00174     def get_ctrls(self):
00175         """
00176         Retrieve currently running controllers
00177         return ["Motor Force", "Position"]
00178         """
00179         running_ctrls = []
00180 
00181         # find controller manager in selected namespace
00182 
00183         ctrl_srv_name = self.prefix + 'controller_manager/list_controllers'
00184         try:
00185             rospy.wait_for_service(
00186                 ctrl_srv_name, self.CONTROLLER_MANAGER_DETECTION_TIMEOUT)
00187 
00188         except rospy.ROSException:
00189             # try at root namespace (only in case bimanual setup in a single
00190             # loop and only if no GUI ns)
00191             if self.namespace == "/":
00192                 ctrl_srv_name = 'controller_manager/list_controllers'
00193                 try:
00194                     rospy.wait_for_service(
00195                         ctrl_srv_name, self.CONTROLLER_MANAGER_DETECTION_TIMEOUT)
00196                     self.single_loop = True
00197                     rospy.loginfo("Detected single loop")
00198                 except rospy.ROSException, e:
00199                     rospy.loginfo(
00200                         "Controller manager not running: %s" % str(e))
00201                     rospy.loginfo("Running controller tuner in edit-only mode")
00202                     return self.set_edit_only(running_ctrls)
00203             else:
00204                 return self.set_edit_only(running_ctrls)
00205 
00206         # found a controller manager
00207         controllers = rospy.ServiceProxy(ctrl_srv_name, ListControllers)
00208         resp = None
00209         try:
00210             resp = controllers()
00211         except rospy.ServiceException, e:
00212             rospy.logerr("Service did not process request: %s" % str(e))
00213 
00214         running_ctrls.append("Motor Force")
00215         if resp is not None:
00216             for controller in resp.controller:
00217                 if controller.state == "running":
00218                     # find at the specific pattern of the controller
00219                     splitted = re.split(
00220                         '[tfmrlw][fhr]j[0-5]_', controller.name)
00221                     # only consider shadow (prefix sh_) controllers (drop js
00222                     # ctrl and others)
00223                     if self.controller_prefix in splitted[0]:
00224                         ctrl_type_tmp = ""
00225                         # only consider joint controllers (containing _xxjy_)
00226                         if len(splitted) >= 2:
00227                             ctrl_type_tmp = splitted[1]
00228                         # look at first word of the controller type
00229                         ctrl_type_tmp_splitted = ctrl_type_tmp.split("_")
00230                         for defined_ctrl_type in self.all_controller_types:
00231                             if ctrl_type_tmp_splitted[0].lower() in defined_ctrl_type.lower():
00232                                 running_ctrls.append(defined_ctrl_type)
00233                                 self.edit_only_mode = False
00234                                 return running_ctrls
00235 
00236         rospy.loginfo("No controllers currently running")
00237         rospy.loginfo("Running controller tuner in edit-only mode")
00238         del running_ctrls[:]
00239         return self.set_edit_only(running_ctrls)
00240 
00241     def set_edit_only(self, running_ctrls):
00242         """
00243         Sets all the controllers to defined type for edit-only mode
00244         """
00245         self.edit_only_mode = True
00246         # In edit_only_mode all the controllers are available for editing
00247         for defined_ctrl_type in self.all_controller_types:
00248             running_ctrls.append(defined_ctrl_type)
00249         return running_ctrls
00250 
00251     def refresh_control_mode(self):
00252         """
00253         Effectively change control mode on the realtime loop
00254         """
00255         self.control_mode = rospy.get_param(
00256             'realtime_loop/' + self.prefix + 'default_control_mode', 'FORCE')
00257 
00258     def get_controller_settings(self, controller_type):
00259         """
00260         Parses a file containing the controller settings
00261         and their min and max values, and returns them.
00262         """
00263         ctrl_settings = CtrlSettings(
00264             self.xml_path, controller_type, self.joint_prefix)
00265 
00266         return ctrl_settings
00267 
00268     def load_parameters(self, controller_type, joint_name):
00269         """
00270         Load the parameters from the yaml file.
00271         """
00272         param_name = ""
00273         prefix = self.prefix if self.single_loop is not True else ""
00274         if controller_type == "Motor Force":
00275             # currently the motor_board topics use non-prefixed joint names
00276             # no matter if single or dual loops there is always a prefix for
00277             # motors
00278             param_name = self.prefix + \
00279                 joint_name.strip(
00280                     self.joint_prefix.rstrip("/") + "_").lower() + "/pid"
00281         elif controller_type == "Position":
00282             param_name = prefix + self.controller_prefix + \
00283                 joint_name.lower() + "_position_controller/pid"
00284         elif controller_type == "Muscle Position":
00285             param_name = prefix + self.controller_prefix + \
00286                 joint_name.lower() + "_muscle_position_controller/pid"
00287         elif controller_type == "Velocity":
00288             param_name = prefix + self.controller_prefix + \
00289                 joint_name.lower() + "_velocity_controller/pid"
00290         elif controller_type == "Mixed Position/Velocity":
00291             param_name = [prefix + self.controller_prefix + joint_name.lower() +
00292                           "_mixed_position_velocity_controller/position_pid",
00293                           prefix + self.controller_prefix + joint_name.lower() +
00294                           "_mixed_position_velocity_controller/velocity_pid"]
00295         elif controller_type == "Effort":
00296             param_name = prefix + self.controller_prefix + \
00297                 joint_name.lower() + "_effort_controller"
00298 
00299         return self.pid_loader.get_settings(param_name)
00300 
00301     def set_controller(self, joint_name, controller_type, controller_settings):
00302         """
00303         Sets the controller settings calling the proper service with the correct syntax for controller type.
00304         """
00305         pid_service = None
00306         service_name = ""
00307         prefix = self.prefix if self.single_loop is not True else ""
00308 
00309         if controller_type == "Motor Force":
00310             # /realtime_loop/change_force_PID_FFJ0
00311             # currently use non-prefixed joint names but adds prefix in the middle
00312             # no matter if single or dual loops there is always a prefix for
00313             # motors
00314             service_name = "realtime_loop/" + self.prefix + \
00315                 "change_force_PID_" + joint_name[-4:].upper()
00316             pid_service = rospy.ServiceProxy(service_name, ForceController)
00317 
00318         elif controller_type == "Position":
00319             # /sh_ffj3_position_controller/set_gains
00320             service_name = prefix + self.controller_prefix + \
00321                 joint_name.lower() + "_position_controller/set_gains"
00322             pid_service = rospy.ServiceProxy(service_name, SetPidGains)
00323 
00324         elif controller_type == "Muscle Position":
00325             # /sh_ffj3_position_controller/set_gains
00326             service_name = prefix + self.controller_prefix + \
00327                 joint_name.lower() + "_muscle_position_controller/set_gains"
00328             pid_service = rospy.ServiceProxy(service_name, SetPidGains)
00329 
00330         elif controller_type == "Velocity":
00331             # /sh_ffj3_velocity_controller/set_gains
00332             service_name = prefix + self.controller_prefix + \
00333                 joint_name.lower() + "_velocity_controller/set_gains"
00334             pid_service = rospy.ServiceProxy(service_name, SetPidGains)
00335 
00336         elif controller_type == "Mixed Position/Velocity":
00337             # /sh_ffj3_mixed_position_velocity_controller/set_gains
00338             service_name = prefix + self.controller_prefix + \
00339                 joint_name.lower() + \
00340                 "_mixed_position_velocity_controller/set_gains"
00341             pid_service = rospy.ServiceProxy(
00342                 service_name, SetMixedPositionVelocityPidGains)
00343 
00344         elif controller_type == "Effort":
00345             # /sh_ffj3_effort_controller/set_gains
00346             service_name = prefix + self.controller_prefix + \
00347                 joint_name.lower() + "_effort_controller/set_gains"
00348             pid_service = rospy.ServiceProxy(
00349                 service_name, SetEffortControllerGains)
00350 
00351         else:
00352             rospy.logerr(
00353                 "", controller_type, " is not a recognized controller type.")
00354 
00355         contrlr_settings_converted = {}
00356         for param in controller_settings.items():
00357             contrlr_settings_converted[param[0]] = float(param[1])
00358 
00359         if controller_type == "Motor Force":
00360             try:
00361                 pid_service(int(contrlr_settings_converted["max_pwm"]),
00362                             int(contrlr_settings_converted["sgleftref"]),
00363                             int(contrlr_settings_converted["sgrightref"]),
00364                             int(contrlr_settings_converted["f"]),
00365                             int(contrlr_settings_converted["p"]), int(
00366                                 contrlr_settings_converted["i"]),
00367                             int(contrlr_settings_converted["d"]), int(
00368                                 contrlr_settings_converted["imax"]),
00369                             int(contrlr_settings_converted["deadband"]), int(contrlr_settings_converted["sign"]))
00370             except rospy.ServiceException:
00371                 return False
00372 
00373         elif controller_type == "Position":
00374             try:
00375                 pid_service(
00376                     float(contrlr_settings_converted["p"]), float(
00377                         contrlr_settings_converted["i"]),
00378                     float(contrlr_settings_converted["d"]), float(
00379                         contrlr_settings_converted["i_clamp"]),
00380                     float(contrlr_settings_converted["max_force"]), float(
00381                         contrlr_settings_converted[
00382                             "position_deadband"]),
00383                     int(contrlr_settings_converted["friction_deadband"]))
00384             except rospy.ServiceException:
00385                 return False
00386 
00387         elif controller_type == "Muscle Position":
00388             try:
00389                 pid_service(
00390                     float(contrlr_settings_converted["p"]), float(
00391                         contrlr_settings_converted["i"]),
00392                     float(contrlr_settings_converted["d"]), float(
00393                         contrlr_settings_converted["i_clamp"]),
00394                     float(contrlr_settings_converted["max_force"]), float(
00395                         contrlr_settings_converted[
00396                             "position_deadband"]),
00397                     int(contrlr_settings_converted["friction_deadband"]))
00398             except rospy.ServiceException:
00399                 return False
00400 
00401         elif controller_type == "Velocity":
00402             try:
00403                 pid_service(
00404                     float(contrlr_settings_converted["p"]), float(
00405                         contrlr_settings_converted["i"]),
00406                     float(contrlr_settings_converted["d"]), float(
00407                         contrlr_settings_converted["i_clamp"]),
00408                     float(contrlr_settings_converted["max_force"]), float(
00409                         contrlr_settings_converted[
00410                             "velocity_deadband"]),
00411                     int(contrlr_settings_converted["friction_deadband"]))
00412             except rospy.ServiceException:
00413                 return False
00414 
00415         elif controller_type == "Mixed Position/Velocity":
00416             try:
00417                 pid_service(
00418                     float(contrlr_settings_converted["pos/p"]), float(
00419                         contrlr_settings_converted["pos/i"]),
00420                     float(contrlr_settings_converted["pos/d"]), float(
00421                         contrlr_settings_converted["pos/i_clamp"]),
00422                     float(contrlr_settings_converted["pos/min_velocity"]), float(
00423                         contrlr_settings_converted[
00424                             "pos/max_velocity"]),
00425                     float(contrlr_settings_converted[
00426                           "pos/position_deadband"]),
00427                     float(contrlr_settings_converted["vel/p"]), float(
00428                         contrlr_settings_converted["vel/i"]),
00429                     float(contrlr_settings_converted["vel/d"]), float(
00430                         contrlr_settings_converted["vel/i_clamp"]),
00431                     float(contrlr_settings_converted["vel/max_force"]),
00432                     int(contrlr_settings_converted["vel/friction_deadband"]))
00433             except rospy.ServiceException:
00434                 return False
00435 
00436         elif controller_type == "Effort":
00437             try:
00438                 pid_service(int(contrlr_settings_converted["max_force"]), int(
00439                     contrlr_settings_converted["friction_deadband"]))
00440             except rospy.ServiceException:
00441                 return False
00442         else:
00443             rospy.logerr(
00444                 "", controller_type, " is not a recognized controller type.")
00445             return False
00446         return True
00447 
00448     def save_controller(self, joint_name, controller_type, controller_settings, filename):
00449         """
00450         Saves the controller settings calling the proper service with the correct syntax for controller type
00451         """
00452         param_name = []
00453         prefix = self.prefix if self.single_loop is not True else ""
00454         if controller_type == "Motor Force":
00455             param_name = ["" + joint_name[-4:].lower(), "pid"]
00456         elif controller_type == "Position":
00457             param_name = [prefix + self.controller_prefix +
00458                           joint_name.lower() + "_position_controller", "pid"]
00459         elif controller_type == "Muscle Position":
00460             param_name = [prefix + self.controller_prefix +
00461                           joint_name.lower() + "_muscle_position_controller", "pid"]
00462         elif controller_type == "Velocity":
00463             param_name = [prefix + self.controller_prefix +
00464                           joint_name.lower() + "_velocity_controller", "pid"]
00465         elif controller_type == "Mixed Position/Velocity":
00466             param_name = [prefix + self.controller_prefix +
00467                           joint_name.lower() + "_mixed_position_velocity_controller", "pid"]
00468         elif controller_type == "Effort":
00469             param_name = [prefix + self.controller_prefix +
00470                           joint_name.lower() + "_effort_controller"]
00471         pid_saver = PidSaver(filename)
00472         pid_saver.save_settings(param_name, controller_settings)