Upload folder using huggingface_hub

.gitattributes

@@ -33,3 +33,4 @@ saved_model/**/* filter=lfs diff=lfs merge=lfs -text
 *.zip filter=lfs diff=lfs merge=lfs -text
 *.zst filter=lfs diff=lfs merge=lfs -text
 *tfevents* filter=lfs diff=lfs merge=lfs -text
+assets/fk_validation.png filter=lfs diff=lfs merge=lfs -text

HILSERL_RUNBOOK.md

@@ -0,0 +1,183 @@
+# HIL-SERL Online Training Runbook — "put right banana in the pot" (UR7e)
+
+Everything offline is already built and frozen (see `HILSERL_PREP_RESULTS.md`). This runbook is
+the exact sequence to start **online** HIL-SERL actor–learner training once the **UR7e** is
+physically connected. All robot-only steps are marked **[ROBOT]**.
+
+- **Shared train config (learner + actor):**
+  `hilserl/config/train_hilserl_ur7e.json`
+- **Offline demo buffer:** `theo/banana_in_pot_rl` at `hilserl/banana_rl_lerobot` (51 ep / 21 524 frames)
+- **Reward classifier:** `hilserl/reward_classifier/checkpoint` (`success_threshold=0.7`)
+- **UR7e URDF (pre-generated):** `hilserl/ur7e.urdf` (IK frame `tool0`)
+- **venv:** `lr_env/bin/python`  ·  env vars: `HF_HUB_OFFLINE=1`, `TORCH_HOME=scratchpad/torch_home`,
+  `HF_LEROBOT_HOME=<root>/hilserl`
+
+---
+
+## 0. Robot-PC installs (one-time) **[ROBOT]**
+
+The offline env (`lr_env`) is missing the online/transport/hardware deps on purpose. On the machine
+that will talk to the arm and run the actor+learner, install:
+
+```bash
+# gRPC transport (actor<->learner). NOTE: the offline uv cache only has a cp310 wheel; you
+# need a wheel matching lr_env's Python (3.12). Requires network.
+uv pip install --python lr_env/bin/python 'lerobot[grpcio-dep]'   # grpcio
+
+# HIL-SERL extra: gym-hil + placo (IK) + transformers, etc.
+uv pip install --python lr_env/bin/python 'lerobot[hilserl]'      # pulls placo, gym-hil
+
+# placo (IK solver, required by lerobot/model/kinematics.py:59). If not pulled above:
+uv pip install --python lr_env/bin/python placo
+
+# ur_rtde — UR robot I/O. NOT installed anywhere yet; required for the UR robot driver.
+uv pip install --python lr_env/bin/python ur_rtde
+```
+
+Verify: `lr_env/bin/python -c "import grpc, placo, rtde_control; print('ok')"`.
+
+> Why these are not already installed: they are hardware/transport-only. The entire offline prep
+> (buffer, classifier, EE conversion, config, dry-run) was completed without them. The learner
+> module import fails fast at `lerobot/transport/__init__.py:27` (`require_package("grpcio")`) until
+> grpcio is present — this is the only thing blocking a full end-to-end module launch offline.
+
+---
+
+## 1. (Already done, re-verify) UR7e URDF
+
+`hilserl/ur7e.urdf` is already generated and referenced by the config
+(`env.processor.inverse_kinematics.urdf_path`). To regenerate:
+
+```bash
+source /opt/ros/jazzy/setup.bash
+xacro /opt/ros/jazzy/share/ur_description/urdf/ur.urdf.xacro \
+      ur_type:=ur7e name:=ur7e > hilserl/ur7e.urdf
+```
+
+IK target frame = `tool0` (offset-free flange; Agent D validated recorded TCP == flange). If you
+mount a gripper whose TCP differs, add a `gripper_frame_link` to the URDF and set
+`env.processor.inverse_kinematics.target_frame_name` accordingly.
+
+---
+
+## 2. Fill the robot + teleop blocks in the config **[ROBOT]**
+
+In `hilserl/config/train_hilserl_ur7e.json`, `env.robot` and `env.teleop` are currently `null`
+(robot-free dry-run). Set them for the live arm, e.g.:
+
+```jsonc
+"robot":  { "type": "<ur_follower_type>", "ip": "<UR7e_IP>", /* motors, cameras cam1/cam2 */ },
+"teleop": { "type": "gamepad", "use_gripper": true }
+```
+
+`motor_names = list(env.robot.bus.motors.keys())` feeds the kinematics solver
+(gym_manipulator.py:414-420), so the robot config's motor order must match the 6 UR joints + gripper.
+
+---
+
+## 3. Tune the workspace-specific values against the real arm **[ROBOT]**
+
+These are **TODO placeholders** in the config — draft values are in there, but they MUST be verified
+with the arm before letting the policy move:
+
+| Field | Config path | Current placeholder | How to tune |
+|---|---|---|---|
+| EE safety bounds | `env.processor.inverse_kinematics.end_effector_bounds` | `min[-0.6,-0.6,0.0] max[0.6,0.6,0.6]` (m, base frame) | Jog the arm to the reachable corners of the banana/pot workspace; set a tight box that contains the task but clamps runaways. |
+| Reset pose | `env.processor.reset.fixed_reset_joint_positions` | `[3.05,-1.60,1.90,-1.85,-1.55,-3.30,0.02]` (rad, from dataset joint-range mid) | Set to a safe, repeatable pre-grasp home; verify the arm returns there each episode. |
+| EE step sizes | `env.processor.inverse_kinematics.end_effector_step_sizes` | `{x:0.05,y:0.05,z:0.05}` (m) | **Keep at 0.05** — the offline action = TCP-delta ÷ 0.05, so changing this desyncs the demo actions. |
+| Episode length | `env.processor.reset.control_time_s` | `20.0` s (→ `max_episode_steps = 20*30 = 600`) | Match a comfortable single-attempt duration. |
+
+The reward classifier decision boundary around the release moment is uncalibrated (Agent B caveat);
+`success_threshold=0.7` adds margin. Consider recording a few real **failure/near-miss** episodes
+early to harden it.
+
+---
+
+## 4. (Optional) Crop ROI — only if you crop online **[ROBOT]**
+
+The offline buffer images are **full-frame resized to 128×128, no crop**
+(`image_preprocessing.resize_size=[128,128]`, `crop_params_dict=null`). If you decide to crop online
+to focus on the workspace, you MUST keep offline and online identical:
+
+```bash
+# 1) find the ROI interactively on a recorded dataset
+lr_env/bin/python -m lerobot.rl.crop_dataset_roi --repo-id theo/banana_in_pot_rl
+# 2) put the returned crop_params_dict into env.processor.image_preprocessing.crop_params_dict
+# 3) RE-RUN the crop on the OFFLINE buffer too (hilserl/banana_rl_lerobot) so the demo images
+#    match the online cropped+resized size — otherwise the encoder sees two different distributions.
+```
+
+If you do not crop, skip this entirely (the default is consistent already).
+
+---
+
+## 5. Start the LEARNER (terminal 1) **[ROBOT]**
+
+```bash
+export HF_HUB_OFFLINE=1
+export TORCH_HOME=<root>/scratchpad/torch_home
+export HF_LEROBOT_HOME=<root>/hilserl
+lr_env/bin/python -m lerobot.rl.learner \
+    --config_path hilserl/config/train_hilserl_ur7e.json
+```
+
+The learner: builds the SAC policy + critics, loads the offline demo buffer via
+`ReplayBuffer.from_lerobot_dataset`, opens a gRPC server on `127.0.0.1:50051`, then **idles at the
+online-buffer gate** (`learner.py:412-413`) until the actor sends transitions. This idle state was
+validated offline (see §7 / `HILSERL_PREP_RESULTS.md`).
+
+> **RAM WARNING — full offline buffer ≈ 25 GB host RAM.**
+> `from_lerobot_dataset` eagerly materializes **every** transition (decoded float32 state +
+> next-state images) into a Python list before filling storage; the full 21 524-frame ×
+> 2×(3×128×128) set peaks at **~25 GB** and was OOM-killed on the 31 GB box (Agent C). Storage
+> itself at `offline_buffer_capacity=25000` adds ~9.8 GB (`optimize_memory=True` aliases next_state).
+> **Mitigations** (pick one):
+> - Run on a machine with **≥ 48 GB RAM** (or add swap).
+> - Subset the demos: add `--dataset.episodes='[0,1,...,N]'` (fewer episodes → proportional RAM).
+> - Lower `policy.offline_buffer_capacity` toward the frame count you actually load.
+
+---
+
+## 6. Start the ACTOR (terminal 2, same config) **[ROBOT]**
+
+```bash
+export HF_HUB_OFFLINE=1 TORCH_HOME=<root>/scratchpad/torch_home HF_LEROBOT_HOME=<root>/hilserl
+lr_env/bin/python -m lerobot.rl.actor \
+    --config_path hilserl/config/train_hilserl_ur7e.json
+```
+
+The actor connects to the learner over gRPC, opens cam1/cam2, builds the EE-delta → IK action
+pipeline (`MapTensorToDeltaActionDict → MapDeltaActionToRobotAction → EEReferenceAndDelta →
+EEBoundsAndSafety → GripperVelocityToJoint → InverseKinematicsRLStep`), runs the policy on the arm,
+and streams transitions back. Once ≥ `online_step_before_learning` (100) online transitions arrive,
+the learner starts SAC updates with a 50/50 online/offline RLPD mix (`online_ratio=0.5`).
+
+---
+
+## 7. Human-in-the-loop interventions **[ROBOT]**
+
+- Press the **upper-right trigger** on the gamepad (or **`space`** on the keyboard) to take over and
+  provide a corrective demonstration; release to hand control back to the policy.
+- Intervene heavily at the start, then taper — a healthy run shows the intervention rate dropping as
+  the policy improves (watch it in wandb if `wandb.enable=true`).
+- The success reward comes from the vision classifier (`reward=1` when `prob>0.7`); with
+  `terminate_on_success=true` the episode ends on the first success frame.
+
+---
+
+## 8. Key hyperparameters to tune (config: `hilserl/config/train_hilserl_ur7e.json`)
+
+- `algorithm.temperature_init` (SAC entropy temp) — start `0.01`; too high makes interventions
+  ineffective.
+- `policy.actor_learner_config.policy_parameters_push_frequency` — seconds between weight pushes
+  (default 4; drop to 1–2 for fresher actor weights).
+- `policy.storage_device` — keep `"cpu"` here (12 GB GPU can't hold the offline image buffer). Set
+  `"cuda"` only if you move to a big-VRAM box.
+- `algorithm.utd_ratio` (2) / `algorithm.num_critics` (2) — raise UTD for more updates per step.
+
+---
+
+### Offline validation already passed (no robot)
+`hilserl/config/dryrun_validate_learner.py` reproduced the learner setup path (config parse →
+`make_policy` → `make_algorithm` (SAC) → offline buffer load+sample → gate). It reached the idle gate
+`len(online_buffer)=0 < online_step_before_learning=100`. Full evidence in `HILSERL_PREP_RESULTS.md`.

README.md

@@ -0,0 +1,185 @@
+---
+license: apache-2.0
+library_name: lerobot
+tags:
+  - hil-serl
+  - reinforcement-learning
+  - sac
+  - rlpd
+  - robotics
+  - ur7e
+  - lerobot
+pipeline_tag: robotics
+datasets:
+  - Bigenlight/banana_in_pot
+---
+
+# HIL-SERL offline prep bundle — Put the right banana in the pot (UR7e)
+
+This repository is the **offline-prepared base for HIL-SERL** (Human-in-the-Loop Sample-Efficient
+Robotic reinforcement Learning) on the *"put the right banana in the pot"* task with a
+**Universal Robots UR7e**. It contains **everything up to online RL** — the vision reward
+classifier, the SAC/RLPD training config, the UR7e kinematics (URDF + FK), the offline demo
+buffer builder, and the runbook. The **online phase itself is robot-only** and is not included
+here (it requires the physical arm plus `ur_rtde` + `placo` + a gRPC actor/learner).
+
+Built from [`Bigenlight/banana_in_pot`](https://huggingface.co/datasets/Bigenlight/banana_in_pot)
+(51 teleop demos / 21,524 frames) using [LeRobot](https://github.com/huggingface/lerobot)'s
+HIL-SERL stack.
+
+## Data & hardware setup
+
+| Component | Detail |
+|---|---|
+| Robot | **Universal Robots UR7e** — 6-DOF collaborative arm, joints in radians. |
+| Teleoperation (demos) | **GELLO** low-cost 3D-printed leader arm. |
+| Camera 1 | **Intel RealSense D435** — RGB only |
+| Camera 2 | **Intel RealSense D435if** — RGB only |
+| Camera streams | 1280×720 @ 30 fps, color only (**no depth / IR**). HIL-SERL uses the two RGB views **resized to 128×128**. |
+| Task | *"put the right banana in the pot"* — distractors + silver pot; success = correct banana in the pot. |
+
+## What's in this bundle
+
+```
+banana_in_pot_hilserl/
+├── reward_classifier/
+│   └── checkpoint/              # trained success classifier (config.json + model.safetensors)
+├── config/
+│   └── train_hilserl_ur7e.json  # shared SAC/RLPD learner+actor config (paths relative)
+├── build_offline_buffer.py      # rebuilds the 54 MB RL demo buffer from the dataset
+├── joint_to_ee.py               # UR7e forward kinematics (Joint → EE), placo-free, validated
+├── ur7e.urdf                    # pre-generated UR7e URDF (IK target frame `tool0`)
+├── HILSERL_RUNBOOK.md           # exact online start sequence (robot-only steps marked [ROBOT])
+└── assets/                      # figures embedded below
+```
+
+> **Offline demo buffer is NOT shipped** (it is ~54 MB of video/state transitions). Rebuild it
+> from the dataset with `build_offline_buffer.py` — see [Rebuilding the demo buffer](#rebuilding-the-offline-demo-buffer).
+
+## 1. Reward / success classifier
+
+A vision-based binary success detector trained offline, in the exact LeRobot
+`reward_classifier` format so it drops straight into HIL-SERL.
+
+- **Encoder:** frozen `lerobot/resnet10` (CNN), per-camera `SpatialLearnedEmbeddings` pool +
+  Linear→LayerNorm→Tanh. Following the official LeRobot behavior, the encoder runs under
+  `no_grad` so **only the classifier head trains** (~2.36 M trainable params).
+- **Cameras:** `observation.images.cam1` + `observation.images.cam2`, 128×128, MEAN_STD-normalized.
+- **Labeling** (all 51 demos are successes, so negatives are synthesized): POSITIVE = last 15%
+  of frames **and** gripper re-opened (release into pot); NEGATIVE = first 55% of frames; the
+  55–85% transport margin is excluded. Split **by episode** (val = eps [4,14,24,34,44]).
+- **Deployed with `success_threshold = 0.7`** for release-boundary margin.
+
+| metric | value |
+|---|---|
+| Val accuracy | **99.49%** |
+| Val precision / recall / F1 | 0.979 / 0.991 / 0.985 |
+| Train accuracy (balanced) | 100% |
+| Confusion (val, thr 0.5) | TP=231, TN=1138, FP=5, FN=2 |
+
+![training curves](assets/training_curves.png)
+
+![confusion matrix](assets/confusion_matrix.png)
+
+## 2. Action spec — EE-delta
+
+The demos are absolute joints, but the HIL-SERL policy acts in **end-effector delta** space.
+
+- **Action = base-frame TCP delta ÷ step_size (0.05 m)**, computed by running **FK on the dataset
+  joints** (identical to what online deploy uses as its per-step reference). The gripper is a
+  discrete class `{0=close, 1=stay, 2=open}`.
+- The offline buffer stores a 4-dim action (continuous xyz + discrete gripper); the SAC critic
+  slices `actions[:, :-1]` for the continuous part and a separate discrete critic handles the
+  gripper. Hence `output_features["action"].shape = [3]` with `num_discrete_actions = 3`.
+- Action stats (21,524 frames): p99 ≈ 0.2 in tanh space, `|Δ|>1` = **0.0%** — comfortably inside
+  the `[-1, 1]` range.
+
+### Joint → EE forward kinematics (validated)
+
+`joint_to_ee.py` implements UR7e FK directly from the URDF joint origins (placo-free) and was
+validated against the recorded TCP pose:
+
+| subset | pos err median | rot err median |
+|---|---|---|
+| near-static (‖q̇‖<0.02, n=1884) | **0.85 mm** | **0.16°** |
+| all samples (n=42,833) | 28.0 mm | — |
+
+Sub-mm / sub-0.2° error while static confirms the kinematic chain is accurate; the larger
+moving-sample error is timing jitter between the two async logging streams (joint vs. TCP),
+**not** an FK error. Online deploy IK uses `placo` (Pinocchio), frame `tool0`.
+
+![FK validation](assets/fk_validation.png)
+
+## 3. Training config (SAC + RLPD)
+
+`config/train_hilserl_ur7e.json` is the shared `TrainRLServerPipelineConfig` for the learner and
+actor, fully consistent with the offline buffer:
+
+| block | key values |
+|---|---|
+| `algorithm` (SAC) | `num_critics=2`, `utd_ratio=2`, `discount=0.99`, `temperature_init=0.01`, `grad_clip_norm=10` |
+| mixer | `online_offline`, `online_ratio=0.5` (RLPD 50/50 online/offline) |
+| `policy` (gaussian_actor) | `vision_encoder=lerobot/resnet10` (frozen), `num_discrete_actions=3`, `online_step_before_learning=100`, `storage_device=cpu` |
+| input features | state[7] + cam1[3,128,128] + cam2[3,128,128] |
+| output features | `action[3]` (continuous xyz; gripper via discrete head) |
+| `env` (gym_manipulator) | `resize_size=[128,128]`, EE `step_sizes=0.05`, IK `urdf=ur7e.urdf`/`tool0`, `reward_classifier` thr 0.7, `robot`/`teleop = null` |
+
+The learner setup was validated end-to-end offline: `python -m lerobot.rl.learner` builds the SAC
+policy (2.76 M trainable / 7.67 M total params), loads the offline demo buffer via
+`ReplayBuffer.from_lerobot_dataset`, starts its gRPC server, and **idles at the online-buffer gate**
+(`len(online_buffer) < online_step_before_learning=100`) waiting for the actor — which is exactly
+the "ready for online RL" state.
+
+## 4. The online phase (robot-only, NOT in this repo)
+
+See `HILSERL_RUNBOOK.md` for the exact sequence. In short, on the machine wired to the arm:
+
+1. Install the hardware/transport deps that the offline env intentionally omits: `grpcio`
+   (py3.12 wheel), `placo` (IK), `ur_rtde` (UR I/O), `lerobot[hilserl]`.
+2. Fill `env.robot` / `env.teleop` in the config (UR7e IP + motors + cameras; gamepad teleop).
+3. Tune the workspace placeholders against the real arm: `end_effector_bounds`,
+   `fixed_reset_joint_positions`, episode length. **Keep `end_effector_step_sizes = 0.05`** — the
+   offline action = TCP-delta ÷ 0.05, so changing it desyncs the demo actions.
+4. Start the **learner** (terminal 1) and **actor** (terminal 2) with the same config. The learner
+   idles at the gate until the actor supplies ≥100 online transitions, then SAC updates begin with
+   the 50/50 RLPD mix.
+5. **Human interventions:** gamepad trigger / `space` to take over and give corrective demos; taper
+   the intervention rate as the policy improves.
+
+> **RAM note:** the full offline buffer (`from_lerobot_dataset`) materializes every transition
+> eagerly and peaks at **~25 GB**. Use ≥48 GB RAM, or subset episodes via
+> `--dataset.episodes='[...]'`, or lower `offline_buffer_capacity`.
+
+## Rebuilding the offline demo buffer
+
+The 54 MB RL demo buffer is not shipped. Rebuild it from the LeRobot dataset with the included
+script (it converts absolute-joint demos to the EE-delta + reward + done schema):
+
+```bash
+python build_offline_buffer.py   # writes ./banana_rl_lerobot (repo_id theo/banana_in_pot_rl)
+```
+
+Then point `dataset.root` in `config/train_hilserl_ur7e.json` at the rebuilt directory (the config
+ships with a relative `./banana_rl_lerobot`).
+
+## Related repositories
+
+- **Dataset:** [`Bigenlight/banana_in_pot`](https://huggingface.co/datasets/Bigenlight/banana_in_pot)
+- **ACT imitation-learning policy** for the same task:
+  [`Bigenlight/act_banana_in_pot`](https://huggingface.co/Bigenlight/act_banana_in_pot)
+
+## Caveats & limitations
+
+- **No true failure episodes.** The classifier's negatives are *early-task* frames (approach/grasp),
+  not genuine failed attempts. It reliably separates "task complete" from "task in progress" but has
+  never seen a real failure of a completed-looking state — expect over-confidence on OOD near-miss
+  end states. **Record a handful of real failure/near-miss episodes early in online HIL-SERL** to
+  harden it.
+- The excluded 55–85% transport margin means the **decision boundary around the release moment is
+  uncalibrated**; `success_threshold=0.7` adds margin.
+- **EE safety bounds and reset pose in the config are placeholders** — they MUST be tuned against
+  the real arm before letting the policy move.
+- Only the classifier head trains (frozen-encoder LeRobot quirk) — fine for this easy visual task,
+  the first thing to revisit if a harder task underperforms.
+- All validation here is **offline** (config parse → policy/critic build → buffer load → gate). No
+  online RL results are included; that is the robot-only next step.

build_offline_buffer.py

@@ -0,0 +1,335 @@
+#!/usr/bin/env python
+"""
+build_offline_buffer.py  (Agent C)
+
+Convert the teleop demo dataset `banana_in_pot_lerobot` into the HIL-SERL OFFLINE
+DEMO BUFFER: a standard LeRobot v3 dataset that already carries the RL columns the
+SAC learner ingests via ReplayBuffer.from_lerobot_dataset:
+
+  observation.state         float32 [7]   (ur_q1..6 + grip_pos)  -- MATCHES online obs
+  observation.images.cam1   video 3x128x128  (full-frame, resized)
+  observation.images.cam2   video 3x128x128
+  action                    float32 [4]   [delta_x, delta_y, delta_z, gripper]
+  next.reward               float32 [1]   (Agent B success classifier, thr=0.7)
+  next.done                 bool    [1]   (success onset OR episode end)
+
+WHY these choices (citations in hilserl/rl_dataset_schema.json + notes md):
+- action = base-frame TCP displacement / end_effector_step_sizes  (Agent D rule).
+  We derive the TCP position via FORWARD KINEMATICS of the dataset's OWN joint state
+  (observation.state[:6]), NOT the raw-h5 tcp_pose. Reason: (a) the online deploy
+  reference is `FK(current joints)` recomputed every step (EEReferenceAndDelta,
+  use_latched_reference=False, gym_manipulator.py:507), so FK(joints) is exactly the
+  quantity whose per-step delta the policy must reproduce; (b) it is perfectly
+  frame-aligned to the images/state the policy observes (no async-stream matching,
+  no episode->take mapping). Agent D validated FK == recorded tcp_pose to 0.85 mm at
+  rest across all 51 takes, so this is equivalent to the recorded TCP.
+- gripper action = discrete class {0=close, 1=stay, 2=open} from grip_pos transitions
+  (robot_kinematic_processor.py:408-412 semantics; grip_cmd is NaN-prone so grip_pos).
+- observation.state kept as the raw 7-d joint state: the online env base observation is
+  `agent_pos` = motor-bus joint positions (gym_manipulator.py:173-177) with all
+  ObservationConfig add_* flags defaulting False (envs/configs.py:266-268) -> 7-dim.
+- images 3x128x128 to match Agent B's reward classifier (trained at 128x128); the
+  online env must set image_preprocessing.resize_size=[128,128] to match (documented).
+"""
+from __future__ import annotations
+
+import json
+import os
+import sys
+
+import numpy as np
+import torch
+import torch.nn.functional as F
+
+HERE = os.path.dirname(os.path.abspath(__file__))
+ROOT = os.path.dirname(HERE)
+sys.path.insert(0, HERE)
+
+from lerobot.datasets.lerobot_dataset import LeRobotDataset  # noqa: E402
+from lerobot.rewards.classifier.modeling_classifier import Classifier  # noqa: E402
+
+# ---------------------------------------------------------------- config
+SRC_ROOT = os.path.join(ROOT, "banana_in_pot_lerobot")
+OUT_ROOT = os.path.join(HERE, "banana_rl_lerobot")
+REPO_ID = "theo/banana_in_pot_rl"
+CKPT = os.path.join(HERE, "reward_classifier", "checkpoint")
+
+STEP_SIZE = 0.05          # metres per unit; end_effector_step_sizes x=y=z
+SUCCESS_THRESHOLD = 0.7   # Agent B caveat
+SUCCESS_REWARD = 1.0
+IMG = 128
+GRIP_EPS = 0.03           # deadzone on Δgrip_pos for discrete gripper class
+GRIPPER_CLOSE, GRIPPER_STAY, GRIPPER_OPEN = 0.0, 1.0, 2.0
+DEVICE = "cuda" if torch.cuda.is_available() else "cpu"
+
+# ---------------------------------------------------------------- UR7e FK
+# scipy-free re-implementation of Agent D's validated FK (hilserl/joint_to_ee.py):
+# same default_kinematics.yaml joint origins, same RPY(xyz, URDF/extrinsic) + Rz(theta)
+# chain shoulder->wrist_3, base_link frame, no tool offset (recorded TCP == flange).
+import yaml  # noqa: E402
+
+_KIN_YAML = "/opt/ros/jazzy/share/ur_description/config/ur7e/default_kinematics.yaml"
+_LINK_ORDER = ["shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3"]
+
+
+def _Rx(a):
+    c, s = np.cos(a), np.sin(a); return np.array([[1, 0, 0], [0, c, -s], [0, s, c]])
+
+
+def _Ry(a):
+    c, s = np.cos(a), np.sin(a); return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])
+
+
+def _Rz(a):
+    c, s = np.cos(a), np.sin(a); return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
+
+
+def _rpyxyz_to_T(p):
+    T = np.eye(4)
+    T[:3, :3] = _Rz(p["yaw"]) @ _Ry(p["pitch"]) @ _Rx(p["roll"])  # URDF RPY = extrinsic xyz
+    T[:3, 3] = [p["x"], p["y"], p["z"]]
+    return T
+
+
+with open(_KIN_YAML) as _f:
+    _KIN = yaml.safe_load(_f)["kinematics"]
+_FIXED = [_rpyxyz_to_T(_KIN[k]) for k in _LINK_ORDER]
+
+
+def _Rz4(a):
+    T = np.eye(4); T[:3, :3] = _Rz(a); return T
+
+
+def fk_batch(q_rad_seq):
+    """(N,6) joint angles [rad] -> (N,4,4) TCP pose in base_link frame."""
+    out = np.zeros((len(q_rad_seq), 4, 4))
+    for n, q in enumerate(np.asarray(q_rad_seq, dtype=float)):
+        T = np.eye(4)
+        for i in range(6):
+            T = T @ _FIXED[i] @ _Rz4(q[i])
+        out[n] = T
+    return out
+
+
+def resize_uint8_hwc(chw_float01: torch.Tensor) -> np.ndarray:
+    """(3,H,W) float[0,1] -> (128,128,3) uint8 HWC (matches Agent B cache + LeRobot video convention)."""
+    x = F.interpolate(chw_float01.unsqueeze(0), size=(IMG, IMG), mode="bilinear", align_corners=False)
+    return (x[0].permute(1, 2, 0).clamp(0, 1).numpy() * 255).astype(np.uint8)
+
+
+def norm_cam(uint8_hwc: np.ndarray, mean: torch.Tensor, std: torch.Tensor) -> torch.Tensor:
+    """(N,128,128,3) uint8 -> (N,3,128,128) MEAN_STD-normalized (exactly Agent B inference)."""
+    t = torch.from_numpy(uint8_hwc.astype(np.float32) / 255.0).permute(0, 3, 1, 2)
+    return (t - mean[None, :, None, None]) / std[None, :, None, None]
+
+
+def discrete_gripper(grip: np.ndarray) -> np.ndarray:
+    """grip_pos (N,) -> discrete gripper command per frame (N,).
+    grip_pos increases when closing (grasp ~0.55), decreases when opening (~0.01).
+    Online: 0=close, 1=stay, 2=open. Command at frame t reproduces grip[t]->grip[t+1]."""
+    n = len(grip)
+    cls = np.full(n, GRIPPER_STAY, dtype=np.float32)
+    dg = np.diff(grip)  # (n-1,)
+    cls[:-1] = np.where(dg > GRIP_EPS, GRIPPER_CLOSE, np.where(dg < -GRIP_EPS, GRIPPER_OPEN, GRIPPER_STAY))
+    return cls
+
+
+def main():
+    print(f"device={DEVICE}  src={SRC_ROOT}\n")
+    ds = LeRobotDataset(REPO_ID, root=SRC_ROOT)
+    task_str = ds.meta.tasks.index[0] if hasattr(ds.meta.tasks, "index") else list(ds.meta.tasks)[0]
+    print("task string:", task_str)
+
+    # per-camera normalization stats (same source as Agent B)
+    stats = json.load(open(os.path.join(SRC_ROOT, "meta", "stats.json")))
+    mean1 = torch.tensor(np.array(stats["observation.images.cam1"]["mean"]).reshape(3), dtype=torch.float32)
+    std1 = torch.tensor(np.array(stats["observation.images.cam1"]["std"]).reshape(3), dtype=torch.float32)
+    mean2 = torch.tensor(np.array(stats["observation.images.cam2"]["mean"]).reshape(3), dtype=torch.float32)
+    std2 = torch.tensor(np.array(stats["observation.images.cam2"]["std"]).reshape(3), dtype=torch.float32)
+
+    clf = Classifier.from_pretrained(CKPT).to(DEVICE).eval()
+
+    # ---- output dataset schema (RL columns) ----
+    features = {
+        "observation.state": {"dtype": "float32", "shape": [7],
+                              "names": ["ur_q1", "ur_q2", "ur_q3", "ur_q4", "ur_q5", "ur_q6", "grip_pos"]},
+        "observation.images.cam1": {"dtype": "video", "shape": [IMG, IMG, 3],
+                                    "names": ["height", "width", "channels"]},
+        "observation.images.cam2": {"dtype": "video", "shape": [IMG, IMG, 3],
+                                    "names": ["height", "width", "channels"]},
+        "action": {"dtype": "float32", "shape": [4], "names": ["delta_x", "delta_y", "delta_z", "gripper"]},
+        "next.reward": {"dtype": "float32", "shape": [1], "names": None},
+        "next.done": {"dtype": "bool", "shape": [1], "names": None},
+    }
+    if os.path.exists(OUT_ROOT):
+        import shutil
+        shutil.rmtree(OUT_ROOT)
+    out = LeRobotDataset.create(repo_id=REPO_ID, fps=ds.fps, root=OUT_ROOT,
+                                features=features, use_videos=True)
+
+    # ---- group frames by episode (single ordered pass) ----
+    from_idx = ds.meta.episodes["dataset_from_index"]
+    to_idx = ds.meta.episodes["dataset_to_index"]
+    n_ep = ds.meta.total_episodes
+
+    all_actions = []
+    grip_class_counts = np.zeros(3, dtype=int)
+    n_success_ep = 0
+    reward_timeline_ep0 = None
+    prob_timeline_ep0 = None
+
+    for ep in range(n_ep):
+        lo, hi = int(from_idx[ep]), int(to_idx[ep])
+        idxs = list(range(lo, hi))
+        n = len(idxs)
+
+        states = np.zeros((n, 7), dtype=np.float32)
+        c1 = np.zeros((n, IMG, IMG, 3), dtype=np.uint8)
+        c2 = np.zeros((n, IMG, IMG, 3), dtype=np.uint8)
+        for j, i in enumerate(idxs):
+            f = ds[i]
+            states[j] = f["observation.state"].numpy()
+            c1[j] = resize_uint8_hwc(f["observation.images.cam1"])
+            c2[j] = resize_uint8_hwc(f["observation.images.cam2"])
+
+        # --- action: FK(joints) -> base-frame TCP; delta / step_size ---
+        pos = fk_batch(states[:, :6])[:, :3, 3]  # (n,3) metres
+        actions = np.zeros((n, 4), dtype=np.float32)
+        actions[:-1, :3] = np.diff(pos, axis=0) / STEP_SIZE
+        actions[:, 3] = discrete_gripper(states[:, 6])
+        all_actions.append(actions)
+        for cval in (GRIPPER_CLOSE, GRIPPER_STAY, GRIPPER_OPEN):
+            grip_class_counts[int(cval)] += int((actions[:, 3] == cval).sum())
+
+        # --- reward: Agent B classifier over frames (batched) ---
+        probs = np.zeros(n, dtype=np.float32)
+        with torch.no_grad():
+            for s in range(0, n, 256):
+                e = min(n, s + 256)
+                b1 = norm_cam(c1[s:e], mean1, std1).to(DEVICE)
+                b2 = norm_cam(c2[s:e], mean2, std2).to(DEVICE)
+                probs[s:e] = clf.predict([b1, b2]).probabilities.cpu().numpy()
+        reward = (probs > SUCCESS_THRESHOLD).astype(np.float32)
+
+        # --- done: success onset OR episode end ---
+        done = np.zeros(n, dtype=bool)
+        succ = np.where(reward > 0.5)[0]
+        if len(succ):
+            n_success_ep += 1
+            done[succ[0]] = True     # success onset
+        done[-1] = True              # episode end
+
+        if ep == 0:
+            reward_timeline_ep0 = reward.copy()
+            prob_timeline_ep0 = probs.copy()
+
+        for j in range(n):
+            out.add_frame({
+                "observation.state": states[j],
+                "observation.images.cam1": c1[j],
+                "observation.images.cam2": c2[j],
+                "action": actions[j],
+                "next.reward": np.array([reward[j]], dtype=np.float32),
+                "next.done": np.array([done[j]], dtype=bool),
+                "task": task_str,
+            })
+        out.save_episode()
+        print(f"ep {ep:02d}: n={n:4d}  success_onset={'-' if not len(succ) else succ[0]:>4}  "
+              f"reward_frames={int(reward.sum()):4d}  |Δp|/step max={np.abs(actions[:,:3]).max():.3f}")
+
+    out.finalize()
+
+    # ---------------------------------------------------------- stats report
+    A = np.concatenate(all_actions, axis=0)
+    cont = A[:, :3]
+    rep = {
+        "total_frames": int(A.shape[0]),
+        "n_episodes": int(n_ep),
+        "n_episodes_with_success_onset": int(n_success_ep),
+        "step_size_m": STEP_SIZE,
+        "success_threshold": SUCCESS_THRESHOLD,
+        "action_continuous_xyz": {
+            "min": cont.min(0).tolist(), "max": cont.max(0).tolist(),
+            "q01": np.quantile(cont, 0.01, axis=0).tolist(),
+            "q50": np.quantile(cont, 0.50, axis=0).tolist(),
+            "q99": np.quantile(cont, 0.99, axis=0).tolist(),
+            "frac_abs_gt_1": float((np.abs(cont) > 1.0).mean()),
+        },
+        "gripper_class_counts": {"close(0)": int(grip_class_counts[0]),
+                                 "stay(1)": int(grip_class_counts[1]),
+                                 "open(2)": int(grip_class_counts[2])},
+    }
+    print("\n==== ACTION / REWARD STATS ====")
+    print(json.dumps(rep, indent=2))
+    json.dump(rep, open(os.path.join(HERE, "action_reward_stats.json"), "w"), indent=2)
+
+    # ---------------------------------------------------------- plots
+    try:
+        import matplotlib
+        matplotlib.use("Agg")
+        import matplotlib.pyplot as plt
+        fig, ax = plt.subplots(1, 3, figsize=(16, 4))
+        for d, name in zip(range(3), ["Δx", "Δy", "Δz"]):
+            ax[0].hist(cont[:, d], bins=120, alpha=0.5, label=name)
+        ax[0].axvline(-1, color="k", ls="--", lw=0.8); ax[0].axvline(1, color="k", ls="--", lw=0.8)
+        ax[0].set_title(f"EE-delta action (÷ step={STEP_SIZE} m)"); ax[0].set_xlabel("normalized delta"); ax[0].legend()
+        ax[1].bar(["close(0)", "stay(1)", "open(2)"], grip_class_counts, color=["#d62728", "#7f7f7f", "#2ca02c"])
+        ax[1].set_title("gripper discrete class counts")
+        ax[2].plot(prob_timeline_ep0, label="classifier prob")
+        ax[2].plot(reward_timeline_ep0, label="next.reward", lw=2)
+        ax[2].axhline(SUCCESS_THRESHOLD, color="k", ls="--", lw=0.8, label=f"thr={SUCCESS_THRESHOLD}")
+        ax[2].set_title("episode 0: reward timeline"); ax[2].set_xlabel("frame"); ax[2].legend()
+        fig.tight_layout()
+        fig.savefig(os.path.join(HERE, "rl_dataset_action_reward.png"), dpi=110)
+        print("saved rl_dataset_action_reward.png")
+    except Exception as e:
+        print("plot skipped:", e)
+
+    # ---------------------------------------------------------- schema json
+    schema = {
+        "repo_id": REPO_ID,
+        "root": OUT_ROOT,
+        "codebase_version": "v3.0",
+        "fps": int(ds.fps),
+        "total_episodes": int(n_ep),
+        "total_frames": int(A.shape[0]),
+        "rl_columns": {
+            "action": {"dtype": "float32", "shape": [4],
+                       "names": ["delta_x", "delta_y", "delta_z", "gripper"],
+                       "semantics": "xyz = base-frame TCP delta / end_effector_step_sizes(=0.05 m); "
+                                    "gripper = discrete class {0=close,1=stay,2=open}"},
+            "next.reward": {"dtype": "float32", "shape": [1],
+                            "semantics": f"Agent B success classifier prob>{SUCCESS_THRESHOLD} -> {SUCCESS_REWARD}"},
+            "next.done": {"dtype": "bool", "shape": [1],
+                          "semantics": "True at success onset AND at episode end"},
+        },
+        "state_keys_for_replay_buffer": ["observation.state",
+                                         "observation.images.cam1", "observation.images.cam2"],
+        "observation.state": {"dtype": "float32", "shape": [7],
+                              "names": ["ur_q1", "ur_q2", "ur_q3", "ur_q4", "ur_q5", "ur_q6", "grip_pos"],
+                              "note": "matches online gym_manipulator agent_pos (all ObservationConfig add_* False)"},
+        "observation.images.cam1": {"dtype": "video", "stored_shape_hwc": [IMG, IMG, 3],
+                                    "decoded_shape_chw": [3, IMG, IMG], "range": "float[0,1]"},
+        "observation.images.cam2": {"dtype": "video", "stored_shape_hwc": [IMG, IMG, 3],
+                                    "decoded_shape_chw": [3, IMG, IMG], "range": "float[0,1]"},
+        "policy_output_features": {"action": {"type": "ACTION", "shape": [4]}},
+        "policy_input_features": {
+            "observation.state": {"type": "STATE", "shape": [7]},
+            "observation.images.cam1": {"type": "VISUAL", "shape": [3, IMG, IMG]},
+            "observation.images.cam2": {"type": "VISUAL", "shape": [3, IMG, IMG]},
+        },
+        "online_env_requirements": {
+            "image_preprocessing.resize_size": [IMG, IMG],
+            "inverse_kinematics.end_effector_step_sizes": {"x": STEP_SIZE, "y": STEP_SIZE, "z": STEP_SIZE},
+            "policy.num_discrete_actions": 3,
+            "reward_classifier.success_threshold": SUCCESS_THRESHOLD,
+            "note": "crop_dataset_roi may be re-applied online; if ROI crop is used, re-run it on THIS "
+                    "dataset too so offline images match the online cropped size.",
+        },
+    }
+    json.dump(schema, open(os.path.join(HERE, "rl_dataset_schema.json"), "w"), indent=2)
+    print("saved rl_dataset_schema.json")
+
+
+if __name__ == "__main__":
+    main()

config/train_hilserl_ur7e.json

@@ -0,0 +1,196 @@
+{
+  "output_dir": "hilserl/outputs/banana_sac",
+  "job_name": "banana_hilserl_ur7e",
+  "seed": 1,
+  "resume": false,
+  "num_workers": 4,
+  "batch_size": 256,
+  "steps": 100000,
+  "log_freq": 500,
+  "save_freq": 5000,
+  "save_checkpoint": true,
+
+  "mixer": "online_offline",
+  "online_ratio": 0.5,
+
+  "dataset": {
+    "repo_id": "theo/banana_in_pot_rl",
+    "root": "./banana_rl_lerobot",
+    "use_imagenet_stats": false
+  },
+
+  "algorithm": {
+    "type": "sac",
+    "actor_lr": 3e-4,
+    "critic_lr": 3e-4,
+    "temperature_lr": 3e-4,
+    "discount": 0.99,
+    "use_backup_entropy": true,
+    "critic_target_update_weight": 0.005,
+    "num_critics": 2,
+    "num_subsample_critics": null,
+    "critic_network_kwargs": {
+      "hidden_dims": [256, 256],
+      "activate_final": true,
+      "final_activation": null
+    },
+    "discrete_critic_network_kwargs": {
+      "hidden_dims": [256, 256],
+      "activate_final": true,
+      "final_activation": null
+    },
+    "temperature_init": 0.01,
+    "target_entropy": null,
+    "utd_ratio": 2,
+    "policy_update_freq": 1,
+    "grad_clip_norm": 10.0,
+    "use_torch_compile": false
+  },
+
+  "policy": {
+    "type": "gaussian_actor",
+    "push_to_hub": false,
+    "device": "cuda",
+    "storage_device": "cpu",
+    "vision_encoder_name": "lerobot/resnet10",
+    "freeze_vision_encoder": true,
+    "image_encoder_hidden_dim": 32,
+    "shared_encoder": true,
+    "num_discrete_actions": 3,
+    "image_embedding_pooling_dim": 8,
+    "state_encoder_hidden_dim": 256,
+    "latent_dim": 256,
+
+    "online_steps": 1000000,
+    "online_buffer_capacity": 100000,
+    "offline_buffer_capacity": 25000,
+    "async_prefetch": false,
+    "online_step_before_learning": 100,
+
+    "normalization_mapping": {
+      "VISUAL": "MEAN_STD",
+      "STATE": "MIN_MAX",
+      "ENV": "MIN_MAX",
+      "ACTION": "MIN_MAX"
+    },
+
+    "input_features": {
+      "observation.state":       { "type": "STATE",  "shape": [7] },
+      "observation.images.cam1": { "type": "VISUAL", "shape": [3, 128, 128] },
+      "observation.images.cam2": { "type": "VISUAL", "shape": [3, 128, 128] }
+    },
+    "output_features": {
+      "action": { "type": "ACTION", "shape": [3] }
+    },
+
+    "dataset_stats": {
+      "observation.images.cam1": {
+        "mean": [0.485, 0.456, 0.406],
+        "std":  [0.229, 0.224, 0.225]
+      },
+      "observation.images.cam2": {
+        "mean": [0.485, 0.456, 0.406],
+        "std":  [0.229, 0.224, 0.225]
+      },
+      "observation.state": {
+        "min": [2.506379, -2.391092, 1.200254, -3.141332, -2.102934, -4.914474, 0.0118],
+        "max": [3.561876, -1.021552, 2.487616, -1.266623, -1.415739, -1.831519, 0.898]
+      },
+      "action": {
+        "min": [-1.0, -1.0, -1.0],
+        "max": [1.0, 1.0, 1.0]
+      }
+    },
+
+    "actor_network_kwargs": {
+      "hidden_dims": [256, 256],
+      "activate_final": true
+    },
+    "policy_kwargs": {
+      "use_tanh_squash": true,
+      "std_min": -5,
+      "std_max": 2,
+      "init_final": 0.05
+    },
+    "actor_learner_config": {
+      "learner_host": "127.0.0.1",
+      "learner_port": 50051,
+      "policy_parameters_push_frequency": 4
+    },
+    "concurrency": {
+      "actor": "processes",
+      "learner": "processes"
+    }
+  },
+
+  "env": {
+    "type": "gym_manipulator",
+    "name": "real_robot",
+    "fps": 30,
+    "task": "put right banana in the pot",
+
+    "robot": null,
+    "teleop": null,
+
+    "processor": {
+      "control_mode": "gamepad",
+      "max_gripper_pos": 100.0,
+      "observation": {
+        "display_cameras": false,
+        "add_joint_velocity_to_observation": false,
+        "add_current_to_observation": false,
+        "add_ee_pose_to_observation": false
+      },
+      "image_preprocessing": {
+        "crop_params_dict": null,
+        "resize_size": [128, 128]
+      },
+      "gripper": {
+        "use_gripper": true,
+        "gripper_penalty": 0.0
+      },
+      "reset": {
+        "fixed_reset_joint_positions": [3.05, -1.60, 1.90, -1.85, -1.55, -3.30, 0.02],
+        "reset_time_s": 5.0,
+        "control_time_s": 20.0,
+        "terminate_on_success": true
+      },
+      "inverse_kinematics": {
+        "urdf_path": "./ur7e.urdf",
+        "target_frame_name": "tool0",
+        "end_effector_bounds": {
+          "min": [-0.6, -0.6, 0.0],
+          "max": [0.6, 0.6, 0.6]
+        },
+        "end_effector_step_sizes": {
+          "x": 0.05,
+          "y": 0.05,
+          "z": 0.05
+        }
+      },
+      "reward_classifier": {
+        "pretrained_path": "./reward_classifier/checkpoint",
+        "success_threshold": 0.7,
+        "success_reward": 1.0
+      }
+    },
+
+    "features": {
+      "observation.state":       { "type": "STATE",  "shape": [7] },
+      "observation.images.cam1": { "type": "VISUAL", "shape": [3, 128, 128] },
+      "observation.images.cam2": { "type": "VISUAL", "shape": [3, 128, 128] },
+      "action":                  { "type": "ACTION", "shape": [3] }
+    },
+    "features_map": {
+      "observation.state": "observation.state",
+      "observation.images.cam1": "observation.images.cam1",
+      "observation.images.cam2": "observation.images.cam2",
+      "action": "action"
+    }
+  },
+
+  "wandb": {
+    "enable": false,
+    "project": "banana_in_pot_hilserl"
+  }
+}

joint_to_ee.py

@@ -0,0 +1,235 @@
+#!/usr/bin/env python
+"""
+joint_to_ee.py — UR7e joint <-> end-effector conversion for HIL-SERL prep.
+
+WHAT HIL-SERL EXPECTS (see notes below / joint_to_ee_notes.md):
+  The RL policy emits a flat action vector [delta_x, delta_y, delta_z, gripper]
+  (4-d when use_gripper=True). delta_{x,y,z} are DIMENSIONLESS, expressed in the
+  robot BASE frame, and are converted to metres inside gym_manipulator's action
+  pipeline by multiplying with `end_effector_step_sizes[{x,y,z}]`
+  (lerobot/robots/so_follower/robot_kinematic_processor.py:124-135). The policy
+  never commands rotation (target_wx/wy/wz are hard-wired to 0.0 in
+  lerobot/processor/delta_action_processor.py:114-116), so orientation is held at
+  the current (reference) EE orientation. `gripper` is a velocity-like scalar.
+
+  Reference pose in the RL pipeline is FK(current joints) recomputed EVERY step
+  (use_latched_reference=False, gym_manipulator.py:507). Therefore the absolute
+  target each step is:  target_pos = FK(q_t).pos + step_size * policy_delta,
+  and the learner's action is exactly the base-frame displacement of the TCP
+  between consecutive steps, divided by the step size.
+
+This module provides:
+  (a) fk(q_rad)                    -> 4x4 TCP pose (base frame) from 6 UR joints.
+  (b) joint_traj_to_ee_delta_actions(...) -> the [dx,dy,dz,gripper] action seq.
+      Prefers RECORDED tcp_pose for the EE (exact); FK is the fallback / validator.
+  (c) IK direction for deploy is documented in joint_to_ee_notes.md (placo).
+
+Validated: FK(ur_joint_states) vs recorded tcp_pose = 0.85 mm median error at
+near-static frames across all 51 takes -> the kinematic chain is exact and the
+recorded TCP == robot flange (no tool offset configured). See validate_fk().
+"""
+from __future__ import annotations
+
+import glob
+import os
+
+import numpy as np
+import yaml
+from scipy.spatial.transform import Rotation as R
+
+# ----------------------------------------------------------------------------
+# UR7e forward kinematics, built directly from the ur_description URDF joint
+# origins (default_kinematics.yaml). Each joint i is a fixed RPY+XYZ transform
+# followed by a rotation about the joint's local Z axis.
+# ----------------------------------------------------------------------------
+DEFAULT_KIN_YAML = "/opt/ros/jazzy/share/ur_description/config/ur7e/default_kinematics.yaml"
+_LINK_ORDER = ["shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3"]
+UR_JOINT_NAMES = [
+    "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
+    "wrist_1_joint", "wrist_2_joint", "wrist_3_joint",
+]
+
+
+def _rpyxyz_to_T(p: dict) -> np.ndarray:
+    T = np.eye(4)
+    T[:3, :3] = R.from_euler("xyz", [p["roll"], p["pitch"], p["yaw"]]).as_matrix()
+    T[:3, 3] = [p["x"], p["y"], p["z"]]
+    return T
+
+
+def _load_fixed_transforms(kin_yaml: str = DEFAULT_KIN_YAML) -> list[np.ndarray]:
+    with open(kin_yaml) as f:
+        kin = yaml.safe_load(f)["kinematics"]
+    return [_rpyxyz_to_T(kin[k]) for k in _LINK_ORDER]
+
+
+_FIXED = _load_fixed_transforms()
+
+
+def _rotz(a: float) -> np.ndarray:
+    c, s = np.cos(a), np.sin(a)
+    T = np.eye(4)
+    T[0, 0] = c; T[0, 1] = -s; T[1, 0] = s; T[1, 1] = c
+    return T
+
+
+def fk(q_rad, tool: np.ndarray | None = None) -> np.ndarray:
+    """Forward kinematics of the UR7e TCP (flange) in the robot BASE frame.
+
+    Args:
+        q_rad: 6 joint angles [shoulder_pan..wrist_3] in RADIANS (URDF order,
+               which is exactly ur_joint_states q1..q6 in our h5).
+        tool:  optional 4x4 flange->TCP offset. Our dataset has NO tool offset
+               (recorded tcp_pose == flange, validated to 0.85 mm), so leave None.
+
+    Returns:
+        4x4 homogeneous transform (base_link frame == UR RTDE base frame).
+    """
+    q = np.asarray(q_rad, dtype=float)
+    T = np.eye(4)
+    for i in range(6):
+        T = T @ _FIXED[i] @ _rotz(q[i])
+    if tool is not None:
+        T = T @ tool
+    return T
+
+
+def fk_batch(q_rad_seq: np.ndarray) -> np.ndarray:
+    """FK over an (N,6) array of joint configs -> (N,4,4)."""
+    return np.stack([fk(q) for q in np.asarray(q_rad_seq, dtype=float)], axis=0)
+
+
+def pose_to_pos_quat(T: np.ndarray):
+    """4x4 -> (pos[3], quat[x,y,z,w])."""
+    return T[:3, 3].copy(), R.from_matrix(T[:3, :3]).as_quat()
+
+
+# ----------------------------------------------------------------------------
+# (b) absolute-joint / TCP trajectory  ->  HIL-SERL EE-delta action sequence
+# ----------------------------------------------------------------------------
+def joint_traj_to_ee_delta_actions(
+    tcp_pos: np.ndarray,
+    tcp_quat: np.ndarray | None = None,
+    gripper: np.ndarray | None = None,
+    end_effector_step_sizes: dict | None = None,
+    gripper_max: float = 1.0,
+    include_rotation: bool = False,
+):
+    """Convert a per-timestep TCP trajectory into the HIL-SERL policy action seq.
+
+    HIL-SERL action at step t is the BASE-frame TCP displacement between the pose
+    at t and t+1, divided by the per-axis step size (so the policy operates in a
+    normalised, roughly [-1,1] action space). Rotation is NOT part of the policy
+    action (gym_manipulator holds orientation fixed), but a rotvec delta is
+    returned as extra info when include_rotation=True.
+
+    Prefer passing the RECORDED tcp_pose (x,y,z,quat) for `tcp_pos`/`tcp_quat` —
+    it is the exact robot EE. Use fk_batch(q)[:, :3, 3] only if TCP wasn't logged.
+
+    Args:
+        tcp_pos:  (N,3) TCP positions in metres, base frame.
+        tcp_quat: (N,4) TCP quaternions [x,y,z,w] (only needed if include_rotation).
+        gripper:  (N,) gripper position (e.g. grip_pos). Aligned to `tcp_pos`.
+        end_effector_step_sizes: {"x":sx,"y":sy,"z":sz} metres-per-unit. If None,
+            deltas are returned in METRES (step size = 1). Choose sx.. so the max
+            per-frame delta maps to <=1 (our data: max ~0.053 m/frame at TCP rate).
+        gripper_max: normaliser for gripper -> action[:,3] = gripper / gripper_max.
+        include_rotation: also return per-step rotvec delta (base frame), (N-1,3).
+
+    Returns:
+        actions: (N-1, 4) float32 -> [delta_x, delta_y, delta_z, gripper], the
+                 vector MapTensorToDeltaActionDictStep consumes. gripper column is
+                 0 if `gripper` is None.
+        rot_delta: (N-1,3) rotvec deltas if include_rotation else None.
+    """
+    tcp_pos = np.asarray(tcp_pos, dtype=float)
+    n = len(tcp_pos)
+    if n < 2:
+        raise ValueError("need >=2 timesteps to form deltas")
+
+    dpos = np.diff(tcp_pos, axis=0)  # (N-1,3) base-frame metres
+    if end_effector_step_sizes is not None:
+        s = np.array([end_effector_step_sizes["x"],
+                      end_effector_step_sizes["y"],
+                      end_effector_step_sizes["z"]], dtype=float)
+        dpos = dpos / s
+
+    actions = np.zeros((n - 1, 4), dtype=np.float32)
+    actions[:, :3] = dpos
+    if gripper is not None:
+        g = np.asarray(gripper, dtype=float)[:n]
+        # policy gripper is applied at step t; use the value at the *target* step
+        actions[:, 3] = (g[1:] / gripper_max).astype(np.float32)
+
+    rot_delta = None
+    if include_rotation:
+        if tcp_quat is None:
+            raise ValueError("tcp_quat required when include_rotation=True")
+        Rmats = R.from_quat(np.asarray(tcp_quat, dtype=float))
+        rot_delta = np.zeros((n - 1, 3), dtype=np.float32)
+        for t in range(n - 1):
+            dR = Rmats[t].inv() * Rmats[t + 1]  # relative rotation in EE frame
+            rot_delta[t] = dR.as_rotvec()
+    return actions, rot_delta
+
+
+# ----------------------------------------------------------------------------
+# h5 helpers (reuse recorded tcp_pose; align async streams by timestamp)
+# ----------------------------------------------------------------------------
+def load_take(h5_path: str, interp_joints_to_tcp: bool = True):
+    """Load a take. ur_joint_states (609) and tcp_pose (608) are logged on
+    SEPARATE clocks; we interpolate joints onto the tcp_pose timestamps so every
+    returned row is time-consistent. Returns a dict of aligned arrays."""
+    import h5py
+    with h5py.File(h5_path, "r") as h:
+        q = np.stack([h[f"ur_joint_states/q{i}"][:] for i in range(1, 7)], 1)
+        qd = np.stack([h[f"ur_joint_states/qd{i}"][:] for i in range(1, 7)], 1)
+        tq = h["ur_joint_states/t_rel_s"][:]
+        pos = np.stack([h[f"tcp_pose/{a}"][:] for a in "xyz"], 1)
+        quat = np.stack([h[f"tcp_pose/{a}"][:] for a in ["qx", "qy", "qz", "qw"]], 1)
+        tp = h["tcp_pose/t_rel_s"][:]
+        gpos = h["gripper/grip_pos"][:]
+        tg = h["gripper/t_rel_s"][:]
+    if interp_joints_to_tcp:
+        q = np.stack([np.interp(tp, tq, q[:, j]) for j in range(6)], 1)
+        qd = np.stack([np.interp(tp, tq, qd[:, j]) for j in range(6)], 1)
+        t = tp
+    else:
+        t = tq
+    gpos_i = np.interp(tp, tg, gpos)
+    return {"q": q, "qd": qd, "tcp_pos": pos, "tcp_quat": quat,
+            "gripper": gpos_i, "t": t}
+
+
+# ----------------------------------------------------------------------------
+# (validator) FK vs recorded tcp_pose
+# ----------------------------------------------------------------------------
+def validate_fk(dataset_glob: str, tool: np.ndarray | None = None):
+    """Return per-sample position (mm) / orientation (deg) errors + joint speed."""
+    files = sorted(glob.glob(dataset_glob))
+    perr, oerr, speed = [], [], []
+    for f in files:
+        d = load_take(f)
+        for k in range(len(d["tcp_pos"])):
+            T = fk(d["q"][k], tool=tool)
+            perr.append(np.linalg.norm(T[:3, 3] - d["tcp_pos"][k]) * 1000.0)
+            Rr = R.from_quat(d["tcp_quat"][k]).as_matrix()
+            oerr.append(np.degrees(np.linalg.norm(
+                R.from_matrix(T[:3, :3].T @ Rr).as_rotvec())))
+            speed.append(np.linalg.norm(d["qd"][k]))
+    return np.array(perr), np.array(oerr), np.array(speed), files
+
+
+if __name__ == "__main__":
+    root = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+    g = os.path.join(root, "Put_right_banana_in_the_pot", "take_*", "vectors.h5")
+    perr, oerr, speed, files = validate_fk(g)
+    st = speed < 0.02
+    print(f"{len(files)} takes, {len(perr)} samples")
+    print(f"POS err mm  ALL: med={np.median(perr):.2f} mean={perr.mean():.2f} "
+          f"p95={np.percentile(perr,95):.2f}")
+    print(f"POS err mm  static(|qd|<0.02, n={st.sum()}): "
+          f"med={np.median(perr[st]):.2f} mean={perr[st].mean():.2f} "
+          f"p95={np.percentile(perr[st],95):.2f}")
+    print(f"ROT err deg static: med={np.median(oerr[st]):.2f} "
+          f"mean={oerr[st].mean():.2f}")

reward_classifier/checkpoint/config.json

@@ -0,0 +1,45 @@
+{
+    "type": "reward_classifier",
+    "input_features": {
+        "observation.images.cam1": {
+            "type": "VISUAL",
+            "shape": [
+                3,
+                128,
+                128
+            ]
+        },
+        "observation.images.cam2": {
+            "type": "VISUAL",
+            "shape": [
+                3,
+                128,
+                128
+            ]
+        }
+    },
+    "output_features": {},
+    "device": "cuda",
+    "pretrained_path": null,
+    "pretrained_revision": null,
+    "push_to_hub": false,
+    "repo_id": null,
+    "license": null,
+    "tags": null,
+    "private": null,
+    "name": "reward_classifier",
+    "num_classes": 2,
+    "hidden_dim": 256,
+    "latent_dim": 256,
+    "image_embedding_pooling_dim": 8,
+    "dropout_rate": 0.1,
+    "model_name": "lerobot/resnet10",
+    "model_type": "cnn",
+    "num_cameras": 2,
+    "learning_rate": 0.0001,
+    "weight_decay": 0.01,
+    "grad_clip_norm": 1.0,
+    "normalization_mapping": {
+        "VISUAL": "MEAN_STD"
+    }
+}

reward_classifier/checkpoint/model.safetensors

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2174329c4ff4143f27176d818c3bfb87386ad3eb39336aa552682bf2edea2cf9
+size 29085260

ur7e.urdf

@@ -0,0 +1,297 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from /opt/ros/jazzy/share/ur_description/urdf/ur.urdf.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="ur7e">
+  <!--
+    Base UR robot series xacro macro.
+
+    NOTE this is NOT a URDF. It cannot directly be loaded by consumers
+    expecting a flattened '.urdf' file. See the top-level '.xacro' for that
+    (but note that .xacro must still be processed by the xacro command).
+
+    This file models the base kinematic chain of a UR robot, which then gets
+    parameterised by various configuration files to convert it into a UR3(e),
+    UR5(e), UR10(e) or UR16e, etc.
+
+    NOTE the default kinematic parameters (i.e., link lengths, frame locations,
+    offsets, etc) do not correspond to any particular robot. They are defaults
+    only. There WILL be non-zero offsets between the Forward Kinematics results
+    in TF (i.e., robot_state_publisher) and the values reported by the Teach
+    Pendant.
+
+    For accurate (and robot-specific) transforms, the 'kinematics_parameters_file'
+    parameter MUST point to a .yaml file containing the appropriate values for
+    the targeted robot.
+
+    If using the UniversalRobots/Universal_Robots_ROS_Driver, follow the steps
+    described in the readme of that repository to extract the kinematic
+    calibration from the controller and generate the required .yaml file.
+
+    Main author of the migration to yaml configs Ludovic Delval.
+
+    Contributors to previous versions (in no particular order)
+
+     - Denis Stogl
+     - Lovro Ivanov
+     - Felix Messmer
+     - Kelsey Hawkins
+     - Wim Meeussen
+     - Shaun Edwards
+     - Nadia Hammoudeh Garcia
+     - Dave Hershberger
+     - G. vd. Hoorn
+     - Philip Long
+     - Dave Coleman
+     - Miguel Prada
+     - Mathias Luedtke
+     - Marcel Schnirring
+     - Felix von Drigalski
+     - Felix Exner
+     - Jimmy Da Silva
+     - Ajit Krisshna N L
+     - Muhammad Asif Rana
+  -->
+  <!-- create link fixed to the "world" -->
+  <link name="world"/>
+  <!-- links -  main serial chain -->
+  <!-- base_link serves as the root of the robot's kinematic tree. It follows REP-103
+        conventions (i.e., X+ forward, Y+ left, Z+ up).
+        Since some kinematic solvers forbid having inertia values attached to a root link, we've
+        added the base_link_inertia link that carries the visual, collision and inertia
+        information.
+    -->
+  <link name="base_link"/>
+  <link name="base_link_inertia">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/visual/base.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/collision/base.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="4.0"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
+    </inertial>
+  </link>
+  <link name="shoulder_link">
+    <visual>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/visual/shoulder.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/collision/shoulder.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="3.761"/>
+      <origin rpy="1.570796326794897 0 0" xyz="0.0 -0.00193 -0.02561"/>
+      <inertia ixx="0.0070021" ixy="7.3e-07" ixz="-1.053e-05" iyy="0.00648091" iyz="0.00049994" izz="0.00657286"/>
+    </inertial>
+  </link>
+  <link name="upper_arm_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.138"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/visual/upperarm.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.138"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/collision/upperarm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="8.058"/>
+      <origin rpy="0 0 0" xyz="-0.2125 0.0 0.11336"/>
+      <inertia ixx="0.01505885" ixy="-5.4e-05" ixz="5.63e-06" iyy="0.33388086" iyz="-1.81e-06" izz="0.33247207"/>
+    </inertial>
+  </link>
+  <link name="forearm_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.007"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/visual/forearm.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.007"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/collision/forearm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="2.846"/>
+      <origin rpy="0 0 0" xyz="-0.2422 0.0 0.0265"/>
+      <inertia ixx="0.00399632" ixy="-1.365e-05" ixz="0.00137272" iyy="0.07879254" iyz="-6.6e-06" izz="0.0784851"/>
+    </inertial>
+  </link>
+  <link name="wrist_1_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.127"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/visual/wrist1.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.127"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/collision/wrist1.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.37"/>
+      <origin rpy="1.570796326794897 0 0" xyz="0.0 -0.01634 -0.0018"/>
+      <inertia ixx="0.00165491" ixy="-2.82e-06" ixz="-4.38e-06" iyy="0.00135962" iyz="0.00010157" izz="0.00126279"/>
+    </inertial>
+  </link>
+  <link name="wrist_2_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.0997"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/visual/wrist2.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.0997"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/collision/wrist2.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.3"/>
+      <origin rpy="-1.570796326794897 0 0" xyz="0.0 0.01634 -0.0018"/>
+      <inertia ixx="0.00135617" ixy="-2.74e-06" ixz="4.44e-06" iyy="0.00127827" iyz="-5.048e-05" izz="0.00096614"/>
+    </inertial>
+  </link>
+  <link name="wrist_3_link">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 -0.0005 -0.0989"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/visual/wrist3.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 -0.0005 -0.0989"/>
+      <geometry>
+        <mesh filename="package://ur_description/meshes/ur5e/collision/wrist3.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.365"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 -0.001159"/>
+      <inertia ixx="0.00018694" ixy="6e-08" ixz="-1.7e-07" iyy="0.00018908" iyz="-9.2e-07" izz="0.00025756"/>
+    </inertial>
+  </link>
+  <!-- base_joint fixes base_link to the environment -->
+  <joint name="base_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="world"/>
+    <child link="base_link"/>
+  </joint>
+  <!-- joints - main serial chain -->
+  <joint name="base_link-base_link_inertia" type="fixed">
+    <parent link="base_link"/>
+    <child link="base_link_inertia"/>
+    <!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
+           frames of the robot/controller have X+ pointing backwards.
+           Use the joint between 'base_link' and 'base_link_inertia' (a dummy
+           link/frame) to introduce the necessary rotation over Z (of pi rad).
+      -->
+    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+  </joint>
+  <joint name="shoulder_pan_joint" type="revolute">
+    <parent link="base_link_inertia"/>
+    <child link="shoulder_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0.1625"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="shoulder_lift_joint" type="revolute">
+    <parent link="shoulder_link"/>
+    <child link="upper_arm_link"/>
+    <origin rpy="1.570796327 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="elbow_joint" type="revolute">
+    <parent link="upper_arm_link"/>
+    <child link="forearm_link"/>
+    <origin rpy="0 0 0" xyz="-0.425 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_1_joint" type="revolute">
+    <parent link="forearm_link"/>
+    <child link="wrist_1_link"/>
+    <origin rpy="0 0 0" xyz="-0.3922 0 0.1333"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="28.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_2_joint" type="revolute">
+    <parent link="wrist_1_link"/>
+    <child link="wrist_2_link"/>
+    <origin rpy="1.570796327 0 0" xyz="0 -0.0997 -2.044881182297852e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="28.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <joint name="wrist_3_joint" type="revolute">
+    <parent link="wrist_2_link"/>
+    <child link="wrist_3_link"/>
+    <origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.0996 -2.042830148012698e-11"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="28.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
+    <dynamics damping="0" friction="0"/>
+  </joint>
+  <link name="ft_frame"/>
+  <joint name="wrist_3_link-ft_frame" type="fixed">
+    <parent link="wrist_3_link"/>
+    <child link="ft_frame"/>
+    <origin rpy="3.141592653589793 0 0" xyz="0 0 0"/>
+  </joint>
+  <!-- ROS-Industrial 'base' frame - base_link to UR 'Base' Coordinates transform -->
+  <link name="base"/>
+  <joint name="base_link-base_fixed_joint" type="fixed">
+    <!-- Note the rotation over Z of pi radians - as base_link is REP-103
+           aligned (i.e., has X+ forward, Y+ left and Z+ up), this is needed
+           to correctly align 'base' with the 'Base' coordinate system of
+           the UR controller.
+      -->
+    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="base"/>
+  </joint>
+  <!-- ROS-Industrial 'flange' frame - attachment point for EEF models -->
+  <link name="flange"/>
+  <joint name="wrist_3-flange" type="fixed">
+    <parent link="wrist_3_link"/>
+    <child link="flange"/>
+    <origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
+  </joint>
+  <!-- ROS-Industrial 'tool0' frame - all-zeros tool frame -->
+  <link name="tool0"/>
+  <joint name="flange-tool0" type="fixed">
+    <!-- default toolframe - X+ left, Y+ up, Z+ front -->
+    <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
+    <parent link="flange"/>
+    <child link="tool0"/>
+  </joint>
+</robot>