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 
00021 from xml.etree import ElementTree as ET
00022 from pr2_mechanism_msgs.srv import ListControllers
00023 
00024 from sr_robot_msgs.srv import ForceController, SetEffortControllerGains, SetMixedPositionVelocityPidGains, SetPidGains
00025 from sr_gui_controller_tuner.pid_loader_and_saver import PidLoader, PidSaver
00026 import unicodedata
00027 
00028 class CtrlSettings(object):
00029     """
00030     Parses xml file and reads controller settings
00031     Creates lists for headers, fingers, motors
00032     """
00033 
00034     def __init__(self, xml_path, controller_type):
00035         self.headers = []
00036 
00037         #open and parses the xml config file
00038         xml_file = open(xml_path)
00039         xml_tree = ET.parse(xml_file)
00040         xml_file.close()
00041 
00042         #read the settings from the xml file
00043         ctrl_tree = None
00044         for ctrl in xml_tree.findall("controller"):
00045             ctrl_name = ctrl.attrib['name']
00046             if ctrl_name == controller_type:
00047                 ctrl_tree = ctrl
00048                 break
00049 
00050         if ctrl_tree == None:
00051             rospy.logerr("Couldn't find the settings for the controller " + controller_type)
00052 
00053         #read the headers settings
00054         xml_headers = ctrl.find("headers")
00055         for header in xml_headers.findall("item"):
00056             self.headers.append( header.attrib )
00057 
00058         self.nb_columns = len(self.headers)
00059 
00060         self.hand_item = ["Hand"]
00061         for i in range(0, self.nb_columns - 1):
00062             self.hand_item.append("")
00063 
00064         #read the fingers and the motors from the xml file
00065         self.fingers = []
00066         self.motors = []
00067         all_fingers = xml_tree.find("fingers")
00068         for finger in all_fingers.findall("finger"):
00069             finger_row = [ finger.attrib['name'] ]
00070             for i in range(0, self.nb_columns - 1):
00071                 finger_row.append("")
00072             self.fingers.append( finger_row )
00073 
00074             motors_for_finger = []
00075             for motor in finger.findall("motor"):
00076                 motor_row = [ "", motor.attrib['name'] ]
00077                 for i in range(0, self.nb_columns - 2):
00078                     motor_row.append("")
00079                 motors_for_finger.append( motor_row )
00080 
00081             self.motors.append( motors_for_finger )
00082 
00083 
00084 class SrControllerTunerApp(object):
00085     """
00086     Handles loading, saving and setting of controller settings
00087     """
00088     CONTROLLER_MANAGER_DETECTION_TIMEOUT = 3.0
00089     
00090     def __init__(self, xml_path):
00091         self.xml_path = xml_path
00092         self.all_controller_types = ["Motor Force", "Position", "Velocity",
00093                                      "Mixed Position/Velocity", "Effort", "Muscle Position"]
00094         self.pid_loader = PidLoader()
00095         
00096         self.edit_only_mode = False
00097         self.control_mode = "FORCE"
00098         
00099 
00100     def get_ctrls(self):
00101         """
00102         Retrieve currentlly running controllers
00103         return ["Motor Force", "Position"]
00104         """
00105         
00106         running_ctrls = []
00107 
00108         try:
00109             rospy.wait_for_service('pr2_controller_manager/list_controllers', self.CONTROLLER_MANAGER_DETECTION_TIMEOUT)
00110         
00111             controllers = rospy.ServiceProxy('pr2_controller_manager/list_controllers', ListControllers)
00112             resp = None
00113             try:
00114                 resp = controllers()
00115             except rospy.ServiceException, e:
00116                 rospy.logerr( "Service did not process request: %s"%str(e) )
00117             
00118             running_ctrls.append("Motor Force")
00119             if resp != None:
00120                 for state,controller in zip(resp.state, resp.controllers):
00121                     if state == "running":
00122                         split = controller.split("_")
00123                         ctrl_type_tmp = split[2]
00124                         for defined_ctrl_type in self.all_controller_types:
00125                             if ctrl_type_tmp.lower() in defined_ctrl_type.lower():
00126                                 running_ctrls.append(defined_ctrl_type)
00127                                 self.edit_only_mode = False
00128                                 return running_ctrls
00129             rospy.loginfo( "No controllers currently running" )
00130             rospy.loginfo( "Running controller tuner in edit-only mode" )
00131             self.edit_only_mode = True
00132             del running_ctrls[:]
00133             #In edit_only_mode all the controllers are available for editing
00134             for defined_ctrl_type in self.all_controller_types:
00135                 running_ctrls.append(defined_ctrl_type)
00136         except rospy.ROSException, e:
00137             rospy.loginfo( "Controller manager not running: %s"%str(e) )
00138             rospy.loginfo( "Running controller tuner in edit-only mode" )
00139             self.edit_only_mode = True
00140             #In edit_only_mode all the controllers are available for editing
00141             for defined_ctrl_type in self.all_controller_types:
00142                 running_ctrls.append(defined_ctrl_type)
00143         
00144         return running_ctrls
00145     
00146     def refresh_control_mode(self):
00147         self.control_mode = rospy.get_param('realtime_loop/default_control_mode', 'FORCE')
00148 
00149     def get_controller_settings( self, controller_type ):
00150         """
00151         Parses a file containing the controller settings
00152         and their min and max values, and returns them.
00153         """
00154         ctrl_settings = CtrlSettings(self.xml_path, controller_type)
00155 
00156         return ctrl_settings
00157 
00158     def load_parameters(self, controller_type, joint_name):
00159         """
00160         Load the parameters from the yaml file.
00161         """
00162         param_name = ""
00163         if controller_type == "Motor Force":
00164             param_name = joint_name.lower() +"/pid"
00165         elif controller_type == "Position":
00166             param_name =  "sh_"+ joint_name.lower()+"_position_controller/pid"
00167         elif controller_type == "Muscle Position":
00168             param_name =  "sh_"+ joint_name.lower()+"_muscle_position_controller/pid"
00169         elif controller_type == "Velocity":
00170             param_name =  "sh_"+ joint_name.lower()+"_velocity_controller/pid"
00171         elif controller_type == "Mixed Position/Velocity":
00172             param_name = ["sh_"+ joint_name.lower()+"_mixed_position_velocity_controller/position_pid",
00173                           "sh_"+ joint_name.lower()+"_mixed_position_velocity_controller/velocity_pid" ]
00174         elif controller_type == "Effort":
00175             param_name =  "sh_"+ joint_name.lower()+"_effort_controller"
00176 
00177         return self.pid_loader.get_settings( param_name )
00178 
00179     def set_controller(self, joint_name, controller_type, controller_settings):
00180         """
00181         Sets the controller settings calling the proper service with the correct syntax for controller type.
00182         """
00183         pid_service = None
00184         service_name = ""
00185         if controller_type == "Motor Force":
00186             #/realtime_loop/change_force_PID_FFJ0
00187             service_name =  "realtime_loop/change_force_PID_"+joint_name.upper()
00188             pid_service = rospy.ServiceProxy(service_name, ForceController)
00189 
00190         elif controller_type == "Position":
00191             #/sh_ffj3_position_controller/set_gains
00192             service_name =  "sh_"+joint_name.lower()+"_position_controller/set_gains"
00193             pid_service = rospy.ServiceProxy(service_name, SetPidGains)
00194 
00195         elif controller_type == "Muscle Position":
00196             #/sh_ffj3_position_controller/set_gains
00197             service_name =  "sh_"+joint_name.lower()+"_muscle_position_controller/set_gains"
00198             pid_service = rospy.ServiceProxy(service_name, SetPidGains)
00199 
00200         elif controller_type == "Velocity":
00201             #/sh_ffj3_velocity_controller/set_gains
00202             service_name =  "sh_"+joint_name.lower()+"_velocity_controller/set_gains"
00203             pid_service = rospy.ServiceProxy(service_name, SetPidGains)
00204 
00205         elif controller_type == "Mixed Position/Velocity":
00206             #/sh_ffj3_mixed_position_velocity_controller/set_gains
00207             service_name =  "sh_"+joint_name.lower()+"_mixed_position_velocity_controller/set_gains"
00208             pid_service = rospy.ServiceProxy(service_name, SetMixedPositionVelocityPidGains)
00209 
00210         elif controller_type == "Effort":
00211             #/sh_ffj3_effort_controller/set_gains
00212             service_name =  "sh_"+joint_name.lower()+"_effort_controller/set_gains"
00213             pid_service = rospy.ServiceProxy(service_name, SetEffortControllerGains)
00214 
00215         else:
00216             rospy.logerr( "", controller_type, " is not a recognized controller type." )
00217 
00218         contrlr_settings_converted = {}
00219         for param in controller_settings.items():
00220             contrlr_settings_converted[ param[0] ] = float(param[1])
00221 
00222         if controller_type == "Motor Force":
00223             try:
00224                 pid_service(int(contrlr_settings_converted["max_pwm"]),
00225                             int(contrlr_settings_converted["sgleftref"]),
00226                             int(contrlr_settings_converted["sgrightref"]),
00227                             int(contrlr_settings_converted["f"]),
00228                             int(contrlr_settings_converted["p"]), int(contrlr_settings_converted["i"]),
00229                             int(contrlr_settings_converted["d"]), int(contrlr_settings_converted["imax"]),
00230                             int(contrlr_settings_converted["deadband"]), int(contrlr_settings_converted["sign"]) )
00231             except:
00232                 return False
00233 
00234         elif controller_type == "Position":
00235             try:
00236                 pid_service(float(contrlr_settings_converted["p"]), float(contrlr_settings_converted["i"]),
00237                             float(contrlr_settings_converted["d"]), float(contrlr_settings_converted["i_clamp"]),
00238                             float(contrlr_settings_converted["max_force"]), float(contrlr_settings_converted["position_deadband"]),
00239                             int(contrlr_settings_converted["friction_deadband"]) )
00240             except:
00241                 return False
00242 
00243         elif controller_type == "Muscle Position":
00244             try:
00245                 pid_service(float(contrlr_settings_converted["p"]), float(contrlr_settings_converted["i"]),
00246                             float(contrlr_settings_converted["d"]), float(contrlr_settings_converted["i_clamp"]),
00247                             float(contrlr_settings_converted["max_force"]), float(contrlr_settings_converted["position_deadband"]),
00248                             int(contrlr_settings_converted["friction_deadband"]) )
00249             except:
00250                 return False
00251 
00252         elif controller_type == "Velocity":
00253             try:
00254                 pid_service(float(contrlr_settings_converted["p"]), float(contrlr_settings_converted["i"]),
00255                             float(contrlr_settings_converted["d"]), float(contrlr_settings_converted["i_clamp"]),
00256                             float(contrlr_settings_converted["max_force"]), float(contrlr_settings_converted["velocity_deadband"]),
00257                             int(contrlr_settings_converted["friction_deadband"]) )
00258             except:
00259                 return False
00260 
00261         elif controller_type == "Mixed Position/Velocity":
00262             try:
00263                 pid_service(float(contrlr_settings_converted["pos/p"]), float(contrlr_settings_converted["pos/i"]),
00264                             float(contrlr_settings_converted["pos/d"]), float(contrlr_settings_converted["pos/i_clamp"]),
00265                             float(contrlr_settings_converted["pos/min_velocity"]), float(contrlr_settings_converted["pos/max_velocity"]),
00266                             float(contrlr_settings_converted["pos/position_deadband"]),
00267                             float(contrlr_settings_converted["vel/p"]), float(contrlr_settings_converted["vel/i"]),
00268                             float(contrlr_settings_converted["vel/d"]), float(contrlr_settings_converted["vel/i_clamp"]),
00269                             float(contrlr_settings_converted["vel/max_force"]),
00270                             int(contrlr_settings_converted["vel/friction_deadband"]) )
00271             except:
00272                 return False
00273 
00274         elif controller_type == "Effort":
00275             try:
00276                 pid_service(int(contrlr_settings_converted["max_force"]), int(contrlr_settings_converted["friction_deadband"]) )
00277             except:
00278                 return False
00279         else:
00280             rospy.logerr( "", controller_type, " is not a recognized controller type." )
00281             return False
00282         return True
00283 
00284 
00285     def save_controller(self, joint_name, controller_type, controller_settings, filename):
00286         """
00287         Saves the controller settings calling the proper service with the correct syntax for controller type
00288         """
00289         param_name = []
00290         if controller_type == "Motor Force":
00291             param_name = [""+joint_name.lower() ,"pid"]
00292         elif controller_type == "Position":
00293             param_name =  ["sh_"+joint_name.lower()+"_position_controller" , "pid"]
00294         elif controller_type == "Muscle Position":
00295             param_name =  ["sh_"+joint_name.lower()+"_muscle_position_controller" , "pid"]
00296         elif controller_type == "Velocity":
00297             param_name =  ["sh_"+joint_name.lower()+"_velocity_controller" , "pid"]
00298         elif controller_type == "Mixed Position/Velocity":
00299             param_name =  ["sh_"+joint_name.lower()+"_mixed_position_velocity_controller" , "pid"]
00300         elif controller_type == "Effort":
00301             param_name =  ["sh_"+joint_name.lower()+"_effort_controller"]
00302 
00303         pid_saver = PidSaver(filename)
00304         pid_saver.save_settings(param_name, controller_settings)