feat: add linear sys feedforward

pySimBlocks/blocks/sources/file_source.py

@@ -38,9 +38,12 @@ class FileSource(BlockSource):
 
     - ``.npz`` / ``.npy``: 1D ``(N,)`` treated as ``(N, 1)``, or 2D ``(N, n)``
       where N is the number of samples and n the signal dimension. Each step
-      outputs a ``(n, 1)`` column vector.
+      outputs a ``(n, 1)`` column vector. If the stored array is instead
+      ``(n, N)`` (signals in rows, samples in columns), set ``transpose=True``
+      to have it transposed to ``(N, n)`` before use.
     - ``.csv``: a single column is selected by ``key``, always producing shape
-      ``(N, 1)``. Output per step is ``(1, 1)``.
+      ``(N, 1)``. Output per step is ``(1, 1)``. ``transpose`` is not
+      applicable to CSV input.
 
     Alternatively, when ``use_time=True``, the output is selected by
     looking up the closest past timestamp in a time column bundled with
@@ -52,6 +55,8 @@ class FileSource(BlockSource):
         key: Array key (NPZ) or column name (CSV) to load. None for NPY files.
         repeat: If True, restart from the first sample after the last one.
         use_time: If True, select samples by time lookup instead of index.
+        transpose: If True (NPZ/NPY only), transpose the loaded 2D array
+            before use, i.e. treat it as ``(n, N)`` instead of ``(N, n)``.
     """
 
     VALID_FILE_TYPES = {"npz", "npy", "csv"}
@@ -63,6 +68,7 @@ def __init__(
         key: str | None = None,
         repeat: bool = False,
         use_time: bool = False,
+        transpose: bool = False,
         sample_time: float | None = None,
     ):
         """Initialize a FileSource block.
@@ -77,13 +83,18 @@ def __init__(
             use_time: If True, select samples by nearest past timestamp
                 instead of advancing by step index. Requires a ``"time"``
                 key or column in the file.
+            transpose: If True, transpose the loaded 2D array before use.
+                Only supported for NPZ and NPY inputs, where it allows
+                loading data stored as ``(n, N)`` (signals in rows) instead
+                of the expected ``(N, n)`` (samples in rows).
             sample_time: Sampling period in seconds, or None to use the
                 global simulation dt.
 
         Raises:
             ValueError: If the file extension is unsupported, if ``use_time``
-                is combined with an NPY file or with ``repeat=True``, or if
-                the loaded data is invalid.
+                is combined with an NPY file or with ``repeat=True``, if
+                ``transpose`` is combined with a CSV file, or if the loaded
+                data is invalid.
             FileNotFoundError: If the file does not exist.
         """
         super().__init__(name, sample_time)
@@ -93,6 +104,7 @@ def __init__(
         self.key = key
         self.repeat = self._to_bool(repeat, "repeat")
         self.use_time = self._to_bool(use_time, "use_time")
+        self.transpose = self._to_bool(transpose, "transpose")
 
         if self.use_time and self.file_type == "npy":
             raise ValueError(
@@ -102,6 +114,10 @@ def __init__(
             raise ValueError(
                 f"[{self.name}] repeat cannot be used when use_time=True."
             )
+        if self.transpose and self.file_type == "csv":
+            raise ValueError(
+                f"[{self.name}] transpose is supported only for NPZ and NPY inputs."
+            )
 
         self._time: np.ndarray | None = None
         self._samples = self._load_samples()
@@ -198,14 +214,23 @@ def _load_samples(self) -> np.ndarray:
 
         if arr.ndim == 1:
             arr = arr.reshape(-1, 1)
-        elif arr.ndim != 2:
+        elif arr.ndim == 2:
+            if self.transpose:
+                arr = arr.T
+        else:
             raise ValueError(
                 f"[{self.name}] Loaded data must be 1D or 2D. Got shape {arr.shape}."
             )
 
         if arr.shape[0] == 0:
             raise ValueError(f"[{self.name}] Loaded file contains no samples.")
 
+        if time is not None and time.shape[0] != arr.shape[0]:
+            raise ValueError(
+                f"[{self.name}] time length ({time.shape[0]}) must match number "
+                f"of samples ({arr.shape[0]}) after applying transpose."
+            )
+
         self._time = time
 
         return arr.astype(float, copy=False)

pySimBlocks/blocks/systems/linear_state_space.py

@@ -29,20 +29,25 @@
 class LinearStateSpace(Block):
     """Discrete-time linear state-space system block.
 
-    Implements a strictly proper discrete-time linear system:
+    Implements a discrete-time linear system:
 
         x[k+1] = A x[k] + B u[k]
-
-        y[k]   = C x[k]
 
-    The D matrix is intentionally not supported to avoid algebraic loops.
+        y[k]   = C x[k]          (if D is None)
+        y[k]   = C x[k] + D u[k] (if D is provided)
+
+    When D is None the block is strictly proper (no direct feedthrough).
+    When D is provided the block has direct feedthrough and algebraic loops
+    involving this block will be detected and rejected at compile time.
 
     Attributes:
         A: State transition matrix of shape (n, n).
         B: Input matrix of shape (n, m).
         C: Output matrix of shape (p, n).
+        D: Feedthrough matrix of shape (p, m), or None.
     """
 
+    # Default at class level; overridden per-instance when D is provided.
     direct_feedthrough = False
 
     def __init__(
@@ -51,6 +56,7 @@ def __init__(
         A: ArrayLike,
         B: ArrayLike,
         C: ArrayLike,
+        D: ArrayLike | None = None,
         x0: ArrayLike | None = None,
         sample_time: float | None = None,
     ):
@@ -61,6 +67,9 @@ def __init__(
             A: State transition matrix, array-like of shape (n, n).
             B: Input matrix, array-like of shape (n, m).
             C: Output matrix, array-like of shape (p, n).
+            D: Feedthrough matrix, array-like of shape (p, m), or None.
+                When provided, the block gains direct feedthrough and
+                y[k] = C x[k] + D u[k].
             x0: Initial state vector, array-like of shape (n, 1) or (n,).
                 Defaults to zeros.
             sample_time: Sampling period in seconds, or None to use the
@@ -101,6 +110,23 @@ def __init__(
         self._m = self.B.shape[1]
         self._p = self.C.shape[0]
 
+        # --- D matrix (optional) ---
+        if D is not None:
+            self.D = np.asarray(D, dtype=float)
+            if self.D.ndim != 2:
+                raise ValueError(f"[{self.name}] D must be 2D. Got shape {self.D.shape}.")
+            if self.D.shape != (self._p, self._m):
+                raise ValueError(
+                    f"[{self.name}] D must have shape ({self._p}, {self._m}). "
+                    f"Got {self.D.shape}."
+                )
+            # Override direct_feedthrough at the instance level so the
+            # scheduler sees this block as having direct feedthrough.
+            self.direct_feedthrough = True
+        else:
+            self.D = None
+
+        # --- Initial state ---
         if x0 is None:
             x0_arr = np.zeros((n, 1), dtype=float)
         else:
@@ -143,12 +169,21 @@ def initialize(self, t0: float) -> None:
     def output_update(self, t: float, dt: float) -> None:
         """Compute y and x outputs from the committed state.
 
+        When D is provided, u[k] is read at this step (direct feedthrough).
+
         Args:
             t: Current simulation time in seconds.
             dt: Current time step in seconds.
         """
         x = self.state["x"]
-        self.outputs["y"] = self.C @ x
+        if self.D is not None:
+            u = self.inputs["u"]
+            if u is None:
+                raise RuntimeError(f"[{self.name}] Input 'u' is not connected or not set.")
+            u_vec = self._to_col_vec("u", u, self._m)
+            self.outputs["y"] = self.C @ x + self.D @ u_vec
+        else:
+            self.outputs["y"] = self.C @ x
         self.outputs["x"] = x.copy()
 
     def state_update(self, t: float, dt: float) -> None:

pySimBlocks/blocks/systems/sofa/sofa_controller.py

@@ -20,6 +20,8 @@
 
 from __future__ import annotations
 
+import atexit
+import os
 from pathlib import Path
 from typing import Any, Dict, List
 
@@ -69,10 +71,11 @@ class SofaPysimBlocksController(Sofa.Core.Controller):
         verbose: If True, print logged variables at each control step.
     """
 
-    def __init__(self, name: str = "SofaControllerGui"):
+    def __init__(self, project_yaml: str, name: str = "SofaControllerGui"):
         """Initialize the SOFA–pySimBlocks controller.
 
         Args:
+            project_yaml: Path to the pySimBlocks YAML project file.
             name: Name passed to the SOFA controller base class.
         """
         super().__init__(name=name)
@@ -91,9 +94,16 @@ def __init__(self, name: str = "SofaControllerGui"):
         self.sim: Simulator | None = None
         self.step_index: int = 0
 
-        self.project_yaml: str | None = None
+        self.project_yaml: str = project_yaml
         self._init_failed = False
 
+        print(f"[pySimBlocks] Controller using project_yaml: {project_yaml}")
+
+        # --- dump des logs, uniquement si lancé depuis pySimBlocks GUI ---
+        self._dump_logs_path: Path | None = None
+        if os.environ.get("PYSIMBLOCKS_SOFA_DUMP_LOGS") == "1" and project_yaml is not None:
+            self._dump_logs_path = Path(project_yaml).parent / ".sofa_logs.npz"
+            atexit.register(self._dump_logs)
 
     # --------------------------------------------------------------------------
     # Public methods
@@ -427,3 +437,19 @@ def _adapt_model_for_sofa(self, model_data: Dict[str, Any]) -> Dict[str, Any]:
 
         adapted["blocks"] = adapted_blocks
         return adapted
+
+    def _dump_logs(self) -> None:
+        """Serialize self.sim.logs to .npz at process exit (GUI-triggered runs only)."""
+        if self._dump_logs_path is None or self.sim is None:
+            return
+        try:
+            arrays = {}
+            for var in self.sim.logs:
+                if var == "time":
+                    arrays["time"] = np.asarray(self.sim.logs["time"])
+                    continue
+                arrays[var] = self.sim.get_data(variable=var)
+            np.savez(self._dump_logs_path, **arrays)
+            print(f"[pySimBlocks] Logs dumped to {self._dump_logs_path}")
+        except Exception as e:
+            print(f"[pySimBlocks] WARNING: failed to dump logs: {e}")

pySimBlocks/cli.py

@@ -36,22 +36,12 @@ def _run_gui(project_dir: str | None) -> None:
 
 
 def _run_export(args: argparse.Namespace) -> None:
+    from pySimBlocks.project import generate_run_script
+
     project_yaml = Path(args.project_file) if args.project_file else None
     project_dir = Path(args.project_dir) if args.project_dir else Path(".")
     output = Path(args.out) if args.out else None
-
-    if args.sofa_controller:
-        try:
-            from pySimBlocks.project.generate_sofa_controller import generate_sofa_controller
-            generate_sofa_controller(project_dir=project_dir, project_yaml=project_yaml)
-        except Exception as e:
-            print(f"Error generating SOFA controller: {e}")
-            print("See SOFA integration documentation for troubleshooting.")
-            sys.exit(1)
-    else:
-        from pySimBlocks.project import generate_run_script
-
-        generate_run_script(project_dir=project_dir, project_yaml=project_yaml, output=output)
+    generate_run_script(project_dir=project_dir, project_yaml=project_yaml, output=output)
 
 
 def _run_update() -> None:
@@ -61,6 +51,15 @@ def _run_update() -> None:
     generate_blocks_index()
     print("pySimBlocks update complete.")
 
+def _run_sofa_init(args: argparse.Namespace) -> None:
+    from pySimBlocks.project.generate_sofa_scaffold import generate_sofa_scaffold
+
+    generate_sofa_scaffold(
+        name=args.name,
+        output_dir=args.directory,
+        force=args.force,
+    )
+
 
 def _build_parser() -> argparse.ArgumentParser:
     parser = argparse.ArgumentParser(
@@ -98,11 +97,22 @@ def _build_parser() -> argparse.ArgumentParser:
         help="Project directory containing project.yaml",
     )
     export_parser.add_argument("-o", "--out", help="Output run.py path")
-    export_parser.add_argument(
-        "-s",
-        "--sofa-controller",
+
+    sofa_init_parser = subparsers.add_parser(
+        "sofa-init",
+        help="Generate a starter SOFA scene + controller pair.",
+    )
+    sofa_init_parser.add_argument("name", help="Base name for the generated files (e.g. 'finger').")
+    sofa_init_parser.add_argument(
+        "-d", "--directory",
+        dest="directory",
+        default=None,
+        help="Output directory. Defaults to the current directory.",
+    )
+    sofa_init_parser.add_argument(
+        "-f", "--force",
         action="store_true",
-        help="Update SOFA controller from project.yaml instead of generating run.py.",
+        help="Overwrite existing files.",
     )
 
     subparsers.add_parser("update", help="Regenerate pySimBlocks blocks index.")
@@ -119,6 +129,8 @@ def main(argv: list[str] | None = None) -> None:
         _run_export(args)
     elif args.command == "update":
         _run_update()
+    elif args.command == "sofa-init":
+        _run_sofa_init(args)
     else:
         parser.print_help()
 

pySimBlocks/docs/blocks/sources/file_source.md

@@ -19,6 +19,7 @@ Supported file formats:
 | `key` | str | Mandatory for `*.npz` (array key) and `*.csv` (column name). Unused for `*.npy`. | True |
 | `repeat` | bool | End-of-file behavior. If `false`, outputs zeros after the last sample. If `true`, restarts from the first sample. | True (default: `False`) |
 | `use_time` | bool | If `true` (only for `*.npz` and `*.csv`), uses a `time` signal and applies ZOH: at time `t`, output sample at largest index `i` such that `T[i] <= t`. | True (default: `False`) |
+| `transpose` | bool | If `true` (only for `*.npz` and `*.npy`), transposes the loaded 2D array before use, i.e. treats data stored as `(n, N)` (signals in rows, samples in columns) as `(N, n)`. Not applicable to `*.csv`. | True (default: `False`) |
 | `sample_time` | float | Block sample time. If omitted, global simulation step is used. | True |
 
 ---
@@ -41,10 +42,12 @@ None.
 
 - File format is inferred from `file_path` extension (`.npz`, `.npy`, `.csv`).
 - `npz` / `npy`: array must be 1D `(N,)` or 2D `(N, n)` — N samples, n signal dimension. Output per step: `(n, 1)`.
-- `csv`: `key` selects a single named column, always `(N, 1)`. Output per step: `(1, 1)`.
+  - If your array is stored as `(n, N)` instead (e.g. exported from MATLAB as `states x time`), set `transpose=true` to have it converted to `(N, n)` before use.
+- `csv`: `key` selects a single named column, always `(N, 1)`. Output per step: `(1, 1)`. `transpose` is not applicable.
 - With `use_time=true`, `time` must exist and be strictly increasing.
   - `npz`: requires key `time`.
   - `csv`: requires column `time`.
+- `transpose` is applied before the `time` length check, so `time` must match the number of samples along the resulting first axis (after transpose, if enabled).
 
 ---
 © 2026 Université de Lille & INRIA - Licensed under LGPL-3.0-or-later