step.motion.auto_tune ===================== .. py:module:: step.motion.auto_tune .. autoapi-nested-parse:: Auto-tune PID controllers via system identification and iterative optimization. Three-phase sequential pipeline: Phase 1 - Drive characterization: Measure physical limits (max velocity, acceleration, deceleration) per axis by commanding raw velocities and observing response. Uses the existing CharacterizeDrive step. Results are applied in-memory and persisted so that subsequent phases have accurate constraints. Phase 2 - Velocity controller tuning: Step-response identification using inflection tangent method (ported from Torsten Brischalle's ControlTheory, MIT license). Extracts plant parameters (gain Ks, dead time Tu, time constant Tg), then computes PID gains via CHR set-point-follow formulas (kp, ki, kd). Validates by comparing ISE before/after. Phase 3 - Motion controller tuning: Coordinate descent (Hooke-Jeeves) optimizer that runs real test motions, scores settling time + overshoot + final error, and iteratively adjusts distance.kp/kd and heading.kp/kd. Step classes: AutoTune - full pipeline (characterize, velocity, motion) AutoTuneVelocity - phase 2 only AutoTuneMotion - phase 3 only Classes ------- .. autoapisummary:: step.motion.auto_tune.PlantParams step.motion.auto_tune.StepResponseData step.motion.auto_tune.VelocityTuneResult step.motion.auto_tune.MotionTuneResult step.motion.auto_tune.AutoTuneVelocity step.motion.auto_tune.AutoTuneMotion step.motion.auto_tune.AutoTune Module Contents --------------- .. py:class:: PlantParams Identified plant parameters from step response. .. py:attribute:: Ks :type: float :value: 0.0 .. py:attribute:: Tu :type: float :value: 0.0 .. py:attribute:: Tg :type: float :value: 0.0 .. py:attribute:: method :type: str :value: '' .. py:class:: StepResponseData Raw step response recording. .. py:attribute:: times :type: list[float] :value: [] .. py:attribute:: commanded :type: list[float] :value: [] .. py:attribute:: measured :type: list[float] :value: [] .. py:class:: VelocityTuneResult Result of velocity controller tuning for one axis. .. py:attribute:: axis :type: str :value: '' .. py:attribute:: plant :type: PlantParams .. py:attribute:: pid :type: raccoon.foundation.PidGains .. py:attribute:: ff :type: raccoon.foundation.Feedforward .. py:attribute:: baseline_ise :type: float :value: 0.0 .. py:attribute:: tuned_ise :type: float :value: 0.0 .. py:attribute:: accepted :type: bool :value: False .. py:class:: MotionTuneResult Result of motion controller tuning for one parameter set. .. py:attribute:: param_name :type: str :value: '' .. py:attribute:: initial_kp :type: float :value: 0.0 .. py:attribute:: initial_kd :type: float :value: 0.0 .. py:attribute:: final_kp :type: float :value: 0.0 .. py:attribute:: final_kd :type: float :value: 0.0 .. py:attribute:: initial_score :type: float :value: 0.0 .. py:attribute:: final_score :type: float :value: 0.0 .. py:attribute:: iterations :type: int :value: 0 .. py:class:: AutoTuneVelocity(axes: list[str] = None, persist: bool = True, csv_dir: Optional[str] = '/tmp/auto_tune') Bases: :py:obj:`step.Step` Tune velocity controllers via step-response system identification. This is Phase 2 of the full auto-tune pipeline, usable standalone when drive characterization has already been performed (or when the hardware limits are already configured in ``raccoon.project.yml``). For each velocity axis the process is: 1. **Baseline recording** -- command a step velocity at 50% of the characterized max and record the response at 100 Hz. 2. **Plant identification** -- extract gain (Ks), dead time (Tu), and time constant (Tg) using the inflection tangent method (local quadratic regression to find the inflection point, then tangent-line intersections). Falls back to 10%/63% rise-time estimation if the inflection method fails. 3. **Gain computation** -- compute PID gains via CHR set-point-follow formulas, scaled down because a kV=1.0 feedforward handles steady-state. 4. **Validation** -- apply the new gains and re-run the step response. If the integral of squared error (ISE) improves, the gains are accepted; otherwise the baseline gains are restored. Accepted gains are applied in-memory to the drive's velocity control config and optionally persisted to ``raccoon.project.yml`` under ``robot.drive.vel_config``. Prerequisites: Drive characterization should have been run first so that max velocity values are available for computing the step command magnitude. :param axes: Velocity axes to tune. Each entry is a velocity component name: ``"vx"`` (forward), ``"vy"`` (lateral/strafe), or ``"wz"`` (angular). Default ``["vx", "vy", "wz"]``. :param persist: If ``True``, write accepted gains to ``raccoon.project.yml``. Default ``True``. :param csv_dir: Directory for step-response CSV files (baseline and tuned recordings per axis). Default ``"/tmp/auto_tune"``. Example:: from raccoon.step.motion import auto_tune_velocity # Tune both velocity axes with defaults auto_tune_velocity() # Tune only the forward axis, save CSVs for plotting auto_tune_velocity( axes=["vx"], csv_dir="/tmp/vel_tune_plots", ) .. py:attribute:: axes :value: None .. py:attribute:: persist :value: True .. py:attribute:: csv_dir :value: '/tmp/auto_tune' .. py:attribute:: results :type: dict[str, VelocityTuneResult] .. py:class:: AutoTuneMotion(axes: list[str] = None, persist: bool = True, csv_dir: Optional[str] = '/tmp/auto_tune') Bases: :py:obj:`step.Step` Tune motion PID controllers via iterative real-world optimization. This is Phase 3 of the full auto-tune pipeline, usable standalone when velocity controllers are already tuned and drive limits are characterized. Uses Hooke-Jeeves coordinate descent to optimize the high-level distance and/or heading PID controllers (kp and kd). The process for each parameter set is: 1. **Initial evaluation** -- run a test motion (0.5 m drive for distance, 90-degree turn for heading) with the current gains and compute a weighted score from settling time, overshoot, and final error. 2. **Coordinate descent** -- try perturbing kp up/down by a delta, keep the direction that improves the score. Then do the same for kd. Repeat for up to 10 iterations. When neither direction improves, halve the deltas. Stop when deltas fall below a minimum threshold. 3. **Constraint-aware scoring** -- the optimizer prioritizes fast completion but heavily penalizes candidates that exceed soft limits on overshoot (10 mm / 3 degrees) or final error (10 mm / 2 degrees), preventing "fast but sloppy" solutions. 4. **Secondary speed checks** -- after optimization at full speed, the final gains are tested at lower speeds (50%, 30%) for monitoring purposes (these do not affect the optimization). Trials alternate between forward/backward (or CW/CCW) to reduce directional bias. Prerequisites: Velocity controllers should be tuned first (Phase 2) and drive limits characterized (Phase 1) for best results. The robot needs enough space for 0.5 m drives and 90-degree turns. :param axes: Motion parameters to optimize. Options are ``"distance"`` (forward drive kp/kd), ``"lateral"`` (strafe kp/kd — shares the distance PID but optimizes with lateral trials), and ``"heading"`` (turn kp/kd). Default ``["distance", "lateral", "heading"]``. :param persist: If ``True``, write final gains to ``raccoon.project.yml`` under ``robot.motion_pid``. Default ``True``. :param csv_dir: Directory for diagnostic CSV output. Default ``"/tmp/auto_tune"``. Example:: from raccoon.step.motion import auto_tune_motion # Tune both distance and heading controllers auto_tune_motion() # Tune only the heading controller auto_tune_motion(axes=["heading"]) # Tune without persisting (for experimentation) auto_tune_motion(persist=False) .. py:attribute:: axes :value: None .. py:attribute:: persist :value: True .. py:attribute:: csv_dir :value: '/tmp/auto_tune' .. py:attribute:: results :type: dict[str, MotionTuneResult] .. py:class:: AutoTune(vel_axes: list[str] = None, characterize_axes: list[str] = None, motion_axes: list[str] = None, tune_characterize: bool = True, tune_velocity: bool = True, tune_motion: bool = True, characterize_trials: int = 3, characterize_power_percent: int = 100, persist: bool = True, csv_dir: Optional[str] = '/tmp/auto_tune') Bases: :py:obj:`step.Step` Auto-tune the full drive system: characterize, velocity PID, motion PID. Runs a three-phase sequential pipeline that takes the robot from unknown hardware limits to fully tuned PID controllers. Each phase builds on the results of the previous one: **Phase 1 -- Drive characterization.** Commands raw velocities to measure max velocity, acceleration, and deceleration for each axis. Multiple trials are run and the median is taken. Results are stored so that subsequent phases have accurate physical constraints for profile generation. **Phase 2 -- Velocity controller tuning.** For each velocity axis (e.g., ``vx``, ``vy``, ``wz``), a step-response is recorded at 100 Hz, plant parameters (gain Ks, dead time Tu, time constant Tg) are identified via the inflection tangent method, and PID gains are computed using CHR set-point-follow formulas. A validation step-response is run with the new gains; if the integral of squared error (ISE) improves, the gains are accepted, otherwise the baseline is kept. **Phase 3 -- Motion controller tuning.** Uses Hooke-Jeeves coordinate descent to optimize the high-level distance, lateral, and heading PID controllers. Real test drives, strafes, and turns are executed, scored on a weighted combination of settling time, overshoot, and final error. The optimizer adjusts kp/kd iteratively, halving the search delta when no improvement is found. All results are applied in-memory immediately and, if ``persist`` is enabled, written to ``raccoon.project.yml`` so they survive restarts. This step requires significant clear space and takes several minutes to complete. It is intended for initial robot setup, not competition runs. :param vel_axes: Velocity axes to tune in Phase 2. Each entry is a velocity component name (``"vx"`` for forward, ``"vy"`` for lateral/strafe, ``"wz"`` for angular). Default ``["vx", "vy", "wz"]``. :param characterize_axes: Axes to characterize in Phase 1. Options are ``"forward"``, ``"lateral"``, ``"angular"``. Default ``["forward", "lateral", "angular"]``. :param motion_axes: Motion parameters to optimize in Phase 3. Options are ``"distance"``, ``"lateral"`` (shares the distance PID but optimizes with lateral trials), and ``"heading"``. Default ``["distance", "lateral", "heading"]``. :param tune_characterize: Whether to run Phase 1. Set to ``False`` if the robot's limits are already known. Default ``True``. :param tune_velocity: Whether to run Phase 2. Default ``True``. :param tune_motion: Whether to run Phase 3. Default ``True``. :param characterize_trials: Number of trials per axis in Phase 1. More trials improve robustness but take longer. Default 3. :param characterize_power_percent: Motor power percentage (1--100) for Phase 1 drive characterization. Default 100. :param persist: If ``True``, write all results to ``raccoon.project.yml``. Default ``True``. :param csv_dir: Directory for diagnostic CSV output (step-response recordings, etc.). Default ``"/tmp/auto_tune"``. Example:: from raccoon.step.motion import auto_tune # Full auto-tune with defaults auto_tune() # Skip characterization (already done), tune only velocity + motion auto_tune(tune_characterize=False) # Tune only the forward velocity axis and distance controller auto_tune( vel_axes=["vx"], motion_axes=["distance"], characterize_axes=["forward"], ) # Dry run without persisting to YAML auto_tune(persist=False, csv_dir="/tmp/auto_tune_test") .. py:attribute:: characterize_axes :value: None .. py:attribute:: vel_axes :value: None .. py:attribute:: motion_axes :value: None .. py:attribute:: tune_characterize :value: True .. py:attribute:: tune_velocity :value: True .. py:attribute:: tune_motion :value: True .. py:attribute:: characterize_trials :value: 3 .. py:attribute:: characterize_power_percent :value: 100 .. py:attribute:: persist :value: True .. py:attribute:: csv_dir :value: '/tmp/auto_tune'