pySimBlocks 0.1.0__py3-none-any.whl

pySimBlocks/blocks/__init__.py

@@ -0,0 +1,68 @@
+# ******************************************************************************
+#                                  pySimBlocks
+#                     Copyright (c) 2026 Université de Lille & INRIA
+# ******************************************************************************
+#  This program is free software: you can redistribute it and/or modify it
+#  under the terms of the GNU Lesser General Public License as published by
+#  the Free Software Foundation, either version 3 of the License, or (at your
+#  option) any later version.
+#
+#  This program is distributed in the hope that it will be useful, but WITHOUT
+#  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+#  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License
+#  for more details.
+#
+#  You should have received a copy of the GNU Lesser General Public License
+#  along with this program.  If not, see <https://www.gnu.org/licenses/>.
+# ******************************************************************************
+#  Authors: see Authors.txt
+# ******************************************************************************
+
+from pySimBlocks.blocks.controllers import Pid, StateFeedback
+from pySimBlocks.blocks.interfaces import ExternalInput, ExternalOutput
+from pySimBlocks.blocks.observers import Luenberger
+from pySimBlocks.blocks.operators import (
+    DeadZone, Delay, DiscreteDerivator, DiscreteIntegrator,
+    Gain, Mux, Product, RateLimiter, Saturation, Sum, ZeroOrderHold
+)
+from pySimBlocks.blocks.sources import (
+    Constant,
+    FunctionSource,
+    Ramp,
+    Sinusoidal,
+    Step,
+    WhiteNoise,
+)
+from pySimBlocks.blocks.systems import LinearStateSpace, PolytopicStateSpace
+
+__all__ = [
+    "Pid",
+    "StateFeedback",
+
+    "ExternalInput",
+    "ExternalOutput",
+
+    "Luenberger",
+
+    "DeadZone",
+    "Delay",
+    "DiscreteDerivator",
+    "DiscreteIntegrator",
+    "Gain",
+    "Mux",
+    "Product",
+    "RateLimiter",
+    "Saturation",
+    "Sum",
+    "ZeroOrderHold",
+
+    "Constant",
+    "FunctionSource",
+    "Ramp",
+    "Step",
+    "Sinusoidal",
+    "WhiteNoise",
+
+    "LinearStateSpace",
+    "PolytopicStateSpace",
+]

pySimBlocks/blocks/controllers/__init__.py

@@ -0,0 +1,27 @@
+# ******************************************************************************
+#                                  pySimBlocks
+#                     Copyright (c) 2026 Université de Lille & INRIA
+# ******************************************************************************
+#  This program is free software: you can redistribute it and/or modify it
+#  under the terms of the GNU Lesser General Public License as published by
+#  the Free Software Foundation, either version 3 of the License, or (at your
+#  option) any later version.
+#
+#  This program is distributed in the hope that it will be useful, but WITHOUT
+#  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+#  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License
+#  for more details.
+#
+#  You should have received a copy of the GNU Lesser General Public License
+#  along with this program.  If not, see <https://www.gnu.org/licenses/>.
+# ******************************************************************************
+#  Authors: see Authors.txt
+# ******************************************************************************
+
+from pySimBlocks.blocks.controllers.pid import Pid
+from pySimBlocks.blocks.controllers.state_feedback import StateFeedback
+
+__all__ = [
+    "Pid",
+    "StateFeedback"
+]

pySimBlocks/blocks/controllers/pid.py

@@ -0,0 +1,250 @@
+# ******************************************************************************
+#                                  pySimBlocks
+#                     Copyright (c) 2026 Université de Lille & INRIA
+# ******************************************************************************
+#  This program is free software: you can redistribute it and/or modify it
+#  under the terms of the GNU Lesser General Public License as published by
+#  the Free Software Foundation, either version 3 of the License, or (at your
+#  option) any later version.
+#
+#  This program is distributed in the hope that it will be useful, but WITHOUT
+#  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+#  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License
+#  for more details.
+#
+#  You should have received a copy of the GNU Lesser General Public License
+#  along with this program.  If not, see <https://www.gnu.org/licenses/>.
+# ******************************************************************************
+#  Authors: see Authors.txt
+# ******************************************************************************
+
+import warnings
+
+import numpy as np
+from numpy.typing import ArrayLike
+
+from pySimBlocks.core.block import Block
+
+
+class Pid(Block):
+    """
+    Discrete-time PID controller block.
+
+    Summary:
+        Implements a single-input single-output discrete PID controller,
+        similar to the Simulink PID block. The controller computes a control
+        command from an error signal using proportional, integral and/or
+        derivative actions, depending on the selected control mode.
+
+    Parameters (overview):
+        controller : str
+            Control mode. One of {"P", "PI", "PD", "PID"}.
+        Kp : float
+            Proportional gain.
+        Ki : float
+            Integral gain.
+        Kd : float
+            Derivative gain.
+        u_min : float, optional
+            Minimum output saturation.
+        u_max : float, optional
+            Maximum output saturation.
+        sample_time : float, optional
+            Controller sampling period.
+
+    I/O:
+        Inputs:
+            e : error signal.
+        Outputs:
+            u : control command.
+
+    Notes:
+        - The block is strictly SISO.
+        - Integral action introduces internal state.
+        - If a parameter is not provided, the block falls back to its
+          internal default value.
+        - Output saturation is applied only if u_min and/or u_max are defined.
+    """
+
+    def __init__(
+        self,
+        name: str,
+        controller: str = "PID",
+        Kp: ArrayLike = 0.0,
+        Ki: ArrayLike = 0.0,
+        Kd: ArrayLike = 0.0,
+        u_min: ArrayLike | None = None,
+        u_max: ArrayLike | None = None,
+        integration_method: str = "euler forward",
+        sample_time: float | None = None,
+    ):
+        super().__init__(name, sample_time)
+
+        controller = controller.upper()
+        allowed_types = {"P", "I", "PI", "PD", "PID"}
+        if controller not in allowed_types:
+            raise ValueError(
+                f"[{self.name}] Invalid controller type '{controller}'. Allowed: {allowed_types}"
+            )
+        self.controller = controller
+
+        self.integration_method = integration_method.lower()
+        allowed = ("euler forward", "euler backward")
+        if self.integration_method not in allowed:
+            raise ValueError(
+                f"[{self.name}] Unsupported method '{self.integration_method}'. Allowed: {allowed}"
+            )
+
+        # Gains (SISO, (1,1))
+        self.Kp = self._to_siso("Kp", Kp)
+        self.Ki = self._to_siso("Ki", Ki)
+        self.Kd = self._to_siso("Kd", Kd)
+
+        self.u_min = None if u_min is None else self._to_siso("u_min", u_min)
+        self.u_max = None if u_max is None else self._to_siso("u_max", u_max)
+
+        if self.u_min is not None and self.u_max is not None:
+            if float(self.u_min[0, 0]) > float(self.u_max[0, 0]):
+                raise ValueError(
+                    f"[{self.name}] u_min ({self.u_min.item()}) must be <= u_max ({self.u_max.item()})."
+                )
+
+        # Warnings
+        self._validate_gains()
+
+        # Direct feedthrough policy
+        has_p = "P" in self.controller
+        has_d = "D" in self.controller
+        has_i = "I" in self.controller
+
+        if has_p or has_d:
+            self.direct_feedthrough = True
+        else:
+            # I-only
+            self.direct_feedthrough = (self.integration_method == "euler backward")
+
+        # Ports
+        self.inputs["e"] = None
+        self.outputs["u"] = None
+
+        # State (SISO)
+        self.state["x_i"] = np.zeros((1, 1), dtype=float)
+        self.state["e_prev"] = np.zeros((1, 1), dtype=float)
+        self.next_state["x_i"] = np.zeros((1, 1), dtype=float)
+        self.next_state["e_prev"] = np.zeros((1, 1), dtype=float)
+
+
+    # --------------------------------------------------------------------------
+    # Public methods
+    # --------------------------------------------------------------------------
+    def initialize(self, t0: float):
+        # Start at zero command, keep internal states at zero.
+        self.outputs["u"] = np.zeros((1, 1), dtype=float)
+
+    # ------------------------------------------------------------------
+    def output_update(self, t: float, dt: float):
+        e_in = self.inputs["e"]
+        if e_in is None:
+            raise RuntimeError(f"[{self.name}] Missing input 'e'.")
+
+        e = self._to_siso("e", e_in)
+
+        x_i = self.state["x_i"]
+        e_prev = self.state["e_prev"]
+
+        has_p = "P" in self.controller
+        has_i = "I" in self.controller
+        has_d = "D" in self.controller
+
+        P = self.Kp * e if has_p else np.zeros((1, 1), dtype=float)
+
+        if has_i:
+            if self.integration_method == "euler forward":
+                I = x_i
+            else:
+                I = x_i + self.Ki * e * dt
+        else:
+            I = np.zeros((1, 1), dtype=float)
+
+        D = (self.Kd * (e - e_prev) / dt) if has_d else np.zeros((1, 1), dtype=float)
+
+        u = P + I + D
+
+        # Saturation
+        if self.u_min is not None:
+            u = np.maximum(u, self.u_min)
+        if self.u_max is not None:
+            u = np.minimum(u, self.u_max)
+
+        self.outputs["u"] = u
+
+    # ------------------------------------------------------------------
+    def state_update(self, t: float, dt: float):
+        e_in = self.inputs["e"]
+        if e_in is None:
+            raise RuntimeError(f"[{self.name}] Missing input 'e'.")
+
+        e = self._to_siso("e", e_in)
+
+        has_i = "I" in self.controller
+
+        # Integrator update only if I term is enabled
+        if has_i:
+            x_i_next = self.state["x_i"] + self.Ki * e * dt
+        else:
+            x_i_next = self.state["x_i"].copy()
+
+        # Anti-windup (clamp integral state if saturation is defined)
+        if self.u_min is not None:
+            x_i_next = np.maximum(x_i_next, self.u_min)
+        if self.u_max is not None:
+            x_i_next = np.minimum(x_i_next, self.u_max)
+
+        self.next_state["x_i"] = x_i_next
+        self.next_state["e_prev"] = e.copy()
+
+
+    # --------------------------------------------------------------------------
+    # Private methods
+    # --------------------------------------------------------------------------
+    def _to_siso(self, name: str, value: ArrayLike) -> np.ndarray:
+        """
+        Normalize scalar-like into (1,1). Reject anything else (strict SISO).
+        """
+        if np.isscalar(value):
+            return np.array([[float(value)]], dtype=float)
+
+        arr = np.asarray(value, dtype=float)
+
+        if arr.shape == ():
+            return np.array([[float(arr)]], dtype=float)
+        if arr.shape == (1,):
+            return arr.reshape(1, 1)
+        if arr.shape == (1, 1):
+            return arr
+
+        raise ValueError(
+            f"[{self.name}] '{name}' must be scalar-like ((), (1,), or (1,1)). Got shape {arr.shape}."
+        )
+
+    # ------------------------------------------------------------------
+    def _validate_gains(self) -> None:
+        kp = float(self.Kp[0, 0])
+        ki = float(self.Ki[0, 0])
+        kd = float(self.Kd[0, 0])
+
+        if "P" in self.controller and kp == 0.0:
+            warnings.warn(
+                f"[{self.name}] Kp=0 while controller '{self.controller}' includes a P term.",
+                UserWarning,
+            )
+        if "I" in self.controller and ki == 0.0:
+            warnings.warn(
+                f"[{self.name}] Ki=0 while controller '{self.controller}' includes an I term.",
+                UserWarning,
+            )
+        if "D" in self.controller and kd == 0.0:
+            warnings.warn(
+                f"[{self.name}] Kd=0 while controller '{self.controller}' includes a D term.",
+                UserWarning,
+            )

pySimBlocks/blocks/controllers/state_feedback.py

@@ -0,0 +1,141 @@
+# ******************************************************************************
+#                                  pySimBlocks
+#                     Copyright (c) 2026 Université de Lille & INRIA
+# ******************************************************************************
+#  This program is free software: you can redistribute it and/or modify it
+#  under the terms of the GNU Lesser General Public License as published by
+#  the Free Software Foundation, either version 3 of the License, or (at your
+#  option) any later version.
+#
+#  This program is distributed in the hope that it will be useful, but WITHOUT
+#  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+#  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License
+#  for more details.
+#
+#  You should have received a copy of the GNU Lesser General Public License
+#  along with this program.  If not, see <https://www.gnu.org/licenses/>.
+# ******************************************************************************
+#  Authors: see Authors.txt
+# ******************************************************************************
+
+import numpy as np
+from pySimBlocks.core.block import Block
+
+
+class StateFeedback(Block):
+    """
+    Discrete-time state-feedback controller block.
+
+    Summary:
+        Implements a static discrete-time state-feedback control law:
+            u = G @ r - K @ x
+
+    Parameters:
+        K : array-like, shape (m, n)
+            State feedback gain matrix.
+        G : array-like, shape (m, p)
+            Reference feedforward gain matrix.
+        sample_time : float, optional
+            Block execution period.
+
+    Inputs:
+        r : array (p, 1)
+            Reference vector.
+        x : array (n, 1)
+            State vector.
+
+    Outputs:
+        u : array (m, 1)
+            Control vector.
+
+    Notes:
+        - Stateless block.
+        - This block intentionally enforces column-vector inputs.
+        - No implicit flattening is performed.
+    """
+
+    direct_feedthrough = True
+
+    def __init__(self, name: str, K, G, sample_time: float | None = None):
+        super().__init__(name, sample_time)
+
+        self.K = np.asarray(K, dtype=float)
+        self.G = np.asarray(G, dtype=float)
+
+        if self.K.ndim != 2:
+            raise ValueError(f"[{self.name}] K must be a 2D array (m,n). Got shape {self.K.shape}.")
+        if self.G.ndim != 2:
+            raise ValueError(f"[{self.name}] G must be a 2D array (m,p). Got shape {self.G.shape}.")
+
+        m, n = self.K.shape
+        m2, p = self.G.shape
+
+        if m != m2:
+            raise ValueError(
+                f"[{self.name}] Inconsistent dimensions: "
+                f"K is {self.K.shape} while G is {self.G.shape} (first dimension must match)."
+            )
+
+        # cached expected sizes for input validation
+        self._m = m
+        self._n = n
+        self._p = p
+
+        # Ports
+        self.inputs["r"] = None
+        self.inputs["x"] = None
+        self.outputs["u"] = None
+
+        # freeze input shapes once seen (optional but consistent)
+        self._input_shapes = {}
+
+    # --------------------------------------------------------------------------
+    # Public methods
+    # --------------------------------------------------------------------------
+    def initialize(self, t0: float):
+        r = self.inputs["r"]
+        x = self.inputs["x"]
+        if r is None or x is None:
+            self.outputs["u"] = np.zeros((self._m, 1))
+            return
+
+        try:
+            r = self._require_col_vector("r", self._p)
+            x = self._require_col_vector("x", self._n)
+            self.outputs["u"] = self.G @ r - self.K @ x
+        except Exception as _:
+            self.outputs["u"] = np.zeros((self._m, 1))
+
+    # ------------------------------------------------------------------
+    def output_update(self, t: float, dt: float):
+        r = self._require_col_vector("r", self._p)
+        x = self._require_col_vector("x", self._n)
+
+        self.outputs["u"] = self.G @ r - self.K @ x
+
+    # ------------------------------------------------------------------
+    def state_update(self, t: float, dt: float):
+        pass
+
+
+    # --------------------------------------------------------------------------
+    # Private methods
+    # --------------------------------------------------------------------------
+    def _require_col_vector(self, port: str, expected_rows: int) -> np.ndarray:
+        u = self.inputs[port]
+        if u is None:
+            raise RuntimeError(f"[{self.name}] Input '{port}' is not connected or not set.")
+
+        arr = np.asarray(u, dtype=float)
+
+        if arr.ndim != 2 or arr.shape[1] != 1:
+            raise ValueError(
+                f"[{self.name}] Input '{port}' must be a column vector (n,1). Got shape {arr.shape}."
+            )
+
+        if arr.shape[0] != expected_rows:
+            raise ValueError(
+                f"[{self.name}] Input '{port}' has wrong dimension: expected ({expected_rows},1), got {arr.shape}."
+            )
+
+        return arr

pySimBlocks/blocks/interfaces/__init__.py

@@ -0,0 +1,27 @@
+# ******************************************************************************
+#                                  pySimBlocks
+#                     Copyright (c) 2026 Université de Lille & INRIA
+# ******************************************************************************
+#  This program is free software: you can redistribute it and/or modify it
+#  under the terms of the GNU Lesser General Public License as published by
+#  the Free Software Foundation, either version 3 of the License, or (at your
+#  option) any later version.
+#
+#  This program is distributed in the hope that it will be useful, but WITHOUT
+#  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+#  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License
+#  for more details.
+#
+#  You should have received a copy of the GNU Lesser General Public License
+#  along with this program.  If not, see <https://www.gnu.org/licenses/>.
+# ******************************************************************************
+#  Authors: see Authors.txt
+# ******************************************************************************
+
+from pySimBlocks.blocks.interfaces.external_input import ExternalInput
+from pySimBlocks.blocks.interfaces.external_output import ExternalOutput
+
+__all__ = [
+    "ExternalInput",
+    "ExternalOutput"
+]

pySimBlocks/blocks/interfaces/external_input.py

@@ -0,0 +1,107 @@
+# ******************************************************************************
+#                                  pySimBlocks
+#                     Copyright (c) 2026 Université de Lille & INRIA
+# ******************************************************************************
+#  This program is free software: you can redistribute it and/or modify it
+#  under the terms of the GNU Lesser General Public License as published by
+#  the Free Software Foundation, either version 3 of the License, or (at your
+#  option) any later version.
+#
+#  This program is distributed in the hope that it will be useful, but WITHOUT
+#  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+#  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License
+#  for more details.
+#
+#  You should have received a copy of the GNU Lesser General Public License
+#  along with this program.  If not, see <https://www.gnu.org/licenses/>.
+# ******************************************************************************
+#  Authors: see Authors.txt
+# ******************************************************************************
+
+import numpy as np
+from pySimBlocks.core.block import Block
+
+
+class ExternalInput(Block):
+    """
+    External input interface block.
+
+    Summary:
+        Pass-through block for external real-time measurements.
+
+    Parameters:
+        sample_time: float, optional
+            Execution period of the block. If not specified, the simulator time
+            step is used.
+
+    I/O:
+        Input:
+            in: array (n,1)
+                External measurement value. Scalar, (n,), (n,1) accepted.
+        Output:
+            out: array (n,1)
+                Measurement forwarded to the model as a column vector (n,1).
+    Policy:
+        - Accepts scalar, (n,), (n,1)
+        - Outputs strict (n,1)
+        - Once shape is known, it is frozen (cannot change)
+    """
+
+    direct_feedthrough = True
+
+    def __init__(self, name: str, sample_time: float | None = None):
+        super().__init__(name, sample_time)
+        self.inputs["in"] = None
+        self.outputs["out"] = None
+        self._resolved_shape: tuple[int, int] | None = None
+
+
+    # --------------------------------------------------------------------------
+    # Public methods
+    # --------------------------------------------------------------------------
+    def initialize(self, t0: float) -> None:
+        u = self.inputs["in"]
+        if u is None:
+            self.outputs["out"] = np.zeros((1, 1))
+            return
+
+        self.outputs["out"] = self._to_col_vec(u)
+
+    # ------------------------------------------------------------------
+    def output_update(self, t: float, dt: float) -> None:
+        u = self.inputs["in"]
+        if u is None:
+            raise RuntimeError(f"[{self.name}] Missing input 'in'.")
+        self.outputs["out"] = self._to_col_vec(u)
+
+    # ------------------------------------------------------------------
+    def state_update(self, t: float, dt: float) -> None:
+        pass
+
+
+    # --------------------------------------------------------------------------
+    # Private methods
+    # --------------------------------------------------------------------------
+    def _to_col_vec(self, value) -> np.ndarray:
+        arr = np.asarray(value, dtype=float)
+
+        if arr.ndim == 0:
+            arr = arr.reshape(1, 1)
+        elif arr.ndim == 1:
+            arr = arr.reshape(-1, 1)
+        elif arr.ndim == 2 and arr.shape[1] == 1:
+            pass
+        else:
+            raise ValueError(
+                f"[{self.name}] Input 'in' must be scalar, (n,), or (n,1). Got shape {arr.shape}."
+            )
+
+        # Freeze shape
+        if self._resolved_shape is None:
+            self._resolved_shape = arr.shape
+        elif arr.shape != self._resolved_shape:
+            raise ValueError(
+                f"[{self.name}] Input 'in' shape changed: expected {self._resolved_shape}, got {arr.shape}."
+            )
+
+        return arr