Source code for bdsim.blocks.sampled

r"""
Sampled-time blocks:

- have inputs and outputs
- have discrete-time state variables that are sampled/updated at the times
  specified by the associated clock
- are a subclass of ``SampledBlock`` |rarr| ``Block``

**Timing.**  At each clock tick :math:`t_k` with :math:`k \ge 1` the block computes its
next state from the inputs *at that instant* and the previous state, and that new state is
the block's output until the next tick.  The tick at :math:`t = 0` does not update the
state, so the output holds ``x0`` for the first sample period.  The block formulas below
are written for this: :math:`x_k` is the state held over :math:`[t_k, t_{k+1})`,
and :math:`x_0` is ``x0``.  A block with feedthrough (``DERIV_S``) also sees the current input.

Note that the value *logged* at a tick instant is the one from before that tick's
update, so it appears one sample late.  It is the logging, not the block, that lags.
"""

from __future__ import annotations

import numpy as np
import spatialmath.base as smb  # type: ignore[import-not-found]

from typing import Any, Callable

from bdsim.components import SampledBlock, Clock, SubsystemBlock, deprecated_block
from bdsim.blocks.continuous import (
    _NO_D,
    _NO_I,
    _as_D,
    _conform_D,
    _integral_limits,
    _old_tf_names,
    _tf2ss,
    _TFParams,
)

Vector1D = int | float | tuple[float, ...] | list[float] | np.ndarray

# ------------------------------------------------------------------------


[docs] class ZOH(SampledBlock): r""" :blockname:`ZOH` Zero-order hold. :inputs: 1 :outputs: 1 :states: N .. list-table:: :header-rows: 1 * - Port type - Port number - Types - Description * - Input - 0 - float, ndarray - :math:`x` * - Output - 0 - float, ndarray - :math:`y` The output is the input as sampled at the most recent clock tick, :math:`y_k = u_k` for :math:`k \ge 1`, and ``x0`` during the first sample period. There is no further delay. The state can be a scalar or a vector, this is given by the type of ``x0``. .. note:: If input is not a scalar, ``x0`` must have the shape of the input signal. """ nin = 1 nout = 1
[docs] def __init__(self, clock: Clock, x0: Vector1D = 0, **blockargs: Any) -> None: r""" :param clock: clock source :type clock: Clock :param x0: Initial value of the hold, defaults to 0 :type x0: array_like, optional :param blockargs: :meth:`common block options <bdsim.Block.__init__>` :type blockargs: dict """ super().__init__(clock=clock, nin=1, nout=1, x0=x0, **blockargs) self.add_param("clock", when="never") self.add_param("x0", when="setup")
# print('nstates', self.nstates) def output(self, t: float, inputs: list[Any], x: np.ndarray) -> list[Any]: # print('* output, x is ', self._x) if x.size == 1: return [x.item()] return [x]
[docs] def next(self, t: float, inputs: list[Any], x: np.ndarray) -> np.ndarray: u = smb.getvector(inputs[0]) return u # must be an ndarray
# ------------------------------------------------------------------------ #
[docs] class Hysteresis_S(SampledBlock): r""" :blockname:`HYSTERESIS_S` Discrete-time hysteresis (relay with memory, Schmitt trigger). :inputs: 1 :outputs: 1 :states: N .. list-table:: :header-rows: 1 * - Port type - Port number - Types - Description * - Input - 0 - float, ndarray - :math:`u` * - Output - 0 - float, ndarray - :math:`y` The state is the output, which switches between two levels. It switches on when the input reaches ``switch_on`` and off when the input falls to ``switch_off``. Between the two it keeps its value, which is the memory: .. math:: y_{k+1} = \begin{cases} y_\mathrm{on} & u_k \ge u_\mathrm{on} \\ y_\mathrm{off} & u_k \le u_\mathrm{off} \\ y_k & \text{otherwise} \end{cases} The input is sampled at each tick of the clock and the output is constant between ticks, as for :class:`ZOH`: the output from tick :math:`k` until the next is decided by the input at tick :math:`k`. Until the first tick after the start of the run the output is ``x0``. A logged value at a tick instant is the value held *before* that tick's update. The state can be a scalar or a vector, given by the shape of ``x0`` and of the parameters. The switching points and output levels can each be: - a scalar, in which case the same value applies to every element, or - a vector, of the same shape as the state, that applies elementwise. .. note:: The switching points must satisfy ``switch_off <= switch_on``. If they are equal the block is an ideal relay with no hysteresis, and the input exactly at the switching point switches on. For example, a Schmitt trigger that turns on above 0.6 and off below 0.4:: clock = bd.clock(0.01) trigger = bd.HYSTERESIS_S(clock, switch_on=0.6, switch_off=0.4) :seealso: :class:`ZOH` """ nin = 1 nout = 1
[docs] def __init__( self, clock: Clock, switch_on: Vector1D = 0.5, switch_off: Vector1D = -0.5, output_on: Vector1D = 1.0, output_off: Vector1D = 0.0, x0: Vector1D | None = None, **blockargs: Any, ) -> None: r""" :param clock: clock source :type clock: Clock :param switch_on: input at or above which the output switches on, defaults to 0.5 :type switch_on: float or array_like, optional :param switch_off: input at or below which the output switches off, defaults to -0.5 :type switch_off: float or array_like, optional :param output_on: output level when on, defaults to 1 :type output_on: float or array_like, optional :param output_off: output level when off, defaults to 0 :type output_off: float or array_like, optional :param x0: initial output, which must be ``output_on`` or ``output_off``, defaults to ``output_off`` :type x0: float or array_like, optional :param blockargs: :meth:`common block options <bdsim.Block.__init__>` :type blockargs: dict :raises ValueError: ``switch_off`` is above ``switch_on``, the two output levels are equal, or ``x0`` is not one of them """ parts = [switch_on, switch_off, output_on, output_off] if x0 is not None: parts.append(x0) shape = np.broadcast_shapes(*[np.shape(p) for p in parts]) initial = output_off if x0 is None else x0 initial = np.array(np.broadcast_to(initial, shape), dtype=float).reshape(-1) super().__init__(clock=clock, nin=1, nout=1, x0=initial, **blockargs) self.switch_on = switch_on self.switch_off = switch_off self.output_on = output_on self.output_off = output_off self._check_levels(output_on, output_off, initial) self._check_switch(switch_on, switch_off) self.add_param("clock", when="never") self.add_param("switch_on", coerce=False, validate=self._check_switch_on) self.add_param("switch_off", coerce=False, validate=self._check_switch_off) self.add_param("output_on", coerce=False, validate=self._check_output_on) self.add_param("output_off", coerce=False, validate=self._check_output_off) self.add_param("x0", when="setup")
@staticmethod def _check_switch(switch_on: Any, switch_off: Any) -> None: if np.any(np.asarray(switch_off) > np.asarray(switch_on)): raise ValueError("switch_off must not be above switch_on") @staticmethod def _check_levels(output_on: Any, output_off: Any, x: Any) -> None: on, off = np.asarray(output_on), np.asarray(output_off) if np.any(on == off): raise ValueError("output_on and output_off must differ") x = np.asarray(x) if not np.all((x == on) | (x == off)): raise ValueError("x0 must be output_on or output_off") def _check_switch_on(self, value: Any) -> None: self._check_switch(value, self.switch_off) def _check_switch_off(self, value: Any) -> None: self._check_switch(self.switch_on, value) def _check_output_on(self, value: Any) -> None: self._check_levels(value, self.output_off, self._x0) def _check_output_off(self, value: Any) -> None: self._check_levels(self.output_on, value, self._x0) def output(self, t: float, inputs: list[Any], x: np.ndarray) -> list[Any]: if x.size == 1: return [x.item()] return [x]
[docs] def next(self, t: float, inputs: list[Any], x: np.ndarray) -> np.ndarray: u = np.broadcast_to(np.asarray(inputs[0], dtype=float), x.shape) on = np.broadcast_to(np.asarray(self.output_on, dtype=float), x.shape) off = np.broadcast_to(np.asarray(self.output_off, dtype=float), x.shape) is_on = np.where( u >= self.switch_on, True, np.where(u <= self.switch_off, False, x == on) ) return np.where(is_on, on, off).astype(float)
# ------------------------------------------------------------------------ # def _check_enable(value: Any) -> None: """Validator for ``enable``, a function or ``None``.""" if value is not None and not callable(value): raise ValueError("enable must be callable") def _band_enable(I_band: float | None) -> Callable[..., bool] | None: """The ``enable`` function of an integrator that only integrates when the error is in a band.""" if I_band is None: return None def ifunc(t: float, u: list[Any], x: Any) -> bool: return bool(abs(u[0]) < I_band) return ifunc
[docs] class Integrator_S(SampledBlock): r""" :blockname:`INTEGRATOR_S` Discrete-time integrator. :inputs: 1 :outputs: 1 :states: N .. list-table:: :header-rows: 1 * - Port type - Port number - Types - Description * - Input - 0 - float, ndarray - :math:`x` * - Output - 0 - float, ndarray - :math:`y` This block implements the dynamics of a discrete-time integrator with gain :math:`g`, which is given by .. math:: \begin{aligned} x_k &= x_{k-1} + g \cdot u_k \cdot T \\ y_k &= x_k \end{aligned} This is backward-Euler (rectangular) integration: the input at a tick contributes to the output from that tick, and :math:`x_0` is ``x0``. The state can be a scalar or a vector. The initial state, and type, is given by ``x0``. The shape of the input signal must match ``x0``. The gain is applied to the input, before accumulation, as in the update equation above. The limits ``min`` and ``max`` apply to the resulting state, which is also the output. They are applied at each sample time, so no crossing events are needed. The minimum and maximum output values can be: - a scalar, in which case the same value applies to every element of the state vector, or - a vector, of the same shape as ``x0`` that applies elementwise to the state. .. note:: The minimum and maximum clip the next state, so a state that would pass a limit is held at it until the input reverses. An initial state outside the limits is clipped at the first sample. Integration can be controlled by an ``enable`` function:: enable(t, u, x): bool where the arguments are current time, a list of inputs to the integrator block and the state as an ndarray. If the function returns False then the integrator's state is set to zero. """ nin = 1 nout = 1
[docs] def __init__( self, clock: Clock, x0: Vector1D = 0, gain: float = 1.0, min: Vector1D | None = None, max: Vector1D | None = None, enable: Callable[[float, list[Any], np.ndarray], bool] | None = None, **blockargs: Any, ) -> None: r""" :param clock: clock source :type clock: Clock :param x0: Initial state, defaults to 0 :type x0: array_like, optional :param gain: gain or scaling factor, defaults to 1 :type gain: float :param min: Minimum value of state, defaults to None :type min: float or array_like, optional :param max: Maximum value of state, defaults to None :type max: float or array_like, optional :param enable: function to enable or disable integration :type enable: callable :param blockargs: :meth:`common block options <bdsim.Block.__init__>` :type blockargs: dict """ super().__init__(clock=clock, x0=x0, **blockargs) self.min = min self.max = max self.gain: float = gain self.T = clock.T self.enable = enable if enable is not None and not callable(enable): raise ValueError("enable must be callable") self.add_param("clock", when="never") self.add_param("gain") self.add_param("min", coerce=False) self.add_param("max", coerce=False) self.add_param("enable", coerce=False, validate=_check_enable) self.add_param("x0", when="setup")
@property def min(self) -> np.ndarray | None: """Lower limit of each state, or ``None`` for no limit.""" return self._min @min.setter def min(self, value: Any) -> None: self._min = None if value is None else smb.getvector(value, len(self._x0)) @property def max(self) -> np.ndarray | None: """Upper limit of each state, or ``None`` for no limit.""" return self._max @max.setter def max(self, value: Any) -> None: self._max = None if value is None else smb.getvector(value, len(self._x0)) def output(self, t: float, u: list[Any], x: np.ndarray) -> list[Any]: result = x if isinstance(result, np.ndarray) and result.ndim == 1 and result.size == 1: result = result.item() return [result]
[docs] def next(self, t: float, u: list[Any], x: np.ndarray) -> np.ndarray: # compute next state xnext = x + self.gain * self.T * np.array(u[0]) # apply enable function if it exists if self.enable is not None and not self.enable(t, u, x): # if enable function returns False then integrator output is jammed at zero return np.zeros(x.shape) # apply limits if they exist if self.min is not None or self.max is not None: xnext = np.clip(xnext, self.min, self.max) return xnext
# ------------------------------------------------------------------------ #
[docs] class Deriv_S(SampledBlock): r""" :blockname:`DERIV_S` Discrete-time derivative. :inputs: 1 :outputs: 1 :states: N .. list-table:: :header-rows: 1 * - Port type - Port number - Types - Description * - Input - 0 - float - :math:`x` * - Output - 0 - float - :math:`y` Implements a discrete-time derivative as a backward difference, with transfer function .. math:: g \, \frac{1 - z^{-1}}{T} where :math:`g` is the gain and :math:`T` is the sampling time. The block is causal because the output uses the current input and the input at the previous tick, and no filter is needed. Unlike the continuous-time derivative blocks, this one includes no smoothing filter, so noise on its input is amplified. The state is the input at the most recent tick, so the output is the difference between the current input and the input at that tick, scaled by the gain and :math:`1/T`. The state is ``x0`` during the first sample period. .. versionadded:: 1.2.0 :seealso: :class:`Integrator_S` """ nin = 1 nout = 1
[docs] def __init__( self, clock: Clock, x0: Vector1D = 0, gain: float = 1.0, **blockargs: Any, ) -> None: r""" :param clock: clock source :type clock: Clock :param x0: initial states, defaults to 0 :type x0: array_like, optional :param gain: gain or scaling factor, defaults to 1 :type gain: float :param kwargs: :meth:`common block options <bdsim.Block.__init__>` :type kwargs: dict """ super().__init__(clock=clock, x0=x0, feedthrough=True, **blockargs) self.gain = gain self.add_param("clock", when="never") self.add_param("gain") self.add_param("x0", when="setup")
def output(self, t: float, u: list[Any], x: np.ndarray) -> list[Any]: result = self.gain * (u[0] - x) / self.T if isinstance(result, np.ndarray) and result.ndim == 1 and result.size == 1: result = result.item() return [result]
[docs] def next(self, t: float, u: list[Any], x: np.ndarray) -> np.ndarray: # next state is current input, so output is difference between current and previous input return np.array(u[0])
# ------------------------------------------------------------------------ #
[docs] class LTI_SS_S(SampledBlock): r""" :blockname:`LTI_SS_S` Discrete-time state-space LTI dynamics :inputs: 1 :outputs: 1 :states: N .. list-table:: :header-rows: 1 * - Port type - Port number - Types - Description * - Input - 0 - float, ndarray - :math:`u` * - Output - 0 - float, ndarray - :math:`y` Implements the dynamics of a discrete-time multi-input multi-output (MIMO) linear time invariant (LTI) system described in statespace form. The dynamics are given by .. math:: \begin{aligned} x_k &= \Phi x_{k-1} + \Gamma u_k \\ y_k &= C x_k + D u_k \end{aligned} where :math:`x_0` is ``x0``. The input at a tick takes effect in the output from that tick, as it does for all sampled blocks. :math:`\Phi` and :math:`\Gamma` are the usual symbols for the discrete-time system and input matrices. They are given by the arguments ``A`` and ``B``, the names used for the continuous-time ``LTI_SS``, and are also available as the attributes ``Phi`` and ``Gamma`` (or ``Φ`` and ``Γ``), which are synonyms for ``A`` and ``B``. ``D`` is optional and defaults to zero. If it is nonzero the block has direct feedthrough, the output depends on the input at the same instant, which creates an algebraic loop if the output feeds back to the input without a delay. The output is a single port: a float if ``C`` has one row, and otherwise an ndarray with one element for each row of ``C``, so a system with several outputs is one wire, which a ``DEMUX`` block can split. The input has one port for each column of ``B``, each carrying a scalar. Examples:: lti = bd.LTI_SS_S( clock, A=np.array([[-2.0]]), B=np.array([1.0]), C=np.array([-1.0]) ) is the system :math:`x_k=-2x_{k-1}+u_k, y_k=-x_k`. .. versionadded:: 1.2.0 :seealso: :class:`~bdsim.blocks.sampled.LTI_SISO_S` :class:`~bdsim.blocks.continuous.LTI_SS` """ nin = 1 nout = 1
[docs] def __init__( self, clock: Clock, A: np.ndarray, B: np.ndarray, C: np.ndarray, D: np.ndarray | None = None, x0: np.ndarray | None = None, **blockargs: Any, ) -> None: r""" :param clock: clock source :type clock: Clock :param A: system matrix :math:`\Phi`, also available as the attribute ``Phi`` :type A: ndarray(n,n) :param B: input matrix :math:`\Gamma`, also available as the attribute ``Gamma``; a 1D array is a column vector :type B: ndarray(n,m) :param C: output matrix; a 1D array is a row vector :type C: ndarray(p,n) :param D: feedthrough matrix, defaults to zero :type D: ndarray(p,m), optional :param x0: initial states, defaults to None :type x0: array_like, optional :param blockargs: :meth:`common block options <bdsim.Block.__init__>` :type blockargs: dict """ # check dimensions of A, B, C, D conform # B, C, D can be 1D or 2D, but must conform to A dimensions and are reshaped as required. if A.shape[0] != A.shape[1]: raise ValueError("A must be square") n = A.shape[0] if len(B.shape) == 1: # 1D array, assume it's a column vector and reshape to (n,1) nin = 1 B = B.reshape((n, 1)) else: nin = B.shape[1] if B.shape[0] != n: raise ValueError("B must have same number of rows as A") if len(C.shape) == 1: # 1D array, assume it's a row vector and reshape to (1,n) p = 1 if C.shape[0] != n: raise ValueError("C must have same number of columns as A") C = C.reshape((1, n)) else: p = C.shape[0] if C.shape[1] != n: raise ValueError("C must have same number of columns as A") D = _as_D(D, p, nin) self.A = A self.B = B self.C = C # a nonzero D is direct feedthrough, which may create an algebraic loop, so the # base class is told; without one there is no D to store feedthrough = D is not None and bool(np.any(D != 0)) self._D: np.ndarray | None = D if feedthrough else None super().__init__( clock=clock, ndstates=n, nin=nin, nout=1, # one output port, whatever the rows of C: see the class docstring x0=x0, feedthrough=feedthrough, **blockargs, ) self.add_param("clock", when="never") self.add_param("A") self.add_param("B") self.add_param("C") self.add_param("D") self.add_param("x0", when="setup")
@property def D(self) -> np.ndarray | None: """The feedthrough matrix, ``None`` if it is zero.""" return self._D @D.setter def D(self, value: Any) -> None: self._D = _conform_D(self, value) # the usual names for the discrete-time system and input matrices, as synonyms @property def Phi(self) -> np.ndarray: r"""The system matrix :math:`\Phi`, a synonym for ``A``.""" return self.A # type: ignore[no-any-return] @Phi.setter def Phi(self, value: Any) -> None: self.A = value @property def Gamma(self) -> np.ndarray: r"""The input matrix :math:`\Gamma`, a synonym for ``B``.""" return self.B # type: ignore[no-any-return] @Gamma.setter def Gamma(self, value: Any) -> None: self.B = value Φ = Phi Γ = Gamma def output(self, t: float, u: list[Any], x: np.ndarray) -> list[Any]: y = self.C @ x if self._D is not None: y = y + self._D @ np.array(u) if y.size == 1: return [y.item()] return [y]
[docs] def next(self, t: float, u: list[Any], x: np.ndarray) -> np.ndarray: # Reshape u and x to (N,1), i.e. column vectors, so # no problems with broadcasting between A@x and B@u x = x.reshape(-1, 1) u_arr = np.array(u).reshape(-1, 1) xnext = self.A @ x + self.B @ u_arr return xnext.flatten()
# ------------------------------------------------------------------------ #
[docs] class LTI_SISO_S(_TFParams, LTI_SS_S): r""" :blockname:`LTI_SISO_S` Discrete-time SISO LTI dynamics. :inputs: 1 :outputs: 1 :states: N .. list-table:: :header-rows: 1 * - Port type - Port number - Types - Description * - Input - 0 - float - :math:`u` * - Output - 0 - float - :math:`y` Implements the dynamics of a discrete-time single-input single-output (SISO) linear time invariant (LTI) system described by numerator and denominator polynomial coefficients. The dynamics are given by .. math:: \frac{Y(z)}{U(z)} = \frac{N(z)}{D(z)} Coefficients are given in the order from highest to lowest order, ie. :math:`2z^2 - 4z +3` is ``[2, -4, 3]``, :math:`1 - z^{-1} + 2 z^{-2}` is ``[1, -1, 2]``. The state-space form can be either controller canonical form or observer canonical form, as specified by the ``form`` argument. For either case, the state is the output of a chain of integrators, and the ``order`` argument controls the order in which they are mapped to rows of the state-transition matrix. Examples:: lti = bd.LTI_SISO_S(clock, num=[1, 2], den=[2, 3, -4]) is the transfer function :math:`\frac{z+2}{2z^2+3z-4}`. .. note:: * The state-space realization is not unique. The controller canonical form is more common in control design, while the observer canonical form is more common in estimation. The state ordering convention (the direction of the integrator chain) can be specified by the ``order`` argument, and results in either 1s on the super-diagonal or sub-diagonal of the A matrix for the controller canonical form. * This block is single-input single-output. For a system with more than one input or output, convert the transfer function to state-space form with :func:`scipy.signal.tf2ss`, which takes several numerators with a common denominator, or :func:`control.tf2ss` for a full transfer function matrix, and create the block with :class:`~bdsim.blocks.sampled.LTI_SS_S`. .. versionadded:: 1.2.0 :seealso: :class:`~bdsim.blocks.sampled.LTI_SS_S` :class:`~bdsim.blocks.continuous.LTI_SISO` """ nin = 1 nout = 1
[docs] def __init__( self, clock: Clock, num: Vector1D = 1, den: Vector1D = [1, 1], x0: np.ndarray | None = None, form: str = "ccf", order: str = "backward", verbose: bool = False, **blockargs: Any, ) -> None: r""" :param clock: clock source :type clock: Clock :param num: Numerator coefficients in descending powers of :math:`z`. :type num: array_like :param den: Denominator coefficients in descending powers of :math:`z`. Does not have to be normalized. :type den: array_like :param form: The canonical form of the realization, defaults to 'ccf'. :type form: str, optional :param order: The state ordering convention :type order: str, optional :param verbose: If True, print the state-space matrices, defaults to False :type verbose: bool, optional :return: LTI_SISO_S block :rtype: ``LTI_SISO_S`` instance Coefficients ``num`` and ``den`` can be specified in the form of an array of coefficients, or an array of factors. The coefficients and factors are arrays of coefficients in decreasing powers of :math:`z`, this could be :math:`z^2 + 0.8z + 0.15` or :math:`1 + 0.8z^{-1} + 0.15z^{-2}`, both represented as ``[1, 0.8, 0.15]``. For example: * ``[1, 0.8]`` is the polynomial :math:`z+0.8`, * ``[[1, 0.3], [1, 0.5]]`` is the factors :math:`(z+0.3)(z+0.5)` which are convolved to obtain the coefficients of :math:`z^2 + 0.8z + 0.15`. * ``[2,[1,0.3], [1,0.5]]`` is the factors :math:`2(z+0.3)(z+0.5)` which are convolved to obtain the coefficients of :math:`2z^2 + 1.6z + 0.3` The ``form`` of the realization can be one of: * ``'ccf'`` : Controller Canonical Form. The characteristic equation coefficients appear in a row of **A**. Useful for control design. * ``'ocf'`` : Observer Canonical Form. The characteristic equation coefficients appear in a column of **A**. Useful for estimation. The ``order`` of the integrator chain can be one of: * ``'forward'`` : :math:`x_0` is the output of the first integrator, :math:`x_n-1` is the last. Results in 1s on the super-diagonal for ``'ccf'``. * ``'backward'``: :math:`x_n-1` is the output of the first integrator, :math:`x_0` is the last. Results in 1s on the sub-diagonal for ``'ccf'``. .. note:: **Representation Perspectives:** 1. **Control Perspective (z-domain):** If coefficients represent descending powers of :math:`z`, the states represent forward shifts. Example: :math:`G(z) = \\frac{b_1 z + b_2}{z^2 + a_1 z + a_2}` 2. **DSP Perspective (z⁻¹-domain):** If coefficients represent ascending powers of :math:`z^{-1}`, the states represent physical unit delays. Example: :math:`H(z^{-1}) = \\frac{b_0 + b_1 z^{-1}}{1 + a_1 z^{-1} + a_2 z^{-2}}` In both cases, this function treats the input arrays as ordered coefficients. .. math:: x[k+1] = A x[k] + B u[k] \\\\ y[k] = C x[k] + D u[k] If ``form='ccf'`` and ``order='forward'``, the state :math:`x_i[k]` corresponds to the output of the :math:`i`-th delay element in a tapped delay line. .. note:: - The transfer function is assumed to be strictly proper (:math:`deg(num) < deg(den)`). - The default ``form='ccf', order='backward'`` corresponds to the state-space realization returned by ``scipy.signal.tf2ss`` and MATLAB's ``tf2ss``. - ``num`` and ``den`` are attributes holding the numerator and denominator coefficients (expanded if they were given as factors), and can be changed after the block is created (they are parameters). The state-space matrices are rebuilt from them. The order of the system, the length of ``den`` minus one, cannot change. - The state-space matrices are available in the ``A``, ``B``, ``C``, and ``D`` attributes of the block. - If the numerator degree is less than the denominator degree, the block has no feedthrough and ``D`` is None. The ``feedthrough`` attribute of the block is set to True otherwise. It is used by the scheduler to ensure correct block evaluation order. .. versionchanged:: next The arguments ``N`` and ``D`` are renamed ``num`` and ``den``, because ``D`` is also the feedthrough matrix of the state-space realization. The old keywords still work, with a ``DeprecationWarning``, and will be removed in a later release. """ num, den = _old_tf_names(num, den, blockargs) A, B, C, _D = _tf2ss(num, den, form=form, order=order, verbose=verbose) n = A.shape[0] if x0 is None: x0 = np.zeros((n,)) super().__init__(clock=clock, A=A, B=B, C=C, D=_D, x0=x0, **blockargs) self._init_tf(num, den, form, order, verbose) self.add_param("num", coerce=False) # a polynomial may change length self.add_param("den", coerce=False) self.add_param("clock", when="never") self.add_param("form", when="never") self.add_param("order", when="never") self.add_param("verbose", when="never") self.add_param("x0", when="setup")
# ------------------------------------------------------------------------ #
[docs] class PID_S(SubsystemBlock): r""" :blockname:`PID_S` Discrete-time PID control. :inputs: 2 :outputs: 1 :states: 2 .. list-table:: :header-rows: 1 * - Port type - Port number - Types - Description * - Input - 0 - float - :math:`x`, plant output * - Input - 1 - float - :math:`x^*`, demanded output * - Output - 0 - any - :math:`u`, control to plant Implements the dynamics of a discrete-timePID controller as a ``bdsim`` subsystem. There are three possible structures: * Parallel structure:: ┌───────> P ─────────┐ │ │ │ ▼ e ────┼───> D(z-1)/Tz ──> SUM ───> y │ ▲ │ │ └───> IT/(z-1) ──────┘ .. math:: \begin{aligned} e &= x^* - x \\ \frac{Y(z)}{E(z)} &= P + I\frac{T}{z-1} + D\frac{z-1}{Tz} \end{aligned} * Ideal or standard structure. Described by ANSI/ISA-S75:: ┌────────────────────┐ │ │ │ ▼ e ────> P ──┼───> D(z-1)/Tz ──> SUM ───> y │ ▲ │ │ └───> IT/(z-1) ──────┘ .. math:: \begin{aligned} e &= x^* - x \\ \frac{Y(z)}{E(z)} &= P \left(1 + I\frac{T}{z-1} + D\frac{z-1}{Tz}\right) \end{aligned} * Series structure. Archaic form, essentially a PI controller followed by a PD controller:: ┌───────────────────┐ ┌────────────────────┐ │ │ │ │ │ ▼ │ ▼ e ────> P ──┴──> IT/(z-1) ───> SUM ──┴──> D(z-1)/Tz ──> SUM ───> y .. math:: \begin{aligned} e &= x^* - x \\ \frac{Y(z)}{E(z)} &= P \left(1 + I\frac{T}{z-1} + D\frac{z-1}{Tz}\right) \end{aligned} Sometimes the gains are written in terms of time constants :math:`T_i = 1/I` and :math:`Td = D`. The PID controller is a subsytem comprising a number of blocks. **Which terms are built.** The controller is a subsystem of ordinary blocks, and a term whose gain is zero is normally left out, to save blocks and states. The proportional term is always built. * By default (``terms=None``) the I term is built only if ``I`` is not zero, and the D term only if ``D`` is not zero. So ``bd.PID_S(clock, P=3, I=1)`` is a PI controller and ``bd.PID_S(clock, P=3)`` is proportional only. * ``terms`` overrides this. It is one of ``"P"``, ``"PI"``, ``"PD"`` or ``"PID"``, and exactly those terms are built, **even if their gain is zero**. A term that is built can have its gain changed after the block is created; a term that was left out cannot be added later, because the structure is fixed when the block is created. For example, a PD controller that starts with no derivative action:: pid = bd.PID_S(clock, P=3, D=0, terms="PD") * A non-zero gain for a term that ``terms`` does not build is an error, not silently ignored. So is a value of ``terms`` other than the four above. * The cost of building a term at zero gain is its state: an integrator state that stays at zero, or the state of the discrete derivative. The gain equivalence between the three structures is given by .. list-table:: :header-rows: 1 :align: center * - structure - :math:`P` - :math:`I` - :math:`D` * - parallel - :math:`P_p` - :math:`I_p` - :math:`D_p` * - ideal - :math:`P_i` - :math:`P_i I_i` - :math:`P_i D_i` * - series - :math:`P_s (1 + D_s I_s)` - :math:`P_s I_s` - :math:`P_s D_s` The derivate is computed by a ``DERIV_S`` block which is a simple first-order difference, which will exacerbate any noise on its input signal. The integration is performed by an ``INTEGRATOR_S`` block, which has some options to limit the integrator state and to enable/disable integration based on the error signal. If ``I_limit`` is provided it specifies the limits of the integrator state, before multiplication by ``I``. If ``I_limit`` is: * a scalar :math:`a` the integrator state is clipped to the interval :math:`[-a, a]` * a 2-tuple :math:`(a,b)` the integrator state is clipped to the interval :math:`[a, b]` If ``I_limit`` is provided it specifies the limits of the integrator output. The integrator state is clipped to these limits, which can be used to prevent integrator windup. The limits can be specified as: * a scalar :math:`s` the band is the interval :math:`[-s, s]` * a 2-tuple :math:`(a,b)` the band is the interval :math:`[a, b]` If ``I_band`` is provided the integrator is reset to zero whenever the error :math:`e` is outside the band given by ``I_band`` which is: * a scalar :math:`s` the band is the interval :math:`[-s, s]` * a 2-tuple :math:`(a,b)` the band is the interval :math:`[a, b]` Examples:: pid = bd.PID_S(clock, P=3, D=2, I=1) # PID pid = bd.PID_S(clock, P=3, I=1) # PI, no D block pid = bd.PID_S(clock, P=3) # P only pid = bd.PID_S(clock, P=3, D=0, terms="PD") # PD, with a D block that starts at zero .. warning:: This block has a direct feedthrough term (by definition), and depending on how it is used in a system it may introduce an an algebraic loop which will be flagged at ``compile`` time. .. versionadded:: 1.2.0 :seealso: :class:`Deriv_S` :class:`Integrator_S` """ nin = 2 nout = 1
[docs] def __init__( self, clock: Clock, P: float = 0.0, I: float = 0.0, D: float = 0.0, I_limit: float | tuple[float, ...] | list[float] | None = None, I_band: float | None = None, structure: str = "parallel", terms: str | None = None, **blockargs: Any, ) -> None: r""" :param clock: clock source :type clock: Clock :param P: proportional gain, defaults to 0 :type P: float :param D: derivative gain, defaults to 0 :type D: float :param I: integral gain, defaults to 0 :type I: float :param D_pole: filter pole for derivative estimate, defaults to 1 rad/s :type D_pole: float :param I_limit: integral limit :type I_limit: float or 2-tuple :param I_band: band within which integral action is active :type I_band: float :param structure: the structure of the PID implementation, "parallel" (default), "series", "standard|ideal" :param terms: the terms to build, one of "P", "PI", "PD" or "PID", even if a gain is zero; the default is P, plus I and D if their gain is not zero :type terms: str, optional :raises ValueError: ``terms`` is not one of those, or a gain is non-zero for a term that is not built :param blockargs: :meth:`common block options <bdsim.Block.__init__>` :type blockargs: dict """ bd = blockargs["bd"] subsystem = bd.runtime.blockdiagram(name="PID") Pblock = subsystem.GAIN(P) # proportional gain block if terms is None: # the terms with a non-zero gain; P is always built terms = "P" + ("I" if I != 0 else "") + ("D" if D != 0 else "") elif terms not in ("P", "PI", "PD", "PID"): raise ValueError( f"terms must be one of 'P', 'PI', 'PD' or 'PID', got {terms!r}" ) else: for name, gain in (("I", I), ("D", D)): if gain != 0 and name not in terms: raise ValueError( f"{name}={gain} but terms={terms!r} does not build the {name} term" ) if "I" in terms: # if the I term is required, create the block lo, hi = _integral_limits(I_limit) Iblock = subsystem.INTEGRATOR_S( clock, gain=I, name="I", min=lo, max=hi, enable=_band_enable(I_band), ) # integral block with gain I if "D" in terms: # if the D term is required, create the blocks Dblock = subsystem.DERIV_S(clock, gain=D) # derivative block error_sum = subsystem.SUM("-+", name="errsum") # error summing junction inp = subsystem.INPORT(2) # PID block inputs outp = subsystem.OUTPORT(1) # PID block output subsystem.connect(inp, error_sum) # for each case sum the various terms if structure == "parallel": if terms == "PID": out_sum = subsystem.SUM("+++", name="SUM") subsystem.connect(error_sum, Pblock, Dblock, Iblock) subsystem.connect(Pblock, out_sum[0]) subsystem.connect(Iblock, out_sum[1]) subsystem.connect(Dblock, out_sum[2]) elif terms == "PI": out_sum = subsystem.SUM("++", name="SUM") subsystem.connect(error_sum, Pblock, Iblock) subsystem.connect(Pblock, out_sum[0]) subsystem.connect(Iblock, out_sum[1]) elif terms == "PD": out_sum = subsystem.SUM("++", name="SUM") subsystem.connect(error_sum, Pblock, Dblock) subsystem.connect(Pblock, out_sum[0]) subsystem.connect(Dblock, out_sum[1]) else: # "P": no summing junction out_sum = Pblock subsystem.connect(error_sum, Pblock) subsystem.connect(out_sum, outp) elif structure == "series": # cascade of P, I and D blocks with summing junctions between them subsystem.connect(error_sum, Pblock) if "I" in terms: sum1 = subsystem.SUM("++", name="SUM_I") subsystem.connect(Pblock, Iblock, sum1[1]) subsystem.connect(Iblock, sum1[0]) I_out = sum1 else: I_out = Pblock if "D" in terms: sum2 = subsystem.SUM("++", name="SUM_D") subsystem.connect(I_out, Dblock, sum2[1]) subsystem.connect(Dblock, sum2[0]) D_out = sum2 else: D_out = I_out subsystem.connect(D_out, outp) elif structure in ("standard", "ideal"): # P in series with parallel D and I if terms == "PID": out_sum = subsystem.SUM("+++", name="SUM") subsystem.connect(error_sum, Pblock) subsystem.connect(Pblock, out_sum[0], Iblock, Dblock) subsystem.connect(Iblock, out_sum[1]) subsystem.connect(Dblock, out_sum[2]) elif terms == "PI": out_sum = subsystem.SUM("++", name="SUM") subsystem.connect(error_sum, Pblock) subsystem.connect(Pblock, out_sum[0], Iblock) subsystem.connect(Iblock, out_sum[1]) elif terms == "PD": out_sum = subsystem.SUM("++", name="SUM") subsystem.connect(error_sum, Pblock) subsystem.connect(Pblock, out_sum[0], Dblock) subsystem.connect(Dblock, out_sum[1]) else: # "P": no summing junction out_sum = Pblock subsystem.connect(error_sum, Pblock) subsystem.connect(out_sum, outp) super().__init__(subsystem=subsystem, **blockargs) self._Iblock = Iblock if "I" in terms else None self._I_band = I_band self.add_param("P", forward=(Pblock, "K")) if "I" in terms: self.add_param("I", forward=(Iblock, "gain")) self.add_param("I_limit", coerce=False) self.add_param("I_band", coerce=False) else: for name in ("I", "I_limit", "I_band"): self.add_param(name, unavailable=_NO_I) if "D" in terms: self.add_param("D", forward=(Dblock, "gain")) else: self.add_param("D", unavailable=_NO_D) self.add_param("clock", when="never") self.add_param("structure", when="never") self.add_param("terms", when="never")
@property def I_limit(self) -> float | tuple[float | None, float | None] | None: """ Limit on the integral term: ``None`` for none, a number for the limit :math:`\\pm` that number, or a (lower, upper) pair. """ if self._Iblock is None: return None lo, hi = self._Iblock.min, self._Iblock.max lo = None if lo is None else float(lo[0]) hi = None if hi is None else float(hi[0]) if lo is None and hi is None: return None if lo is not None and hi is not None and lo == -hi: return hi return lo, hi @I_limit.setter def I_limit(self, value: Any) -> None: if self._Iblock is None: raise ValueError(_NO_I) lo, hi = _integral_limits(value) self._Iblock.set_param("min", lo) self._Iblock.set_param("max", hi) @property def I_band(self) -> float | None: """The integral term only integrates while the error is smaller than this, or ``None``.""" return self._I_band @I_band.setter def I_band(self, value: float | None) -> None: if self._Iblock is None: raise ValueError(_NO_I) if value is not None and not value > 0: raise ValueError("I_band must be positive") self._Iblock.set_param("enable", _band_enable(value)) self._I_band = value
# --------------------------------------------------------------------------- # Compatibility shims # ------------------------------------------------------------------------ # class _NoiseMixin: r""" What ANOISE_S and MNOISE_S share: a noise sample held in the discrete state. The sample for tick :math:`k` is drawn in ``next()`` from a generator seeded with ``[seed, k]``, so it is a pure function of the seed and the tick index. That makes a run repeatable whatever the solver does between ticks, and harmless if ``next()`` is also called for ticks of other clocks, where the result is discarded. """ _mean: float # mean of each sample sigma: Vector1D seed: int | None def _check_sigma(self, value: Any) -> None: sigma = np.asarray(value, dtype=float) if sigma.ndim > 1: raise ValueError( f"sigma must be a scalar or a vector, got shape {sigma.shape}" ) if np.any(sigma < 0): raise ValueError("sigma must not be negative") def _check_seed(self, value: Any) -> None: if value is not None and (int(value) != value or value < 0): raise ValueError( f"seed must be a non-negative integer or None, got {value}" ) def _check_shapes(self) -> None: shape = np.shape(self.sigma) n = self._x0.size # type: ignore[attr-defined] if shape not in ((), (n,)): raise ValueError( f"sigma has shape {shape} but the block has {n} state element(s)" ) def reset(self) -> None: """A seeded block repeats every run, an unseeded one starts each run afresh.""" if self.seed is None: self._run_seed = int(np.random.SeedSequence().entropy) # type: ignore[arg-type] else: self._run_seed = int(self.seed) def configure(self) -> None: self._check_shapes() sig = self.insig[0] # type: ignore[attr-defined] if sig is None or sig.shape is None: return n = self._x0.size # type: ignore[attr-defined] size = int(np.prod(sig.shape)) if n != 1 and tuple(sig.shape) != self._x0.shape: # type: ignore[attr-defined] raise ValueError( f"{self.name}: input has shape {tuple(sig.shape)} but the noise has " # type: ignore[attr-defined] f"{n} element(s)" ) if n == 1 and size != 1: raise ValueError( f"{self.name}: input has {size} elements but the noise has one " # type: ignore[attr-defined] "sample per tick. Give sigma, or x0, one element per input element " "for independent noise on each" ) def next(self, t: float, u: list[Any], x: np.ndarray) -> np.ndarray: k = int(round((t - self._clock.offset) / self.T)) # type: ignore[attr-defined] rng = np.random.default_rng([self._run_seed, max(k, 0)]) return np.atleast_1d(rng.normal(self._mean, self.sigma, size=self._x0.shape)) # type: ignore[attr-defined] def _sample(self, x: np.ndarray) -> Any: return x.item() if x.size == 1 else x
[docs] class Anoise_S(_NoiseMixin, SampledBlock): r""" :blockname:`ANOISE_S` Additive noise block. :inputs: 1 :outputs: 1 :states: N .. list-table:: :header-rows: 1 * - Port type - Port number - Types - Description * - Input - 0 - float, ndarray - :math:`u` * - Output - 0 - float, ndarray - :math:`u + n` The output is the input plus a sample :math:`n` of zero-mean Gaussian noise with standard deviation ``sigma``. A new sample is drawn at each tick of the clock and held until the next, so the noise is piecewise constant and the solver sees a function of time. The input passes through continuously. The sample for tick :math:`k` depends only on ``seed`` and :math:`k`, so a seeded block gives the same noise in every run and whatever the solver does. If ``seed`` is ``None`` each run uses a new seed. ``sigma`` is a scalar or a vector. If the input is a vector the noise has one element per input element, independent of each other, so ``sigma`` (or ``x0``) must have that length. A vector ``sigma`` gives each element its own standard deviation. As for all sampled blocks the tick at :math:`t=0` is not applied, so the first sample period carries ``x0`` instead of noise. It is zero by default. For example:: anoise = bd.ANOISE_S(clock, sigma=0.1, seed=1) :seealso: :class:`Mnoise_S` :func:`numpy.random.default_rng` """ nin = 1 nout = 1 _mean = 0.0
[docs] def __init__( self, clock: Clock, sigma: Vector1D = 1, seed: int | None = None, x0: Vector1D | None = None, **blockargs: Any, ) -> None: r""" :param clock: clock source :param sigma: standard deviation of the noise, defaults to 1 :param seed: seed for the random number generator, defaults to None :param x0: noise during the first sample period, defaults to zero with the length of ``sigma`` :param blockargs: :meth:`common block options <bdsim.Block.__init__>` """ sigma_array = np.asarray(sigma, dtype=float) if x0 is None: x0 = np.zeros(sigma_array.size) super().__init__(clock=clock, x0=x0, feedthrough=True, **blockargs) self.sigma = sigma self.seed = seed self._check_sigma(sigma) self._check_seed(seed) self._check_shapes() self.reset() self.add_param("clock", when="never") self.add_param("sigma", coerce=False, validate=self._check_sigma) self.add_param("seed", coerce=False, when="setup", validate=self._check_seed) self.add_param("x0", when="setup")
def output(self, t: float, u: list[Any], x: np.ndarray) -> list[Any]: return [u[0] + self._sample(x)]
[docs] class Mnoise_S(_NoiseMixin, SampledBlock): r""" :blockname:`MNOISE_S` Multiplicative noise block. :inputs: 1 :outputs: 1 :states: N .. list-table:: :header-rows: 1 * - Port type - Port number - Types - Description * - Input - 0 - float, ndarray - :math:`u` * - Output - 0 - float, ndarray - :math:`u \times n` The output is the input times a sample :math:`n` of Gaussian noise with unit mean and standard deviation ``sigma``. A new sample is drawn at each tick of the clock and held until the next, so the noise is piecewise constant and the solver sees a function of time. The input passes through continuously. The sample for tick :math:`k` depends only on ``seed`` and :math:`k`, so a seeded block gives the same noise in every run and whatever the solver does. If ``seed`` is ``None`` each run uses a new seed. ``sigma`` is a scalar or a vector. If the input is a vector the noise has one element per input element, independent of each other, so ``sigma`` (or ``x0``) must have that length. A vector ``sigma`` gives each element its own standard deviation. As for all sampled blocks the tick at :math:`t=0` is not applied, so the first sample period carries ``x0`` instead of noise. It is one by default. For example:: mnoise = bd.MNOISE_S(clock, sigma=0.1, seed=1) :seealso: :class:`Anoise_S` :func:`numpy.random.default_rng` """ nin = 1 nout = 1 _mean = 1.0
[docs] def __init__( self, clock: Clock, sigma: Vector1D = 1, seed: int | None = None, x0: Vector1D | None = None, **blockargs: Any, ) -> None: r""" :param clock: clock source :param sigma: standard deviation of the noise, defaults to 1 :param seed: seed for the random number generator, defaults to None :param x0: noise during the first sample period, defaults to one with the length of ``sigma`` :param blockargs: :meth:`common block options <bdsim.Block.__init__>` """ sigma_array = np.asarray(sigma, dtype=float) if x0 is None: x0 = np.ones(sigma_array.size) super().__init__(clock=clock, x0=x0, feedthrough=True, **blockargs) self.sigma = sigma self.seed = seed self._check_sigma(sigma) self._check_seed(seed) self._check_shapes() self.reset() self.add_param("clock", when="never") self.add_param("sigma", coerce=False, validate=self._check_sigma) self.add_param("seed", coerce=False, when="setup", validate=self._check_seed) self.add_param("x0", when="setup")
def output(self, t: float, u: list[Any], x: np.ndarray) -> list[Any]: return [u[0] * self._sample(x)]
# ------------------------------------------------------------------------ #
[docs] @deprecated_block("Integrator_S") class DIntegrator(Integrator_S): r""" :blockname:`DINTEGRATOR <INTEGRATOR_S>` Deprecated: use ``Integrator_S`` instead. """
if __name__ == "__main__": # pragma: no cover from pathlib import Path import subprocess import sys root = Path(__file__).resolve().parents[3] test_file = ( root / "tests" / "blocks" / f"test_blocks_{Path(__file__).stem.lower()}.py" ) if not test_file.exists(): print(f"No module unit tests found for {Path(__file__).name}: {test_file}") raise SystemExit(0) raise SystemExit(subprocess.call([sys.executable, "-m", "pytest", str(test_file)]))