ArthurGoldFishKing/pi05_task1_handoff

pi0.5 policy: EBiM Task 1 (cable routing), FR3 Duo mobile

Contents

Path	What
pretrained_model/	The policy: LoRA adapter, config, normalization stats, tokenizer (57 MB). Use this folder as the policy path.
requirements.txt	pip freeze of the training environment
reference/preprocess.py	The exact image resize/pad code used to build the training images: use it for live images
reference/policy_adapter_sim.py	The simulation version of a policy node, for reference only (see Reference code)

What it is

• pi0.5 LoRA fine-tune (rank 32, alpha 32) of lerobot/pi05_base. The base weights are downloaded from Hugging Face on first load.
• Training: 240,000 steps (batch size 2, gradient accumulation 8, which is about 30,000 optimizer updates), AdamW, lr 1e-4 with cosine decay to 1e-5, bfloat16. Final training loss was about 0.05.
• Dataset: the Task 1 real-robot recordings ebim-benchmark/ebim-task1-realrobotdata-lerobot-part1 + -part2 (Hugging Face). That is 50 episodes, 352,655 frames at 20 Hz, task "cable routing".

Requirements

• LeRobot 0.6.2, git commit 30074f7f1358b3c015ae1750017200e86e9c4eb6. Python 3.12, torch 2.11.0, transformers 5.5.4, peft 0.21.0, numpy 2.2.6. See requirements.txt, and choose the torch build that matches your CUDA driver.
• Hugging Face: lerobot/pi05_base (about 14 GB) is downloaded on first load. google/paligemma-3b-pt-224 is gated: accept its license on huggingface.co, then run huggingface-cli login (or set HF_TOKEN).
• GPU: about 9.6 GB of VRAM at peak, so a 12 GB or larger NVIDIA GPU is recommended. On an RTX 4070 Ti SUPER, the first call takes about 0.8 s and later calls about 0.2 s per 50-action chunk.

Inputs

Column names are those of the real-robot dataset (meta/info.json).

observation.state: numpy float32, shape (16,)

Index	Dataset column	Units
0–6	franka_robot_left_measured_joint_states_left_fr3v2_joint1…7	rad
7	franka_robot_left_joint_states_robotiq_85_left_knuckle_joint	rad, 0 = open, about 0.88 = closed
8–14	franka_robot_right_measured_joint_states_right_fr3v2_joint1…7	rad
15	franka_robot_right_joint_states_robotiq_85_left_knuckle_joint	rad, 0 = open, about 0.86 = closed

Images: numpy uint8, (224, 224, 3), HWC, RGB, values 0–255, no batch dimension

Key	Camera	Recorded at
observation.images.head	ZED Mini	672×376
observation.images.wrist_left	Left wrist camera	640×480
observation.images.wrist_right	Right wrist camera	640×480

• Resize like the training data, using this exact code: resize_with_pad() in reference/preprocess.py. It scales each image to fit inside 224×224 while keeping its aspect ratio (head → 224×126, wrists → 224×168), using torch bilinear interpolation with antialiasing, then centers it on a black canvas. Don't reimplement it with OpenCV, PIL or torchvision: their antialiasing differs, so the pixels won't match training. (This code is verified bit-identical to the pipeline that made the training images.)
• Head camera lens correction. The dataset's head images went through zed_equidistant_repair in your MCAP-to-LeRobot conversion. The live head feed must go through the same function, before resizing.
• ⚠️ Color order. ROS camera topics are often bgr8. Convert to RGB before resizing: the model was trained on RGB, and a swap is hard to notice (see Step 0 below).
• Task: the string "cable routing".

prepare_observation_for_inference (used in the example below) adds the batch dimension and converts the arrays to what the model consumes: images become torch float32 (1, 3, 224, 224) in [0, 1] (CHW), and the state becomes (1, 16) float32. Pass it a copy of your observation dict, because it modifies its input in place.

Outputs

A chunk of 50 × 23 float32 actions: absolute targets in the dataset's action columns, spaced 50 ms apart (20 Hz).

Index	Dataset column	Units
0–6	left_gello_joint_states_left_fr3v2_joint1…7	rad (GELLO leader-arm joint angles)
7	left_gripper_target_width_percent_value	0–1, 1 = open
8–14	right_gello_joint_states_right_fr3v2_joint1…7	rad
15	right_gripper_target_width_percent_value	0–1, 1 = open
16–21	swerve_drive_controller_cmd_vel_{linear,angular}_{x,y,z}	m/s, rad/s (only x, y and angular z are used, within ±0.05)
22	spine_target_height_value	m; the dataset range is 0.268–0.385 (the measured spine height, spine_state_joint_states_spine_z, is 0.269–0.347)

• Gripper directions are opposite. In the state, the knuckle angle is 0 when open. In the action, the width fraction is 1 when open.
• Chunk execution. The training config sets n_action_steps = 50 = chunk_size: execute all 50 actions (2.5 s) before using a new chunk. Request the next chunk while the current one is still running, so the robot never stalls on the 0.2–0.8 s inference time. For more reactive closed-loop control, you can execute fewer actions per chunk at inference time (e.g. 10–25, or set cfg.n_action_steps), but this hasn't been tested.
• Latency. A new chunk is computed from an observation taken about 0.2 s earlier, and its action 0 belongs to that moment. When switching to it, skip its first ceil(latency / 0.05) actions (about 4 at 0.2 s) instead of starting from action 0. Otherwise the arm jumps back to where it was when the observation was taken. Measure the latency from observation timestamp to chunk arrival.

Inference example

This was tested against pretrained_model/ in the training environment, with dummy inputs. Run it from this folder.

from types import SimpleNamespace

import numpy as np
import torch
from lerobot.configs.policies import PreTrainedConfig
from lerobot.policies.factory import make_policy, make_pre_post_processors
from lerobot.policies.utils import prepare_observation_for_inference

POLICY_PATH = "pretrained_model"
CAMERAS = ["head", "wrist_left", "wrist_right"]

# 1. Load: make_policy only needs the feature *descriptions*, not the dataset.
cfg = PreTrainedConfig.from_pretrained(POLICY_PATH)
cfg.pretrained_path = POLICY_PATH
features = {
    "observation.state": {"dtype": "float32", "shape": [16], "names": None},
    "action": {"dtype": "float32", "shape": [23], "names": None},
    **{f"observation.images.{c}": {"dtype": "video", "shape": [224, 224, 3],
                                   "names": ["height", "width", "channel"]} for c in CAMERAS},
}
policy = make_policy(cfg, ds_meta=SimpleNamespace(features=features))
policy.eval()
preprocessor, postprocessor = make_pre_post_processors(cfg, pretrained_path=POLICY_PATH)
device = torch.device(cfg.device)

# 2. One observation (replace with live data): state float32 (16,), images uint8 HWC RGB 224x224.
obs = {"observation.state": np.zeros(16, dtype=np.float32)}
for c in CAMERAS:
    obs[f"observation.images.{c}"] = np.zeros((224, 224, 3), dtype=np.uint8)

# 3. Inference: numpy -> batched torch -> normalize/tokenize -> 50-step chunk -> un-normalize.
with torch.inference_mode():
    batch = prepare_observation_for_inference(dict(obs), device, task="cable routing")  # copy: it mutates its input
    batch = preprocessor(batch)
    chunk = policy.predict_action_chunk(batch)       # (1, 50, 23), normalized
    actions = postprocessor(chunk)[0].float().cpu().numpy()  # (50, 23), dataset units
print(actions.shape, actions[0])

Running the policy through GELLO

The idea: replace the GELLO state publisher with a policy node, and leave the follower controller and teleop stack unchanged.

cameras + joint states → policy node → /left/gello/joint_states, /right/gello/joint_states → follower controller → Franka

All topic and joint names below are inferred from the dataset column names and the simulation code. Confirm each one on the robot (ros2 topic list, ros2 topic echo).

Signal	Inferred topic	Type
Arm targets (action 0–6, 8–14)	/left/gello/joint_states, /right/gello/joint_states, joints {left,right}_fr3v2_joint1…7	sensor_msgs/JointState
Gripper targets (action 7, 15)	/left/gripper/gripper_client/target_gripper_width_percent, /right/…	std_msgs/Float32
Base (action 16–21)	/swerve_drive_controller/cmd_vel	geometry_msgs/Twist (or TwistStamped)
Spine (action 22)	a spine target-height topic	unknown

0. Offline check, without the robot. Take a recorded MCAP episode and run it through your own policy-node pipeline: ZED correction, BGR → RGB, resize_with_pad, and state assembly. Then check two things:

   • Actions: compare the first action of each chunk with the recorded action at that frame. For reference, running the policy on the dataset's own frames (4 episodes, 160 chunks, every 50 frames) gave these first-action errors:

     	Arm joints, median / mean / p90	Grippers, mean
     Correct pipeline	0.021 / 0.032 / 0.072 rad	0.019
     Left and right arm states swapped	0.084 / 0.159 / 0.275 rad	0.030
     Gripper state as open fraction instead of knuckle angle	0.027 / 0.094 / 0.105 rad	0.031

     Errors near the first row mean your state assembly matches training. These episodes were part of the training data, so real-robot errors will be somewhat higher.

   • Images: check them separately, because the action comparison barely detects image mistakes. In the same test, BGR images gave a mean of 0.034 rad and stretched (unpadded) images 0.044 rad, close to the correct 0.032. Instead, show your 224×224 output next to resize_with_pad() applied to the dataset frame for the same timestamp. Compare them side by side and per channel: colors, lens distortion and black borders must match. The dataset frame already has the ZED correction applied. Don't rely on an average pixel difference: these scenes are mostly gray, so a color swap changes the mean by only 2–4 gray levels.

1. Shadow mode, with no robot motion from the policy. Teleoperate with GELLO as usual while the policy runs and publishes to a separate topic, such as /policy/left/joint_states.

   How to compare: each call returns a 2.5 s chunk, so compare the first action of each chunk with the /left/gello/joint_states reading at the moment the observation was taken. They should be close.

   If left joints 1, 2 or 7 are clearly mirrored, stop. Signs of that: they move in the opposite direction, and they're off by about the offset (policy value ≈ offset − topic value, with offsets of about −2.15, −0.27 and +0.37 rad). It means the action isn't in that topic's convention. See the left-arm mirror note below.

2. Make the policy the only publisher. Don't start franka_gello_state_publisher (or unplug GELLO), so nothing else publishes on the GELLO topics.

3. Publish like GELLO. Send sensor_msgs/JointState with the same joint names as the GELLO publisher, at 20 Hz, plus the two gripper-width topics. Keep base and spine off at first, with the robot parked in front of the table.

4. Avoid a jump on the first action. Start with the arms near the training start pose, and clamp the change per step. The median first frame of all 50 episodes, as measured joints in rad:

   • left: [-1.07, -0.13, 1.04, -2.76, 1.18, 1.93, 0.17]
   • right: [0.80, -0.38, -0.80, -2.79, -1.18, 1.76, -0.17]
   • grippers open

   In the data, 99% of per-step changes are under 0.026 rad (99.9% under 0.06 rad), so a starting clamp of about 0.01 rad per step is conservative.

5. Low speed, someone at the e-stop. Relax the limits gradually.

Reference code

reference/policy_adapter_sim.py is the simulation version of the policy node, copied unchanged from the benchmark repo, for reference only. It expects the Isaac Sim bridge, and it imports COMPACT_STATE_NAMES, fit_resolution and pad_to_size from the repo's baseline/preprocess.py. Those functions are included here as reference/preprocess.py; point its sys.path line at reference/. It:

• uses the sim topics (/isaac/* for state and cameras, /bridge/* for commands) and the sim gripper joint name (left_robotiq_opening);
• sends action[0:7] / [8:15] directly as joint targets;
• drops the base and spine actions (action[16:23]).

To adapt it for the real robot:

• Use the real topic names for state, cameras and commands.
• Publish arm actions to the GELLO joint-state topics and gripper actions to the gripper-width topics, not to the joint controller.
• Apply the ZED zed_equidistant_repair to the head image.
• Convert BGR → RGB where needed, then resize with reference/preprocess.py's resize_with_pad.
• Skip the first ceil(latency / 0.05) actions of each new chunk.
• Use the real gripper joint (robotiq_85_left_knuckle_joint, in rad) for the state.
• Look up joints by name, not by message index (msg.position[:7], position[0]).
• Add dry-run mode, per-step clamping, a stale-observation timeout and a NaN check.

Safety notes

• Publish each action dimension to the same topic its dataset column was recorded from.
• Left-arm mirror, cause unconfirmed. In the dataset, left-arm joints 1, 2 and 7 follow action ≈ offset − measured, with offsets of about −2.15, −0.27 and +0.37 rad. The offsets drift from session to session (j7 ranges over about 0.55 rad across episodes). The right arm and left joints 3–6 map 1:1. Shadow mode (step 1 above) is designed to catch this.
• Only tested in simulation. The policy has never run in closed loop on a real robot.
• Start with a dry run (log actions without sending them), then low speed, with someone at the e-stop.