diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 0cce668..623fb79 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -87,7 +87,7 @@ jobs: run: make init - name: Run Pytest - run: make test + run: uv run pytest --cov=src --cov-report=term-missing --no-cov-on-fail --cov-report=xml --cov-fail-under=80 --ignore=tests/utils_test/csv_inspector_test.py --ignore=tests/utils_test/csv_player_test.py - name: Upload coverage XML artifact if: ${{ matrix.python-version == '3.12' }} diff --git a/README.md b/README.md index a25c35a..82fcf13 100644 --- a/README.md +++ b/README.md @@ -38,7 +38,7 @@ uv publish # make sure your version in pyproject.toml is updated or -Update the version number in pyproject.toml and imu_module/__init__.py +Update the version number in pyproject.toml and hip_controller/__init__.py Commit your changes and add a git tag v Push the tag git push --tag @@ -128,18 +128,7 @@ docker build -t hip-controller . docker run hip-controller ``` -## How to Import -Import the main module: -```python -from hip_controller import definitions -``` - -Import specific components: -```python -from hip_controller.control.app import AppController -from hip_controller.control.signal_processing.kalman_filter import KalmanFilter -from hip_controller.plotter.simulator import Simulator ``` ## Architecture and Flow @@ -161,12 +150,13 @@ The main entry point is `app.py` in the control module, which orchestrates the f Run the main application: ```python from hip_controller.control.app import WalkOnController -from hip_controller.definitions import SensorSignal -logger.info("Initializing the lower limb controller.") +from hip_controller.definitions import SensorSignal, BasicConfig +logger.info("Initializing the lower limb controller.") -self.controller_left = WalkOnController(reverse=False, plot=False) -self.controller_right = WalkOnController(reverse=True, plot=False) +config = BasicConfig(filtered=False) +self.controller_left = WalkOnController(left_limb=True, config=config) +self.controller_right = WalkOnController(left_limb=False, config=config) while True: signal_left = SensorSignal(timestamp=timestamp_left, angle_rad=data_left.quat.to_euler(seq="xyz").z, velocity_rad_per_sec=data_left.device_data.gyro.z) @@ -314,6 +304,9 @@ motor_command = limb_controller.step(signal) The following tree shows the important permanent files. Run `make tree` to update. ``` +├── .claude +│ └── skills +│ └── code-review-nathalie.md ├── data │ ├── evaluation_raw_data │ │ ├── incline_walk @@ -793,7 +786,6 @@ The following tree shows the important permanent files. Run `make tree` to updat │ │ ├── AB11_turn_and_step_1_right-turn_angle.csv │ │ ├── AB12_turn_and_step_1_right-turn_angle.csv │ │ └── AB13_turn_and_step_1_right-turn_angle.csv -│ ├── logs │ └── sensor_data │ ├── arduino2_2026_03_23.csv │ ├── arduino_2026_03_23.csv @@ -809,17 +801,17 @@ The following tree shows the important permanent files. Run `make tree` to updat │ ├── compare_all.py │ ├── compare_matlab.py │ ├── compare_matlab_module.py +│ ├── controller_simulator.py │ ├── csv_converter.py -│ ├── csv_utils.py +│ ├── csv_player.py │ ├── evaluation_matplotlib.py │ ├── evaluation_record.py │ ├── live_comparison_plot.py -│ ├── main.py │ ├── mat_to_csv.py │ ├── normalize_output_time.py -│ ├── reference_versus_calculated.py -│ ├── script.py -│ └── simulator.py +│ ├── plot_scenarios.py +│ ├── readme.md +│ └── reference_versus_calculated.py ├── src │ └── hip_controller │ ├── control @@ -834,6 +826,7 @@ The following tree shows the important permanent files. Run `make tree` to updat │ │ │ └── pid_controller.py │ │ ├── signal_processing │ │ │ ├── drift_removal.py +│ │ │ ├── filtering.py │ │ │ ├── sensor_preprocessor.py │ │ │ └── velocity_estimation.py │ │ ├── __init__.py @@ -848,6 +841,7 @@ The following tree shows the important permanent files. Run `make tree` to updat │ │ ├── csv_player.py │ │ └── live_phase_portrait.py │ ├── utils +│ │ ├── csv_utils.py │ │ ├── math_utils.py │ │ ├── state_space.py │ │ └── utils.py @@ -892,6 +886,7 @@ The following tree shows the important permanent files. Run `make tree` to updat ├── .gitignore ├── .pre-commit-config.yaml ├── .python-version +├── CLAUDE.md ├── CONTRIBUTING.md ├── Dockerfile ├── LICENSE diff --git a/pyproject.toml b/pyproject.toml index 8bc223d..6dd5ac1 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,6 @@ [project] name = "hip_controller" -version = "0.1.1" +version = "0.1.2" description = "Lower limb exosuit hip controller." readme = "README.md" authors = [ diff --git a/scripts/controller_simulator.py b/scripts/controller_simulator.py index 88f877c..b636cf6 100644 --- a/scripts/controller_simulator.py +++ b/scripts/controller_simulator.py @@ -12,25 +12,20 @@ from matplotlib import ticker from pyqtgraph import QtCore, QtWidgets # pragma: no cover - -from hip_controller.filters.second_order_low_pass_filter import ( - SecondOrderLowPassFilter, -) +from hip_controller.control.app import WalkOnController from hip_controller.definitions import ( DEFAULT_LOG_LEVEL, BasicConfig, LowPassFilterConfig, SolverType, - ExosuitData ) - -from src.hip_controller.plotter.csv_player import CSVPlayer -from dataclasses import dataclass -from scripts.csv_player import ScriptPlayer, ComparisonData -from hip_controller.control.app import WalkOnController -from scripts.live_comparison_plot import TimePlotterComparisonWindow +from hip_controller.filters.second_order_low_pass_filter import ( + SecondOrderLowPassFilter, +) from hip_controller.utils.utils import setup_logger - +from scripts.csv_player import ComparisonData, ScriptPlayer +from scripts.live_comparison_plot import TimePlotterComparisonWindow +from src.hip_controller.plotter.csv_player import CSVPlayer def simulate_comparison_dynamic( @@ -125,9 +120,9 @@ def update() -> None: - sensor_data : ExosuitData = player.get_sensor_data_from_csv() - controller_left.step(sensor_data.left) - controller_right.step(sensor_data.right) + step = player.get_sensor_data_from_csv() + controller_left.step(step.sensor_data.left) + controller_right.step(step.sensor_data.right) # setInterval in miliseconds. Update each 10ms timer.setInterval(10) @@ -275,7 +270,9 @@ def plot_notch_filter_debug( if __name__ == "__main__": simulate(input_name="x", expected_output_name="y", func=lpf.step, path=TESTING_DIR / "controller_test/low_level_testing/low_level_testing_data/second_order_lpf_2026_03_06.csv") """ - from hip_controller.control.signal_processing.sensor_preprocessor import SensorPreprocessor + from hip_controller.control.signal_processing.sensor_preprocessor import ( + SensorPreprocessor, + ) from hip_controller.definitions import PreprocessorConfig preprocessor = SensorPreprocessor(PreprocessorConfig()) diff --git a/scripts/readme.md b/scripts/readme.md new file mode 100644 index 0000000..e4c6fef --- /dev/null +++ b/scripts/readme.md @@ -0,0 +1,124 @@ +# Scripts Folder + +This document describes the purpose of each script in `scripts/` and how they are intended to be used. + + +## Script descriptions + +### `compare_all.py` +A comparison utility for MATLAB reference data and module output data. It parses matched MATLAB/output CSV pairs, computes RMSE/MAPE metrics, and generates comparison plots. + +### `compare_matlab.py` +Same purpose as `compare_all.py`: compare Simulink/MATLAB CSV data against module output data. Use it to inspect phase and motor command agreement between MATLAB and Python results. + +### `compare_matlab_module.py` +A second comparison helper with the same MATLAB vs module output workflow. It is also intended for RMSE/MAPE calculation and plotting of matched CSV pairs. + +### `controller_simulator.py` +A GUI-focused simulation script. It can replay CSV data through the controller and display live comparison plots. Key functions: +- `simulate_controller_with_data(...)` for full controller playback +- `simulate_comparison_dynamic(...)` for comparing actual output with expected output + +### `csv_converter.py` +CSV processing utilities for walking data. Primary helpers: +- `concatenate_stairs(...)` to merge stair trial CSVs per participant +- `combine_two_files(...)` to join two CSVs while keeping the selected columns + +### `csv_player.py` +A stateful CSV player for real-time-style playback. It loads a CSV once and returns one row at a time so GUI/plotting components can simulate sensor streaming. + +### `evaluation_matplotlib.py` +Plotting utilities for evaluation output. It contains functions to draw gait phase, filtered signals, amplitude, and motor command figures from evaluation CSV files. + +### `evaluation_record.py` +The evaluation preprocessing pipeline. It reads raw sensor CSVs from `data/evaluation_raw_data/`, downsamples and converts them, and writes the resulting evaluation CSV files under `scripts/evaluation_output/`. + +### `live_comparison_plot.py` +A PyQt6 plot window for live signal comparison. Used by simulator components to show input, actual output, and expected output in real time. + +### `mat_to_csv.py` +MAT file conversion helper. It extracts `left` and `right` arrays from `.mat` files and writes them as CSV, preserving folder structure. + +### `normalize_output_time.py` +Normalizes output CSV time columns so `time (s)` starts at `0.00` and increments by `0.01`. Useful for preparing evaluation/output files for plotting or comparison. + +### `plot_scenarios.py` +A comparison plot module for preprocessing validation. It replays CSV rows through a callable, then plots actual vs expected output and residuals in a 3-panel figure. + +### `reference_versus_calculated.py` +A gait-phase comparison tool. It builds a reference phase signal from left/right cycle boundaries and compares it against calculated gait phase output, including RMSE reporting and plots. + +## Usage notes + +- Many scripts are library-style and are best used by importing their functions from Python. +- `evaluation_record.py`, `mat_to_csv.py`, and `normalize_output_time.py` include `__main__` runners for direct execution. +- Visualization scripts generally require `matplotlib`, and GUI scripts require `PyQt6`/`pyqtgraph`. + +For exact call patterns, open the corresponding script and inspect the top-level functions or the `if __name__ == '__main__'` section. + +## Examples + +### Run directly from the shell + +```bash +python scripts/evaluation_record.py +python scripts/mat_to_csv.py +python scripts/normalize_output_time.py +``` + +### Compare MATLAB vs module CSV outputs + +```python +from scripts.compare_matlab import parse_matlab_module, compute_metrics +from pathlib import Path + +matlab_csv = Path('data/evaluation_raw_data/matlab_ref.csv') +output_csv = Path('data/evaluation_output/module_out.csv') +comparison = parse_matlab_module(matlab_csv, output_csv) +metrics = compute_metrics(comparison) +``` + +### Use the CSV player for real-time-style replay + +```python +from scripts.csv_player import ScriptPlayer + +player = ScriptPlayer(Path('data/evaluation_raw_data/some_walk.csv')) +while player.has_next_line(): + row = player.get_data_from_csv('angle_right (rad)', 'gait_phase_right (rad)') +``` + +### Build a preprocessing comparison plot + +```python +from scripts.plot_scenarios import plot_preprocessor_comparison +from hip_controller.definitions import SensorSignal +from hip_controller.control.signal_processing.sensor_preprocessor import SensorPreprocessor, PreprocessorConfig + +preprocessor = SensorPreprocessor(PreprocessorConfig()) +plot_preprocessor_comparison( + csv_path='scripts/evaluation_output/normal_walk/AB01_normal_walk.csv', + time_col='time (s)', + input_col='angle_right (rad)', + expected_output_col='filtered_velocity_right (rad/s)', + build_signal=lambda t, a: SensorSignal(timestamp=t, angle_rad=a, velocity_rad_per_sec=0.0), + run_callable=preprocessor.filter, + extract_output=lambda sig: sig.velocity_rad_per_sec, +) +``` + +### Simulate controller playback + +```python +from scripts.controller_simulator import simulate_controller_with_data +from pathlib import Path + +simulate_controller_with_data(csv_path=Path('data/evaluation_raw_data/normal_walk/AB01_normal_walk.csv')) +``` + +### Normalize evaluation output time values + +```python +from scripts.normalize_output_time import normalize_output_folder +normalize_output_folder('scripts/evaluation_output', 'scripts/normalized_output') +``` diff --git a/src/hip_controller/__init__.py b/src/hip_controller/__init__.py index f05a609..cbb7a9b 100644 --- a/src/hip_controller/__init__.py +++ b/src/hip_controller/__init__.py @@ -14,14 +14,19 @@ try: import tomli as tomllib except ImportError as err: - raise ImportError("Python 3.10 requires the 'tomli' package: pip install tomli") from err + raise ImportError( + "Python 3.10 requires the 'tomli' package: pip install tomli" + ) from err from importlib.metadata import PackageNotFoundError, version from pathlib import Path try: __version__ = version("hip-controller") except PackageNotFoundError: - # this path leads to: src/hip_controller/__init__.py → src/ → repo_root/ → pyproject.toml - pyproject = Path(__file__).resolve().parent.parent.parent / "pyproject.toml" - with open(pyproject, "rb") as f: - __version__ = dict(tomllib.load(f))["project"]["version"] + try: + # this path leads to: src/hip_controller/__init__.py → src/ → repo_root/ → pyproject.toml + pyproject = Path(__file__).resolve().parent.parent.parent / "pyproject.toml" + with open(pyproject, "rb") as f: + __version__ = dict(tomllib.load(f))["project"]["version"] + except FileNotFoundError: + __version__ = "0.0.0+unknown" diff --git a/src/hip_controller/__main__.py b/src/hip_controller/__main__.py index 519f624..80dd8d2 100644 --- a/src/hip_controller/__main__.py +++ b/src/hip_controller/__main__.py @@ -8,29 +8,83 @@ from pathlib import Path # pragma: no cover from loguru import logger +from pandas import DataFrame from pyqtgraph import QtCore, QtWidgets # pragma: no cover from hip_controller.control.app import WalkOnController +from hip_controller.control.motor_reference_control.amplitude_modulation import ( + AscendStairsMode, + DescendStairsMode, + LevelGroundMode, + ModeStrategy, +) from hip_controller.definitions import ( DEFAULT_LOG_LEVEL, BasicConfig, - ExosuitData, LogLevel, + RecordedSensorData, ) # pragma: no cover +from hip_controller.plotter.csv_inspector import plot as csv_inspector_plot from hip_controller.plotter.csv_player import CSVPlayer from hip_controller.utils.utils import setup_logger +MOTOR_LEFT_COLUMN = "motor_command_left (rad)" +MOTOR_RIGHT_COLUMN = "motor_command_right (rad)" +FILTERED_ANG_LEFT_COLUMN = "filtered_angle_left (rad)" +FILTERED_ANG_RIGHT_COLUMN = "filtered_angle_right (rad)" +FILTERED_VEL_LEFT_COLUMN = "filtered_vel_left (rad/s)" +FILTERED_VEL_RIGHT_COLUMN = "filtered_vel_right (rad/s)" +PORTRAIT_RADIUS_LEFT_COLUMN = "Portrait Radius Left" +PORTRAIT_RADIUS_RIGHT_COLUMN = "Portrait Radius Right" +SCALED_PORTRAIT_RADIUS_LEFT_COLUMN = "Scaled Portrait Radius Left" +SCALED_PORTRAIT_RADIUS_RIGHT_COLUMN = "Scaled Portrait Radius Right" +SIGMOID_SCALING_LEFT_COLUMN = "Sigmoid Scaling Left" +SIGMOID_SCALING_RIGHT_COLUMN = "Sigmoid Scaling Right" +SCALED_SIGMOID_SCALING_LEFT_COLUMN = "Scaled Sigmoid Scaling Left" +SCALED_SIGMOID_SCALING_RIGHT_COLUMN = "Scaled Sigmoid Scaling Right" +AMPLITUDE_LEFT_COLUMN = "Amplitude Left" +AMPLITUDE_RIGHT_COLUMN = "Amplitude Right" +GAIT_PHASE_LEFT_COLUMN = "Gait Phase Left (rad)" +GAIT_PHASE_RIGHT_COLUMN = "Gait Phase Right (rad)" +MOTION_MAPPING_LEFT_COLUMN = "Motion Mapping Left" +MOTION_MAPPING_RIGHT_COLUMN = "Motion Mapping Right" +VELOCITY_SURROGATE_LEFT_COLUMN = "Velocity Surrogate Left (rad/s)" +VELOCITY_SURROGATE_RIGHT_COLUMN = "Velocity Surrogate Right (rad/s)" +VELOCITY_LPF_ANGLE_LEFT_COLUMN = "Velocity-LPF Angle Left (rad)" +VELOCITY_LPF_ANGLE_RIGHT_COLUMN = "Velocity-LPF Angle Right (rad)" +DRIFT_REMOVED_ANGLE_LEFT_COLUMN = "Drift-Removed Angle Left (rad)" +DRIFT_REMOVED_ANGLE_RIGHT_COLUMN = "Drift-Removed Angle Right (rad)" -def main( +# Integer classification -> locomotion mode. Instances are cached so we don't +# rebuild a ModeStrategy on every sample. Unknown values fall back to Level +# Ground (the safe default that matches pre-classification behavior). +_MODES_BY_CLASSIFICATION: dict[int, ModeStrategy] = { + 0: LevelGroundMode(), + 1: AscendStairsMode(), + 2: DescendStairsMode(), +} + + +def _mode_for(classification: int) -> ModeStrategy: + """Map a classification integer to its locomotion-mode strategy.""" + return _MODES_BY_CLASSIFICATION.get(classification, _MODES_BY_CLASSIFICATION[0]) + + +def main( # noqa: PLR0915, C901 log_level: str = DEFAULT_LOG_LEVEL, stderr_level: str = DEFAULT_LOG_LEVEL, csv_path: Path = BasicConfig.read_data_from_path, + fast: bool = False, ) -> None: # pragma: no cover """Run the main pipeline. :param log_level: The log level to use. :param stderr_level: The std err level to use. :param str csv_path: Path to the CSV file used for simulated real-time playback. The user could pass in the path of a file as well. + :param bool fast: When True, skip the live phase-portrait plots, process every CSV + row as fast as Python can, then open the resulting output CSV in the + :func:`hip_controller.plotter.csv_inspector.plot` window. When False + (default), runs in real-time with the live plot windows. :return: None # Example @@ -38,23 +92,208 @@ def main( """ setup_logger(log_level=log_level, stderr_level=stderr_level) + # QApplication is created unconditionally: fast mode still needs one for the + # csv_inspector_plot() call at the end, and live mode needs one for the + # phase-portrait windows. Reusing a single instance avoids a second + # QApplication construction inside csv_inspector_plot. app = QtWidgets.QApplication([]) player = CSVPlayer(csv_path) - controller_left = WalkOnController(reverse=True, plot=True, filtered=True) - controller_right = WalkOnController(reverse=False, plot=True, filtered=True) + plot = not fast + config = BasicConfig( + filtered=False, + left_limb_plot=plot, + right_limb_plot=plot, + ) + + controller_left = WalkOnController(left_limb=True, config=config) + controller_right = WalkOnController(left_limb=False, config=config) + timer = QtCore.QTimer() - def update() -> None: - """Update the controller with the next line of CSV data.""" - if not player.has_next_line(): - timer.stop() + # Track the previous main-switch state so we can reset the controllers on a + # falling edge (1 -> 0). The reset puts the preprocessor back into its + # "first call" state so that, on the next rising edge, velocity derivation + # starts fresh from the raw angle. + state = {"prev_switch": False} + + # Buffer of per-sample rows (inputs + motor commands) written to disk when + # playback finishes or the user interrupts with Ctrl+C. Values are mostly + # floats; main_switch and classification_* are ints, hence the wider type. + output_rows: list[dict[str, float | int]] = [] + output_path = csv_path.with_name(f"{csv_path.stem}_output.csv").resolve() + logger.info(f"Simulation results will be written to '{output_path}'.") + + def save_results() -> None: + """Persist the accumulated input/output rows to a CSV next to the input file.""" + if not output_rows: return + DataFrame(output_rows).to_csv(output_path, index=False) + logger.success(f"Saved {len(output_rows)} simulation rows to '{output_path}'.") + + def process_step() -> bool: + """Pull one row from the CSV, run the controllers, append to ``output_rows``. + + :return: ``True`` if a row was processed, ``False`` at end-of-file. + :rtype: bool + """ + if not player.has_next_line(): + return False - sensor_data: ExosuitData = player.get_sensor_data_from_csv() - controller_left.step(sensor_data.left) - controller_right.step(sensor_data.right) + step = player.get_sensor_data_from_csv() + sensor_data = step.sensor_data + main_switch = step.main_switch + controller_left.amplitude_modulation.set_mode( + _mode_for(step.classification_left) + ) + controller_right.amplitude_modulation.set_mode( + _mode_for(step.classification_right) + ) + + if main_switch: + motor_command_left = controller_left.step(sensor_data.left) + motor_command_right = controller_right.step(sensor_data.right) + else: + if state["prev_switch"]: + controller_left.reset() + controller_right.reset() + motor_command_left = 0.0 + motor_command_right = 0.0 + + state["prev_switch"] = main_switch + + # last_filtered_signal / last_intermediates / etc. are None before the + # first step or after a reset; write NaN in those cases via float('nan') + # so downstream consumers can distinguish "no value" from a real zero. + # Locals (rather than chained attribute access) so pyright can narrow + # the Optionals reliably when building the row dict below. + filt_left = controller_left.last_filtered_signal + filt_right = controller_right.last_filtered_signal + amp_left = controller_left.amplitude_modulation.last_intermediates + amp_right = controller_right.amplitude_modulation.last_intermediates + pre_left = controller_left.pre_processor + pre_right = controller_right.pre_processor + vel_surrogate_left = pre_left.last_velocity_surrogate_rad_per_sec + vel_surrogate_right = pre_right.last_velocity_surrogate_rad_per_sec + vel_lpf_left = pre_left.last_velocity_lpf_angle_rad + vel_lpf_right = pre_right.last_velocity_lpf_angle_rad + drift_left = pre_left.last_drift_removed_angle_rad + drift_right = pre_right.last_drift_removed_angle_rad + gait_phase_left = controller_left.last_gait_phase_rad + gait_phase_right = controller_right.last_gait_phase_rad + mapping_left = controller_left.motion_reference_controller.last_mapping_value + mapping_right = controller_right.motion_reference_controller.last_mapping_value + nan = float("nan") + # SensorSignal.timestamp is Optional in the dataclass; CSVPlayer always + # synthesizes one if the column is absent, so this is effectively never + # None in practice — but pyright can't see that. + timestamp_value = ( + sensor_data.left.timestamp + if sensor_data.left.timestamp is not None + else nan + ) + + output_rows.append( + { + RecordedSensorData.timestamp: timestamp_value, + RecordedSensorData.ang_left: sensor_data.left.angle_rad, + RecordedSensorData.ang_right: sensor_data.right.angle_rad, + RecordedSensorData.vel_left: sensor_data.left.velocity_rad_per_sec, + RecordedSensorData.vel_right: sensor_data.right.velocity_rad_per_sec, + RecordedSensorData.main_switch: int(main_switch), + "classification_left": step.classification_left, + "classification_right": step.classification_right, + FILTERED_ANG_LEFT_COLUMN: filt_left.angle_rad if filt_left else nan, + FILTERED_VEL_LEFT_COLUMN: ( + filt_left.velocity_rad_per_sec if filt_left else nan + ), + FILTERED_ANG_RIGHT_COLUMN: ( + filt_right.angle_rad if filt_right else nan + ), + FILTERED_VEL_RIGHT_COLUMN: ( + filt_right.velocity_rad_per_sec if filt_right else nan + ), + VELOCITY_SURROGATE_LEFT_COLUMN: ( + vel_surrogate_left if vel_surrogate_left is not None else nan + ), + VELOCITY_SURROGATE_RIGHT_COLUMN: ( + vel_surrogate_right if vel_surrogate_right is not None else nan + ), + VELOCITY_LPF_ANGLE_LEFT_COLUMN: ( + vel_lpf_left if vel_lpf_left is not None else nan + ), + VELOCITY_LPF_ANGLE_RIGHT_COLUMN: ( + vel_lpf_right if vel_lpf_right is not None else nan + ), + DRIFT_REMOVED_ANGLE_LEFT_COLUMN: ( + drift_left if drift_left is not None else nan + ), + DRIFT_REMOVED_ANGLE_RIGHT_COLUMN: ( + drift_right if drift_right is not None else nan + ), + PORTRAIT_RADIUS_LEFT_COLUMN: ( + amp_left.portrait_radius if amp_left else nan + ), + PORTRAIT_RADIUS_RIGHT_COLUMN: ( + amp_right.portrait_radius if amp_right else nan + ), + SCALED_PORTRAIT_RADIUS_LEFT_COLUMN: ( + amp_left.scaled_portrait_radius if amp_left else nan + ), + SCALED_PORTRAIT_RADIUS_RIGHT_COLUMN: ( + amp_right.scaled_portrait_radius if amp_right else nan + ), + SIGMOID_SCALING_LEFT_COLUMN: ( + amp_left.sigmoid_scaling if amp_left else nan + ), + SIGMOID_SCALING_RIGHT_COLUMN: ( + amp_right.sigmoid_scaling if amp_right else nan + ), + SCALED_SIGMOID_SCALING_LEFT_COLUMN: ( + amp_left.scaled_sigmoid_scaling if amp_left else nan + ), + SCALED_SIGMOID_SCALING_RIGHT_COLUMN: ( + amp_right.scaled_sigmoid_scaling if amp_right else nan + ), + AMPLITUDE_LEFT_COLUMN: amp_left.amplitude if amp_left else nan, + AMPLITUDE_RIGHT_COLUMN: amp_right.amplitude if amp_right else nan, + GAIT_PHASE_LEFT_COLUMN: ( + gait_phase_left if gait_phase_left is not None else nan + ), + GAIT_PHASE_RIGHT_COLUMN: ( + gait_phase_right if gait_phase_right is not None else nan + ), + MOTION_MAPPING_LEFT_COLUMN: ( + mapping_left if mapping_left is not None else nan + ), + MOTION_MAPPING_RIGHT_COLUMN: ( + mapping_right if mapping_right is not None else nan + ), + MOTOR_LEFT_COLUMN: motor_command_left, + MOTOR_RIGHT_COLUMN: motor_command_right, + } + ) + return True + + if fast: + # Process every row as fast as Python allows, save once, then hand + # off the result file to the CSV inspector for visual inspection. + while process_step(): + pass + save_results() + logger.info("Opening result in CSV inspector.") + csv_inspector_plot(output_path) + return + + # Live mode: drive the controllers from a Qt timer so the plot windows + # update in real time. + def update() -> None: + """Qt timer slot: process one row and reschedule the timer.""" + if not process_step(): + timer.stop() + save_results() + return # setInterval in miliseconds. Update each 10ms timer.setInterval(10) @@ -62,10 +301,27 @@ def sigint_handler(signal, frame) -> None: """Handle SIGINT (Ctrl+C) gracefully.""" logger.success("Keyboard interrupted with ^C.") timer.stop() + save_results() app.quit() timer.timeout.connect(slot=update) signal.signal(signal.SIGINT, sigint_handler) + + # Save results no matter how the app exits: end-of-CSV in update(), + # Ctrl+C in sigint_handler, or the user closing the plot windows. The + # aboutToQuit signal fires once at shutdown for all of these paths; + # save_results is idempotent (returns early when output_rows is empty), + # so duplicate calls from the EOF/Ctrl+C paths are harmless. + app.aboutToQuit.connect(save_results) + + # PyQt's event loop is implemented in C and doesn't yield to the Python + # interpreter often enough for signal handlers (Ctrl+C) to be delivered. + # A no-op QTimer firing every 200 ms forces a return to Python so the + # SIGINT handler installed above actually runs. + keepalive = QtCore.QTimer() + keepalive.timeout.connect(lambda: None) + keepalive.start(200) + timer.start(0) app.exec() @@ -95,15 +351,35 @@ def sigint_handler(signal, frame) -> None: "--file-path", "-p", default=Path(BasicConfig.read_data_from_path), - choices=list(LogLevel()), - help="Path to the CSV file used for simulated real-time playback. The file has to contain columns name 'angle_left (rad)', 'vel_left (rad/s)', 'angle_right (rad)', 'vel_right (rad/s)', additinally 'time (s)'.", + help=( + "Path to the CSV file used for simulated real-time playback. " + "Required columns: 'angle_left (rad)', 'angle_right (rad)'. " + "Optional columns: 'time (s)' (else synthesized from sample index), " + "'main_switch' (0/1 per row; defaults to 1 when absent), " + "'vel_left (rad/s)', 'vel_right (rad/s)' (else velocity is derived " + "from the raw angle by the controller's preprocessor), " + "'classification_left' / 'classification_right' (0=Level Ground, " + "1=Ascend Stairs, 2=Descend Stairs; defaults to 0 when absent). " + "Alternative header names are accepted, see CSVPlayer.COLUMN_ALIASES." + ), required=False, type=Path, ) + parser.add_argument( + "--fast", + "-f", + action="store_true", + help=( + "Run the simulation as fast as possible without the live phase-" + "portrait plot windows, then open the result CSV in the inspector. " + "Default (omitted) is live mode with real-time plots." + ), + ) args = parser.parse_args() main( log_level=args.log_level, stderr_level=args.stderr_level, csv_path=args.file_path, + fast=args.fast, ) diff --git a/src/hip_controller/control/app.py b/src/hip_controller/control/app.py index 1d45903..c64cf33 100644 --- a/src/hip_controller/control/app.py +++ b/src/hip_controller/control/app.py @@ -10,7 +10,7 @@ from hip_controller.control.signal_processing.sensor_preprocessor import ( SensorPreprocessor, ) -from hip_controller.definitions import PreprocessorConfig, SensorSignal +from hip_controller.definitions import BasicConfig, SensorSignal class WalkOnController: @@ -19,33 +19,51 @@ class WalkOnController: This controller implements a gait phase-based control strategy for a single limb, which can be used for both unilateral and bilateral hip flexion exosuits. The controller processes raw sensor signals to compute the current gait phase, applies amplitude modulation based on the sensor signals, and generates motor velocity commands for the exosuit's actuators. """ - def __init__(self, reverse: bool, plot: bool = False, filtered=False): + def __init__(self, left_limb: bool, config: BasicConfig): """Initialize the controller. - :param bool reverse: Whether to reverse the motor command output (for mirrored wiring). - :param bool plot: Whether to enable live plotting of the controller's internal states. - :param bool filtered: Whether to use pre-filtered sensor signals instead of raw signals. + :param bool left_limb: True if the controller is for left lower limb, False if for right lower limb. + :param BasicConfig config: Configurations including whether to reverse the motor command output (for mirrored wiring), whether to enable live plotting of the controller's internal states, whether to use pre-filtered sensor signals instead of raw signals and so on. :return: None """ - self.plot = plot - self.filtered = filtered - if plot: + self.filtered = config.filtered + if left_limb: + self.plot = config.left_limb_plot + self.amplitude_modulation = AmplitudeModulation( + reverse=config.left_limb_reverse + ) + else: + self.plot = config.right_limb_plot + self.amplitude_modulation = AmplitudeModulation( + reverse=config.right_limb_reverse + ) + + if self.plot: from hip_controller.plotter.live_phase_portrait import PortraitWindow # Execute the Qt plot application. - self.plotter = PortraitWindow(left=not reverse) + self.plotter = PortraitWindow(left=left_limb) self.plotter.show() - self.pre_processor = SensorPreprocessor(PreprocessorConfig()) + self.pre_processor = SensorPreprocessor(basic_config=config) self.gait_controller = GaitController() # due to different wire settings one of them might need to be reversed - mirrored with -1 - self.amplitude_modulation = AmplitudeModulation(reverse=reverse) + self.motion_reference_controller = MotionReferenceController() self._prev_timestamp: float | None = None + # Most recent signal passed downstream from the preprocessor (raw input + # when filtered=True, otherwise the filtered angle + derived velocity). + # Exposed so external code (e.g. the simulator) can log it. + self.last_filtered_signal: SensorSignal | None = None + + # Most recent gait phase produced by the gait controller (rad). None + # until the first step or after a reset. + self.last_gait_phase_rad: float | None = None + def step(self, curr_signal: SensorSignal) -> float: """Step the controller ahead. @@ -59,11 +77,13 @@ def step(self, curr_signal: SensorSignal) -> float: filtered_signal = curr_signal else: filtered_signal = self.pre_processor.filter(raw_signal=curr_signal) + self.last_filtered_signal = filtered_signal # Gait phase calculation gait_phase = self.gait_controller.update_and_compute( curr_signal=filtered_signal ) + self.last_gait_phase_rad = gait_phase # Apply amplitude modulation amplitude = self.amplitude_modulation.compute_amplitude(signal=filtered_signal) @@ -73,6 +93,13 @@ def step(self, curr_signal: SensorSignal) -> float: gait_phase=gait_phase, amplitude=amplitude ) + # Safety gate: only assist during hip flexion (positive angle). + # Negative filtered angle indicates extension / unclean signal -- in + # both cases driving the tendon further would be wrong, so cut the + # command to zero. + if filtered_signal.angle_rad < 0: + motor_command = 0.0 + # Plotting if self.plot and curr_signal.timestamp is not None: steady = self.gait_controller.get_signal_steady_state() @@ -91,3 +118,7 @@ def reset(self) -> None: """ # TODO add reset functions for gait controller, motor controller and so on.. self.pre_processor.reset() + self.last_filtered_signal = None + self.last_gait_phase_rad = None + self.amplitude_modulation.last_intermediates = None + self.motion_reference_controller.last_mapping_value = None diff --git a/src/hip_controller/control/motor_reference_control/amplitude_modulation.py b/src/hip_controller/control/motor_reference_control/amplitude_modulation.py index fa48c69..68b9e13 100644 --- a/src/hip_controller/control/motor_reference_control/amplitude_modulation.py +++ b/src/hip_controller/control/motor_reference_control/amplitude_modulation.py @@ -23,6 +23,28 @@ class ModeParameters: gain: float +@dataclass +class AmplitudeIntermediates: + """Per-sample intermediate values produced inside ``compute_amplitude``. + + Exposed so external code (e.g. the simulator) can log the full pipeline: + portrait radius -> scaled portrait radius -> sigmoid -> scaled sigmoid -> + final amplitude. + + :portrait_radius: ``sqrt(angle**2 + velocity**2)`` of the input signal. + :scaled_portrait_radius: ``portrait_radius * mode.scale``. + :sigmoid_scaling: Sigmoid output in [0, 1] (before gain/reverse). + :scaled_sigmoid_scaling: ``sigmoid_scaling * mode.gain`` (before reverse). + :amplitude: Final amplitude (``scaled_sigmoid_scaling * reverse``). + """ + + portrait_radius: float + scaled_portrait_radius: float + sigmoid_scaling: float + scaled_sigmoid_scaling: float + amplitude: float + + class ModeStrategy(ABC): """Abstract mode class.""" @@ -49,8 +71,8 @@ class AscendStairsMode(ModeStrategy): def get_parameters(self) -> ModeParameters: """Get parameters for ascending stairs.""" return ModeParameters( - scale=SCALE_LEVEL_MODE - 0.6, - sigmoid_power=SIGMOID_POWER + 100, + scale=SCALE_LEVEL_MODE - 0, # -0.6 + sigmoid_power=SIGMOID_POWER + 50, # +100 gain=AMPLITUDE_GAIN - 2, ) @@ -61,8 +83,8 @@ class DescendStairsMode(ModeStrategy): def get_parameters(self) -> ModeParameters: """Get parameters for descending stairs.""" return ModeParameters( - scale=SCALE_LEVEL_MODE - 0.5, - sigmoid_power=SIGMOID_POWER + 100, + scale=SCALE_LEVEL_MODE + 2.0, # -0.5 + sigmoid_power=SIGMOID_POWER + 50, # +100 gain=AMPLITUDE_GAIN + 0.5, ) @@ -80,6 +102,10 @@ def __init__(self, reverse: bool): else: self.reverse_amplitude: int = 1 + # Most recent per-stage values from compute_amplitude(); None until the + # first call. Exposed for logging by external code. + self.last_intermediates: AmplitudeIntermediates | None = None + def set_mode(self, mode: ModeStrategy): """Switch mode at runtime.""" self._mode = mode @@ -104,14 +130,23 @@ def compute_amplitude(self, signal: SensorSignal) -> float: """ params = self._mode.get_parameters() - scaled_portrait_radius = ( - self._compute_portrait_radius(signal=signal) * params.scale - ) + portrait_radius = self._compute_portrait_radius(signal=signal) + scaled_portrait_radius = portrait_radius * params.scale - amplitude = self.apply_sigmoid_scaling( + sigmoid_scaling = self.apply_sigmoid_scaling( value=scaled_portrait_radius, power=params.sigmoid_power ) - return (amplitude * params.gain) * self.reverse_amplitude + scaled_sigmoid_scaling = sigmoid_scaling * params.gain + amplitude = scaled_sigmoid_scaling * self.reverse_amplitude + + self.last_intermediates = AmplitudeIntermediates( + portrait_radius=portrait_radius, + scaled_portrait_radius=scaled_portrait_radius, + sigmoid_scaling=sigmoid_scaling, + scaled_sigmoid_scaling=scaled_sigmoid_scaling, + amplitude=amplitude, + ) + return amplitude @staticmethod def apply_sigmoid_scaling(value: float, power: int) -> float: diff --git a/src/hip_controller/control/motor_reference_control/motor_reference_controller.py b/src/hip_controller/control/motor_reference_control/motor_reference_controller.py index 63d4848..b9d334a 100644 --- a/src/hip_controller/control/motor_reference_control/motor_reference_controller.py +++ b/src/hip_controller/control/motor_reference_control/motor_reference_controller.py @@ -15,6 +15,11 @@ def __init__(self) -> None: # Initialize the mid-level controller with a 1-D Lookup Table for motion mapping. self.motion_mapping = MotionMapping() + # Most recent motion-mapping (cubic-spline) output, before amplitude + # scaling and saturation. None until the first compute_motor_command + # call or after a reset. Exposed for logging by external code. + self.last_mapping_value: float | None = None + def compute_motor_command(self, gait_phase: float, amplitude: float) -> float: """Compute the motor command based on the gait phase and amplitude. @@ -29,6 +34,7 @@ def compute_motor_command(self, gait_phase: float, amplitude: float) -> float: ) mapping_value = self.motion_mapping.spline(value=sinusoidal_behavior_gait_phase) + self.last_mapping_value = float(mapping_value) motor_command = mapping_value * amplitude diff --git a/src/hip_controller/control/signal_processing/drift_removal.py b/src/hip_controller/control/signal_processing/drift_removal.py index 46660cf..a2721b2 100644 --- a/src/hip_controller/control/signal_processing/drift_removal.py +++ b/src/hip_controller/control/signal_processing/drift_removal.py @@ -11,9 +11,7 @@ from hip_controller.definitions import LowPassFilterConfig, NotchConfig from hip_controller.filters.notch_filter import NotchFilter -from hip_controller.filters.second_order_low_pass_filter import ( - SecondOrderLowPassFilter, -) +from hip_controller.filters.second_order_low_pass_filter import SecondOrderLowPassFilter class DriftRemovalStrategy(ABC): diff --git a/src/hip_controller/control/signal_processing/filtering.py b/src/hip_controller/control/signal_processing/filtering.py index f08a3ef..4c5d5a9 100644 --- a/src/hip_controller/control/signal_processing/filtering.py +++ b/src/hip_controller/control/signal_processing/filtering.py @@ -10,7 +10,12 @@ from abc import ABC, abstractmethod -from hip_controller.definitions import LowPassFilterConfig, SogiFllConfig +from hip_controller.definitions import ( + KalmanFilterConfig, + LowPassFilterConfig, + SogiFllConfig, +) +from hip_controller.filters.kalman_filter import KalmanFilter from hip_controller.filters.second_order_low_pass_filter import ( SecondOrderLowPassFilter, ) @@ -59,19 +64,24 @@ def __init__(self, config: SogiFllConfig) -> None: """ self._sogi_filter: SogiFllFilter = SogiFllFilter(config=config) + # Quadrature output of the inner SOGI on the most recent call. This is + # a smoothed proxy for velocity (90 deg phase-shifted from + # angle_surrogate). Exposed so external code can log or gate on it. + self.last_quadrature: float = 0.0 + def filter(self, angle_rad: float, time_difference: float) -> float: """Estimate velocity using SOGI phase-locked structure. - :param float angle: Drift-compensated angle [rad]. + :param float angle_rad: Drift-compensated angle [rad]. :param float time_difference: Time elapsed since previous sample [s]. - :param float gyro_velocity: Unused in this implementation. - :return: (angle_surrogate, velocity_quadrature). - :rtype: tuple[float, float] + :return: angle_surrogate. + :rtype: float """ - angle_surrogate, _ = self._sogi_filter.filter( + angle_surrogate, quadrature = self._sogi_filter.filter( raw_theta_rad=angle_rad, time_difference=time_difference ) + self.last_quadrature = quadrature return angle_surrogate def reset(self) -> None: @@ -80,6 +90,7 @@ def reset(self) -> None: :return: None """ self._sogi_filter.reset() + self.last_quadrature = 0.0 class LowPassFiltering(FilteringStrategy): @@ -87,15 +98,12 @@ class LowPassFiltering(FilteringStrategy): The filter tracks the slow drift component; subtracting its output from the raw angle acts as a high-pass and yields *angle_no_drift_low_pass*. - - :param lpf: A configured :class:`SecondOrderLowPassFilter` instance whose - cut-off frequency sits well below the motion band. """ def __init__(self, config: LowPassFilterConfig) -> None: """Create a low-pass drift removal strategy. - :param SecondOrderLowPassFilter lpf: Low-pass filter for drift estimation. + :param config: Low-pass filter configuration for drift estimation. :return: None :rtype: None """ @@ -104,7 +112,7 @@ def __init__(self, config: LowPassFilterConfig) -> None: def filter(self, angle_rad: float, time_difference: float) -> float: """Execute one drift-removal step. - :param float raw_angle: Raw angle reading [rad]. + :param float angle_rad: Raw angle reading [rad]. :param float time_difference: Difference dt between current timestamp and previous timestamp. :return: Drift-compensated angle [rad]. :rtype: float @@ -120,3 +128,32 @@ def reset(self) -> None: :return: None """ self._low_pass_filter.reset() + + +class KalmanFiltering(FilteringStrategy): + """Kalman filter.""" + + def __init__(self, config: KalmanFilterConfig): + """Initialize the Kalman filter. + + :param config: Kalman filter configuration. + """ + self._kalman_filter = KalmanFilter(config=config) + + def filter(self, angle_rad: float, time_difference: float) -> float: + """Execute one Kalman filter step. + + :param angle_rad: Raw angle in rad. + :param time_difference: Difference dt between current timestamp and previous timestamp. + :return: Drift-compensated angle in rad. + """ + return self._kalman_filter.filter( + angle_rad=angle_rad, time_difference=time_difference + ) + + def reset(self) -> None: + """Reset the filter to a known initial condition. + + :return: None + """ + self._kalman_filter.reset() diff --git a/src/hip_controller/control/signal_processing/sensor_preprocessor.py b/src/hip_controller/control/signal_processing/sensor_preprocessor.py index 509c856..5eb81ca 100644 --- a/src/hip_controller/control/signal_processing/sensor_preprocessor.py +++ b/src/hip_controller/control/signal_processing/sensor_preprocessor.py @@ -1,25 +1,44 @@ -"""Two-stage sensor preprocessing pipeline: drift removal followed by velocity estimation. +"""Three-stage sensor preprocessing pipeline: drift removal, filtering, and velocity estimation. -There are two strategies for drift removal and four strategies for velocity estimation implemented in the control module, which can be selected and configured in the :class:`PreprocessorConfig` when initializing the :class:`WalkOnController`. +There are two strategies for drift removal, three strategies for filtering, and three strategies for velocity estimation +implemented in the control module, which can be selected and configured in the :class:`PreprocessorConfig` when initializing the :class:`WalkOnController`. The drift removal strategies include: ``LowPassDriftRemoval`` and ``NotchDriftRemoval``. -The velocity estimation strategies include: ``SogifllVelocityEstimation``, ``LowPassVelocityEstimation``, ``DiscreteDerivativeVelocityEstimation``, and ``GyroscopeVelocityEstimation``. +The filtering strategies include: ``LowPassFiltering``, ``SogiFllFiltering``, and ``KalmanFiltering``. + +The velocity estimation strategies include: ``LowPassVelocityEstimation``, ``DiscreteDerivativeVelocityEstimation``, and ``GyroscopeVelocityEstimation``. """ from __future__ import annotations from hip_controller.control.signal_processing.drift_removal import ( DriftRemovalStrategy, + LowPassDriftRemoval, + NotchDriftRemoval, ) from hip_controller.control.signal_processing.filtering import ( FilteringStrategy, + KalmanFiltering, + LowPassFiltering, SogiFllFiltering, ) from hip_controller.control.signal_processing.velocity_estimation import ( + DiscreteDerivativeVelocityEstimation, + GyroscopeVelocityEstimation, + LowPassVelocityEstimation, VelocityEstimationStrategy, ) -from hip_controller.definitions import PreprocessorConfig, SensorSignal +from hip_controller.definitions import ( + BASELINE_REMOVAL_SAMPLE_NUM, + BasicConfig, + DriftRemovalMethod, + FilteringMethod, + PreprocessorConfig, + SensorSignal, + VelocityEstimationMethod, + VelocityInputAngle, +) class SensorPreprocessor: @@ -31,22 +50,45 @@ class SensorPreprocessor: """ - def __init__(self, config: PreprocessorConfig) -> None: + def __init__(self, basic_config: BasicConfig) -> None: """Initialize the sensor pre-processor. - :param PreprocessorConfig config: Preprocessor configuration. + :param basic_config: controller configuration. :return: None """ - self.config = config - self._drift_removal: DriftRemovalStrategy = config.drift_removal_strategy - self._sogi_fll: FilteringStrategy = SogiFllFiltering( - config=config.filtering_sogifll_config - ) - self._velocity_estimation: VelocityEstimationStrategy = ( - config.velocity_estimation_strategy - ) + self._basic_config: BasicConfig = basic_config + self._velocity_input_angle = PreprocessorConfig.velocity_input_angle + + self._drift_removal: DriftRemovalStrategy + self._filtering: FilteringStrategy + self._velocity_estimation: VelocityEstimationStrategy self._prev_timestamp: float | None = None + self._baseline: float = 0.0 + self._baseline_count: int = 0 + self._baseline_sum: float = 0.0 + + self._init_strategies() + + # SOGI-FLL quadrature output from the most recent filter() call. + # Reflects a smoothed velocity-like signal (90 deg phase-shifted from + # the SOGI in-phase angle). None until the first non-trivial filter() + # call. Exposed for logging by external code. + self.last_velocity_surrogate_rad_per_sec: float | None = None + + # Angle as seen *inside* the velocity-estimation LPF, i.e. the LPF's + # smoothed output that is then differentiated to produce the velocity. + # For LowPassVelocityEstimation this is the second-order-LPF-filtered + # version of velocity_input_angle_rad; for other strategies it's the + # first element of their (angle, velocity) return tuple. Useful for + # diagnosing where velocity spikes come from. None on first call / + # after reset. + self.last_velocity_lpf_angle_rad: float | None = None + + # Output of the drift-removal stage (LPF subtraction or notch), + # measured between drift removal and SOGI. None on first call / + # after reset. + self.last_drift_removed_angle_rad: float | None = None def filter(self, raw_signal: SensorSignal) -> SensorSignal: """Run one preprocessing step and return a :class:`SensorSignal`. @@ -54,6 +96,19 @@ def filter(self, raw_signal: SensorSignal) -> SensorSignal: :return: Preprocessed :class:`SensorSignal` with timestamp of the current sample [s], raw angle from the sensor [rad] and gyroscope angular rate [rad/s] read from sensor. :rtype: SensorSignal """ + # Baseline capture by taking avg of first N samples + if self._baseline_count < BASELINE_REMOVAL_SAMPLE_NUM: + self._baseline_count += 1 + self._baseline_sum += raw_signal.angle_rad + + if self._baseline_count == BASELINE_REMOVAL_SAMPLE_NUM: + self._baseline = self._baseline_sum / BASELINE_REMOVAL_SAMPLE_NUM + + raw_signal.angle_rad = 0.0 + else: + # normal operation: baseline removal + raw_signal.angle_rad -= self._baseline + if self._prev_timestamp is None or raw_signal.timestamp is None: self._prev_timestamp = raw_signal.timestamp return raw_signal @@ -65,25 +120,44 @@ def filter(self, raw_signal: SensorSignal) -> SensorSignal: # check dt too big if time_difference > 1.0: - self._drift_removal = self.config.drift_removal_strategy - self._velocity_estimation = self.config.velocity_estimation_strategy - time_difference = 0.01 + self.reset() + return raw_signal self._prev_timestamp = raw_signal.timestamp angle_no_drift_rad = self._drift_removal.filter( raw_angle=raw_signal.angle_rad, time_difference=time_difference ) + self.last_drift_removed_angle_rad = angle_no_drift_rad - angle_out_rad = self._sogi_fll.filter( + angle_out_rad = self._filtering.filter( angle_rad=angle_no_drift_rad, time_difference=time_difference ) + # Surface the SOGI quadrature for downstream logging / experimentation. + # The SogiFllFiltering wrapper caches it on every filter() call; other + # FilteringStrategy implementations (none yet) would need to expose the + # same attribute. + self.last_velocity_surrogate_rad_per_sec = getattr( + self._filtering, "last_quadrature", None + ) - _, velocity_out_rad_per_sec = self._velocity_estimation.filter( - angle_rad=angle_out_rad, - time_difference=time_difference, - gyro_velocity_rad_per_sec=raw_signal.velocity_rad_per_sec, + # See PreprocessorConfig.velocity_input_angle for the trade-off between + # latency / smoothness (more filtering) and freshness (less filtering). + if self._velocity_input_angle == VelocityInputAngle.RAW: + velocity_input_angle_rad = raw_signal.angle_rad + elif self._velocity_input_angle == VelocityInputAngle.DRIFT_REMOVED: + velocity_input_angle_rad = angle_no_drift_rad + else: + velocity_input_angle_rad = angle_out_rad + + velocity_lpf_angle_rad, velocity_out_rad_per_sec = ( + self._velocity_estimation.filter( + angle_rad=velocity_input_angle_rad, + time_difference=time_difference, + gyro_velocity_rad_per_sec=raw_signal.velocity_rad_per_sec, + ) ) + self.last_velocity_lpf_angle_rad = velocity_lpf_angle_rad return SensorSignal( timestamp=raw_signal.timestamp, @@ -91,13 +165,80 @@ def filter(self, raw_signal: SensorSignal) -> SensorSignal: velocity_rad_per_sec=velocity_out_rad_per_sec, ) + def _init_strategies(self): + """Get instance of different options of drift removal, filtering, and velocity estimation.""" + if self._basic_config.drift_removal_method == DriftRemovalMethod.LOW_PASS: + self._drift_removal = LowPassDriftRemoval( + PreprocessorConfig.drift_removal_second_order_lpf_config + ) + + elif self._basic_config.drift_removal_method == DriftRemovalMethod.NOTCH: + self._drift_removal = NotchDriftRemoval( + PreprocessorConfig.drift_removal_notch_config + ) + else: + raise ValueError( + f"Unrecognized drift-removal method: {self._basic_config.drift_removal_method}" + ) + + if self._basic_config.filtering_method == FilteringMethod.SOGI: + self._filtering = SogiFllFiltering( + PreprocessorConfig.filtering_sogifll_config + ) + + elif self._basic_config.filtering_method == FilteringMethod.LOW_PASS: + self._filtering = LowPassFiltering( + PreprocessorConfig.filtering_lowpass_config + ) + + elif self._basic_config.filtering_method == FilteringMethod.KALMAN: + self._filtering = KalmanFiltering( + PreprocessorConfig.filtering_kalman_config + ) + + else: + raise ValueError( + f"Unrecognized filtering method: {self._basic_config.filtering_method}" + ) + + if ( + self._basic_config.velocity_estimation_method + == VelocityEstimationMethod.DISCRETE_DERIVATIVE + ): + self._velocity_estimation = DiscreteDerivativeVelocityEstimation() + + elif ( + self._basic_config.velocity_estimation_method + == VelocityEstimationMethod.LOW_PASS + ): + self._velocity_estimation = LowPassVelocityEstimation( + PreprocessorConfig.velocity_estimation_low_pass_config + ) + + elif ( + self._basic_config.velocity_estimation_method + == VelocityEstimationMethod.GYROSCOPE + ): + self._velocity_estimation = GyroscopeVelocityEstimation() + + else: + raise ValueError( + f"Unrecognized velocity-estimation method: {self._basic_config.velocity_estimation_method}" + ) + def reset(self) -> None: """Reset the Signal Preprocessor if exosuit is disconnected or timeout occured. :return: None """ self._prev_timestamp = None + self._baseline: float = 0.0 + self._baseline_count: int = 0 + self._baseline_sum: float = 0.0 + self.last_velocity_surrogate_rad_per_sec = None + self.last_velocity_lpf_angle_rad = None + self.last_drift_removed_angle_rad = None self._drift_removal.reset() - self._sogi_fll.reset() + self._filtering.reset() self._velocity_estimation.reset() diff --git a/src/hip_controller/definitions.py b/src/hip_controller/definitions.py index 8fd05d5..c9c9fda 100644 --- a/src/hip_controller/definitions.py +++ b/src/hip_controller/definitions.py @@ -1,19 +1,26 @@ """Common definitions for this module.""" + import sys +from dataclasses import asdict, dataclass, field + +from numpy.typing import NDArray -from dataclasses import asdict, dataclass -from enum import auto if sys.version_info >= (3, 11): - from enum import StrEnum + from enum import StrEnum, auto else: from enum import Enum + class StrEnum(str, Enum): """String enum backport for Python <3.11.""" + + from math import pi from pathlib import Path import numpy as np +from hip_controller.utils.state_space import StateSpaceLinear + np.set_printoptions(precision=3, floatmode="fixed", suppress=True) @@ -29,27 +36,27 @@ class StrEnum(str, Enum): LOG_DIR: Path = DATA_DIR / "logs" -@dataclass(frozen=True) -class BasicConfig: - """Basic configurations for the hip controller.""" +class DriftRemovalMethod(StrEnum): + """Drift removal strategy options.""" - # if the graph is displayed or not - left_limb_plot: bool = True - right_limb_plot: bool = True + LOW_PASS = auto() + NOTCH = auto() - # if the wiring settings are reversed or not - left_limb_reverse: bool = False - right_limb_reverse: bool = True - # either read data from imu or read data from csv file using csv player - read_from_imu: bool = False +class FilteringMethod(StrEnum): + """Filtering strategy options.""" - # the path where data is read from - read_data_from_path: Path = ( - DATA_DIR / "sensor_data" / "data_input_filtered_2026_01_09.csv" - ) + SOGI = auto() + KALMAN = auto() + LOW_PASS = auto() - frequency: int = 100 + +class VelocityEstimationMethod(StrEnum): + """Velocity estimation strategy options.""" + + DISCRETE_DERIVATIVE = auto() + LOW_PASS = auto() + GYROSCOPE = auto() class SolverType(StrEnum): @@ -65,17 +72,54 @@ class SolverType(StrEnum): Maps to Simulink continuous integrator + ode4 solver. """ - FORWARD_EULER = "forward_euler" - BACKWARD_EULER = "backward_euler" - TRAPEZOIDAL = "trapezoidal" - RUNGE_KUTTA = "rk4" + FORWARD_EULER = auto() + BACKWARD_EULER = auto() + TRAPEZOIDAL = auto() + RUNGE_KUTTA = auto() + + +@dataclass(frozen=True) +class BasicConfig: + """Basic configurations for the hip controller.""" + + # general frequency + frequency: int = 100 + + # if data is pre filtered - skip the pre processing + filtered: bool = False + + # if the graph is displayed or not + left_limb_plot: bool = False + right_limb_plot: bool = False + + # if the wiring settings are reversed or not + left_limb_reverse: bool = False + right_limb_reverse: bool = True + + # either read data from imu or read data from csv file using csv player + read_from_imu: bool = False + + # the path where data is read from + read_data_from_path: Path = ( + DATA_DIR / "sensor_data" / "data_input_filtered_2026_01_09.csv" + ) + + # select which DriftRemovalMethod, VelocityEstimationMethod + drift_removal_method: DriftRemovalMethod = DriftRemovalMethod.LOW_PASS + + filtering_method: FilteringMethod = FilteringMethod.SOGI + velocity_estimation_method: VelocityEstimationMethod = ( + VelocityEstimationMethod.DISCRETE_DERIVATIVE + ) + # cut-off frequency for the 2ndOrderLP filter + cut_off_freq_low_pass_rad_per_sec: float = 80.0 @dataclass class LowPassFilterConfig: """Settings for the second-order low-pass filter containing cut_off_frequency, damping_ratio, initial_condition, solver_type.""" - cut_off_frequency_rad_per_sec: float = 20.0 # in rad/s + cut_off_frequency_rad_per_sec: float = 60.0 # in rad/s damping_ratio: float = 1.0 # 1.0 = critically damped initial_condition: float = 0.0 solver_type: SolverType = ( @@ -83,6 +127,21 @@ class LowPassFilterConfig: ) # SolverType enum of numerical integration strategy +@dataclass(frozen=True) +class KalmanFilterConfig: + """Settings for the Kalman filter.""" + + process_noise: NDArray = field(default_factory=lambda: 2e-2 * np.eye(2)) + measurement_noise: NDArray = field(default_factory=lambda: 0.75 * np.eye(1)) + state_space: StateSpaceLinear = field( + default_factory=lambda: StateSpaceLinear( + A=np.array([[1.0, 0.01], [0.0, 1.0]]), C=np.array([[1.0, 0.0]]) + ) + ) + initial_state: NDArray = field(default_factory=lambda: np.array([0.0, 0.0])) + initial_covariance: NDArray = field(default_factory=lambda: 10 * np.eye(2)) + + # Pre processing @@ -90,8 +149,8 @@ class LowPassFilterConfig: class NotchConfig: """Configurations for the notch function.""" - center_freq_hz: float - bandwidth_3db_hz: float + center_freq_hz: float = 0.0 + bandwidth_3db_hz: float = 0.1 sample_rate_hz: float = BasicConfig.frequency @@ -113,36 +172,36 @@ class SogiFllConfig: """ # cadence bounds (walking/running range) - lower_cadence_bound: float = 0.2 - upper_cadence_bound: float = 4.0 + lower_cadence_bound: float = 0.5 # 0.2 -> extremely slow walking + upper_cadence_bound: float = 1.8 # 4.0 -> very fast running - # Tune only if the portrait is ringy or too sluggish: + # Tune only if the portrait is ringy or too sluggish:s # - increase to 1.2-1.4 if theta/theta_quad look underdamped / not tracking well # - decrease to 0.8-0.9 if very noisy and jitter is observed - sogi_adaptation_gain: float = 1.0 + sogi_adaptation_gain: float = 1.0 # 0.7 #1.0 # Frequency adaptation speed: # - increase to track speed changes faster # - decrease if noisy/jittery (sensor/noise dependent) - fll_adaptation_gain: float = 1.0 + fll_adaptation_gain: float = 1.0 # 1.0 # lock thresholds (amplitude/noise dependent) - lower_energy_threshold: float = 1e-4 - upper_energy_threshold: float = 1e-2 + lower_energy_threshold: float = 1e-4 # 5e-4 #1e-4 + upper_energy_threshold: float = 1e-2 # 5e-2 #1e-2 # Tune only if internal frequency becomes jittery or too laggy: # - decrease to 0.2 for smoother (more lag) # - increase to 0.5 for faster (more jitter) - frequency_estimate_smoother_bandwidth: float = 0.30 + frequency_estimate_smoother_bandwidth: float = 0.30 # 0.20 #0.30 # Tune only if lock flickers or reacts too slowly: # - decrease (0.3) to reduce flicker # - increase (0.8-1.0) for faster start/stop response - lock_state_smoother_bandwidth: float = 0.50 + lock_state_smoother_bandwidth: float = 0.50 # 0.30 #0.50 # [Hz] initial guess (walking/running general default) # Tune only if you want faster lock at startup: # - set near typical cadence in your trials (walk ~1-2 Hz, run ~2-3 Hz) - initial_frequency_guess: float = 1.4 + initial_frequency_guess: float = 1.0 # 1.0 # % state decay when standing # Tune only if oscillator rings too long after stopping: @@ -155,75 +214,47 @@ class SogiFllConfig: numerical_safety_floor: float = 1e-9 -class DriftRemovalMethod(StrEnum): - """Drift removal strategy options.""" - - LOW_PASS = auto() - NOTCH = auto() - +class VelocityInputAngle(StrEnum): + """Which angle is fed to the velocity-estimation stage. -class VelocityEstimationMethod(StrEnum): - """Velocity estimation strategy options.""" + RAW -- ``raw_signal.angle_rad`` straight from the sensor. + DRIFT_REMOVED -- output of the drift-removal stage (LPF or notch). + FILTERED -- output of the SOGI-FLL stage (current default). + """ - SOGI = auto() - DISCRETE_DERIVATIVE = auto() - LOW_PASS = auto() - GYROSCOPE = auto() + RAW = auto() + DRIFT_REMOVED = auto() + FILTERED = auto() class PreprocessorConfig: """Configurations for the sensor preprocessor.""" - # Select methods for drift removal and velocity estimation filtering - drift_removal_method: DriftRemovalMethod = DriftRemovalMethod.LOW_PASS - velocity_estimation_method: VelocityEstimationMethod = ( - VelocityEstimationMethod.DISCRETE_DERIVATIVE - ) + # Selects which angle is fed into the velocity-estimation stage. + # See VelocityInputAngle for the options. Default keeps the historical + # behavior (use the SOGI-FLL filtered angle). + velocity_input_angle: VelocityInputAngle = VelocityInputAngle.FILTERED # Configurations for the filters drift_removal_second_order_lpf_config: LowPassFilterConfig = LowPassFilterConfig( cut_off_frequency_rad_per_sec=1.25, damping_ratio=1.0, initial_condition=0.0 ) - drift_removal_notch_config: NotchConfig = NotchConfig( - center_freq_hz=0.0, bandwidth_3db_hz=0.1, sample_rate_hz=BasicConfig.frequency - ) + drift_removal_notch_config: NotchConfig = NotchConfig() + filtering_sogifll_config: SogiFllConfig = SogiFllConfig() + filtering_lowpass_config: LowPassFilterConfig = LowPassFilterConfig() + filtering_kalman_config: KalmanFilterConfig = KalmanFilterConfig() + filtering_second_order_lpf_config: LowPassFilterConfig = LowPassFilterConfig( + cut_off_frequency_rad_per_sec=90.0, damping_ratio=1.0, initial_condition=0.0 + ) + velocity_estimation_low_pass_config: LowPassFilterConfig = LowPassFilterConfig( cut_off_frequency_rad_per_sec=20.0, damping_ratio=1.0, initial_condition=0.0 ) - @property - def drift_removal_strategy(self): - """Get instance of different options of drift removal.""" - from hip_controller.control.signal_processing.drift_removal import ( - LowPassDriftRemoval, - NotchDriftRemoval, - ) - - if self.drift_removal_method == DriftRemovalMethod.LOW_PASS: - return LowPassDriftRemoval(self.drift_removal_second_order_lpf_config) - else: - return NotchDriftRemoval(self.drift_removal_notch_config) - - @property - def velocity_estimation_strategy(self): - """Get instance of different options of velocity estimation.""" - from hip_controller.control.signal_processing.velocity_estimation import ( - DiscreteDerivativeVelocityEstimation, - GyroscopeVelocityEstimation, - LowPassVelocityEstimation, - ) - - if ( - self.velocity_estimation_method - == VelocityEstimationMethod.DISCRETE_DERIVATIVE - ): - return DiscreteDerivativeVelocityEstimation() - elif self.velocity_estimation_method == VelocityEstimationMethod.LOW_PASS: - return LowPassVelocityEstimation(self.filtering_second_order_lpf_config) - else: - return GyroscopeVelocityEstimation() +# baseline removal using first N samples +BASELINE_REMOVAL_SAMPLE_NUM = 10 # centering & normalization VALUE_NEAR_ZERO = 1e-6 @@ -237,12 +268,8 @@ def velocity_estimation_strategy(self): # Amplitude modulation SCALE_LEVEL_MODE = 1 -SIGMOID_POWER = 50 -AMPLITUDE_GAIN = -6.5 # Motor position desidered amplitude (rad) - -# Kalman filter definitions -PROCESS_NOISE = 2e-2 -MEASUREMENT_NOISE = 0.75 +SIGMOID_POWER = 50 # 50 +AMPLITUDE_GAIN = -7 # Motor position desidered amplitude (rad) # Cubic Spline Interpolation @@ -321,6 +348,7 @@ class RecordedSensorData: vel_left: str = "vel_left (rad/s)" ang_right: str = "angle_right (rad)" vel_right: str = "vel_right (rad/s)" + main_switch: str = "main_switch" fake_frequency_hz: int = BasicConfig.frequency @@ -329,7 +357,7 @@ class RecordedSensorData: class PIDConfig: """Configurations for PID controller.""" - proportional_gain: float = 14.0 + proportional_gain: float = 8.0 integral_gain: float = 0.0 derivative_gain: float = 0.02 output_limits: tuple[float, float] | None = None diff --git a/src/hip_controller/filters/kalman_filter.py b/src/hip_controller/filters/kalman_filter.py index d3d8e2d..d120351 100644 --- a/src/hip_controller/filters/kalman_filter.py +++ b/src/hip_controller/filters/kalman_filter.py @@ -3,9 +3,8 @@ import numpy as np from numpy.typing import NDArray -from hip_controller.definitions import MEASUREMENT_NOISE, PROCESS_NOISE +from hip_controller.definitions import KalmanFilterConfig from hip_controller.utils.math_utils import symmetrize_matrix -from hip_controller.utils.state_space import StateSpaceLinear class KalmanFilter: @@ -13,32 +12,19 @@ class KalmanFilter: def __init__( self, - state_space: StateSpaceLinear, - initial_x: np.ndarray, - initial_covariance: np.ndarray, - process_noise: NDArray | None = None, - measurement_noise: NDArray | None = None, + config: KalmanFilterConfig, ) -> None: """Initialize the Kalman Filter. - :param state_space: linear state space model - :param initial_x: Initial state estimate - :param initial_covariance: Initial error covariance - :param process_noise: Process noise covariance - :param measurement_noise: Measurement noise covariance + :param config: Kalman filter configuration. :return: None """ - self.state_space = state_space - if process_noise is None: - process_noise = PROCESS_NOISE * np.eye(len(state_space.A)) - self.Q: np.ndarray = process_noise - - if measurement_noise is None: - measurement_noise = MEASUREMENT_NOISE * np.eye(len(state_space.C)) - self.R: np.ndarray = measurement_noise - - self.x: np.ndarray = initial_x - self.cov: np.ndarray = initial_covariance + self.config = config # save initial state for resets + self.state_space = config.state_space + self.Q: NDArray = config.process_noise + self.R: NDArray = config.measurement_noise + self.x: NDArray = config.initial_state + self.cov: NDArray = config.initial_covariance def predict(self, u: NDArray | None = None) -> None: """Predict the next state and error covariance. @@ -65,3 +51,24 @@ def update(self, z: NDArray) -> NDArray: self.cov = symmetrize_matrix(cov) return z - self.state_space.C @ self.x + + def filter(self, angle_rad: float, time_difference: float) -> float: + """Execute one filter step, containing a prediction step and an update step. + + :param angle_rad: Raw angle in rad. + :param time_difference: Difference dt between current timestamp and previous timestamp. + :return: Drift-compensated angle in rad. + """ + # modify the state transition matrix with the given time step + self.state_space.A[0, 1] = time_difference + self.predict(u=None) + self.update(z=np.array([angle_rad])) + return float(self.x[0]) + + def reset(self) -> None: + """Reset the Kalman filter to its initial condition. + + :return: None + """ + self.x = self.config.initial_state + self.cov = self.config.initial_covariance diff --git a/src/hip_controller/plotter/__init__.py b/src/hip_controller/plotter/__init__.py new file mode 100644 index 0000000..9757353 --- /dev/null +++ b/src/hip_controller/plotter/__init__.py @@ -0,0 +1,5 @@ +"""Plotting utilities for the hip controller package.""" + +from hip_controller.plotter.csv_inspector import plot + +__all__ = ["plot"] diff --git a/src/hip_controller/plotter/__main__.py b/src/hip_controller/plotter/__main__.py new file mode 100644 index 0000000..30761c2 --- /dev/null +++ b/src/hip_controller/plotter/__main__.py @@ -0,0 +1,70 @@ +"""Command-line entry point for the modular CSV plotter. + +Usage:: + + python -m hip_controller.plotter path/to/file.csv [--frequency 100] + python -m hip_controller.plotter path/to/file.csv --no-time-only-zoom +""" + +from __future__ import annotations + +import argparse +from pathlib import Path + +from hip_controller.definitions import BasicConfig +from hip_controller.plotter.csv_inspector import plot + + +def main(argv: list[str] | None = None) -> int: + """Parse CLI arguments and launch the CSV inspector. + + :param argv: optional argument list (defaults to ``sys.argv[1:]``); exposed + to make the entry point easy to drive from tests. + :type argv: list[str] or None + :return: process exit code (always 0 once the GUI window closes). + :rtype: int + """ + parser = argparse.ArgumentParser( + prog="python -m hip_controller.plotter", + description=( + "Modular CSV plotter (Simulink-Data-Inspector-style) for the " + "hip-controller package. The X axis is synthesized from " + "--frequency; no time column is required in the CSV." + ), + ) + parser.add_argument( + "csv", + type=Path, + help="Path to a CSV file with a header row.", + ) + parser.add_argument( + "--frequency", + type=int, + default=BasicConfig.frequency, + help=( + "Sampling frequency in Hz used to synthesize the time axis. " + f"Defaults to BasicConfig.frequency ({BasicConfig.frequency})." + ), + ) + parser.add_argument( + "--no-time-only-zoom", + dest="time_only_zoom", + action="store_false", + help=( + "Start with both X and Y zoom enabled. By default the Y axis is " + "locked and only the time axis responds to the mouse wheel." + ), + ) + parser.set_defaults(time_only_zoom=True) + args = parser.parse_args(argv) + + plot( + csv_path=args.csv, + frequency_hz=args.frequency, + time_only_zoom=args.time_only_zoom, + ) + return 0 + + +if __name__ == "__main__": # pragma: no cover + raise SystemExit(main()) diff --git a/src/hip_controller/plotter/csv_inspector.py b/src/hip_controller/plotter/csv_inspector.py new file mode 100644 index 0000000..1d06f17 --- /dev/null +++ b/src/hip_controller/plotter/csv_inspector.py @@ -0,0 +1,955 @@ +"""Modular CSV plotter for the hip controller. + +Provides a Simulink-Data-Inspector-style GUI for inspecting CSV recordings: + +- Vertically stacked subplots with a linked (shared) time axis. +- A *single* signal panel on the left: click a subplot to make it "active", + then tick which CSV columns appear in that subplot. +- Each subplot has a small overlaid toolbar in its upper-right corner that + switches mouse-interaction modes: + + * Pan -- left-drag translates the view + * T-Zoom -- left-drag pans; wheel zooms X only (default) + * Zoom -- left-drag draws a zoom rectangle; wheel zooms X and Y + * Pick -- click a data point to read its value in the status bar + +- The X axis is synthesized from the sampling frequency + (``BasicConfig.frequency`` by default), so the CSV need not carry its own + time column. + +Public entry point: :func:`plot`. +""" + +from __future__ import annotations + +import sys +from pathlib import Path +from typing import Any, ClassVar + +import numpy as np +import pandas as pd +import pyqtgraph as pg +from loguru import logger +from pandas.api.types import is_numeric_dtype +from PyQt6 import QtCore, QtGui, QtWidgets + +from hip_controller.definitions import BasicConfig + +# Column names matched case-insensitively against these prefixes are treated +# as timestamp columns and excluded from the plottable signal list, because +# the X axis is synthesized from the sample frequency. +_TIME_COLUMN_PREFIXES: tuple[str, ...] = ("time", "timestamp", "t (") + + +def discover_plottable_columns(dataframe: pd.DataFrame) -> list[str]: + """Return the subset of CSV columns that should appear as selectable signals. + + Keeps only numeric columns and drops anything whose header looks like a + timestamp, because the time axis is synthesized from the sample frequency + rather than read from the file. + + :param pandas.DataFrame dataframe: parsed CSV. + :return: ordered list of plottable column names. + :rtype: list[str] + """ + plottable: list[str] = [] + for col in dataframe.columns: + if not is_numeric_dtype(dataframe[col]): + continue + lowered = str(col).lower().strip() + if any(lowered.startswith(prefix) for prefix in _TIME_COLUMN_PREFIXES): + continue + plottable.append(str(col)) + return plottable + + +def synthesize_time_vector(n_samples: int, frequency_hz: int) -> np.ndarray: + """Synthesize a uniform time vector (seconds) from a sample count and frequency. + + :param int n_samples: number of rows in the CSV. + :param int frequency_hz: sampling frequency (samples per second). + :return: 1-D array of timestamps in seconds, length ``n_samples``. + :rtype: numpy.ndarray + :raises ValueError: if ``frequency_hz`` is non-positive. + """ + if frequency_hz <= 0: + raise ValueError(f"frequency_hz must be positive, got {frequency_hz}.") + return np.arange(n_samples, dtype=np.float64) / float(frequency_hz) + + +class _ColorSwatch(QtWidgets.QPushButton): # pragma: no cover + """Small color square that opens a color picker when clicked.""" + + color_changed = QtCore.pyqtSignal(QtGui.QColor) + + def __init__( + self, initial: QtGui.QColor, parent: QtWidgets.QWidget | None = None + ) -> None: + """Build a swatch displaying ``initial`` and emitting on user changes.""" + super().__init__(parent) + self._color: QtGui.QColor = QtGui.QColor(initial) + self.setFixedSize(18, 18) + self.setToolTip("Click to change this signal's line color.") + self._refresh_style() + self.clicked.connect(self._on_clicked) + + def color(self) -> QtGui.QColor: + """Return the swatch's current color.""" + return QtGui.QColor(self._color) + + def set_color(self, color: QtGui.QColor) -> None: + """Set the swatch color without emitting ``color_changed``.""" + self._color = QtGui.QColor(color) + self._refresh_style() + + def _refresh_style(self) -> None: + rgba = self._color + self.setStyleSheet( + f"background-color: rgba({rgba.red()}, {rgba.green()}, " + f"{rgba.blue()}, {rgba.alpha()});" + "border: 1px solid #555; border-radius: 2px;", + ) + + def _on_clicked(self) -> None: + picked = QtWidgets.QColorDialog.getColor( + self._color, + self, + "Pick line color", + ) + if picked.isValid(): + self._color = picked + self._refresh_style() + self.color_changed.emit(picked) + + +class _SubplotWidget(QtWidgets.QFrame): # pragma: no cover + """One subplot: a ``pyqtgraph.PlotWidget`` plus an overlaid mode toolbar. + + Owns its curves and legend so the parent window only has to manage + high-level layout (how many subplots and which columns go where). + + Signals: + + * ``activated()`` -- emitted on any user interaction inside this subplot; + the parent uses it to know which subplot the side-panel checkboxes + should target. + * ``point_picked(time_sec, value, name)`` -- emitted in Pick mode when the + user clicks near a data point; the parent displays the readout. + """ + + MODE_PAN: str = "pan" + MODE_TIME_ZOOM: str = "time_zoom" + MODE_GENERAL_ZOOM: str = "general_zoom" + MODE_PICKER: str = "picker" + + activated = QtCore.pyqtSignal() + point_picked = QtCore.pyqtSignal(float, float, str) + + def __init__( + self, + index: int, + initial_mode: str = MODE_TIME_ZOOM, + parent: QtWidgets.QWidget | None = None, + ) -> None: + """Build one subplot with its own plot widget and mode toolbar. + + :param int index: zero-based subplot index, used for the title. + :param str initial_mode: starting mouse-interaction mode. + :param QtWidgets.QWidget parent: optional Qt parent. + """ + super().__init__(parent) + self.setObjectName("subplotFrame") + self.setFrameShape(QtWidgets.QFrame.Shape.NoFrame) + + self._index: int = index + self._mode: str = initial_mode + + self.plot_widget: pg.PlotWidget = pg.PlotWidget() + self.plot_widget.showGrid(x=True, y=True, alpha=0.3) + self.plot_widget.setLabel("bottom", "time", units="s") + self.plot_widget.setTitle(f"Subplot {index + 1}") + self._legend: pg.LegendItem = self.plot_widget.addLegend(offset=(10, 10)) + + # Curves currently displayed: column name → PlotDataItem. + self.curves: dict[str, pg.PlotDataItem] = {} + + # Marker shown in Pick mode. + self._pick_marker: pg.ScatterPlotItem | None = None + self._pick_label: pg.TextItem | None = None + + layout = QtWidgets.QVBoxLayout(self) + layout.setContentsMargins(0, 0, 0, 0) + layout.addWidget(self.plot_widget) + + self._toolbar, self._mode_buttons = self._build_mode_toolbar() + self._toolbar.setParent(self) + self._toolbar.raise_() + + self.set_active(False) + # scene() is typed as Optional in PyQt stubs and sigMouseClicked is a + # pyqtgraph-specific signal that PyQt's stubs don't know about. + scene = self.plot_widget.scene() + assert scene is not None + scene.sigMouseClicked.connect(self._on_scene_clicked) # pyright: ignore[reportAttributeAccessIssue] + self.set_mode(self._mode) + + # --- toolbar construction ---------------------------------------- + + def _build_mode_toolbar( + self, + ) -> tuple[QtWidgets.QFrame, dict[str, QtWidgets.QToolButton]]: + """Build the floating mode toolbar shown in the plot's upper-right corner.""" + bar = QtWidgets.QFrame() + bar.setObjectName("modeBar") + bar.setStyleSheet( + "#modeBar { background: rgba(255, 255, 255, 220); " + "border: 1px solid #888; border-radius: 4px; }" + "QToolButton { padding: 2px 6px; }" + "QToolButton:checked { background: #cfe1f7; border: 1px solid #4a90e2; " + "border-radius: 3px; }", + ) + row = QtWidgets.QHBoxLayout(bar) + row.setContentsMargins(3, 3, 3, 3) + row.setSpacing(2) + + buttons: dict[str, QtWidgets.QToolButton] = {} + group = QtWidgets.QButtonGroup(bar) + group.setExclusive(True) + + entries: list[tuple[str, str, str]] = [ + (self.MODE_PAN, "Pan", "Pan: left-drag translates the view."), + ( + self.MODE_TIME_ZOOM, + "T-Zoom", + "Time-only zoom: wheel zooms the X axis; Y auto-fits (default).", + ), + ( + self.MODE_GENERAL_ZOOM, + "Zoom", + "General zoom: left-drag draws a zoom rectangle; wheel zooms X and Y.", + ), + ( + self.MODE_PICKER, + "Pick", + "Data cursor: click near a curve point to read its value.", + ), + ] + for mode, label, tooltip in entries: + btn = QtWidgets.QToolButton() + btn.setText(label) + btn.setToolTip(tooltip) + btn.setCheckable(True) + btn.setAutoRaise(True) + btn.clicked.connect(self.activated.emit) + btn.clicked.connect(lambda _checked, m=mode: self.set_mode(m)) + group.addButton(btn) + row.addWidget(btn) + buttons[mode] = btn + return bar, buttons + + # --- curve management -------------------------------------------- + + def add_curve( + self, + name: str, + time_sec: np.ndarray, + y_values: np.ndarray, + pen: QtGui.QPen, + ) -> None: + """Plot one column on this subplot (no-op if already present).""" + if name in self.curves: + return + item = self.plot_widget.plot(time_sec, y_values, pen=pen, name=name) + self.curves[name] = item + self._update_left_label() + + def remove_curve(self, name: str) -> None: + """Remove one column from this subplot (no-op if absent).""" + item = self.curves.pop(name, None) + if item is None: + return + self.plot_widget.removeItem(item) + try: + self._legend.removeItem(name) + except (KeyError, AttributeError): + # Older pyqtgraph builds may raise if the entry is already gone. + pass + self._clear_pick_marker() + self._update_left_label() + + def remove_all_curves(self) -> None: + """Remove every curve currently shown on this subplot.""" + for name in list(self.curves.keys()): + self.remove_curve(name) + + def set_curve_pen(self, name: str, pen: QtGui.QPen) -> None: + """Update an existing curve's pen (color/width) in place.""" + item = self.curves.get(name) + if item is None: + return + item.setPen(pen) + # Re-stamp the legend sample so its swatch reflects the new pen. + try: + self._legend.removeItem(name) + except (KeyError, AttributeError): + pass + self._legend.addItem(item, name) + + def _update_left_label(self) -> None: + """Show the column name on the Y axis when exactly one curve is plotted.""" + if len(self.curves) == 1: + self.plot_widget.setLabel("left", next(iter(self.curves))) + else: + self.plot_widget.setLabel("left", "") + + # --- mode handling ----------------------------------------------- + + def set_mode(self, mode: str) -> None: + """Switch this subplot's mouse-interaction mode. + + :param str mode: one of the ``MODE_*`` class constants. + """ + self._mode = mode + btn = self._mode_buttons.get(mode) + if btn is not None and not btn.isChecked(): + btn.setChecked(True) + self._apply_mode() + if mode != self.MODE_PICKER: + self._clear_pick_marker() + + def _apply_mode(self) -> None: + """Configure ViewBox and cursor to match ``self._mode``.""" + plot_item = self.plot_widget.getPlotItem() + assert plot_item is not None + view_box = plot_item.getViewBox() + assert view_box is not None + viewport = self.plot_widget.viewport() + assert viewport is not None + if self._mode == self.MODE_PAN: + view_box.setMouseMode(pg.ViewBox.PanMode) + view_box.setMouseEnabled(x=True, y=True) + viewport.setCursor(QtCore.Qt.CursorShape.OpenHandCursor) + elif self._mode == self.MODE_TIME_ZOOM: + view_box.setMouseMode(pg.ViewBox.PanMode) + view_box.setMouseEnabled(x=True, y=False) + # Intentionally do NOT re-enable Y auto-range here: switching INTO + # T-Zoom should preserve whatever Y range the user has set in + # another mode. Y is just locked from mouse input, not refit. + viewport.setCursor(QtCore.Qt.CursorShape.SizeHorCursor) + elif self._mode == self.MODE_GENERAL_ZOOM: + view_box.setMouseMode(pg.ViewBox.RectMode) + view_box.setMouseEnabled(x=True, y=True) + viewport.setCursor(QtCore.Qt.CursorShape.CrossCursor) + elif self._mode == self.MODE_PICKER: + view_box.setMouseMode(pg.ViewBox.PanMode) + view_box.setMouseEnabled(x=False, y=False) + viewport.setCursor(QtCore.Qt.CursorShape.CrossCursor) + + # --- active styling ---------------------------------------------- + + def set_active(self, active: bool) -> None: + """Toggle the visual highlight that marks the active subplot.""" + if active: + self.setStyleSheet( + "#subplotFrame { border: 2px solid #4a90e2; border-radius: 3px; }", + ) + else: + self.setStyleSheet( + "#subplotFrame { border: 1px solid #cccccc; border-radius: 3px; }", + ) + + # --- click handling (activate + picker) -------------------------- + + def _on_scene_clicked(self, event: Any) -> None: + """Activate this subplot on any click; in Pick mode, report the nearest point. + + ``event`` is a ``pg.GraphicsScene.mouseEvents.MouseClickEvent`` at + runtime, but that internal pyqtgraph type isn't exposed via stubs, so + we accept ``Any`` rather than chasing a private import. + """ + self.activated.emit() + if self._mode != self.MODE_PICKER: + return + if event.button() != QtCore.Qt.MouseButton.LeftButton: + return + plot_item = self.plot_widget.getPlotItem() + assert plot_item is not None + view_box = plot_item.getViewBox() + assert view_box is not None + scene_pos = event.scenePos() + if not self.plot_widget.sceneBoundingRect().contains(scene_pos): + return + view_point = view_box.mapSceneToView(scene_pos) + nearest = self._find_nearest_point( + click_x=float(view_point.x()), + click_y=float(view_point.y()), + ) + if nearest is None: + return + time_sec, value, name = nearest + self._show_pick_marker(time_sec=time_sec, value=value, name=name) + self.point_picked.emit(time_sec, value, name) + + def _find_nearest_point( + self, + click_x: float, + click_y: float, + ) -> tuple[float, float, str] | None: + """Return ``(x, y, curve_name)`` for the data point closest to the click.""" + plot_item = self.plot_widget.getPlotItem() + assert plot_item is not None + view_box = plot_item.getViewBox() + assert view_box is not None + pixel_w, pixel_h = view_box.viewPixelSize() + pixel_w = pixel_w or 1.0 + pixel_h = pixel_h or 1.0 + + best: tuple[float, float, str] | None = None + best_dist = float("inf") + for name, item in self.curves.items(): + data = item.getData() + if data is None: + continue + x_data, y_data = data + if x_data is None or y_data is None or len(x_data) == 0: + continue + idx = int(np.argmin(np.abs(x_data - click_x))) + x_val = float(x_data[idx]) + y_val = float(y_data[idx]) + dx = (x_val - click_x) / pixel_w + dy = (y_val - click_y) / pixel_h + dist = (dx * dx + dy * dy) ** 0.5 + if dist < best_dist: + best_dist = dist + best = (x_val, y_val, name) + return best + + def _show_pick_marker(self, time_sec: float, value: float, name: str) -> None: + """Draw / move the picker marker and its text label at the given point.""" + if self._pick_marker is None: + self._pick_marker = pg.ScatterPlotItem( + size=12, + pen=pg.mkPen("k", width=1), + brush=pg.mkBrush(255, 80, 80, 220), + ) + self.plot_widget.addItem(self._pick_marker) + self._pick_marker.setData([time_sec], [value]) + + if self._pick_label is None: + self._pick_label = pg.TextItem(anchor=(0.0, 1.0), color="k") + self.plot_widget.addItem(self._pick_label) + self._pick_label.setText(f"{name}\nt={time_sec:.3f}s, y={value:.4g}") + self._pick_label.setPos(time_sec, value) + + def _clear_pick_marker(self) -> None: + """Remove the picker marker / label from this subplot if present.""" + if self._pick_marker is not None: + self.plot_widget.removeItem(self._pick_marker) + self._pick_marker = None + if self._pick_label is not None: + self.plot_widget.removeItem(self._pick_label) + self._pick_label = None + + # --- layout ------------------------------------------------------ + + def resizeEvent(self, a0: QtGui.QResizeEvent | None) -> None: # noqa: N802 + """Keep the mode toolbar anchored to the upper-right corner. + + ``a0`` is named to match the PyQt6 base-class signature so the + override is recognized by the type checker. + """ + super().resizeEvent(a0) + self._toolbar.adjustSize() + margin = 6 + x = self.width() - self._toolbar.width() - margin + self._toolbar.move(max(0, x), margin) + + +class CSVInspectorWindow(QtWidgets.QMainWindow): # pragma: no cover + """Main window for the modular CSV plotter. + + The user picks the number of subplots from the left panel, clicks a + subplot to make it active, then ticks which CSV columns appear in it. + All subplots share their X axis, so panning / zooming time stays in + sync. + """ + + _MAX_SUBPLOTS: int = 8 + + # Class-level reference list that keeps every open inspector window alive + # so the garbage collector doesn't reap one when the "Open in New Window" + # handler returns. Cleared per-window in closeEvent. + _open_windows: ClassVar[list[CSVInspectorWindow]] = [] + + def __init__( + self, + csv_path: Path, + frequency_hz: int = BasicConfig.frequency, + time_only_zoom: bool = True, + ) -> None: + """Build the GUI for one CSV file. + + :param pathlib.Path csv_path: path to a CSV file with a header row. + :param int frequency_hz: sampling frequency in Hz used to synthesize + the time axis. Defaults to ``BasicConfig.frequency``. + :param bool time_only_zoom: starting mouse-interaction mode for every + subplot. ``True`` (default) selects the time-only zoom mode, which + matches the Simulink Data Inspector feel. ``False`` selects the + general (X+Y) zoom mode. + """ + super().__init__() + + pg.setConfigOption("background", "w") + pg.setConfigOption("foreground", "k") + pg.setConfigOption("antialias", True) + + self._csv_path: Path = Path(csv_path) + self._frequency_hz: int = int(frequency_hz) + self._initial_mode: str = ( + _SubplotWidget.MODE_TIME_ZOOM + if time_only_zoom + else _SubplotWidget.MODE_GENERAL_ZOOM + ) + + self._dataframe: pd.DataFrame = pd.DataFrame() + self._columns: list[str] = [] + self._time_sec: np.ndarray = np.empty(0, dtype=np.float64) + self._column_colors: dict[str, QtGui.QColor] = {} + self._subplot_signals: list[set[str]] = [] + self._subplots: list[_SubplotWidget] = [] + self._signal_checkboxes: dict[str, QtWidgets.QCheckBox] = {} + self._signal_swatches: dict[str, _ColorSwatch] = {} + self._active_index: int = 0 + + self._load_csv(self._csv_path) + self._init_column_colors() + self._subplot_signals = [set(self._columns[: min(2, len(self._columns))])] + + self.resize(1200, 800) + self.setWindowTitle(f"CSV Inspector — {self._csv_path.name}") + status_bar = self.statusBar() + assert status_bar is not None + status_bar.showMessage("Ready.") + + self._build_menu() + self._build_layout() + self._apply_layout() + + # Register so a strong reference outlives the constructing scope. + CSVInspectorWindow._open_windows.append(self) + + # --- data loading ------------------------------------------------- + + def _init_column_colors(self) -> None: + """Assign a stable default color to each column from pyqtgraph's palette.""" + self._column_colors = {} + hues = max(len(self._columns), 6) + for idx, col in enumerate(self._columns): + self._column_colors[col] = pg.intColor(idx, hues=hues) + + def _load_csv(self, csv_path: Path) -> None: + """Read a CSV from disk and refresh ``_columns`` / ``_time_sec``. + + :raises ValueError: if the CSV exposes no numeric, non-time columns. + """ + logger.info(f"Loading CSV '{csv_path}'.") + self._dataframe = pd.read_csv(csv_path) + self._columns = discover_plottable_columns(self._dataframe) + if not self._columns: + raise ValueError( + f"CSV '{csv_path}' has no numeric (non-time) columns to plot.", + ) + self._time_sec = synthesize_time_vector( + n_samples=len(self._dataframe), + frequency_hz=self._frequency_hz, + ) + + # --- UI construction ---------------------------------------------- + + def _build_menu(self) -> None: + """Construct the File and View menus. + + QMainWindow.menuBar(), QMenuBar.addMenu(), and QMenu.addAction() are + all typed as Optional in the PyQt stubs even though they always return + a real object on a QMainWindow that owns a menu bar. Asserts narrow + the types for the checker without changing runtime behavior. + """ + menu = self.menuBar() + assert menu is not None + file_menu = menu.addMenu("&File") + assert file_menu is not None + + open_action = file_menu.addAction("&Open CSV…") + assert open_action is not None + open_action.setShortcut("Ctrl+O") + open_action.triggered.connect(self._on_open_csv) + + open_new_action = file_menu.addAction("Open CSV in &New Window…") + assert open_new_action is not None + open_new_action.setShortcut("Ctrl+Shift+O") + open_new_action.triggered.connect(self._on_open_csv_new_window) + + file_menu.addSeparator() + quit_action = file_menu.addAction("&Quit") + assert quit_action is not None + quit_action.setShortcut("Ctrl+Q") + quit_action.triggered.connect(self.close) + + view_menu = menu.addMenu("&View") + assert view_menu is not None + reset_action = view_menu.addAction("Reset view (auto-range)") + assert reset_action is not None + reset_action.setShortcut("Ctrl+R") + reset_action.triggered.connect(self._on_reset_view) + + def _build_layout(self) -> None: + """Build the central plot area and the left-side signal panel.""" + self._plot_area = QtWidgets.QSplitter(QtCore.Qt.Orientation.Vertical) + self.setCentralWidget(self._plot_area) + + dock = QtWidgets.QDockWidget("Signals", self) + dock.setAllowedAreas( + QtCore.Qt.DockWidgetArea.LeftDockWidgetArea + | QtCore.Qt.DockWidgetArea.RightDockWidgetArea, + ) + + panel = QtWidgets.QWidget(dock) + outer = QtWidgets.QVBoxLayout(panel) + outer.setContentsMargins(8, 8, 8, 8) + + outer.addWidget(QtWidgets.QLabel(f"File: {self._csv_path.name}")) + outer.addWidget( + QtWidgets.QLabel( + f"X axis: time (s) synthesized at {self._frequency_hz} Hz", + ), + ) + + count_row = QtWidgets.QHBoxLayout() + count_row.addWidget(QtWidgets.QLabel("Number of subplots:")) + self._count_spin = QtWidgets.QSpinBox() + self._count_spin.setRange(1, self._MAX_SUBPLOTS) + self._count_spin.setValue(len(self._subplot_signals)) + self._count_spin.valueChanged.connect(self._on_subplot_count_changed) + count_row.addWidget(self._count_spin) + count_row.addStretch(1) + outer.addLayout(count_row) + + outer.addWidget(_make_separator()) + + self._active_label = QtWidgets.QLabel() + self._active_label.setStyleSheet("font-weight: bold;") + outer.addWidget(self._active_label) + + outer.addWidget( + QtWidgets.QLabel("Tick a column to add it to the active subplot:"), + ) + + signals_box = QtWidgets.QGroupBox("Signals") + signals_layout = QtWidgets.QVBoxLayout(signals_box) + self._populate_signal_rows(signals_layout) + + scroll = QtWidgets.QScrollArea() + scroll.setWidgetResizable(True) + scroll.setWidget(signals_box) + outer.addWidget(scroll, stretch=1) + + dock.setWidget(panel) + self.addDockWidget(QtCore.Qt.DockWidgetArea.LeftDockWidgetArea, dock) + + # --- layout sync -------------------------------------------------- + + def _apply_layout(self) -> None: + """Reconcile the GUI with the current ``_subplot_signals`` state.""" + n_subplots = len(self._subplot_signals) + self._sync_subplots(n_subplots) + self._active_index = min(self._active_index, n_subplots - 1) + self._refresh_curves() + self._refresh_signal_checkboxes() + self._refresh_active_indicator() + for subplot in self._subplots: + subplot.plot_widget.enableAutoRange(axis="x", enable=True) + + def _sync_subplots(self, n_subplots: int) -> None: + """Add or remove subplot widgets to match ``n_subplots`` and re-link X axes.""" + while len(self._subplots) > n_subplots: + subplot = self._subplots.pop() + subplot.remove_all_curves() + subplot.setParent(None) + subplot.deleteLater() + + while len(self._subplots) < n_subplots: + idx = len(self._subplots) + subplot = _SubplotWidget(index=idx, initial_mode=self._initial_mode) + subplot.activated.connect( + lambda i=idx: self._on_subplot_activated(i), + ) + subplot.point_picked.connect(self._on_point_picked) + self._plot_area.addWidget(subplot) + self._subplots.append(subplot) + + if self._subplots: + base_view = self._subplots[0].plot_widget + for subplot in self._subplots[1:]: + subplot.plot_widget.setXLink(base_view) + + def _refresh_curves(self) -> None: + """Reconcile curves on every subplot against ``_subplot_signals``.""" + for i, subplot in enumerate(self._subplots): + desired = self._subplot_signals[i] + for stale in set(subplot.curves.keys()) - desired: + subplot.remove_curve(stale) + for col in sorted(desired): + if col in subplot.curves: + continue + y_values = self._dataframe[col].to_numpy(dtype=np.float64) + subplot.add_curve( + name=col, + time_sec=self._time_sec, + y_values=y_values, + pen=self._pen_for_column(col), + ) + + def _refresh_signal_checkboxes(self) -> None: + """Sync the side-panel checkboxes to the active subplot's signal set.""" + if not self._subplots: + return + active_set = self._subplot_signals[self._active_index] + for col, cb in self._signal_checkboxes.items(): + cb.blockSignals(True) + cb.setChecked(col in active_set) + cb.blockSignals(False) + + def _refresh_active_indicator(self) -> None: + """Update the 'Editing: Subplot N' label and per-subplot border highlight.""" + for i, subplot in enumerate(self._subplots): + subplot.set_active(i == self._active_index) + if self._subplots: + self._active_label.setText(f"Editing: Subplot {self._active_index + 1}") + else: + self._active_label.setText("") + + def _pen_for_column(self, column: str) -> QtGui.QPen: + """Return the pen for ``column`` using its currently-selected color.""" + color = self._column_colors.get(column) or QtGui.QColor("#888888") + return pg.mkPen(color=color, width=2) + + def _populate_signal_rows(self, layout: QtWidgets.QVBoxLayout) -> None: + """Build one [color-swatch][checkbox] row per column into ``layout``.""" + for col in self._columns: + row = QtWidgets.QWidget() + row_layout = QtWidgets.QHBoxLayout(row) + row_layout.setContentsMargins(0, 0, 0, 0) + row_layout.setSpacing(6) + + swatch = _ColorSwatch(self._column_colors[col]) + swatch.color_changed.connect( + lambda color, name=col: self._on_color_changed(name, color), + ) + row_layout.addWidget(swatch) + + cb = QtWidgets.QCheckBox(col) + cb.toggled.connect( + lambda checked, name=col: self._on_signal_toggled(name, checked), + ) + row_layout.addWidget(cb, stretch=1) + + layout.addWidget(row) + self._signal_checkboxes[col] = cb + self._signal_swatches[col] = swatch + layout.addStretch(1) + + # --- slots -------------------------------------------------------- + + def _on_subplot_count_changed(self, value: int) -> None: + """Handle the subplot-count spin box: grow or shrink the model.""" + current = len(self._subplot_signals) + if value > current: + for _ in range(value - current): + self._subplot_signals.append(set()) + else: + self._subplot_signals = self._subplot_signals[:value] + self._apply_layout() + + def _on_signal_toggled(self, column: str, checked: bool) -> None: + """Add or remove ``column`` from the active subplot.""" + if not self._subplots: + return + target = self._subplot_signals[self._active_index] + if checked: + target.add(column) + else: + target.discard(column) + self._refresh_curves() + + def _on_subplot_activated(self, index: int) -> None: + """Make ``index`` the active subplot (the one the side panel edits).""" + if index == self._active_index: + return + self._active_index = index + self._refresh_signal_checkboxes() + self._refresh_active_indicator() + + def _on_point_picked(self, time_sec: float, value: float, name: str) -> None: + """Display the picked data point in the status bar.""" + status_bar = self.statusBar() + assert status_bar is not None + status_bar.showMessage( + f"{name} t = {time_sec:.4f} s y = {value:.6g}", + ) + + def _on_color_changed(self, column: str, color: QtGui.QColor) -> None: + """Update the stored color for ``column`` and restyle every curve using it.""" + self._column_colors[column] = QtGui.QColor(color) + pen = self._pen_for_column(column) + for subplot in self._subplots: + subplot.set_curve_pen(column, pen) + swatch = self._signal_swatches.get(column) + if swatch is not None: + swatch.set_color(color) + + def _on_reset_view(self) -> None: + """Auto-range both axes on every subplot.""" + for subplot in self._subplots: + subplot.plot_widget.enableAutoRange(axis="x", enable=True) + subplot.plot_widget.enableAutoRange(axis="y", enable=True) + + def _on_open_csv_new_window(self) -> None: + """Open a CSV in a *new* inspector window (this one stays open). + + Useful for comparing two or more recordings side-by-side. The new + window is appended to ``CSVInspectorWindow._open_windows`` so it + survives past the end of this method. + """ + path_str, _ = QtWidgets.QFileDialog.getOpenFileName( + self, + "Open CSV in New Window", + str(self._csv_path.parent), + "CSV files (*.csv)", + ) + if not path_str: + return + try: + new_window = CSVInspectorWindow( + csv_path=Path(path_str), + frequency_hz=self._frequency_hz, + time_only_zoom=self._initial_mode == _SubplotWidget.MODE_TIME_ZOOM, + ) + except (ValueError, OSError, pd.errors.ParserError) as exc: + QtWidgets.QMessageBox.critical(self, "Failed to load CSV", str(exc)) + return + new_window.show() + + def closeEvent(self, a0: QtGui.QCloseEvent | None) -> None: # noqa: N802 + """Drop ourselves from the global open-windows list on close. + + ``a0`` is named to match the PyQt6 base-class signature so the + override is recognized by the type checker. + """ + try: + CSVInspectorWindow._open_windows.remove(self) + except ValueError: + pass + super().closeEvent(a0) + + def _on_open_csv(self) -> None: + """Open a new CSV in the running window via a file dialog.""" + path_str, _ = QtWidgets.QFileDialog.getOpenFileName( + self, + "Open CSV", + str(self._csv_path.parent), + "CSV files (*.csv)", + ) + if not path_str: + return + new_path = Path(path_str) + try: + self._load_csv(new_path) + except (ValueError, OSError, pd.errors.ParserError) as exc: + QtWidgets.QMessageBox.critical(self, "Failed to load CSV", str(exc)) + return + + self._csv_path = new_path + self.setWindowTitle(f"CSV Inspector — {new_path.name}") + + for subplot in self._subplots: + subplot.remove_all_curves() + subplot.setParent(None) + subplot.deleteLater() + self._subplots.clear() + + self._init_column_colors() + self._subplot_signals = [set(self._columns[: min(2, len(self._columns))])] + self._active_index = 0 + self._count_spin.blockSignals(True) + self._count_spin.setValue(1) + self._count_spin.blockSignals(False) + self._rebuild_signal_checkboxes() + self._apply_layout() + + def _rebuild_signal_checkboxes(self) -> None: + """Rebuild the side-panel signal rows against the current columns.""" + signals_box = self._find_signals_groupbox() + if signals_box is None: + return + layout = signals_box.layout() + # _build_layout() always installs a QVBoxLayout here, but QGroupBox.layout() + # is typed as Optional[QLayout]. Narrow it for both pyright and runtime. + if not isinstance(layout, QtWidgets.QVBoxLayout): + return + while layout.count(): + item = layout.takeAt(0) + if item is None: + break + widget = item.widget() + if widget is not None: + widget.setParent(None) + widget.deleteLater() + self._signal_checkboxes.clear() + self._signal_swatches.clear() + self._populate_signal_rows(layout) + + def _find_signals_groupbox(self) -> QtWidgets.QGroupBox | None: + """Locate the 'Signals' group box inside the side-panel dock.""" + for dock in self.findChildren(QtWidgets.QDockWidget): + for box in dock.findChildren(QtWidgets.QGroupBox): + if box.title() == "Signals": + return box + return None + + +def _make_separator() -> QtWidgets.QFrame: # pragma: no cover + """Return a thin horizontal divider for use in the side panel.""" + line = QtWidgets.QFrame() + line.setFrameShape(QtWidgets.QFrame.Shape.HLine) + line.setFrameShadow(QtWidgets.QFrame.Shadow.Sunken) + return line + + +def plot( + csv_path: str | Path, + frequency_hz: int = BasicConfig.frequency, + time_only_zoom: bool = True, +) -> None: + """Open the modular CSV inspector for the given file. + + The time axis is synthesized as ``numpy.arange(n_samples) / frequency_hz``; + no time column is required in the CSV. + + :param csv_path: path to a CSV file with a header row. + :type csv_path: str or pathlib.Path + :param int frequency_hz: sampling frequency in Hz used to synthesize the + time axis. Defaults to ``BasicConfig.frequency``. + :param bool time_only_zoom: starting interaction mode for every subplot. + ``True`` (default) is the Simulink-Data-Inspector-style time-only + zoom; ``False`` is general (X + Y) zoom. Either mode can also be + switched per subplot from its in-plot toolbar. + """ + path = Path(csv_path) + app = QtWidgets.QApplication.instance() or QtWidgets.QApplication(sys.argv) + window = CSVInspectorWindow( + csv_path=path, + frequency_hz=frequency_hz, + time_only_zoom=time_only_zoom, + ) + window.show() + app.exec() diff --git a/src/hip_controller/plotter/csv_player.py b/src/hip_controller/plotter/csv_player.py index ce18b2c..6d31aaf 100644 --- a/src/hip_controller/plotter/csv_player.py +++ b/src/hip_controller/plotter/csv_player.py @@ -1,5 +1,6 @@ """Stateful CSV player that simulates real-time data arrival.""" +from dataclasses import dataclass from pathlib import Path from loguru import logger @@ -7,6 +8,65 @@ from hip_controller.definitions import ExosuitData, RecordedSensorData, SensorSignal +# Default classification value used when the Classification Left / Right +# columns are absent (or contain NaN / unmappable values). Maps to Level +# Ground mode in the application-level dispatch. +DEFAULT_CLASSIFICATION = 0 + +# Accepted header names for each logical column. The first entry is the +# canonical name (matches RecordedSensorData where applicable) and is what +# the tests / data pipeline write out; the others are tolerated on input so +# externally produced CSVs (e.g. MATLAB exports) don't need to be renamed +# before playback. +COLUMN_ALIASES: dict[str, tuple[str, ...]] = { + "timestamp": (RecordedSensorData.timestamp, "Time [s]", "time"), + "ang_left": (RecordedSensorData.ang_left, "Angle Left Raw [rad]"), + "ang_right": (RecordedSensorData.ang_right, "Angle Right Raw [rad]"), + "vel_left": (RecordedSensorData.vel_left, "Vel Left Raw [rad]"), + "vel_right": (RecordedSensorData.vel_right, "Vel Right Raw [rad]"), + "main_switch": (RecordedSensorData.main_switch, "Main Switch"), + "classification_left": ("classification_left", "Classification Left"), + "classification_right": ("classification_right", "Classification Right"), +} + + +@dataclass +class PlayerStep: + """One row of CSV playback: sensor signals + per-sample control inputs. + + :sensor_data: Raw (or recorded) left/right angle and velocity signals + plus the timestamp shared by both legs. + :main_switch: Whether the controller should run this sample (``True``) or + be held idle (``False``). Defaults to ``True`` when the column is absent. + :classification_left: Locomotion-mode classification for the left leg + (0 = Level Ground, 1 = Ascend Stairs, 2 = Descend Stairs). Defaults to + :data:`DEFAULT_CLASSIFICATION` when the column is absent. + :classification_right: Locomotion-mode classification for the right leg. + """ + + sensor_data: ExosuitData + main_switch: bool + classification_left: int + classification_right: int + + +def _resolve_column( + available_columns: list[str], candidates: tuple[str, ...] +) -> str | None: + """Return the first candidate header present in ``available_columns``. + + :param list[str] available_columns: Column names found in the loaded CSV. + :param tuple[str, ...] candidates: Accepted header names for one logical + column, ordered by preference (canonical first). + :return: The matching column name, or ``None`` if none of the candidates + are present. + :rtype: str | None + """ + for name in candidates: + if name in available_columns: + return name + return None + class CSVPlayer: """The CSV file is loaded fully once using pandas. @@ -25,13 +85,44 @@ def __init__(self, csv_path: Path) -> None: :param str csv_path: Path to the CSV file containing time, angle, and velocity columns. Default takes the file path from RecordedSensorData setup in definitions. """ logger.info(f"Loading CSV file '{csv_path}'.") - self.dataframe = read_csv(csv_path) + # sep=None + engine='python' lets pandas sniff the delimiter, so both + # comma- and semicolon-separated files load without manual configuration. + # decimal=',' covers European-locale exports (e.g. MATLAB on German + # systems) that write "3,14" instead of "3.14". + self.dataframe = read_csv(csv_path, sep=None, engine="python", decimal=",") + # Strip incidental whitespace from headers so " angle_left (rad)" still + # matches "angle_left (rad)". + self.dataframe.columns = [str(c).strip() for c in self.dataframe.columns] self.counter = 0 - self.has_timestamp: bool = ( - RecordedSensorData.timestamp in self.dataframe.columns + available = list(self.dataframe.columns) + self._col_timestamp = _resolve_column(available, COLUMN_ALIASES["timestamp"]) + self._col_ang_left = _resolve_column(available, COLUMN_ALIASES["ang_left"]) + self._col_ang_right = _resolve_column(available, COLUMN_ALIASES["ang_right"]) + self._col_vel_left = _resolve_column(available, COLUMN_ALIASES["vel_left"]) + self._col_vel_right = _resolve_column(available, COLUMN_ALIASES["vel_right"]) + self._col_main_switch = _resolve_column( + available, COLUMN_ALIASES["main_switch"] + ) + self._col_classification_left = _resolve_column( + available, COLUMN_ALIASES["classification_left"] + ) + self._col_classification_right = _resolve_column( + available, COLUMN_ALIASES["classification_right"] ) + if self._col_ang_left is None or self._col_ang_right is None: + raise KeyError( + "CSV is missing a left/right hip-angle column. Expected one of " + f"{COLUMN_ALIASES['ang_left']} and one of " + f"{COLUMN_ALIASES['ang_right']}. Found columns: {available}" + ) + + @property + def has_timestamp(self) -> bool: + """Whether a timestamp column was found in the CSV.""" + return self._col_timestamp is not None + def has_next_line(self) -> bool: """Check whether more data is available. @@ -40,28 +131,67 @@ def has_next_line(self) -> bool: """ return self.counter < len(self.dataframe) - def get_sensor_data_from_csv(self) -> ExosuitData: + def get_sensor_data_from_csv(self) -> PlayerStep: """Get the recorded data from csv line by line. - :return: timestamp, angle_left, velocity_left, angle_right, velocity_right packed together as an Exosuit dataclass - :rtype: ExosuitData + Velocity columns are optional: when missing, ``velocity_rad_per_sec`` is + set to 0.0 and the controller is expected to derive velocity from the + raw angle internally (run with ``filtered=False``). + + The main switch column is optional: when missing it defaults to ``True`` + (controller always active). + + The classification columns are optional: when missing they default to + :data:`DEFAULT_CLASSIFICATION` (Level Ground). + + :return: :class:`PlayerStep` bundling sensor signals, main switch and + per-leg locomotion classifications for this sample. + :rtype: PlayerStep """ row = self.dataframe.iloc[self.counter] self.counter += 1 - if self.has_timestamp: - timestamp = float(row[RecordedSensorData.timestamp]) + if self._col_timestamp is not None: + timestamp = float(row[self._col_timestamp]) else: timestamp = self.counter / RecordedSensorData.fake_frequency_hz - return ExosuitData( + vel_left = ( + float(row[self._col_vel_left]) if self._col_vel_left is not None else 0.0 + ) + vel_right = ( + float(row[self._col_vel_right]) if self._col_vel_right is not None else 0.0 + ) + main_switch = ( + bool(row[self._col_main_switch]) + if self._col_main_switch is not None + else True + ) + classification_left = ( + int(row[self._col_classification_left]) + if self._col_classification_left is not None + else DEFAULT_CLASSIFICATION + ) + classification_right = ( + int(row[self._col_classification_right]) + if self._col_classification_right is not None + else DEFAULT_CLASSIFICATION + ) + + exosuit_data = ExosuitData( left=SensorSignal( timestamp=timestamp, - angle_rad=float(row[RecordedSensorData.ang_left]), - velocity_rad_per_sec=float(row[RecordedSensorData.vel_left]), + angle_rad=float(row[self._col_ang_left]), + velocity_rad_per_sec=vel_left, ), right=SensorSignal( timestamp=timestamp, - angle_rad=float(row[RecordedSensorData.ang_right]), - velocity_rad_per_sec=float(row[RecordedSensorData.vel_right]), + angle_rad=float(row[self._col_ang_right]), + velocity_rad_per_sec=vel_right, ), ) + return PlayerStep( + sensor_data=exosuit_data, + main_switch=main_switch, + classification_left=classification_left, + classification_right=classification_right, + ) diff --git a/src/matlab-files/still_to_implement_controlling.slx b/src/matlab-files/still_to_implement_controlling.slx new file mode 100644 index 0000000..578967d Binary files /dev/null and b/src/matlab-files/still_to_implement_controlling.slx differ diff --git a/tests/conftest.py b/tests/conftest.py index c54532a..736e420 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -2,10 +2,9 @@ import os import sys -from hip_controller.definitions import StrEnum from pathlib import Path -from hip_controller.definitions import TESTING_DIR +from hip_controller.definitions import TESTING_DIR, StrEnum # Add the src directory to the path so that the quaternion_ekf package can be imported my_path = os.path.dirname(os.path.abspath(__file__)) diff --git a/tests/controller_test/app_test.py b/tests/controller_test/app_test.py index 90d6e3c..132e496 100644 --- a/tests/controller_test/app_test.py +++ b/tests/controller_test/app_test.py @@ -9,6 +9,7 @@ SensorSignal, WalkOnController, ) +from hip_controller.definitions import BasicConfig from tests.conftest import ( DATA_REFERENCE_MOTION_RIGHT, REL_TOL, @@ -18,7 +19,8 @@ def test_controller_right(): """Test the main function with the right lower limb data.""" - controller = WalkOnController(reverse=True, plot=False, filtered=True) + config = BasicConfig(filtered=True) + controller = WalkOnController(left_limb=False, config=config) df = read_csv(filepath_or_buffer=DATA_REFERENCE_MOTION_RIGHT) diff --git a/tests/controller_test/pre_process_testing/drift_removal_test.py b/tests/controller_test/pre_process_testing/drift_removal_test.py index abd06aa..2f40a0b 100644 --- a/tests/controller_test/pre_process_testing/drift_removal_test.py +++ b/tests/controller_test/pre_process_testing/drift_removal_test.py @@ -51,7 +51,7 @@ def test_low_pass_drift_removal() -> None: def test_notch_drift_removal() -> None: - """Test LowPassDriftRemoval against expected outputs.""" + """Test NotchDriftRemoval against expected outputs.""" # Load test data df = pd.read_csv(DATA_PRE_PROCESSING) diff --git a/tests/controller_test/pre_process_testing/filtering_test.py b/tests/controller_test/pre_process_testing/filtering_test.py index 99d0f8a..9af5bdf 100644 --- a/tests/controller_test/pre_process_testing/filtering_test.py +++ b/tests/controller_test/pre_process_testing/filtering_test.py @@ -3,7 +3,9 @@ from numpy import testing from pandas import read_csv -from hip_controller.control.signal_processing.filtering import SogiFllFiltering +from hip_controller.control.signal_processing.filtering import ( + SogiFllFiltering, +) from hip_controller.control.signal_processing.velocity_estimation import ( DiscreteDerivativeVelocityEstimation, ) diff --git a/tests/controller_test/pre_process_testing/kalman_test.py b/tests/controller_test/pre_process_testing/kalman_test.py index 7b21029..a836dc9 100644 --- a/tests/controller_test/pre_process_testing/kalman_test.py +++ b/tests/controller_test/pre_process_testing/kalman_test.py @@ -2,6 +2,7 @@ import numpy as np +from hip_controller.definitions import KalmanFilterConfig from hip_controller.filters.kalman_filter import KalmanFilter from hip_controller.utils.state_space import StateSpaceLinear @@ -22,12 +23,14 @@ def test_kalman_filter_initialization() -> None: C = np.eye(2) ss = StateSpaceLinear(A=A, C=C) - # Act - kf = KalmanFilter( + config = KalmanFilterConfig( state_space=ss, - initial_x=np.zeros((2, 1)), + initial_state=np.zeros((2, 1)), initial_covariance=np.eye(2), ) + + # Act + kf = KalmanFilter(config=config) for _i in range(10): kf.predict() _ = kf.update(z=np.array([[0.0]])) diff --git a/tests/utils_test/csv_inspector_test.py b/tests/utils_test/csv_inspector_test.py new file mode 100644 index 0000000..84f7ab2 --- /dev/null +++ b/tests/utils_test/csv_inspector_test.py @@ -0,0 +1,69 @@ +"""Tests for the pure helpers in :mod:`hip_controller.plotter.csv_inspector`. + +The GUI itself (``CSVInspectorWindow``) is not unit-tested because it requires +a Qt event loop and a display; it is annotated ``# pragma: no cover`` for the +same reason as ``live_phase_portrait.py``. +""" + +from __future__ import annotations + +import numpy as np +import pandas as pd +from pytest import raises + +from hip_controller.plotter.csv_inspector import ( + discover_plottable_columns, + synthesize_time_vector, +) + + +def test_discover_plottable_columns_filters_time_and_strings() -> None: + """Time-like and non-numeric columns must be excluded.""" + df = pd.DataFrame( + { + "time (s)": [0.0, 0.01, 0.02], + "angle_left (rad)": [0.1, 0.2, 0.3], + "vel_left (rad/s)": [1.0, 1.1, 1.2], + "label": ["a", "b", "c"], + }, + ) + assert discover_plottable_columns(df) == [ + "angle_left (rad)", + "vel_left (rad/s)", + ] + + +def test_discover_plottable_columns_preserves_csv_column_order() -> None: + """The output order must follow the CSV's column order, not be re-sorted.""" + df = pd.DataFrame( + { + "b_signal": [1.0, 2.0], + "a_signal": [3.0, 4.0], + "Timestamp": [0.0, 0.1], + }, + ) + assert discover_plottable_columns(df) == ["b_signal", "a_signal"] + + +def test_discover_plottable_columns_handles_empty_dataframe() -> None: + """An empty CSV yields an empty signal list without raising.""" + assert discover_plottable_columns(pd.DataFrame()) == [] + + +def test_synthesize_time_vector_uses_inverse_frequency() -> None: + """t[i] must equal i / frequency_hz.""" + time_sec = synthesize_time_vector(n_samples=4, frequency_hz=100) + np.testing.assert_allclose(time_sec, [0.0, 0.01, 0.02, 0.03]) + + +def test_synthesize_time_vector_length_matches_n_samples() -> None: + """The returned vector must have exactly n_samples entries.""" + assert synthesize_time_vector(n_samples=250, frequency_hz=50).shape == (250,) + + +def test_synthesize_time_vector_rejects_non_positive_frequency() -> None: + """Zero or negative frequency must raise ValueError.""" + with raises(ValueError): + synthesize_time_vector(n_samples=10, frequency_hz=0) + with raises(ValueError): + synthesize_time_vector(n_samples=10, frequency_hz=-100) diff --git a/tests/utils_test/csv_player_test.py b/tests/utils_test/csv_player_test.py index 560cf02..2593130 100644 --- a/tests/utils_test/csv_player_test.py +++ b/tests/utils_test/csv_player_test.py @@ -48,17 +48,22 @@ def test_csv_player_reads_rows_in_order(tmp_path): player = CSVPlayer(csv_path) - t0 = player.get_sensor_data_from_csv() - t1 = player.get_sensor_data_from_csv() + step0 = player.get_sensor_data_from_csv() + step1 = player.get_sensor_data_from_csv() - assert t0 == ExosuitData( + assert step0.sensor_data == ExosuitData( left=SensorSignal(timestamp=0.0, angle_rad=1.0, velocity_rad_per_sec=0.1), right=SensorSignal(timestamp=0.0, angle_rad=4.0, velocity_rad_per_sec=0.4), ) - assert t1 == ExosuitData( + assert step1.sensor_data == ExosuitData( left=SensorSignal(timestamp=0.1, angle_rad=2.0, velocity_rad_per_sec=0.2), right=SensorSignal(timestamp=0.1, angle_rad=5.0, velocity_rad_per_sec=0.5), ) + # Optional columns absent in fixture -> defaults applied. + assert step0.main_switch is True + assert step1.main_switch is True + assert step0.classification_left == 0 + assert step0.classification_right == 0 def test_csv_player_index_increments(tmp_path): diff --git a/uv.lock b/uv.lock index 75fd7f4..3823cf6 100644 --- a/uv.lock +++ b/uv.lock @@ -441,7 +441,7 @@ wheels = [ [[package]] name = "hip-controller" -version = "0.1.1" +version = "0.1.2" source = { editable = "." } dependencies = [ { name = "loguru" },