# FireAnt / M20 SDK Deploy

This ROS 2 project runs the 8-DoF FireAnt Stage-2 policy in MuJoCo while retaining the original
16-DoF M20 path. FireAnt is the default.

## FireAnt alignment

| Item | Deployment value |
|---|---|
| Robot/DDS joint order | FL knee/wheel, FR knee/wheel, HL knee/wheel, HR knee/wheel |
| Policy joint order | FL/FR/HL/HR knees, then FL/FR/HL/HR wheels |
| Actor | 63 observations, 8 actions, 50 Hz |
| Low-level loop | 200 Hz; MuJoCo step 1 kHz |
| Knee control | position action scale 0.25, `kp=80`, `kd=2`, limit 75 Nm |
| Wheel control | velocity action scale 5.0, `kp=0`, `kd=0.6`, limit 28.68 Nm |
| Wheel | radius 0.096 m, width 0.083 m, mass 1.34 kg |
| Knee module | mass 0.612 kg |
| Base | mass 15.882 kg |

The actor observation is reconstructed in the exact training order: angular velocity, projected
gravity, command, relative joint position, joint velocity, last action, wheel/support contact
states, wheel/support force magnitudes, wheel-slip proxy and applied torque. The simulator sends
base velocity and contact data through the unused entries 8-15 of the fixed 16-slot DDS joint
message. This extension is for sim2sim; sim2real requires equivalent estimators/sensors.

The MuJoCo wheel geoms have their own friction values. Contact restitution is effectively zero.
`--randomize` samples wheel friction, masses, inertias, base COM and control gains once from the
training-time ranges.

## Policy export

The repository already contains `policy/fire_ant_policy.onnx`, exported from:

```text
rl_training/logs/rsl_rl/deeprobotics_m20_support/
2026-09-12_17-52-15_stage2_multiterrain/model_39899.pt
```

Re-export after selecting a different checkpoint:

```bash
cd /home/ycpy/huangweifeng/hwf_workspace/rl_training_huoyi/M20_sdk_deploy
python3 scripts/export_fire_ant_policy.py \
  --checkpoint /absolute/path/to/model_xxx.pt \
  --output policy/fire_ant_policy.onnx
```

The exporter checks the ONNX graph and compares its output against PyTorch.

## Build

Install the simulator dependency in the Python environment that has ROS 2 `rclpy` and `drdds`:

```bash
python3 -m pip install "numpy<2" mujoco
```

Then build in a ROS 2 Humble workspace. The `drdds` package must be built or sourced first.

```bash
cd /home/ycpy/huangweifeng/hwf_workspace/rl_training_huoyi
source /opt/ros/humble/setup.bash
source /path/to/the/workspace/containing/drdds/install/setup.bash
colcon build --packages-select m20_sdk_deploy --cmake-args -DBUILD_PLATFORM=x86
source install/setup.bash
```

## FireAnt sim2sim

Use the same ROS domain in both terminals. Start MuJoCo first so the controller immediately receives
valid joint and IMU data.

Terminal 1 — deterministic flat-ground sanity check:

```bash
export ROS_DOMAIN_ID=1
source /opt/ros/humble/setup.bash
source /path/to/the/workspace/containing/drdds/install/setup.bash
python3 /home/ycpy/huangweifeng/hwf_workspace/rl_training_huoyi/M20_sdk_deploy/interface/robot/simulation/mujoco_simulation_ros2.py \
  --robot fire_ant --terrain flat
```

Terminal 2 — controller and Stage-2 policy:

```bash
export ROS_DOMAIN_ID=1
source /opt/ros/humble/setup.bash
source /path/to/the/workspace/containing/drdds/install/setup.bash
source /home/ycpy/huangweifeng/hwf_workspace/rl_training_huoyi/install/setup.bash
ros2 run m20_sdk_deploy rl_deploy
```

Keyboard sequence:

1. Press `z` and wait about 1.5 seconds for the default posture.
2. Press `c` to enter RL control.
3. Hold `w` to move forward; `s` requests backward motion and `q/e` turn.
4. Press `x` to lower the robot or `r` for joint damping.

Stage-2 was trained with zero lateral command, so `a/d` are intentionally disabled for FireAnt.
The policy's Stage-2 forward command range is 0.25-1.2 m/s; the keyboard uses 1.2 m/s while held.
With no movement key held, a deployment deadman override holds the default knee pose and commands
zero wheel speed, avoiding the zero-command creep caused by Stage-2's positive-only command sampling.

After the flat test, run the mixed 8/12/16 cm obstacle course:

```bash
python3 /home/ycpy/huangweifeng/hwf_workspace/rl_training_huoyi/M20_sdk_deploy/interface/robot/simulation/mujoco_simulation_ros2.py \
  --robot fire_ant --terrain rough
```

Robustness check with one reproducible domain-randomization sample:

```bash
python3 /home/ycpy/huangweifeng/hwf_workspace/rl_training_huoyi/M20_sdk_deploy/interface/robot/simulation/mujoco_simulation_ros2.py \
  --robot fire_ant --terrain rough --randomize --seed 1
```

Add `--headless` for unattended runs and `--realtime-factor 0` to run as fast as possible.

## Original M20 compatibility

The old model, interface and 57-to-16 ONNX policy remain available:

```bash
# Simulator
python3 interface/robot/simulation/mujoco_simulation_ros2.py --robot m20 --terrain rough

# Controller
ros2 run m20_sdk_deploy rl_deploy --robot=m20
```