control/
Thrust allocation, station-keeping, depth/heading control, mode-switch hand-over to upper-body manipulation, and dock-A subsurface approach controller for Free Humanoid Submersible.
This directory will hold:
- Thrust allocation. Pseudo-inverse + quadratic-programming thrust allocator that maps a desired body-frame wrench (force + torque, 6 DoF) to per-thruster thrust commands. Handles thruster saturation, redundancy resolution (the 8-thruster layout is over-actuated for the 6-DoF problem), and graceful degradation under thruster failure. Cycle: ~50 Hz.
- Station-keeping MPC. Horizon ~2 s, cycle ~50 Hz. State: 6-DoF pose (depth + heading + horizontal position from USBL fix) + 6-DoF velocity (DVL bottom-track + IMU integration). Control: per-thruster thrust commands (allocated through the thrust allocator). Disturbance model: depth-varying current. Constraints: depth limits, thruster saturation, thrust-allocation feasibility.
- Depth / heading control inner loop. Per-axis PID at ~200 Hz on the depth and heading channels, beneath the station-keeping MPC. Handles fast-loop disturbance rejection.
- Per-thruster ESC PID. ~500 Hz per-thruster speed PID running on the thruster ESC firmware. The control directory documents the desired speed-vs-thrust map; the firmware implements the inner loop.
- Mode-switch hand-over. State machine that coordinates thrust-allocation reconfiguration with upper-body manipulation hand-over. When entering
manipulation_static_hover from station_keeping_hover, the thrust allocator is reconfigured to compensate for the reaction forces from arm motion (the platform pushes water as it moves an arm); the whole-body MPC integrates upper-body motion with thrust allocation. When exiting, the inverse handover.
- Dock approach controller. Fine-positioning relative to the dock-A subsurface optical/acoustic fiducial. Multi-modal sensor fusion: USBL coarse-position, multibeam-imaging-sonar mid-range, downward camera array close-range visual-servo, head stereo for fine alignment. Targets ±50 mm positional accuracy at the rake-bar / intake-screen / hull-mating-fixture work points (looser than the centaur’s ±20 mm because of underwater visibility limits and station-keeping floor).
- Manipulation policy. Inherited from the bipedal sibling — imitation-learning baseline (ACT or diffusion-policy variant), retrained with simulated-fluid-disturbance data for the submersible operating envelope. Genesis MPM/fluid simulation is the primary training-data generator; pool-trial real-robot data is the ground-truth validation set (Phase 3+).
- Whole-body MPC for upper-body manipulation while in
manipulation_static_hover. Inherits the bipedal sibling’s whole-body MPC; integrates the platform’s translational and rotational disturbance from arm motion as a constraint on the thrust allocator.
Cross-references
Status
v0.0 scaffold. The thrust allocator + station-keeping MPC + manipulation hand-over state machine is the Phase 1 deliverable, closed-loop in Genesis-fluid simulation against simulated current disturbances. The dock-A subsurface approach controller is a Phase 2 deliverable. The whole-body MPC integration with the thrust allocator is a Phase 3 deliverable.