Add real-arm deployment code and final RL policy

.gitattributes

@@ -33,3 +33,27 @@ 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
+rl_deploy/calib_debug/p1_1792287841_after.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_debug/p1_1792287841_ref.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_debug/p2_1792287871_after.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_debug/p2_1792287871_ref.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_debug/p3_1792287891_after.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_debug/p3_1792287891_ref.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_debug/p4_1792287916_after.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_debug/p4_1792287916_ref.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_debug/p5_1792287936_after.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_debug/p5_1792287936_ref.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_debug/p6_1792287959_after.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_debug/p6_1792287959_ref.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_debug/p7_1792287977_after.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_debug/p7_1792287977_ref.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_debug/p8_1792287997_after.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_debug/p8_1792287997_ref.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_point_1.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_point_2.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_point_3.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_point_4.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_point_5.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_point_6.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_point_7.png filter=lfs diff=lfs merge=lfs -text
+rl_deploy/calib_point_8.png filter=lfs diff=lfs merge=lfs -text

rl_deploy/README.md

@@ -0,0 +1,47 @@
+# RL candy-pick policy on the real SO101
+
+Trained in Isaac Lab (`Isaac-SO101-CandyPick-Direct`, state-based PPO). The policy sees joint
+angles plus the blue candy's table position, which is detected once from the phone camera. So the
+phone camera has to be calibrated against the table first.
+
+None of these scripts move the arm unless you pass `--go` to `deploy_rl.py` and type `go`.
+
+## 1. Calibrate the phone against the table (once per camera/robot placement)
+
+```bash
+cd ~/B1Hackathon/tasks/candy_sort/rl_deploy && ~/miniconda3/envs/lerobot/bin/python calibrate_table.py --points 8
+```
+The follower's torque is switched off. For each point, put one blue candy on the table and press
+Enter. Then move the arm by hand until the gripper tip touches the middle of the candy, and press
+Enter. Spread the points over the area where candies go. The script saves `table_calib.json` and
+prints the fit error, which should be under 2 cm.
+
+## 2. Get the policy (export runs on the L40S)
+
+```bash
+ssh drishti-karma-devarshi 'cd ~/work/twin && ~/miniconda3/envs/lerobot/bin/python export_policy.py $(ls -d ~/work/IsaacLab/logs/rsl_rl/so101_candy_pick/*v2* | tail -1)/model_1400.pt ~/work/twin/rl_policy.pt'
+```
+```bash
+scp drishti-karma-devarshi:work/twin/rl_policy.pt ~/B1Hackathon/tasks/candy_sort/rl_deploy/policy.pt
+```
+Replace `model_1400.pt` with whichever checkpoint the thread recommends.
+
+## 3. Run
+
+Dry run first. It detects the candy, prints its table position and the targets it would send, and
+does not move the arm:
+```bash
+cd ~/B1Hackathon/tasks/candy_sort/rl_deploy && ~/miniconda3/envs/lerobot/bin/python deploy_rl.py --policy policy.pt
+```
+Real run (the arm moves for 10 s; keep a hand on the power switch):
+```bash
+cd ~/B1Hackathon/tasks/candy_sort/rl_deploy && ~/miniconda3/envs/lerobot/bin/python deploy_rl.py --policy policy.pt --go
+```
+
+## Notes
+- Candies must lie in the trained area. That's where you picked them in the grid recordings, in
+  front of the arm: about x -0.03..0.15 m and y -0.29..-0.15 m in the policy frame. The towel stays
+  where it was during those recordings.
+- `so101_fk.py` is the arm's forward kinematics, from the official URDF. Its tip positions agree with
+  the simulator's to 1 cm (median) on 146 real poses.
+- Only one blue candy is targeted per run. Candies the calibration places on the towel are ignored.

rl_deploy/calib_points.json

@@ -0,0 +1,162 @@
+[
+ {
+  "uv": [
+   283.3632596685083,
+   232.50552486187846
+  ],
+  "area": 724,
+  "joints_deg": [
+   -22.681318681318682,
+   -1.2307692307692308,
+   41.18681318681319,
+   39.07692307692308,
+   -2.5054945054945055,
+   1.1026878015161956
+  ],
+  "tip": [
+   0.0885746699621676,
+   -0.2211262158968657,
+   -0.0018728227502649436
+  ]
+ },
+ {
+  "uv": [
+   223.27258566978193,
+   174.84112149532712
+  ],
+  "area": 642,
+  "joints_deg": [
+   -6.065934065934066,
+   13.098901098901099,
+   21.846153846153847,
+   39.42857142857143,
+   -2.4175824175824174,
+   1.1026878015161956
+  ],
+  "tip": [
+   0.04521679450712202,
+   -0.28363020045294185,
+   0.00034617923123554314
+  ]
+ },
+ {
+  "uv": [
+   324.1647940074906,
+   146.14606741573033
+  ],
+  "area": 534,
+  "joints_deg": [
+   10.54945054945055,
+   -5.450549450549451,
+   41.714285714285715,
+   58.59340659340659,
+   -2.32967032967033,
+   1.1026878015161956
+  ],
+  "tip": [
+   -0.009996185007877991,
+   -0.19178363754922642,
+   0.006818864733087288
+  ]
+ },
+ {
+  "uv": [
+   323.44,
+   294.63470588235293
+  ],
+  "area": 1700,
+  "joints_deg": [
+   -41.67032967032967,
+   7.824175824175824,
+   25.45054945054945,
+   52.879120879120876,
+   -2.5934065934065935,
+   1.1026878015161956
+  ],
+  "tip": [
+   0.14299164473120482,
+   -0.19023792065397205,
+   0.003980910785919246
+  ]
+ },
+ {
+  "uv": [
+   446.7759784075574,
+   253.0425101214575
+  ],
+  "area": 1482,
+  "joints_deg": [
+   -52.30769230769231,
+   -23.64835164835165,
+   56.30769230769231,
+   63.51648351648352,
+   84.26373626373626,
+   1.1026878015161956
+  ],
+  "tip": [
+   0.09268488377845423,
+   -0.1230287165562727,
+   0.013203728373691447
+  ]
+ },
+ {
+  "uv": [
+   177.62869198312237,
+   300.67148884870403
+  ],
+  "area": 1659,
+  "joints_deg": [
+   -32.879120879120876,
+   30.24175824175824,
+   -1.2747252747252746,
+   42.15384615384615,
+   -17.53846153846154,
+   1.1716057891109581
+  ],
+  "tip": [
+   0.17211300438132343,
+   -0.2762982688066562,
+   -0.005484126509456877
+  ]
+ },
+ {
+  "uv": [
+   203.57355679702047,
+   143.64245810055866
+  ],
+  "area": 537,
+  "joints_deg": [
+   3.6043956043956045,
+   25.406593406593405,
+   5.934065934065934,
+   46.81318681318681,
+   -16.923076923076923,
+   1.1716057891109581
+  ],
+  "tip": [
+   0.00809345599756148,
+   -0.30200666202286097,
+   -0.007828562295707622
+  ]
+ },
+ {
+  "uv": [
+   326.9448818897638,
+   185.59186351706037
+  ],
+  "area": 762,
+  "joints_deg": [
+   -11.516483516483516,
+   -11.516483516483516,
+   47.07692307692308,
+   57.45054945054945,
+   -17.36263736263736,
+   1.1026878015161956
+  ],
+  "tip": [
+   0.0432111876213237,
+   -0.18203129452029243,
+   0.008625103296823854
+  ]
+ }
+]

rl_deploy/calibrate_table.py

@@ -0,0 +1,143 @@
+"""Calibrate the phone camera against the table, using the arm itself as the measuring stick.
+
+The script never moves the arm: the follower's torque is switched OFF and you move it by hand.
+For every point it asks you to:
+  A. clear the spot (no candy there yet, hands out of view)      -> reference photo
+  B. place ONE blue candy at a new spot (hands out of view)       -> photo; only blue that is NEW
+     compared with the reference counts, so blue things already in the scene are ignored
+  C. check the detection (preview saved as calib_point_N.png) and confirm it
+  D. touch the gripper tip to the middle of the candy             -> joint angles -> tip position
+Points are saved after every step to calib_points.json, so a crash loses nothing: run again with
+--resume to continue. With 6+ points it fits the pixel -> table map and saves table_calib.json.
+
+    cd ~/B1Hackathon/tasks/candy_sort/rl_deploy && ~/miniconda3/envs/lerobot/bin/python calibrate_table.py --points 8
+    ~/miniconda3/envs/lerobot/bin/python calibrate_table.py --points 8 --resume     # continue after a crash
+"""
+
+import argparse
+import json
+import os
+import time
+
+import cv2
+import numpy as np
+
+from common import CALIB, HERE, Phone, apply_h, blue_blobs, blue_mask, blue_report, connect_robot, fit_homography, read_joints
+from so101_fk import SO101FK
+
+POINTS = os.path.join(HERE, "calib_points.json")
+MIN_PIXEL_GAP = 20  # a new point must be at least this far (px) from earlier ones
+
+p = argparse.ArgumentParser()
+p.add_argument("--points", type=int, default=8)
+p.add_argument("--resume", action="store_true", help="keep the points in calib_points.json and add more")
+p.add_argument("--phone-url", default=None, help="phone stream URL (default: PHONE_URL env or http://127.0.0.1:4747/video)")
+a = p.parse_args()
+
+pts = json.load(open(POINTS)) if (a.resume and os.path.exists(POINTS)) else []
+if pts:
+    print(f"Resuming with {len(pts)} saved point(s).")
+elif os.path.exists(POINTS):
+    print(f"Starting fresh (the old {os.path.basename(POINTS)} will be overwritten; use --resume to keep it).")
+
+
+def save():
+    json.dump(pts, open(POINTS, "w"), indent=1)
+
+
+def fit_and_report():
+    uv = np.array([q["uv"] for q in pts])
+    xy = np.array([q["tip"][:2] for q in pts])
+    H = fit_homography(uv, xy)
+    err = np.linalg.norm(apply_h(H, uv) - xy, axis=1)
+    loo = []  # leave-one-out: how well each point is predicted by the others
+    for i in range(len(pts)):
+        m = np.arange(len(pts)) != i
+        loo.append(np.linalg.norm(apply_h(fit_homography(uv[m], xy[m]), uv[i:i + 1])[0] - xy[i]))
+    print("\nPoint  pixel (u, v)      tip (x, y, z) [m]          fit err  leave-one-out err")
+    for i, q in enumerate(pts):
+        print(f"{i + 1:5d}  ({q['uv'][0]:6.1f}, {q['uv'][1]:5.1f})  ({q['tip'][0]:+.3f}, {q['tip'][1]:+.3f}, {q['tip'][2]:+.3f})"
+              f"   {1000 * err[i]:5.1f} mm   {1000 * loo[i]:6.1f} mm")
+    print(f"Fit error median {1000 * np.median(err):.1f} mm, max {1000 * err.max():.1f} mm; "
+          f"leave-one-out median {1000 * np.median(loo):.1f} mm, max {1000 * max(loo):.1f} mm")
+    return H, err, loo
+
+
+fk = SO101FK()
+robot = connect_robot()
+robot.bus.disable_torque()
+print("\nFollower torque is OFF: you move the arm by hand. Nothing in this script moves it.")
+print("Hold the arm when the script ends: it stays limp.\n")
+phone = Phone(a.phone_url) if a.phone_url else Phone()
+try:
+    while len(pts) < a.points:
+        k = len(pts) + 1
+        print(f"\n===== Point {k}/{a.points} =====")
+        input(f"[A] Make sure there is NO candy at the spot you will use next and your hands are out of view.\n"
+              f"    Press Enter to take the reference photo...")
+        ref = phone.frame()
+        input("[B] Put ONE blue candy at a new spot in the candy area (not where earlier points were),\n"
+              "    take your hands out of view, press Enter...")
+        img = phone.frame()
+        new = sorted(blue_blobs(img, ignore=ref), key=lambda b: -b["area"])
+        allb = blue_blobs(img)
+        # diagnostics: raw photos and blue masks are saved for every attempt
+        dd = os.path.join(HERE, "calib_debug")
+        os.makedirs(dd, exist_ok=True)
+        tag = f"p{k}_{int(time.time())}"
+        cv2.imwrite(os.path.join(dd, f"{tag}_ref.png"), cv2.cvtColor(ref, cv2.COLOR_RGB2BGR))
+        cv2.imwrite(os.path.join(dd, f"{tag}_after.png"), cv2.cvtColor(img, cv2.COLOR_RGB2BGR))
+        cv2.imwrite(os.path.join(dd, f"{tag}_bluemask.png"), blue_mask(img))
+        rep = blue_report(img)
+        diff = float(np.abs(img.astype(np.int16) - ref.astype(np.int16)).mean())
+        print(f"    photo {img.shape[1]}x{img.shape[0]}, brightness {rep['mean_brightness']:.0f}, change vs reference {diff:.1f} "
+              f"(near 0 = the two photos are the same frame)")
+        print(f"    blue pixels {rep['blue_px']}; largest blue patch {rep['largest_area']} px at {rep['largest_at']} "
+              f"(blobs under 25 px are ignored); photos saved in calib_debug/{tag}_*.png")
+        print(f"    blue blobs in the photo: {len(allb)}; NEW since the reference photo: {len(new)}")
+        for j, b in enumerate(new):
+            print(f"      new blob {j + 1}: pixel ({b['uv'][0]:.0f}, {b['uv'][1]:.0f}), {b['area']} px")
+        if not new:
+            print("    No new blue candy found. Check it is in view and not hidden by the arm, then redo this point.")
+            continue
+        b = new[0]
+        near = [i + 1 for i, q in enumerate(pts) if np.linalg.norm(np.array(q["uv"]) - b["uv"]) < MIN_PIXEL_GAP]
+        if near:
+            print(f"    This is the same spot as point(s) {near}. Move the candy somewhere new and redo this point.")
+            continue
+        dbg = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
+        for j, bb in enumerate(new):
+            cv2.circle(dbg, tuple(int(v) for v in bb["uv"]), 8, (0, 0, 255) if j == 0 else (0, 255, 255), 2)
+        prev = os.path.join(HERE, f"calib_point_{k}.png")
+        cv2.imwrite(prev, dbg)
+        print(f"[C] Using new blob 1 (red circle in {os.path.basename(prev)}).")
+        if input("    Is that the candy you placed? [Y/n] ").strip().lower().startswith("n"):
+            print("    Skipped. Redo this point.")
+            continue
+        input("[D] Move the gripper by hand until its tip touches the MIDDLE of that candy, press Enter...")
+        q = read_joints(robot)
+        tip = fk.tcp(q)
+        print(f"    joints [deg] {np.round(q, 1).tolist()}")
+        print(f"    tip position {np.round(tip, 3).tolist()} m (z should be about 0.00 to 0.02)")
+        if not -0.03 < tip[2] < 0.04:
+            print("    The tip is not at table height: lower the gripper onto the candy and redo this point.")
+            continue
+        pts.append({"uv": b["uv"].tolist(), "area": b["area"], "joints_deg": q.tolist(), "tip": tip.tolist()})
+        save()
+        print(f"    Saved point {k} to {os.path.basename(POINTS)}. You can now remove the candy.")
+        if len(pts) >= 4:
+            fit_and_report()
+finally:
+    phone.close()
+    robot.disconnect()
+
+if len(pts) < 6:
+    raise SystemExit(f"Only {len(pts)} points; need at least 6. Run again with --resume.")
+H, err, loo = fit_and_report()
+json.dump({"H": H.tolist(), "image_size": [640, 360], "points": pts, "err_mm": (1000 * err).tolist(),
+           "loo_mm": [1000 * v for v in loo]}, open(CALIB, "w"), indent=1)
+print(f"\nSaved {CALIB}")
+if max(loo) > 0.02:
+    worst = int(np.argmax(loo)) + 1
+    print(f"Leave-one-out error above 2 cm (worst: point {worst}). Delete it from {os.path.basename(POINTS)} "
+          f"and add a point with --resume, or add more points.")

rl_deploy/common.py

@@ -0,0 +1,164 @@
+"""Shared pieces for the RL policy on the real SO101: robot/camera setup, blue-candy detection,
+and the phone-pixel -> table homography."""
+
+import json
+import os
+import time
+
+import cv2
+import numpy as np
+
+HERE = os.path.dirname(os.path.abspath(__file__))
+CALIB = os.path.join(HERE, "table_calib.json")
+FOLLOWER_PORT = "/dev/serial/by-id/usb-1a86_USB_Single_Serial_5AE6057813-if00"
+FOLLOWER_ID = "my_follower_arm"
+PHONE_URL = os.environ.get("PHONE_URL", "http://127.0.0.1:4747/video")  # DroidCam on localhost; PHONE_URL=... to override
+IMG_W, IMG_H = 640, 360  # the phone view is used at this size (same as the half-res datasets)
+JOINTS = ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"]
+
+
+def connect_robot():
+    from lerobot.robots.so_follower.config_so_follower import SOFollowerRobotConfig
+    from lerobot.robots.so_follower.so_follower import SOFollower
+
+    robot = SOFollower(SOFollowerRobotConfig(port=FOLLOWER_PORT, id=FOLLOWER_ID))
+    robot.connect(calibrate=False)
+    return robot
+
+
+def read_joints(robot):
+    obs = robot.get_observation()
+    return np.array([obs[f"{n}.pos"] for n in JOINTS], dtype=np.float64)
+
+
+class Phone:
+    """DroidCam stream, resized to 640x360 RGB.
+
+    A background thread reads the stream continuously and keeps only the newest frame, so a photo
+    is always current (OpenCV otherwise hands back frames buffered seconds earlier, which made the
+    "before" and "after" photos identical). Reconnects if the HTTP stream drops."""
+
+    def __init__(self, url=PHONE_URL):
+        import threading
+
+        self.url = url
+        self.cap = None
+        self.latest, self.stamp, self.count = None, 0.0, 0
+        self.lock = threading.Lock()
+        self.running = True
+        self._open()
+        self.thread = threading.Thread(target=self._reader, daemon=True)
+        self.thread.start()
+        t0 = time.time()
+        while self.latest is None and time.time() - t0 < 10:
+            time.sleep(0.05)
+        if self.latest is None:
+            raise RuntimeError(f"no frames from {self.url} within 10 s")
+
+    def _open(self):
+        if self.cap is not None:
+            self.cap.release()
+        self.cap = cv2.VideoCapture(self.url)
+        if not self.cap.isOpened():
+            raise RuntimeError(f"cannot open phone stream {self.url} (is DroidCam running? close any browser tab showing it)")
+
+    def _reader(self):
+        fails = 0
+        while self.running:
+            ok, img = self.cap.read()
+            if ok and img is not None:
+                fails = 0
+                with self.lock:
+                    self.latest, self.stamp = img, time.time()
+                    self.count += 1
+                continue
+            fails += 1
+            print(f"  (phone stream dropped, reconnecting: attempt {fails})")
+            time.sleep(1.0)
+            try:
+                self._open()
+            except RuntimeError as e:
+                print("  ", e)
+
+    def frame(self, max_age=0.5, timeout=10.0):
+        """Newest frame (taken after this call started), 640x360 RGB."""
+        t_call = time.time()
+        while time.time() - t_call < timeout:
+            with self.lock:
+                img, stamp = self.latest, self.stamp
+            if img is not None and stamp > t_call and time.time() - stamp < max_age:
+                return cv2.cvtColor(cv2.resize(img, (IMG_W, IMG_H), interpolation=cv2.INTER_AREA), cv2.COLOR_BGR2RGB)
+            time.sleep(0.02)
+        raise RuntimeError("no fresh phone frame for %.0f s; check the DroidCam app and connection" % timeout)
+
+    def close(self):
+        self.running = False
+        self.thread.join(timeout=2)
+        self.cap.release()
+
+
+def blue_mask(rgb):
+    hsv = cv2.cvtColor(rgb, cv2.COLOR_RGB2HSV)
+    m = cv2.inRange(hsv, (95, 90, 70), (125, 255, 255))  # OpenCV hue 0..180: blue ~ 100-125
+    return cv2.morphologyEx(m, cv2.MORPH_OPEN, np.ones((3, 3), np.uint8))
+
+
+def blue_report(rgb):
+    """Why a photo has (no) blue blobs: blue pixel count and the largest blue component."""
+    m = blue_mask(rgb)
+    n, _, stats, cent = cv2.connectedComponentsWithStats(m)
+    big = max(range(1, n), key=lambda i: stats[i, cv2.CC_STAT_AREA], default=None)
+    hsv = cv2.cvtColor(rgb, cv2.COLOR_RGB2HSV)
+    return {"blue_px": int((m > 0).sum()), "largest_area": int(stats[big, cv2.CC_STAT_AREA]) if big else 0,
+            "largest_at": cent[big].round(0).tolist() if big else None, "mean_brightness": float(hsv[..., 2].mean())}
+
+
+def blue_blobs(rgb, min_px=25, ignore=None):
+    """Blue wrapper blobs: list of dicts with pixel centroid, area and the blob's long-axis angle.
+    ``ignore``: a reference image; blue that was already there (dilated a little) is left out."""
+    m = blue_mask(rgb)
+    if ignore is not None:
+        m = cv2.bitwise_and(m, cv2.bitwise_not(cv2.dilate(blue_mask(ignore), np.ones((9, 9), np.uint8))))
+    n, lab, stats, cent = cv2.connectedComponentsWithStats(m)
+    out = []
+    for i in range(1, n):
+        if stats[i, cv2.CC_STAT_AREA] < min_px:
+            continue
+        ys, xs = np.nonzero(lab == i)
+        pts = np.c_[xs, ys].astype(np.float64)
+        c = pts.mean(0)
+        evals, evecs = np.linalg.eigh(np.cov((pts - c).T))
+        major = evecs[:, np.argmax(evals)]
+        out.append({"uv": c, "area": int(stats[i, cv2.CC_STAT_AREA]), "axis_uv": major})
+    return out
+
+
+def fit_homography(uv, xy):
+    """Least-squares DLT homography (u, v) -> (x, y)."""
+    A = []
+    for (u, v), (x, y) in zip(uv, xy):
+        A.append([-u, -v, -1, 0, 0, 0, x * u, x * v, x])
+        A.append([0, 0, 0, -u, -v, -1, y * u, y * v, y])
+    _, _, Vt = np.linalg.svd(np.array(A))
+    return Vt[-1].reshape(3, 3) / Vt[-1, -1]
+
+
+def apply_h(H, uv):
+    uv = np.atleast_2d(uv)
+    q = np.c_[uv, np.ones(len(uv))] @ H.T
+    return q[:, :2] / q[:, 2:3]
+
+
+def load_calib():
+    if not os.path.exists(CALIB):
+        raise SystemExit(f"No {CALIB}. Run calibrate_table.py first.")
+    return np.array(json.load(open(CALIB))["H"])
+
+
+def candy_on_table(H, blob):
+    """Table x, y [m] and yaw [rad] of a blob (yaw from the blob's long axis mapped onto the table)."""
+    c = blob["uv"]
+    p0, p1 = apply_h(H, [c - 8 * blob["axis_uv"], c + 8 * blob["axis_uv"]])
+    d = p1 - p0
+    xy = apply_h(H, c)[0]
+    return xy, float(np.arctan2(d[1], d[0]))

rl_deploy/deploy_rl.py

@@ -0,0 +1,156 @@
+"""Run the Isaac Lab RL candy-pick policy on the real SO101 (state-based: joints + detected candy).
+
+Same observation and action conventions as Isaac-SO101-CandyPick-Direct:
+  obs  = arm joints [rad] + gripper opening [0..1], joint velocities x0.1, candy (x, y, sin2yaw,
+         cos2yaw) from the phone at the start, towel (x, y), last action, episode time fraction
+  act  = 5 arm-joint target deltas (x0.06 rad per 30 Hz step, clamped to the real-demo ranges)
+         + gripper command (-1 closed .. +1 = LeRobot 45 open)
+
+By default it only detects the candy and prints what it would do (DRY RUN, arm not moved).
+Add --go to move the arm; you are asked to confirm first. Keep a hand near the power switch.
+
+    cd ~/B1Hackathon/tasks/candy_sort/rl_deploy
+    ~/miniconda3/envs/lerobot/bin/python deploy_rl.py --policy policy.pt            # dry run
+    ~/miniconda3/envs/lerobot/bin/python deploy_rl.py --policy policy.pt --go       # moves the arm
+Safety (real arm only): targets change by at most --max-step-deg per step (default 1 deg = 30 deg/s,
+about a third of what the sim policy may command) and actions are low-pass filtered (--smooth).
+Every run saves ../eval_videos/rl_<policy>_<time>.mp4 (phone + wrist) and a .csv of measured vs
+commanded joints, unless --no-video.
+"""
+
+import argparse
+import time
+
+import numpy as np
+import torch
+
+from common import HERE, JOINTS, Phone, blue_blobs, candy_on_table, connect_robot, load_calib, read_joints
+
+WRIST_DEV = "/dev/v4l/by-id/usb-Ausha_Webcam_Ausha_Webcam-video-index0"
+
+# Must match so101_candy/candy_env_cfg.py
+REAL_ARM_RANGE_DEG = np.array([(-44.9, 80.9), (-102.9, 32.6), (-12.2, 93.1), (0.2, 104.3), (-61.1, 99.7)])
+ARM_SCALE_RAD = 0.06
+GRIP_OPEN_LR, GRIP_CLOSED_LR = 45.0, 0.0
+GRIP_LO, GRIP_HI = -0.174533, 1.74533  # sim gripper joint limits [rad] (LeRobot 0..100 maps onto them)
+TOWEL_XY = (-0.20, -0.17)
+TOWEL_HALF = (0.08, 0.12)
+CANDY_X, CANDY_Y = (-0.06, 0.18), (-0.32, -0.12)  # spawn box of the sim task, with some margin
+EPISODE_STEPS = 300  # 10 s at 30 Hz
+HZ = 30.0
+
+p = argparse.ArgumentParser()
+p.add_argument("--policy", required=True, help="TorchScript policy exported from Isaac Lab (policy.pt)")
+p.add_argument("--go", action="store_true", help="actually move the arm")
+p.add_argument("--seconds", type=float, default=10.0)
+p.add_argument("--max-step-deg", type=float, default=1.0,
+               help="safety: max change of any joint target per 30 Hz step [deg] (sim policy allows ~3.4)")
+p.add_argument("--smooth", type=float, default=0.5, help="safety: action low-pass, 0 = off, 0.5 = average with last")
+p.add_argument("--no-video", action="store_true", help="don't save the phone/wrist video and the per-step log")
+p.add_argument("--phone-url", default=None, help="phone stream URL (default: PHONE_URL env or http://127.0.0.1:4747/video)")
+a = p.parse_args()
+
+policy = torch.jit.load(a.policy, map_location="cpu").eval()
+H = load_calib()
+
+# 1) Detect the target candy (blue, not already on the towel, inside the trained workspace).
+phone = Phone(a.phone_url) if a.phone_url else Phone()
+img = phone.frame()
+cands = []
+for b in blue_blobs(img):
+    xy, yaw = candy_on_table(H, b)
+    on_towel = abs(xy[0] - TOWEL_XY[0]) < TOWEL_HALF[0] and abs(xy[1] - TOWEL_XY[1]) < TOWEL_HALF[1]
+    in_ws = CANDY_X[0] <= xy[0] <= CANDY_X[1] and CANDY_Y[0] <= xy[1] <= CANDY_Y[1]
+    print(f"blue blob at pixel {b['uv'].round(0)} -> table {xy.round(3)} m, yaw {np.degrees(yaw):.0f} deg"
+          f"{' (on towel)' if on_towel else ''}{' (outside trained area)' if not in_ws else ''}")
+    if not on_towel and in_ws:
+        cands.append((xy, yaw))
+if not cands:
+    raise SystemExit("No blue candy inside the trained workspace. Move one closer to the arm and retry.")
+xy, yaw = min(cands, key=lambda c: np.linalg.norm(c[0] - np.array([0.06, -0.22])))
+candy_obs = np.array([xy[0], xy[1], np.sin(2 * yaw), np.cos(2 * yaw)])
+print(f"target candy: {xy.round(3)} m, yaw {np.degrees(yaw):.0f} deg")
+
+robot = connect_robot()
+q = read_joints(robot)
+print("current joints [deg]:", q.round(1))
+if a.go:
+    if input("The arm WILL move for %.0f s. Type 'go' to start: " % a.seconds).strip() != "go":
+        robot.disconnect()
+        raise SystemExit("aborted")
+else:
+    print("DRY RUN: computing actions without sending them (add --go to move the arm)")
+
+# Documentation: phone (+ wrist if available) video and a per-step CSV of commanded vs measured joints.
+import csv
+import os
+from datetime import datetime
+
+import cv2
+
+rec = None
+if not a.no_video:
+    os.makedirs(os.path.join(HERE, "..", "eval_videos"), exist_ok=True)
+    stem = os.path.join(HERE, "..", "eval_videos", f"rl_{os.path.splitext(os.path.basename(a.policy))[0]}_{datetime.now():%Y%m%d_%H%M%S}"
+                        + ("" if a.go else "_dryrun"))
+    wrist = cv2.VideoCapture(WRIST_DEV)
+    wrist = wrist if wrist.isOpened() else None
+    vw = cv2.VideoWriter(stem + ".mp4", cv2.VideoWriter_fourcc(*"mp4v"), HZ, (640 + (320 if wrist else 0), 360))
+    logf = open(stem + ".csv", "w", newline="")
+    log = csv.writer(logf)
+    log.writerow(["t"] + [f"meas_{n}" for n in JOINTS] + [f"target_{n}" for n in JOINTS] + [f"act_{i}" for i in range(6)])
+    rec = (vw, wrist, log, logf)
+    print(f"recording to {stem}.mp4 / .csv")
+
+target = q.copy()  # LeRobot units
+last_act = np.zeros(6)
+prev_cmd = np.zeros(6)
+prev_q, prev_t = q.copy(), time.perf_counter()
+steps = int(a.seconds * HZ)
+try:
+    for k in range(steps):
+        t0 = time.perf_counter()
+        q = read_joints(robot)
+        dt = max(t0 - prev_t, 1e-3)
+        q_rad = np.deg2rad(q[:5])
+        grip_open = q[5] / 100.0
+        vel = np.r_[np.deg2rad(q[:5] - prev_q[:5]), (q[5] - prev_q[5]) / 100.0 * (GRIP_HI - GRIP_LO)] / dt
+        prev_q, prev_t = q.copy(), t0
+        obs = np.r_[q_rad, grip_open, 0.1 * vel, candy_obs, TOWEL_XY, last_act, min(k, EPISODE_STEPS) / EPISODE_STEPS]
+        with torch.no_grad():
+            act = policy(torch.tensor(obs, dtype=torch.float32)[None])[0].numpy()
+        act = np.clip(act, -1.0, 1.0)
+        last_act = act  # the policy sees its own (unsmoothed) action, as in sim
+        cmd = a.smooth * prev_cmd + (1.0 - a.smooth) * act if a.smooth > 0 else act
+        prev_cmd = cmd
+        step = np.clip(np.rad2deg(ARM_SCALE_RAD * cmd[:5]), -a.max_step_deg, a.max_step_deg)
+        target[:5] = np.clip(target[:5] + step, REAL_ARM_RANGE_DEG[:, 0], REAL_ARM_RANGE_DEG[:, 1])
+        act = cmd
+        target[5] = GRIP_CLOSED_LR + (GRIP_OPEN_LR - GRIP_CLOSED_LR) * (act[5] + 1.0) / 2.0
+        if a.go:
+            robot.send_action({f"{n}.pos": float(v) for n, v in zip(
+                ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"], target)})
+        elif k % 15 == 0:
+            print(f"t={k / HZ:4.1f}s target [deg]: {target.round(1)}")
+        if rec:
+            vw, wrist, log, _ = rec
+            frame = cv2.cvtColor(phone.frame(max_age=1.0), cv2.COLOR_RGB2BGR)
+            if wrist is not None:
+                ok, w = wrist.read()
+                w = cv2.resize(w, (320, 240)) if ok else np.zeros((240, 320, 3), np.uint8)
+                frame = np.concatenate([frame, np.vstack([w, np.zeros((120, 320, 3), np.uint8)])], axis=1)
+            cv2.putText(frame, f"t={k / HZ:4.1f}s {'GO' if a.go else 'DRY'}", (10, 25), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
+            vw.write(frame)
+            log.writerow([round(k / HZ, 3)] + q.round(2).tolist() + target.round(2).tolist() + np.round(act, 3).tolist())
+        time.sleep(max(0.0, 1.0 / HZ - (time.perf_counter() - t0)))
+except KeyboardInterrupt:
+    print("\nstopped by Ctrl-C")
+finally:
+    robot.disconnect()
+    phone.close()
+    if rec:
+        rec[0].release()
+        if rec[1] is not None:
+            rec[1].release()
+        rec[3].close()
+        print("saved video and per-step log next to", os.path.basename(stem))

rl_deploy/export_policy.py

@@ -0,0 +1,42 @@
+"""Turn an rsl_rl checkpoint (model_XXXX.pt) of Isaac-SO101-CandyPick-Direct into a TorchScript policy.
+
+The exported module takes the raw 25-d observation and returns the deterministic action (mean),
+with the observation normalizer baked in. No Isaac Sim needed, so it can run while training.
+
+    python export_policy.py <run dir>/model_1200.pt policy.pt
+"""
+
+import sys
+
+import torch
+from torch import nn
+
+EPS = 1e-2  # rsl_rl EmpiricalNormalization
+
+
+class Policy(nn.Module):
+    def __init__(self, sd):
+        super().__init__()
+        self.register_buffer("mean", sd["obs_normalizer._mean"].clone())
+        self.register_buffer("std", sd["obs_normalizer._std"].clone())
+        layers, i = [], 0
+        while f"mlp.{i}.weight" in sd:
+            w = sd[f"mlp.{i}.weight"]
+            lin = nn.Linear(w.shape[1], w.shape[0])
+            lin.weight.data.copy_(w)
+            lin.bias.data.copy_(sd[f"mlp.{i}.bias"])
+            layers.append(lin)
+            if f"mlp.{i + 2}.weight" in sd:
+                layers.append(nn.ELU())
+            i += 2
+        self.mlp = nn.Sequential(*layers)
+
+    def forward(self, obs: torch.Tensor) -> torch.Tensor:
+        return self.mlp((obs - self.mean) / (self.std + 1e-2))  # EPS
+
+
+ckpt = torch.load(sys.argv[1], map_location="cpu", weights_only=False)
+pol = Policy(ckpt["actor_state_dict"]).eval()
+ts = torch.jit.script(pol)
+ts.save(sys.argv[2])
+print("saved", sys.argv[2], "from iteration", ckpt.get("iter"), "| obs dim", pol.mean.shape[-1])

rl_deploy/policy.pt

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

rl_deploy/policy_final.pt

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1188b99a9d7ea964525e1a63286635d08c3b6c189ec891ca305bd3bf6997758c
+size 206389

rl_deploy/so101_fk.py

@@ -0,0 +1,83 @@
+"""Forward kinematics of the SO101 from the official so101_new_calib URDF (numpy only).
+
+Joint angles are LeRobot degrees (0 = middle of the calibrated range), which map 1:1 onto the URDF
+joints in radians. Positions are returned in the frame the RL policy uses: the robot base on the
+table, z up, with the arm's zero pan pointing along -y (Isaac Lab's so101_new_calib USD, mounted
+3 cm into the table as in the sim task). See _to_policy_frame.
+"""
+
+import os
+import xml.etree.ElementTree as ET
+
+import numpy as np
+
+URDF = os.path.join(os.path.dirname(os.path.abspath(__file__)), "so101_new_calib.urdf")
+ARM = ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"]
+
+
+def _rpy(r, p, y):
+    cr, sr, cp, sp, cy, sy = np.cos(r), np.sin(r), np.cos(p), np.sin(p), np.cos(y), np.sin(y)
+    return np.array([[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
+                     [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
+                     [-sp, cp * sr, cp * cr]])
+
+
+def _axis_angle(axis, a):
+    axis = np.asarray(axis, float) / np.linalg.norm(axis)
+    K = np.array([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]], [-axis[1], axis[0], 0]])
+    return np.eye(3) + np.sin(a) * K + (1 - np.cos(a)) * K @ K
+
+
+def _tf(R, t):
+    T = np.eye(4)
+    T[:3, :3], T[:3, 3] = R, t
+    return T
+
+
+class SO101FK:
+    def __init__(self, urdf=URDF):
+        root = ET.parse(urdf).getroot()
+        self.joints = {}
+        for j in root.findall("joint"):
+            o = j.find("origin")
+            xyz = [float(v) for v in (o.get("xyz") if o is not None else "0 0 0").split()]
+            rpy = [float(v) for v in (o.get("rpy") if o is not None else "0 0 0").split()]
+            ax = j.find("axis")
+            self.joints[j.get("name")] = {
+                "parent": j.find("parent").get("link"), "child": j.find("child").get("link"),
+                "T": _tf(_rpy(*rpy), xyz), "axis": [float(v) for v in ax.get("xyz").split()] if ax is not None else None,
+                "type": j.get("type"),
+            }
+        self.by_child = {v["child"]: k for k, v in self.joints.items()}
+
+    def link_pose(self, link, state_deg):
+        """4x4 pose of ``link`` in the policy's base frame for a LeRobot state (deg)."""
+        q = {n: np.deg2rad(state_deg[i]) for i, n in enumerate(ARM)}
+        chain = []
+        while link in self.by_child:
+            name = self.by_child[link]
+            chain.append(name)
+            link = self.joints[name]["parent"]
+        T = np.eye(4)
+        for name in reversed(chain):
+            j = self.joints[name]
+            T = T @ j["T"]
+            if j["type"] == "revolute" and name in q:
+                T = T @ _tf(_axis_angle(j["axis"], q[name]), [0, 0, 0])
+        return _to_policy_frame() @ T
+
+    def tcp(self, state_deg):
+        """Gripper tip position (``gripper_frame_link``), metres, policy base frame."""
+        return self.link_pose("gripper_frame_link", state_deg)[:3, 3]
+
+
+def _to_policy_frame():
+    # URDF base_link -> sim policy frame. Fitted (least squares, rigid) so URDF tip positions match
+    # Isaac Lab's so101_new_calib USD on 146 real grasp/release poses: about -87 deg about z plus a
+    # small offset; residual 10 mm median, 27 mm max (the two models differ slightly).
+    R = np.array([[5.790e-02, 9.983e-01, 2.300e-03],
+                  [-9.983e-01, 5.790e-02, -4.000e-04],
+                  [-5.000e-04, -2.300e-03, 1.000e00]])
+    U, _, Vt = np.linalg.svd(R)
+    # (fitted on poses already lowered by the 3 cm mount, so that offset is applied first)
+    return _tf(U @ Vt, [0.0049, -0.0160, 0.0392]) @ _tf(np.eye(3), [0.0, 0.0, -0.03008])

rl_deploy/so101_new_calib.urdf

@@ -0,0 +1,453 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Generated using onshape-to-robot -->
+<!-- Onshape https://cad.onshape.com/documents/7715cc284bb430fe6dab4ffd/w/4fd0791b683777b02f8d975a/e/826c553ede3b7592eb9ca800 -->
+<robot name="so101_new_calib">
+
+  <!-- Materials -->
+  <material name="3d_printed">
+    <color rgba="1.0 0.82 0.12 1.0"/>
+  </material>
+  <material name="sts3215">
+    <color rgba="0.1 0.1 0.1 1.0"/>
+  </material>
+
+  <!-- Link base -->
+  <link name="base_link">
+    <inertial>
+      <origin xyz="0.0137179 -5.19711e-05 0.0334843" rpy="0 0 0"/>
+      <mass value="0.147"/>
+      <inertia ixx="0.000114686" ixy="-4.59787e-07" ixz="4.97151e-06" iyy="0.000136117" iyz="9.75275e-08" izz="0.000130364"/>
+    </inertial>
+    <!-- Part base_motor_holder_so101_v1 -->
+    <visual>
+      <origin xyz="-0.00636471 -9.94414e-05 -0.0024" rpy="1.5708 -1.67685e-15 1.5708"/>
+      <geometry>
+        <mesh filename="assets/base_motor_holder_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.00636471 -9.94414e-05 -0.0024" rpy="1.5708 -1.67685e-15 1.5708"/>
+      <geometry>
+        <mesh filename="assets/base_motor_holder_so101_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part base_so101_v2 -->
+    <visual>
+      <origin xyz="-0.00636471 -8.97657e-09 -0.0024" rpy="1.5708 -2.78073e-29 1.5708"/>
+      <geometry>
+        <mesh filename="assets/base_so101_v2.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.00636471 -8.97657e-09 -0.0024" rpy="1.5708 -2.78073e-29 1.5708"/>
+      <geometry>
+        <mesh filename="assets/base_so101_v2.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part sts3215_03a_v1 -->
+    <visual>
+      <origin xyz="0.0263353 -8.97657e-09 0.0437" rpy="-8.21148e-16 7.84513e-18 1.249e-15"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0263353 -8.97657e-09 0.0437" rpy="-8.21148e-16 7.84513e-18 1.249e-15"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part waveshare_mounting_plate_so101_v2 -->
+    <visual>
+      <origin xyz="-0.0309827 -0.000199441 0.0474" rpy="1.5708 -1.35493e-14 1.5708"/>
+      <geometry>
+        <mesh filename="assets/waveshare_mounting_plate_so101_v2.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0309827 -0.000199441 0.0474" rpy="1.5708 -1.35493e-14 1.5708"/>
+      <geometry>
+        <mesh filename="assets/waveshare_mounting_plate_so101_v2.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Link shoulder -->
+  <link name="shoulder_link">
+    <inertial>
+      <origin xyz="-0.0307604 -1.66727e-05 -0.0252713" rpy="0 0 0"/>
+      <mass value="0.100006"/>
+      <inertia ixx="8.3759e-05" ixy="7.55525e-08" ixz="-1.16342e-06" iyy="8.10403e-05" iyz="1.54663e-07" izz="2.39783e-05"/>
+    </inertial>
+    <!-- Part sts3215_03a_v1_2 -->
+    <visual>
+      <origin xyz="-0.0303992 0.000422241 -0.0417" rpy="1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0303992 0.000422241 -0.0417" rpy="1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part motor_holder_so101_base_v1 -->
+    <visual>
+      <origin xyz="-0.0675992 -0.000177759 0.0158499" rpy="1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="assets/motor_holder_so101_base_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0675992 -0.000177759 0.0158499" rpy="1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="assets/motor_holder_so101_base_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part rotation_pitch_so101_v1 -->
+    <visual>
+      <origin xyz="0.0122008 2.22413e-05 0.0464" rpy="-1.5708 2.35221e-33 0"/>
+      <geometry>
+        <mesh filename="assets/rotation_pitch_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0122008 2.22413e-05 0.0464" rpy="-1.5708 2.35221e-33 0"/>
+      <geometry>
+        <mesh filename="assets/rotation_pitch_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Link upper_arm -->
+  <link name="upper_arm_link">
+    <inertial>
+      <origin xyz="-0.0898471 -0.00838224 0.0184089" rpy="0 0 0"/>
+      <mass value="0.103"/>
+      <inertia ixx="4.08002e-05" ixy="-1.97819e-05" ixz="-4.03016e-08" iyy="0.000147318" iyz="8.97326e-09" izz="0.000142487"/>
+    </inertial>
+    <!-- Part sts3215_03a_v1_3 -->
+    <visual>
+      <origin xyz="-0.11257 -0.0155 0.0187" rpy="-3.14159 -5.27356e-16 -1.5708"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.11257 -0.0155 0.0187" rpy="-3.14159 -5.27356e-16 -1.5708"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part upper_arm_so101_v1 -->
+    <visual>
+      <origin xyz="-0.065085 0.012 0.0182" rpy="3.14159 -0 -1.30911e-30"/>
+      <geometry>
+        <mesh filename="assets/upper_arm_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.065085 0.012 0.0182" rpy="3.14159 -0 -1.30911e-30"/>
+      <geometry>
+        <mesh filename="assets/upper_arm_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Link lower_arm -->
+  <link name="lower_arm_link">
+    <inertial>
+      <origin xyz="-0.0980701 0.00324376 0.0182831" rpy="0 0 0"/>
+      <mass value="0.104"/>
+      <inertia ixx="2.87438e-05" ixy="7.41152e-06" ixz="1.26409e-06" iyy="0.000159844" iyz="-4.90188e-08" izz="0.00014529"/>
+    </inertial>
+    <!-- Part under_arm_so101_v1 -->
+    <visual>
+      <origin xyz="-0.0648499 -0.032 0.0182" rpy="3.14159 -0 6.67202e-31"/>
+      <geometry>
+        <mesh filename="assets/under_arm_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0648499 -0.032 0.0182" rpy="3.14159 -0 6.67202e-31"/>
+      <geometry>
+        <mesh filename="assets/under_arm_so101_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part motor_holder_so101_wrist_v1 -->
+    <visual>
+      <origin xyz="-0.0648499 -0.032 0.018" rpy="-3.14159 -2.55351e-15 -1.83387e-30"/>
+      <geometry>
+        <mesh filename="assets/motor_holder_so101_wrist_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0648499 -0.032 0.018" rpy="-3.14159 -2.55351e-15 -1.83387e-30"/>
+      <geometry>
+        <mesh filename="assets/motor_holder_so101_wrist_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part sts3215_03a_v1_4 -->
+    <visual>
+      <origin xyz="-0.1224 0.0052 0.0187" rpy="-3.14159 -7.88861e-31 -3.14159"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.1224 0.0052 0.0187" rpy="-3.14159 -7.88861e-31 -3.14159"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Link wrist -->
+  <link name="wrist_link">
+    <inertial>
+      <origin xyz="-0.000103312 -0.0386143 0.0281156" rpy="0 0 0"/>
+      <mass value="0.079"/>
+      <inertia ixx="3.68263e-05" ixy="1.7893e-08" ixz="-5.28128e-08" iyy="2.5391e-05" iyz="3.6412e-06" izz="2.1e-05"/>
+    </inertial>
+    <!-- Part sts3215_03a_no_horn_v1 -->
+    <visual>
+      <origin xyz="8.32667e-17 -0.0424 0.0306" rpy="1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_no_horn_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="8.32667e-17 -0.0424 0.0306" rpy="1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_no_horn_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part wrist_roll_pitch_so101_v2 -->
+    <visual>
+      <origin xyz="0 -0.028 0.0181" rpy="-1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="assets/wrist_roll_pitch_so101_v2.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.028 0.0181" rpy="-1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="assets/wrist_roll_pitch_so101_v2.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Link gripper -->
+  <link name="gripper_link">
+    <inertial>
+      <origin xyz="0.000213627 0.000245138 -0.025187" rpy="0 0 0"/>
+      <mass value="0.087"/>
+      <inertia ixx="2.75087e-05" ixy="-3.35241e-07" ixz="-5.7352e-06" iyy="4.33657e-05" iyz="-5.17847e-08" izz="3.45059e-05"/>
+    </inertial>
+    <!-- Part sts3215_03a_v1_5 -->
+    <visual>
+      <origin xyz="0.0077 0.0001 -0.0234" rpy="-1.5708 -5.19179e-17 -1.66533e-16"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0077 0.0001 -0.0234" rpy="-1.5708 -5.19179e-17 -1.66533e-16"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part wrist_roll_follower_so101_v1 -->
+    <visual>
+      <origin xyz="8.32667e-17 -0.000218214 0.000949706" rpy="-3.14159 -5.55112e-17 0"/>
+      <geometry>
+        <mesh filename="assets/wrist_roll_follower_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="8.32667e-17 -0.000218214 0.000949706" rpy="-3.14159 -5.55112e-17 0"/>
+      <geometry>
+        <mesh filename="assets/wrist_roll_follower_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Gripper frame (dummy link + fixed joint) -->
+  <link name="gripper_frame_link">
+    <origin xyz="0 0 0" rpy="0 -0 0"/>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-9"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+  </link>
+
+  <joint name="gripper_frame_joint" type="fixed">
+    <origin xyz="-0.0079 -0.000218121 -0.0981274" rpy="0 3.14159 0"/>
+    <parent link="gripper_link"/>
+    <child link="gripper_frame_link"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+
+  <!-- Link moving_jaw_so101_v1 -->
+  <link name="moving_jaw_so101_v1_link">
+    <inertial>
+      <origin xyz="-0.00157495 -0.0300244 0.0192755" rpy="0 0 0"/>
+      <mass value="0.012"/>
+      <inertia ixx="6.61427e-06" ixy="-3.19807e-07" ixz="-5.90717e-09" iyy="1.89032e-06" iyz="-1.09945e-07" izz="5.28738e-06"/>
+    </inertial>
+    <!-- Part moving_jaw_so101_v1 -->
+    <visual>
+      <origin xyz="-5.55112e-17 -5.55112e-17 0.0189" rpy="9.53145e-17 6.93889e-18 1.24077e-24"/>
+      <geometry>
+        <mesh filename="assets/moving_jaw_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-5.55112e-17 -5.55112e-17 0.0189" rpy="9.53145e-17 6.93889e-18 1.24077e-24"/>
+      <geometry>
+        <mesh filename="assets/moving_jaw_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Joint from gripper to moving_jaw_so101_v1 -->
+  <joint name="gripper" type="revolute">
+    <origin xyz="0.0202 0.0188 -0.0234" rpy="1.5708 -5.24284e-08 -1.41553e-15"/>
+    <parent link="gripper_link"/>
+    <child link="moving_jaw_so101_v1_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-0.174533" upper="1.74533"/>
+  </joint>
+
+  <transmission name="gripper_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="gripper">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor6">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from wrist to gripper -->
+  <joint name="wrist_roll" type="revolute">
+    <origin xyz="5.55112e-17 -0.0611 0.0181" rpy="1.5708 0.0486795 3.14159"/>
+    <parent link="wrist_link"/>
+    <child link="gripper_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-2.74385" upper="2.84121"/>
+  </joint>
+
+  <transmission name="wrist_roll_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_roll">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor5">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from lower_arm to wrist -->
+  <joint name="wrist_flex" type="revolute">
+    <origin xyz="-0.1349 0.0052 3.62355e-17" rpy="4.02456e-15 8.67362e-16 -1.5708"/>
+    <parent link="lower_arm_link"/>
+    <child link="wrist_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-1.65806" upper="1.65806"/>
+  </joint>
+
+  <transmission name="wrist_flex_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_flex">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor4">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from upper_arm to lower_arm -->
+  <!-- Note: 5-degree calibration offset applied to joint limits -->
+  <joint name="elbow_flex" type="revolute">
+    <origin xyz="-0.11257 -0.028 1.73763e-16" rpy="-3.63608e-16 8.74301e-16 1.5708"/>
+    <parent link="upper_arm_link"/>
+    <child link="lower_arm_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-1.69" upper="1.69"/>
+  </joint>
+
+  <transmission name="elbow_flex_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="elbow_flex">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor3">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from shoulder to upper_arm -->
+  <joint name="shoulder_lift" type="revolute">
+    <origin xyz="-0.0303992 -0.0182778 -0.0542" rpy="-1.5708 -1.5708 0"/>
+    <parent link="shoulder_link"/>
+    <child link="upper_arm_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-1.74533" upper="1.74533"/>
+  </joint>
+
+  <transmission name="shoulder_lift_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_lift">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor2">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from base to shoulder -->
+  <joint name="shoulder_pan" type="revolute">
+    <origin xyz="0.0388353 -8.97657e-09 0.0624" rpy="3.14159 4.18253e-17 -3.14159"/>
+    <parent link="base_link"/>
+    <child link="shoulder_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-1.91986" upper="1.91986"/>
+  </joint>
+
+  <transmission name="shoulder_pan_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_pan">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor1">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission> 
+
+</robot>

rl_deploy/table_calib.json

@@ -0,0 +1,205 @@
+{
+ "H": [
+  [
+   -0.00020641936356017595,
+   0.001605983436679173,
+   -0.17743518923975418
+  ],
+  [
+   0.0009714184562970813,
+   -0.0002591502281196994,
+   -0.5384275139337167
+  ],
+  [
+   5.463862597621938e-05,
+   0.0019026390841230777,
+   1.0
+  ]
+ ],
+ "image_size": [
+  640,
+  360
+ ],
+ "points": [
+  {
+   "uv": [
+    283.3632596685083,
+    232.50552486187846
+   ],
+   "area": 724,
+   "joints_deg": [
+    -22.681318681318682,
+    -1.2307692307692308,
+    41.18681318681319,
+    39.07692307692308,
+    -2.5054945054945055,
+    1.1026878015161956
+   ],
+   "tip": [
+    0.0885746699621676,
+    -0.2211262158968657,
+    -0.0018728227502649436
+   ]
+  },
+  {
+   "uv": [
+    223.27258566978193,
+    174.84112149532712
+   ],
+   "area": 642,
+   "joints_deg": [
+    -6.065934065934066,
+    13.098901098901099,
+    21.846153846153847,
+    39.42857142857143,
+    -2.4175824175824174,
+    1.1026878015161956
+   ],
+   "tip": [
+    0.04521679450712202,
+    -0.28363020045294185,
+    0.00034617923123554314
+   ]
+  },
+  {
+   "uv": [
+    324.1647940074906,
+    146.14606741573033
+   ],
+   "area": 534,
+   "joints_deg": [
+    10.54945054945055,
+    -5.450549450549451,
+    41.714285714285715,
+    58.59340659340659,
+    -2.32967032967033,
+    1.1026878015161956
+   ],
+   "tip": [
+    -0.009996185007877991,
+    -0.19178363754922642,
+    0.006818864733087288
+   ]
+  },
+  {
+   "uv": [
+    323.44,
+    294.63470588235293
+   ],
+   "area": 1700,
+   "joints_deg": [
+    -41.67032967032967,
+    7.824175824175824,
+    25.45054945054945,
+    52.879120879120876,
+    -2.5934065934065935,
+    1.1026878015161956
+   ],
+   "tip": [
+    0.14299164473120482,
+    -0.19023792065397205,
+    0.003980910785919246
+   ]
+  },
+  {
+   "uv": [
+    446.7759784075574,
+    253.0425101214575
+   ],
+   "area": 1482,
+   "joints_deg": [
+    -52.30769230769231,
+    -23.64835164835165,
+    56.30769230769231,
+    63.51648351648352,
+    84.26373626373626,
+    1.1026878015161956
+   ],
+   "tip": [
+    0.09268488377845423,
+    -0.1230287165562727,
+    0.013203728373691447
+   ]
+  },
+  {
+   "uv": [
+    177.62869198312237,
+    300.67148884870403
+   ],
+   "area": 1659,
+   "joints_deg": [
+    -32.879120879120876,
+    30.24175824175824,
+    -1.2747252747252746,
+    42.15384615384615,
+    -17.53846153846154,
+    1.1716057891109581
+   ],
+   "tip": [
+    0.17211300438132343,
+    -0.2762982688066562,
+    -0.005484126509456877
+   ]
+  },
+  {
+   "uv": [
+    203.57355679702047,
+    143.64245810055866
+   ],
+   "area": 537,
+   "joints_deg": [
+    3.6043956043956045,
+    25.406593406593405,
+    5.934065934065934,
+    46.81318681318681,
+    -16.923076923076923,
+    1.1716057891109581
+   ],
+   "tip": [
+    0.00809345599756148,
+    -0.30200666202286097,
+    -0.007828562295707622
+   ]
+  },
+  {
+   "uv": [
+    326.9448818897638,
+    185.59186351706037
+   ],
+   "area": 762,
+   "joints_deg": [
+    -11.516483516483516,
+    -11.516483516483516,
+    47.07692307692308,
+    57.45054945054945,
+    -17.36263736263736,
+    1.1026878015161956
+   ],
+   "tip": [
+    0.0432111876213237,
+    -0.18203129452029243,
+    0.008625103296823854
+   ]
+  }
+ ],
+ "err_mm": [
+  5.768327483946577,
+  11.168090471611537,
+  10.273185868475645,
+  2.103149561433357,
+  10.31306901446774,
+  4.800515995559936,
+  7.8180204156784345,
+  14.80895048151171
+ ],
+ "loo_mm": [
+  10.140745687901573,
+  14.396130379327072,
+  23.67257095374759,
+  4.647962466997839,
+  50.30546068867357,
+  21.273626361652976,
+  56.748608120772445,
+  17.030096025168582
+ ]
+}