seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

feat(controls): adaptive PID balance controller with gain scheduling (Issue #136) #149

Merged
sl-jetson merged 1 commits from sl-controls/issue-136-adaptive-pid into main 2026-03-02 09:51:20 -05:00
Conversation 0 Commits 1 Files Changed 11 +1809
sl-controls commented 2026-03-02 09:39:11 -05:00
Collaborator
Copy Link

Summary

  • pid_controller.py — GainSet (clamped kp/ki/kd), PIDController (anti-windup, D-on-error, output clamp), GainScheduler (empty/light/heavy weight table, exponential blending at gain_blend_alpha per tick, safety bounds, manual override, instant revert-to-defaults), InstabilityDetector (dual criteria: tilt threshold + oscillation sign-reversal count)
  • weight_estimator.py — WeightEstimator (rolling-window current→weight, steady-state gated by |tilt|, change detection), CalibrationRoutine (IDLE→ROCKING→SETTLING→DONE/FAILED; sinusoidal rocking output, settling current sampling, weight from avg current)
  • adaptive_pid_node.py — 100 Hz ROS2 node; /saltybot/imu + /saltybot/motor_current → /saltybot/balance_effort + /saltybot/weight_estimate + /saltybot/pid_state (JSON); /saltybot/calibrate_balance Trigger service; IMU watchdog; override_enabled dynamic reconfigure; instability → revert + PID reset + WARN log
  • config/adaptive_pid_params.yaml, launch/adaptive_pid.launch.py, package.xml, setup.py/cfg
  • test/test_adaptive_pid.py — 68/68 unit tests passing

How to use

# Launch
ros2 launch saltybot_adaptive_pid adaptive_pid.launch.py

# Trigger calibration
ros2 service call /saltybot/calibrate_balance std_srvs/srv/Trigger

# Manual gain override (dynamic reconfigure)
ros2 param set /adaptive_pid override_enabled true
ros2 param set /adaptive_pid override_kp 12.0
ros2 param set /adaptive_pid override_enabled false  # back to auto

# Monitor
ros2 topic echo /saltybot/pid_state

Test plan

  • 68 unit tests pass (pure Python, no ROS2)
  • Verify gain transitions logged on weight class change
  • Trigger calibration service, confirm DONE + weight estimate in logs
  • Inject large tilt oscillations, confirm WARN + revert logged
  • Confirm override_enabled bypasses scheduling
  • Verify /saltybot/pid_state JSON fields on topic echo

Closes #136

🤖 Generated with Claude Code

## Summary - **pid_controller.py** — `GainSet` (clamped kp/ki/kd), `PIDController` (anti-windup, D-on-error, output clamp), `GainScheduler` (empty/light/heavy weight table, exponential blending at `gain_blend_alpha` per tick, safety bounds, manual override, instant revert-to-defaults), `InstabilityDetector` (dual criteria: tilt threshold + oscillation sign-reversal count) - **weight_estimator.py** — `WeightEstimator` (rolling-window current→weight, steady-state gated by |tilt|, change detection), `CalibrationRoutine` (IDLE→ROCKING→SETTLING→DONE/FAILED; sinusoidal rocking output, settling current sampling, weight from avg current) - **adaptive_pid_node.py** — 100 Hz ROS2 node; `/saltybot/imu` + `/saltybot/motor_current` → `/saltybot/balance_effort` + `/saltybot/weight_estimate` + `/saltybot/pid_state` (JSON); `/saltybot/calibrate_balance` Trigger service; IMU watchdog; `override_enabled` dynamic reconfigure; instability → revert + PID reset + WARN log - **config/adaptive_pid_params.yaml**, **launch/adaptive_pid.launch.py**, **package.xml**, **setup.py/cfg** - **test/test_adaptive_pid.py** — 68/68 unit tests passing ## How to use ```bash # Launch ros2 launch saltybot_adaptive_pid adaptive_pid.launch.py # Trigger calibration ros2 service call /saltybot/calibrate_balance std_srvs/srv/Trigger # Manual gain override (dynamic reconfigure) ros2 param set /adaptive_pid override_enabled true ros2 param set /adaptive_pid override_kp 12.0 ros2 param set /adaptive_pid override_enabled false # back to auto # Monitor ros2 topic echo /saltybot/pid_state ``` ## Test plan - [x] 68 unit tests pass (pure Python, no ROS2) - [ ] Verify gain transitions logged on weight class change - [ ] Trigger calibration service, confirm DONE + weight estimate in logs - [ ] Inject large tilt oscillations, confirm WARN + revert logged - [ ] Confirm override_enabled bypasses scheduling - [ ] Verify /saltybot/pid_state JSON fields on topic echo Closes #136 🤖 Generated with [Claude Code](https://claude.com/claude-code)
sl-controls added 1 commit 2026-03-02 09:39:11 -05:00 
Pure modules (no ROS2 dep, fully unit-tested):
- pid_controller.py:
    GainSet — (kp,ki,kd) with safety clamp helper
    PIDController — anti-windup integral, D-on-error, output clamping
    GainScheduler — 3-class weight table (empty/light/heavy), exponential
      gain blending (alpha per tick), safety bounds clamping, manual
      override, immediate revert-to-defaults on instability
    InstabilityDetector — dual criteria: tilt threshold (>50% of window)
      + sign-reversal count (oscillation)

- weight_estimator.py:
    WeightEstimator — rolling-window current→weight, steady-state gating
      (|tilt|≤threshold), change detection (threshold_kg)
    CalibrationRoutine — IDLE→ROCKING→SETTLING→DONE/FAILED state machine;
      sinusoidal rocking output, settling current sampling, weight estimate
      from avg current; abort() / restart supported

- adaptive_pid_node.py: 100 Hz ROS2 node
    Sub: /saltybot/imu (Imu, pitch from quaternion), /saltybot/motor_current
    Pub: /saltybot/balance_effort (Float32), /saltybot/weight_estimate,
         /saltybot/pid_state (JSON: gains, class, weight_kg, flags)
    Srv: /saltybot/calibrate_balance (std_srvs/Trigger)
    IMU watchdog (0.5 s), dynamic reconfigure via override_enabled param,
    instability → revert + PID reset, structured INFO/WARN logging

- config/adaptive_pid_params.yaml, launch/adaptive_pid.launch.py,
  package.xml, setup.py, setup.cfg
- test/test_adaptive_pid.py: 68/68 unit tests passing

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson merged commit 732045a086 into main 2026-03-02 09:51:20 -05:00
Sign in to join this conversation.
Reviewers
No Reviewers
Labels
No items
No Label
Milestone
No items
No Milestone
Projects
Clear projects
No project
Assignees
Clear assignees
shaytac sl-controls sl-firmware sl-jetson sl-mechanical sl-perception sl-uwb sl-webui seb
No Assignees
1 Participants
Notifications
Due Date
No due date set.
Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#149
No description provided.
Delete Branch "sl-controls/issue-136-adaptive-pid"

Deleting a branch is permanent. Although the deleted branch may continue to exist for a short time before it actually gets removed, it CANNOT be undone in most cases. Continue?