Source code for package_template.pid

#!/usr/bin/env python

""" Brief description of the pid module.

    More advanced description of this module, e.g.
    This module contains a 1D PID controller and utilities for managing the
    gains and the controller parameters.

    Copyright (c) 2017-2019, New York University and Max Planck Gesellschaft,
    License BSD-3-Clause
"""


# Python 3 compatibility, has to be called just after the hashbang.
from __future__ import print_function, division
from pathlib import Path


[docs]class DefaultConfiguration(object): """PID configuration Configuration object with default values for kp, kd and ki can be used as input argument to create an instance of PID Attributes: kp: Proportional gain. kd: Derivative gain. ki: Integral gain. """ kp = 1 kd = 1 ki = 1 def __init__(self): self.kp = 1 self.kd = 1 self.ki = 1 def __repr__(self): return "%s(kp=%r, kd=%r, ki=%r)" % ( self.__class__.__name__, self.kp, self.kd, self.ki, )
[docs]class RosConfiguration: """ROS param configuration This contains the name of the ros parameter server keys for the PID gains. Attributes: ROSPARAM_KP: Key for reading kp gain. ROSPARAM_KD: Key for reading kd gain. ROSPARAM_KI: Key for reading ki gain. """ ROSPARAM_KP = "kp" ROSPARAM_KD = "kd" ROSPARAM_KI = "ki"
[docs]class ConfigFileConfiguration: """ Path to default configuration file, relative to the pid package """ ## Relative path to the default configuration fole relative_path = str(Path("config") / "test_pid_gains.yaml")
# "PythonPID" : to differentiate with cpp bindings PID # see /srcpy/wrappers.cpp
[docs]class PythonPID: """ Simple 1D PID controller Attributes: _configuration: The PID gains. _integral: The integral term. """ def __init__(self, configuration): """Constructor, initiallize the PID gains from a given configuration. Args: configuration: Any object with "kp", "kd" and "ki" attributes (as float) """ self._configuration = configuration self._integral = 0
[docs] def get_gains(self): """Get the gains in a dictionary, keys: "kp", "kd" and "ki" Returns: Dict `--` The PID gains. """ return { "kp": self._configuration.kp, "kd": self._configuration.kp, "ki": self._configuration.ki, }
[docs] def reset_integral(self): """ Reset integral part of the PID to 0.0 """ self._integral = 0.0
[docs] def compute(self, position, velocity, position_target, delta_time): """Compute the force related to the pid controller. This function is not stateless, as it performs integration. Call reset_integral() to reset the integral part. Args: position: Float `--` current position velocity: Float `--` current velocity position_target: Float `--` target position delta_time: Float `--` time passed since last measurement. Used for integral computation Returns: Float `--` computed force """ position_error = position_target - position self._integral += delta_time * position_error return ( position_error * self._configuration.kp - velocity * self._configuration.kd + self._integral * self._configuration.ki )
def __str__(self): """ Convert the object into a string """ return ( "PID controller: kp:" + str(self._configuration.kp) + " kd:" + str(self._configuration.kd) + " ki:" + str(self._configuration.ki) )
def _read_yaml_config_file(file_path): """Parse a yaml file to get the PID gains. Convenience function for reading pid configuration from yaml file. Args: file_path: str `--` Path relative to the execution path or global path. """ # importing yaml and reading yaml file import yaml assert Path(file_path).is_file() with open(file_path, "r") as f: yaml_load_object = yaml.load(f, Loader=yaml.Loader) # checking the yaml file had the excepted entries expected_attributes = ["kp", "kd", "ki"] if isinstance(yaml_load_object, DefaultConfiguration): for attribute in expected_attributes: if not hasattr(yaml_load_object, attribute): raise Exception( "Configuration file " + str(file_path) + " is expected to have the " + str(attribute) + " entry" ) Config = yaml_load_object elif isinstance(yaml_load_object, dict): for attribute in expected_attributes: if not attribute in yaml_load_object.keys(): raise Exception( "Configuration file " + str(file_path) + " is expected to have the " + str(attribute) + " entry" ) # creating a config object with correct attributes class Config(object): pass for attribute in expected_attributes: try: setattr(Config, attribute, float(yaml_load_object[attribute])) except Exception: raise Exception( "failed to convert " + attribute + "(" + str(yaml_load_object[attribute]) + ") to float (file: " + str(file_path) + ")" ) # constructing and returning controller return PythonPID(Config)
[docs]def get_default_pid(): """Factory for default PID controller. See PID and see DefaultConfiguration. Returns: PID `--` a new PID controller based on the DefaultConfiguration. """ return PythonPID(DefaultConfiguration)
[docs]def get_ros1_params_pid(verbose=True): """Get a PID controller parameterized through ROS params Assumes roscore is running and suitable parameters have been written in the server. Args: verbose: Bool `--` True: prints (stdout) the ros parameters it reads. Returns: PID `--` A PID object based on gains read from the ROS parameter server. """ # importing ros and checking roscore is running import rospy if rospy.is_shutdown(): raise Exception("failed to read ros parameters: ros is shutdown") # placeholder for the config class config: kp = None kd = None ki = None # reading the gains from ros parameter server parameters = [ RosConfiguration.ROSPARAM_KP, RosConfiguration.ROSPARAM_KD, RosConfiguration.ROSPARAM_KI, ] gains = ["kp", "kd", "ki"] # if requested, printing the parameters it is about to read if verbose: print("reading ros parameters: " + ", ".join(parameters)) for parameter, gain in zip(parameters, gains): if not rospy.has_param(parameter): raise Exception( "ros parameter server does not have the requested parameter: " + str(parameter) + " (current parameters: " + ", ".join(rospy.get_param_names()) + ")" ) try: value = rospy.get_param(parameter) setattr(config, gain, value) except Exception as e: raise Exception( "failed to read ros parameter " + str(parameter) + ": " + str(e) ) # constructing and returning controller return PythonPID(config)
[docs]def get_config_file_pid(config_file_path=None, verbose=True): """Reads a yaml file and return a corresponding PID controller. Args: config_file_path: str `--` Path to configuration file relative to the script where this function is defined is specified in the ConfigFileConfiguration object. If None, uses default config file (in config folder), else used specified path verbose: Bool `--` If True, prints path to config file used to standard output Returns: PID `--` A PID based on gains read from default configuration file """ if config_file_path is None: # getting abs path to this script abs_path_config = str( Path(__file__).resolve().parent.parent.parent / ConfigFileConfiguration.relative_path ) else: abs_path_config = config_file_path # checking file exists if not Path(abs_path_config).is_file(): raise Exception( "failed to find configuration file: " + str(abs_path_config) ) # printing path to config file if asked if verbose: print("reading pid gains from: ", abs_path_config) # constructing and returning the controller return _read_yaml_config_file(abs_path_config)