From 89dc6909621ba08f6697f4a1060a05f2dbe07d87 Mon Sep 17 00:00:00 2001 From: NGierden Date: Mon, 8 Jun 2026 16:55:35 +0200 Subject: [PATCH] Wire per-sample locomotion mode classification; add CSV inspector Integrates the locomotion-mode pipeline end-to-end and adds supporting diagnostics + offline-replay tooling. Control logic: - __main__: read classification_left/right per row and call amplitude_modulation.set_mode each sample; respect main_switch with controller reset on the falling edge; add --fast batch mode that dumps every internal intermediate to _output.csv and opens the result in the new CSV inspector. - WalkOnController: expose last_filtered_signal and last_gait_phase_rad; safety-gate the motor command to 0 when the filtered angle is negative. - AmplitudeModulation: cache per-stage values in a new AmplitudeIntermediates dataclass; retune Ascend/Descend stair-mode scale/power parameters. - MotorReferenceController / SogiFllFiltering / SensorPreprocessor: expose last_* attributes for offline logging of internal signals. - PreprocessorConfig: new VelocityInputAngle option (RAW / DRIFT_REMOVED / FILTERED); default keeps the previous behavior. - definitions: retune SOGI cadence bounds, initial frequency guess, LPF cutoffs, AMPLITUDE_GAIN (-6.5 -> -7), PID P gain (14 -> 8). Add main_switch column constant. Tooling: - CSVPlayer: tolerate semicolon delimiters, European decimals, alternate header names, missing timestamp/velocity/main_switch/ classification columns. Returns a new PlayerStep bundle. - plotter: new Simulink-Data-Inspector-style csv_inspector module with linked-axis stacked subplots and per-subplot pan/zoom/pick tools. Runnable via 'python -m hip_controller.plotter'. - __init__: fall back to '0.0.0+unknown' when neither installed package metadata nor pyproject.toml is reachable. - Tests updated for the PlayerStep return type; new csv_inspector tests added. Also includes a small ruff-format-driven consolidation of duplicate imports in tests/conftest.py. Co-Authored-By: Claude Opus 4.7 --- scripts/controller_simulator.py | 29 +- src/hip_controller/__init__.py | 15 +- src/hip_controller/__main__.py | 295 +++++- src/hip_controller/control/app.py | 22 + .../amplitude_modulation.py | 53 +- .../motor_reference_controller.py | 6 + .../control/signal_processing/filtering.py | 9 +- .../signal_processing/sensor_preprocessor.py | 57 +- src/hip_controller/definitions.py | 55 +- src/hip_controller/plotter/__init__.py | 5 + src/hip_controller/plotter/__main__.py | 70 ++ src/hip_controller/plotter/csv_inspector.py | 955 ++++++++++++++++++ src/hip_controller/plotter/csv_player.py | 156 ++- tests/conftest.py | 3 +- tests/utils_test/csv_inspector_test.py | 69 ++ tests/utils_test/csv_player_test.py | 13 +- 16 files changed, 1728 insertions(+), 84 deletions(-) create mode 100644 src/hip_controller/plotter/__init__.py create mode 100644 src/hip_controller/plotter/__main__.py create mode 100644 src/hip_controller/plotter/csv_inspector.py create mode 100644 tests/utils_test/csv_inspector_test.py 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/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..612078e 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,201 @@ 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 + controller_left = WalkOnController(reverse=True, plot=plot, filtered=False) + controller_right = WalkOnController(reverse=False, plot=plot, filtered=False) 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 + + 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) + ) - sensor_data: ExosuitData = player.get_sensor_data_from_csv() - controller_left.step(sensor_data.left) - controller_right.step(sensor_data.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 +294,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 +344,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..fe52a02 100644 --- a/src/hip_controller/control/app.py +++ b/src/hip_controller/control/app.py @@ -46,6 +46,15 @@ def __init__(self, reverse: bool, plot: bool = False, filtered=False): 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 +68,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 +84,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 +109,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/filtering.py b/src/hip_controller/control/signal_processing/filtering.py index f08a3ef..b32bccc 100644 --- a/src/hip_controller/control/signal_processing/filtering.py +++ b/src/hip_controller/control/signal_processing/filtering.py @@ -59,6 +59,11 @@ 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. @@ -69,9 +74,10 @@ def filter(self, angle_rad: float, time_difference: float) -> float: :return: (angle_surrogate, velocity_quadrature). :rtype: tuple[float, 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 +86,7 @@ def reset(self) -> None: :return: None """ self._sogi_filter.reset() + self.last_quadrature = 0.0 class LowPassFiltering(FilteringStrategy): diff --git a/src/hip_controller/control/signal_processing/sensor_preprocessor.py b/src/hip_controller/control/signal_processing/sensor_preprocessor.py index 509c856..3c5337d 100644 --- a/src/hip_controller/control/signal_processing/sensor_preprocessor.py +++ b/src/hip_controller/control/signal_processing/sensor_preprocessor.py @@ -19,7 +19,11 @@ from hip_controller.control.signal_processing.velocity_estimation import ( VelocityEstimationStrategy, ) -from hip_controller.definitions import PreprocessorConfig, SensorSignal +from hip_controller.definitions import ( + PreprocessorConfig, + SensorSignal, + VelocityInputAngle, +) class SensorPreprocessor: @@ -48,6 +52,26 @@ def __init__(self, config: PreprocessorConfig) -> None: self._prev_timestamp: float | None = None + # 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`. @@ -74,16 +98,36 @@ def filter(self, raw_signal: SensorSignal) -> SensorSignal: 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_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._sogi_fll, "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.config.velocity_input_angle == VelocityInputAngle.RAW: + velocity_input_angle_rad = raw_signal.angle_rad + elif self.config.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, @@ -97,6 +141,9 @@ def reset(self) -> None: :return: None """ self._prev_timestamp = None + 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() diff --git a/src/hip_controller/definitions.py b/src/hip_controller/definitions.py index 8fd05d5..beaaa95 100644 --- a/src/hip_controller/definitions.py +++ b/src/hip_controller/definitions.py @@ -1,14 +1,18 @@ """Common definitions for this module.""" -import sys +import sys from dataclasses import asdict, dataclass from enum import auto + if sys.version_info >= (3, 11): from enum import StrEnum else: from enum import Enum + class StrEnum(str, Enum): """String enum backport for Python <3.11.""" + + from math import pi from pathlib import Path @@ -75,7 +79,7 @@ class SolverType(StrEnum): 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 = ( @@ -113,36 +117,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: @@ -162,6 +166,19 @@ class DriftRemovalMethod(StrEnum): NOTCH = auto() +class VelocityInputAngle(StrEnum): + """Which angle is fed to the velocity-estimation stage. + + 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). + """ + + RAW = auto() + DRIFT_REMOVED = auto() + FILTERED = auto() + + class VelocityEstimationMethod(StrEnum): """Velocity estimation strategy options.""" @@ -180,6 +197,11 @@ class PreprocessorConfig: 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 @@ -189,7 +211,7 @@ class PreprocessorConfig: ) filtering_sogifll_config: SogiFllConfig = SogiFllConfig() filtering_second_order_lpf_config: LowPassFilterConfig = LowPassFilterConfig( - cut_off_frequency_rad_per_sec=20.0, damping_ratio=1.0, initial_condition=0.0 + cut_off_frequency_rad_per_sec=90.0, damping_ratio=1.0, initial_condition=0.0 ) @property @@ -237,8 +259,8 @@ def velocity_estimation_strategy(self): # Amplitude modulation SCALE_LEVEL_MODE = 1 -SIGMOID_POWER = 50 -AMPLITUDE_GAIN = -6.5 # Motor position desidered amplitude (rad) +SIGMOID_POWER = 50 # 50 +AMPLITUDE_GAIN = -7 # Motor position desidered amplitude (rad) # Kalman filter definitions PROCESS_NOISE = 2e-2 @@ -321,6 +343,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 +352,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/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/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/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):