library.pid_controller

  1class PID:
  2    """PID controller.
  3
  4    Implements a simple proportional–integral–derivative controller.
  5
  6    Args:
  7        kp (float): Proportional gain.
  8        ki (float): Integral gain.
  9        kd (float): Derivative gain.
 10        setpoint (float, optional): Desired target value. Defaults to 0.0.
 11        output_limits (tuple, optional): Tuple (min, max) to clamp the controller output.
 12            Use None for no limit. Defaults to (None, None).
 13        invert_output (bool): swap sign on output 
 14
 15    Attributes:
 16        kp (float): Proportional gain.
 17        ki (float): Integral gain.
 18        kd (float): Derivative gain.
 19        setpoint (float): Target setpoint.
 20        min_output (float or None): Minimum output limit.
 21        max_output (float or None): Maximum output limit.
 22        invert_output (bool): swap sign on output
 23
 24    """
 25
 26    def __init__(self, kp, ki, kd, setpoint=0.0, 
 27                 output_limits=(None, None),
 28                 invert_output=False):
 29        """
 30        Initialize the PID controller.
 31
 32        Parameters are the same as described in the class docstring.
 33        """
 34        self.kp = kp
 35        self.ki = ki
 36        self.kd = kd
 37
 38        self.setpoint = setpoint
 39
 40        self._last_error = 0.0
 41        self._integral = 0.0
 42        self._last_time = None
 43
 44        # Output limits: (min, max)
 45        self.min_output, self.max_output = output_limits
 46        self.invert_output = invert_output
 47
 48    def reset(self):
 49        """
 50        Reset the controller internal state.
 51
 52        Clears the integral accumulator and last error/time so the controller
 53        behaves as if newly constructed.
 54        """
 55        self._last_error = 0.0
 56        self._integral = 0.0
 57        self._last_time = None
 58
 59    def __call__(self, measurement, dt):
 60        """
 61        Calculate the PID output for a given measurement and timestep.
 62
 63        Args:
 64            measurement (float): The current measured value.
 65            dt (float): Time interval in seconds since the last call. If dt <= 0.0,
 66            the derivative term is treated as zero and the integral is not updated.
 67
 68        Returns:
 69            float: Control output after applying proportional, integral, and
 70            derivative terms, clamped to output_limits if specified.
 71        """
 72        error = self.setpoint - measurement
 73
 74        # proportional
 75        p = self.kp * error
 76
 77        # integral (sum)
 78        if dt > 0.0:
 79            self._integral += error * dt
 80        i = self.ki * self._integral
 81
 82        # derivative (slope)
 83        if dt > 0.0:
 84            derivative = (error - self._last_error) / dt
 85        else:
 86            derivative = 0.0
 87        d = self.kd * derivative
 88
 89        output = p + i + d
 90
 91        # check limits
 92        if self.min_output is not None:
 93            output = max(self.min_output, output)
 94        if self.max_output is not None:
 95            output = min(self.max_output, output)
 96
 97        self._last_error = error
 98
 99        if self.invert_output:
100            output = output * -1
101
102        return output
103
104
105class Controller():
106    """Multi-axis PID controller for coordinated control.
107    
108    Manages multiple PID controllers, one per axis, for coordinated multi-axis
109    control systems such as robotics applications. Each axis can have independent
110    PID gains and setpoints.
111    
112    Args:
113        axes (dict, optional): Dictionary mapping axis names to PID parameter
114            dictionaries. Each parameter dictionary should contain 'kp', 'ki',
115            'kd', and 'setpoint' keys. If None, creates default x, y, z axes
116            with zero gains. Defaults to None.
117    
118    Attributes:
119        DEFAULT_PID (dict): Default PID parameters with all gains set to zero.
120            Used when no axes configuration is provided.
121        axes (dict): Dictionary mapping axis names to PID controller instances.
122    
123    Example:
124        >>> axes = {
125        ...     'x': {'kp': 37, 'ki': 0, 'kd': 1.2, 'setpoint': 0.2},
126        ...     'y': {'kp': 57, 'ki': 0, 'kd': 0.2, 'setpoint': 0.3},
127        ...     'z': {'kp': 12, 'ki': 0, 'kd': 2.2, 'setpoint': 0.01},
128        ... }
129        >>> controller = Controller(axes=axes)
130        >>> measurements = {'x': 0.15, 'y': 0.20, 'z': 0.005}
131        >>> outputs = controller(measurements, dt=0.01)
132    """
133    
134    DEFAULT_PID = {
135        'kp': 0,
136        'ki': 0,
137        'kd': 0,
138        'setpoint': 0
139    }
140    
141    def __init__(self, axes=None):
142        """Initialize the multi-axis controller.
143        
144        Args:
145            axes (dict, optional): Dictionary mapping axis names to PID parameter
146                dictionaries. Each parameter dictionary should contain 'kp', 'ki',
147                'kd', and 'setpoint' keys. If None, creates default x, y, z axes
148                with zero gains. Defaults to None.
149        
150        Example:
151            >>> # Custom axes configuration
152            >>> axes = {
153            ...     'x': {'kp': 10, 'ki': 0.1, 'kd': 1, 'setpoint': 0.5},
154            ...     'y': {'kp': 15, 'ki': 0.2, 'kd': 2, 'setpoint': 0.3}
155            ... }
156            >>> controller = Controller(axes=axes)
157            
158            >>> # Default configuration (all zeros)
159            >>> controller = Controller()
160        """
161        self.axes = axes
162    
163    @property
164    def axes(self):
165        """Get dictionary mapping axis names to PID controller instances.
166        
167        Returns:
168            dict: Dictionary with axis names as keys (e.g., 'x', 'y', 'z') and
169                PID controller instances as values.
170        """
171        return self._axes
172
173    @axes.setter
174    def axes(self, axes):
175        """Set up PID controllers for each axis.
176        
177        Creates individual PID controller instances for each axis based on the
178        provided configuration. If no configuration is provided, creates default
179        x, y, z axes with zero gains.
180        
181        Args:
182            axes (dict or None): Dictionary mapping axis names to PID parameter
183                dictionaries, or None for default configuration.
184        """
185        pid_axes = {}
186        if not axes:
187            my_axes = {'x': self.DEFAULT_PID,
188                       'y': self.DEFAULT_PID,
189                       'z': self.DEFAULT_PID}
190        else:
191            my_axes = axes
192        
193        for a, value in my_axes.items():
194            pid_axes[a] = PID(**value)
195
196        self._axes = pid_axes
197
198    def reset(self, axis='all'):
199        """Reset internal state of one or more PID controllers.
200        
201        Clears the integral accumulator and last error/time for the specified
202        axis or axes, returning them to their initial state.
203        
204        Args:
205            axis (str or list, optional): Specifies which axes to reset:
206                - 'all': Reset all axes (default)
207                - str: Reset single axis by name (e.g., 'x')
208                - list: Reset multiple specific axes (e.g., ['x', 'z'])
209        
210        Raises:
211            KeyError: If a specified axis name does not exist.
212        
213        Example:
214            >>> controller.reset()  # Reset all axes
215            >>> controller.reset('x')  # Reset only x-axis
216            >>> controller.reset(['x', 'y'])  # Reset x and y axes
217        """
218        if axis == 'all':
219            axis_list = list(self.axes.values())
220        elif isinstance(axis, str):
221            # Single axis name
222            axis_list = [self.axes[axis]]
223        else:
224            # Assume it's an iterable of axis names
225            axis_list = [self.axes[a] for a in axis]
226        
227        for a in axis_list:
228            a.reset()
229
230    def __call__(self, measurements, dt):
231        """Compute PID output for all axes.
232        
233        Calculates control outputs for all configured axes based on current
234        measurements and the time step. This is the primary method for getting
235        control commands during simulation or real-time control.
236        
237        Args:
238            measurements (dict): Current measured values for each axis.
239                Keys must match the axis names defined in the controller.
240                Example: {'x': 0.15, 'y': 0.20, 'z': 1.05}
241            dt (float): Time step in seconds since the last update.
242                Must be positive for proper integral and derivative calculations.
243        
244        Returns:
245            dict: Control output (typically velocity commands) for each axis.
246                Keys match the input measurement keys.
247                Example: {'x': 0.05, 'y': -0.03, 'z': 0.01}
248        
249        Raises:
250            KeyError: If measurements dict is missing a required axis.
251        
252        Example:
253            >>> measurements = {'x': 0.15, 'y': 0.20, 'z': 0.005}
254            >>> outputs = controller(measurements, dt=0.01)
255            >>> print(outputs)
256            {'x': 1.85, 'y': 5.7, 'z': 0.06}
257        """
258        return {axis: controller(measurements[axis], dt) 
259                for axis, controller in self.axes.items()}