Upload folder using huggingface_hub

.gitattributes

@@ -33,3 +33,13 @@ 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
+exp_all_changes/videos/ep_01_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_all_changes/videos/ep_02_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_all_changes/videos/ep_03_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_all_changes/videos/ep_04_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_all_changes/videos/ep_05_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_all_changes/videos/ep_06_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_all_changes/videos/ep_07_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_all_changes/videos/ep_08_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_all_changes/videos/ep_09_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_all_changes/videos/ep_10_failed.mp4 filter=lfs diff=lfs merge=lfs -text

exp_all_changes/CHANGES.md

@@ -0,0 +1,20 @@
+# exp_all_changes — what changed vs. main-branch FSM
+
+This version applies **all four proposed fixes simultaneously** to compress the FSM
+into the 20-second leaderboard eval window AND place objects more gently.
+
+| Constant | Original (main) | This version | Why |
+|---|---|---|---|
+| `_PHASE_DURATIONS_PER_OBJECT` | `(180, 130, 20, 160, 170, 15, 30)` = 705 steps | `(80, 70, 25, 70, 100, 30, 25)` = 400 steps | Cuts wasted idle time. Original took ~25s per pair; new ~13s. Lets a learned policy finish inside the 20s eval cap. |
+| `_LIFT_Z_OFFSET` | `0.20` | `0.15` | Reduces vertical travel during transit. (Earlier 0.10 attempt was too low — held cutlery clipped the plate edge.) |
+| `_RELEASE_Z_OFFSET` | `0.09` | `0.06` | Cutlery dropped from 9 cm bounced out of the 15 cm success radius. Lower drop = gentler placement. (Earlier 0.04 attempt put gripper fingers into the table.) |
+| `_MAX_CARTESIAN_DELTA` | `0.018` | `0.020` | Slightly faster IK so the cut phase durations are still enough for the actual motion. (Earlier 0.025 was too aggressive — caused knock-offs.) |
+
+## What the videos should show
+- Faster overall execution (each episode ~13s vs ~25s)
+- Lower transport altitude
+- Cutlery placed more gently next to plate
+
+## What this experiment is **NOT** testing
+- Original FSM behavior (see `exp_heights_only` for that baseline)
+- Eval policy timing — these are FSM rollouts, not trained-policy rollouts

exp_all_changes/SUMMARY.md

@@ -0,0 +1,32 @@
+# FSM Experiment: exp_all_changes
+
+- Date: Mon May 25 03:38:37 PM PST 2026
+- Poses file: `data/umi/test_fsm_v3/object_poses_10_shared.json`
+- Successes: **0
+0** / Failures: **10**
+
+## FSM constants used in this run
+```python
+_MAX_CARTESIAN_DELTA = 0.020  # slight bump from 0.018 (0.025 was too aggressive and caused knock-offs)
+_HOVER_Z_OFFSET = 0.15
+_GRASP_Z_OFFSET = 0.08
+_LIFT_Z_OFFSET = 0.15        # was 0.20; 0.10 was too low (held cutlery clipped plate edge during transport)
+_RELEASE_Z_OFFSET = 0.06     # was 0.09; 0.04 was too low (gripper fingers hit plate/table)
+_PHASE_DURATIONS_PER_OBJECT = (80, 70, 25, 70, 100, 30, 25)  # cut from (180,130,20,160,170,15,30); old idle time made policy reproduce 25s trajectories vs 20s eval cap
+_PHASES_PER_OBJECT = len(_PHASE_DURATIONS_PER_OBJECT)
+def _clamp_delta(delta: torch.Tensor, max_norm: float = _MAX_CARTESIAN_DELTA) -> torch.Tensor:
+    MAX_STEPS: int = len(_PICK_ORDER) * sum(_PHASE_DURATIONS_PER_OBJECT) + 100
+        self._events_dt: list[int] = list(_PHASE_DURATIONS_PER_OBJECT) * len(_PICK_ORDER)
+        target[:, 2] += _HOVER_Z_OFFSET
+        target[:, 2] += _GRASP_Z_OFFSET
+        target[:, 2] += _GRASP_Z_OFFSET
+        target[:, 2] += _LIFT_Z_OFFSET
+        target[:, 2] += _LIFT_Z_OFFSET
+        target[:, 2] += _RELEASE_Z_OFFSET
+        target[:, 2] += _LIFT_Z_OFFSET
+```
+
+## Videos
+Per-episode mp4 in `videos/` named `ep_NN_<success|failed>.mp4`. Side-by-side (wrist | front, 1280x480 @ 10 fps).
+
+See `CHANGES.md` for what was modified in this version and why.

exp_all_changes/cutlery_arrangement.py

@@ -0,0 +1,482 @@
+"""State machine for the Franka cutlery-arrangement task."""
+
+from __future__ import annotations
+
+import math
+
+import torch
+from isaaclab.utils.math import (
+    axis_angle_from_quat,
+    matrix_from_quat,
+    quat_apply,
+    quat_from_euler_xyz,
+    quat_inv,
+    quat_mul,
+)
+
+from leisaac.datagen.state_machine.base import StateMachineBase
+
+# ---------------------------------------------------------------------------
+# Module-level constants
+# ---------------------------------------------------------------------------
+_FORK_NAME = "fork"
+_KNIFE_NAME = "knife"
+_PLATE_NAME = "plate"
+_EE_BODY_NAME = "panda_hand"
+_FRANKA_ARM_JOINT_NAMES = (
+    "panda_joint1",
+    "panda_joint2",
+    "panda_joint3",
+    "panda_joint4",
+    "panda_joint5",
+    "panda_joint6",
+    "panda_joint7",
+)
+
+_GRIPPER_OPEN = 1.0
+_GRIPPER_CLOSE = -1.0
+
+_MAX_CARTESIAN_DELTA = 0.020  # slight bump from 0.018 (0.025 was too aggressive and caused knock-offs)
+_MAX_ROT_DELTA = 0.08
+_IK_DLS_LAMBDA = 0.01
+
+_HOVER_Z_OFFSET = 0.15
+_GRASP_Z_OFFSET = 0.08
+_LIFT_Z_OFFSET = 0.15        # was 0.20; 0.10 was too low (held cutlery clipped plate edge during transport)
+_RELEASE_Z_OFFSET = 0.06     # was 0.09; 0.04 was too low (gripper fingers hit plate/table)
+_GRIPPER_DOWN_ROLL_W = math.pi
+_GRIPPER_DOWN_PITCH_W = 0.0
+_GRIPPER_DOWN_YAW_OFFSET_RANGE = (-0.15, 0.15)
+# Grasp yaw bias (rad) on top of the object's world yaw, before the random
+# jitter. Cutlery items are elongated, so π/2 closes the fingers across the
+# short axis. Per-USD orientation correction lives in env_cfg's
+# ``per_object_yaw_offset``.
+_GRASP_YAW_OFFSET: float = math.pi / 2.0
+# Horizontal retreat (m) toward the robot base applied to approach + grasp
+# targets. Per-object so each cutlery item can be tuned independently
+# (e.g. knife may grab better with no retreat than fork).
+_GRASP_RETREAT_PER_OBJECT: dict[str, float] = {
+    "fork": 0.025,
+    "knife": 0.025,
+}
+
+_PLACE_OFFSET = 0.10
+
+_SUCCESS_MAX_DIST_XY = 0.15
+
+_FRANKA_REST_JOINT_POS = {
+    "panda_joint1": 0.0,
+    "panda_joint2": -math.pi / 4.0,
+    "panda_joint3": 0.0,
+    "panda_joint4": -3.0 * math.pi / 4.0,
+    "panda_joint5": 0.0,
+    "panda_joint6": math.pi / 2.0,
+    "panda_joint7": math.pi / 4.0,
+    "panda_finger_joint1": 0.04,
+    "panda_finger_joint2": 0.04,
+}
+
+# Pick order: fork first (place on +y / left of plate), then knife (place on -y / right)
+_PICK_ORDER = (_KNIFE_NAME, _FORK_NAME)
+_PLACE_X_SIGNS = (-1.0, +1.0)  # knife → -x of plate, fork → +x of plate
+
+_PHASE_DURATIONS_PER_OBJECT = (80, 70, 25, 70, 100, 30, 25)  # cut from (180,130,20,160,170,15,30); old idle time made policy reproduce 25s trajectories vs 20s eval cap
+_PHASES_PER_OBJECT = len(_PHASE_DURATIONS_PER_OBJECT)
+
+
+def _constant_gripper(num_envs: int, device: torch.device, value: float) -> torch.Tensor:
+    return torch.full((num_envs, 1), value, device=device)
+
+
+def _clamp_delta(delta: torch.Tensor, max_norm: float = _MAX_CARTESIAN_DELTA) -> torch.Tensor:
+    norm = torch.linalg.norm(delta, dim=-1, keepdim=True).clamp_min(1e-6)
+    scale = torch.clamp(max_norm / norm, max=1.0)
+    return delta * scale
+
+
+def _shortest_quat(quat: torch.Tensor) -> torch.Tensor:
+    return torch.where(quat[:, 0:1] < 0.0, -quat, quat)
+
+
+def _retreat_xy_toward(
+    target_pos_w: torch.Tensor,
+    anchor_pos_w: torch.Tensor,
+    distance: float,
+) -> torch.Tensor:
+    """Pull ``target_pos_w`` xy toward ``anchor_pos_w`` by ``distance`` metres."""
+    out = target_pos_w.clone()
+    delta_xy = out[:, :2] - anchor_pos_w[:, :2]
+    norm = torch.linalg.norm(delta_xy, dim=-1, keepdim=True).clamp_min(1e-6)
+    out[:, :2] -= distance * (delta_xy / norm)
+    return out
+
+
+def _yaw_from_quat_wxyz(quat_wxyz: torch.Tensor) -> torch.Tensor:
+    """Yaw (rotation about world z) from a (w, x, y, z) quaternion."""
+    w, x, y, z = quat_wxyz[:, 0], quat_wxyz[:, 1], quat_wxyz[:, 2], quat_wxyz[:, 3]
+    siny_cosp = 2.0 * (w * z + x * y)
+    cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
+    return torch.atan2(siny_cosp, cosy_cosp)
+
+
+def _find_body_index(robot, body_name: str) -> int:
+    if hasattr(robot, "find_bodies"):
+        body_ids, _ = robot.find_bodies(body_name)
+        if len(body_ids) > 0:
+            return int(body_ids[0])
+
+    body_names = getattr(robot.data, "body_names", None)
+    if body_names is not None and body_name in body_names:
+        return body_names.index(body_name)
+
+    return -1
+
+
+# ---------------------------------------------------------------------------
+# State machine
+# ---------------------------------------------------------------------------
+
+
+class CutleryArrangementStateMachine(StateMachineBase):
+    """Scripted Franka policy for arranging cutlery around a plate.
+
+    Picks up the fork and places it on the +y (left) side of the plate,
+    then picks up the knife and places it on the -y (right) side.
+
+    Each object goes through 7 phases:
+
+    0. Move above object
+    1. Approach down to object
+    2. Close gripper to grasp
+    3. Lift object upward
+    4. Move above target position (beside plate)
+    5. Lower and release
+    6. Retreat upward
+
+    The action vector is ``[panda_joint1, ..., panda_joint7, gripper]``.
+    """
+
+    MAX_STEPS: int = len(_PICK_ORDER) * sum(_PHASE_DURATIONS_PER_OBJECT) + 100
+
+    def __init__(self) -> None:
+        self._step_count: int = 0
+        self._episode_done: bool = False
+        self._ee_body_idx: int = -1
+        self._jacobi_body_idx: int = -1
+        self._arm_joint_ids: list[int] = []
+        self._jacobi_joint_ids: list[int] = []
+        self._rest_joint_pos: torch.Tensor | None = None
+        self._rest_ee_pos_w: torch.Tensor | None = None
+        self._initial_ee_pos_w: torch.Tensor | None = None
+        self._gripper_down_yaw_w: torch.Tensor | None = None
+        self._gripper_down_yaw_offset_w: torch.Tensor | None = None
+        self._current_object_idx: int = 0
+        self._event: int = 0
+        self._events_dt: list[int] = list(_PHASE_DURATIONS_PER_OBJECT) * len(_PICK_ORDER)
+
+    # ------------------------------------------------------------------
+    # StateMachineBase interface
+    # ------------------------------------------------------------------
+
+    def setup(self, env) -> None:
+        robot = env.scene["robot"]
+        self._ee_body_idx = _find_body_index(robot, _EE_BODY_NAME)
+        joint_names = list(robot.data.joint_names)
+        missing = [j for j in _FRANKA_ARM_JOINT_NAMES if j not in joint_names]
+        if missing:
+            raise ValueError(f"Missing Franka joints {missing} in {joint_names}")
+        self._arm_joint_ids = [joint_names.index(j) for j in _FRANKA_ARM_JOINT_NAMES]
+
+        if self._ee_body_idx < 0:
+            raise ValueError(f"Could not find body '{_EE_BODY_NAME}' in Franka.")
+        if robot.is_fixed_base:
+            self._jacobi_body_idx = self._ee_body_idx - 1
+            self._jacobi_joint_ids = self._arm_joint_ids
+        else:
+            self._jacobi_body_idx = self._ee_body_idx
+            self._jacobi_joint_ids = [jid + 6 for jid in self._arm_joint_ids]
+
+        self._rest_joint_pos = torch.zeros(env.num_envs, len(joint_names), device=env.device)
+        for idx, name in enumerate(joint_names):
+            if name in _FRANKA_REST_JOINT_POS:
+                self._rest_joint_pos[:, idx] = _FRANKA_REST_JOINT_POS[name]
+
+        robot.write_joint_state_to_sim(
+            position=self._rest_joint_pos,
+            velocity=torch.zeros_like(self._rest_joint_pos),
+        )
+        env.sim.step(render=False)
+        env.scene.update(dt=env.physics_dt)
+        self._rest_ee_pos_w = self._ee_pos_w(robot).clone()
+
+    def check_success(self, env) -> bool:
+        plate_pos = env.scene[_PLATE_NAME].data.root_pos_w - env.scene.env_origins
+        fork_pos = env.scene[_FORK_NAME].data.root_pos_w - env.scene.env_origins
+        knife_pos = env.scene[_KNIFE_NAME].data.root_pos_w - env.scene.env_origins
+
+        done = torch.ones(env.num_envs, dtype=torch.bool, device=env.device)
+
+        fork_dist_xy = torch.norm(fork_pos[:, :2] - plate_pos[:, :2], dim=1)
+        knife_dist_xy = torch.norm(knife_pos[:, :2] - plate_pos[:, :2], dim=1)
+
+        done = torch.logical_and(done, fork_dist_xy <= _SUCCESS_MAX_DIST_XY)
+        done = torch.logical_and(done, knife_dist_xy <= _SUCCESS_MAX_DIST_XY)
+        done = torch.logical_and(done, fork_pos[:, 0] > plate_pos[:, 0]) # fork on +x
+        done = torch.logical_and(done, knife_pos[:, 0] < plate_pos[:, 0]) # knife on -x
+
+        return bool(done.all().item())
+
+    def pre_step(self, env) -> None:
+        pass
+
+    def get_action(self, env) -> torch.Tensor:
+        robot = env.scene["robot"]
+        robot.write_joint_damping_to_sim(damping=10.0)
+
+        device = env.device
+        num_envs = env.num_envs
+
+        obj_name = _PICK_ORDER[self._current_object_idx]
+        x_sign = _PLACE_X_SIGNS[self._current_object_idx]
+        obj_pos_w = env.scene[obj_name].data.root_pos_w.clone()
+        obj_quat_w = env.scene[obj_name].data.root_quat_w.clone()
+        plate_pos_w = env.scene[_PLATE_NAME].data.root_pos_w.clone()
+        robot_root_pos_w = robot.data.root_pos_w.clone()
+
+        place_target_w = plate_pos_w.clone()
+        place_target_w[:, 0] += x_sign * _PLACE_OFFSET
+
+        if self._step_count == 0 and self._event == 0:
+            self._initial_ee_pos_w = self._ee_pos_w(robot).clone()
+
+        phase_in_cycle = self._event % _PHASES_PER_OBJECT
+
+        target_quat_w = self._gripper_down_quat_w(
+            obj_quat_w,
+            obj_name,
+            num_envs,
+            device,
+            obj_quat_w.dtype,
+            yaw_offset=_GRASP_YAW_OFFSET,
+        )
+
+        grasp_anchor_w = _retreat_xy_toward(
+            obj_pos_w,
+            robot_root_pos_w,
+            _GRASP_RETREAT_PER_OBJECT.get(obj_name, 0.0),
+        )
+
+        if phase_in_cycle == 0:
+            target_pos_w, gripper_cmd = self._phase_move_above_object(obj_pos_w, num_envs, device)
+        elif phase_in_cycle == 1:
+            target_pos_w, gripper_cmd = self._phase_approach_object(grasp_anchor_w, num_envs, device)
+        elif phase_in_cycle == 2:
+            target_pos_w, gripper_cmd = self._phase_grasp(grasp_anchor_w, num_envs, device)
+        elif phase_in_cycle == 3:
+            target_pos_w, gripper_cmd = self._phase_lift(obj_pos_w, num_envs, device)
+        elif phase_in_cycle == 4:
+            target_pos_w, gripper_cmd = self._phase_move_above_place(place_target_w, num_envs, device)
+        elif phase_in_cycle == 5:
+            target_pos_w, gripper_cmd = self._phase_lower_to_release(place_target_w, num_envs, device)
+        else:
+            target_pos_w, gripper_cmd = self._phase_retreat(place_target_w, num_envs, device)
+
+        return self._joint_position_franka_action(env, target_pos_w, target_quat_w, gripper_cmd)
+
+    # ------------------------------------------------------------------
+    # Phase helpers
+    # ------------------------------------------------------------------
+
+    def _phase_move_above_object(self, obj_pos_w, num_envs, device):
+        target = obj_pos_w.clone()
+        target[:, 2] += _HOVER_Z_OFFSET
+        if self._initial_ee_pos_w is not None:
+            denom = max(self._events_dt[self._event] - 1, 1)
+            alpha = min(self._step_count / denom, 1.0)
+            target = (1.0 - alpha) * self._initial_ee_pos_w + alpha * target
+        return target, _constant_gripper(num_envs, device, _GRIPPER_OPEN)
+
+    def _phase_approach_object(self, obj_pos_w, num_envs, device):
+        target = obj_pos_w.clone()
+        target[:, 2] += _GRASP_Z_OFFSET
+        return target, _constant_gripper(num_envs, device, _GRIPPER_OPEN)
+
+    def _phase_grasp(self, obj_pos_w, num_envs, device):
+        # Hold the same height as the approach phase so the EE doesn't keep
+        # descending while the fingers are closing — that timing race causes
+        # empty grasps.
+        target = obj_pos_w.clone()
+        target[:, 2] += _GRASP_Z_OFFSET
+        return target, _constant_gripper(num_envs, device, _GRIPPER_CLOSE)
+
+    def _phase_lift(self, obj_pos_w, num_envs, device):
+        target = obj_pos_w.clone()
+        target[:, 2] += _LIFT_Z_OFFSET
+        return target, _constant_gripper(num_envs, device, _GRIPPER_CLOSE)
+
+    def _phase_move_above_place(self, place_pos_w, num_envs, device):
+        target = place_pos_w.clone()
+        target[:, 2] += _LIFT_Z_OFFSET
+        return target, _constant_gripper(num_envs, device, _GRIPPER_CLOSE)
+
+    def _phase_lower_to_release(self, place_pos_w, num_envs, device):
+        target = place_pos_w.clone()
+        target[:, 2] += _RELEASE_Z_OFFSET
+        return target, _constant_gripper(num_envs, device, _GRIPPER_CLOSE)
+
+    def _phase_retreat(self, place_pos_w, num_envs, device):
+        target = place_pos_w.clone()
+        target[:, 2] += _LIFT_Z_OFFSET
+        return target, _constant_gripper(num_envs, device, _GRIPPER_OPEN)
+
+    # ------------------------------------------------------------------
+    # Timeline
+    # ------------------------------------------------------------------
+
+    def advance(self) -> None:
+        if self._episode_done:
+            return
+
+        self._step_count += 1
+        if self._step_count < self._events_dt[self._event]:
+            return
+
+        self._event += 1
+        self._step_count = 0
+
+        if self._event >= len(self._events_dt):
+            self._episode_done = True
+            return
+
+        new_obj_idx = self._event // _PHASES_PER_OBJECT
+        if new_obj_idx != self._current_object_idx:
+            self._current_object_idx = new_obj_idx
+            self._initial_ee_pos_w = None
+            self._gripper_down_yaw_w = None
+            self._gripper_down_yaw_offset_w = None
+
+    def reset(self) -> None:
+        self._step_count = 0
+        self._episode_done = False
+        self._event = 0
+        self._current_object_idx = 0
+        self._initial_ee_pos_w = None
+        self._gripper_down_yaw_w = None
+        self._gripper_down_yaw_offset_w = None
+
+    # ------------------------------------------------------------------
+    # IK / control helpers (same as CupStackingStateMachine)
+    # ------------------------------------------------------------------
+
+    def _ee_pos_w(self, robot) -> torch.Tensor:
+        body_idx = self._ee_body_idx if self._ee_body_idx >= 0 else -1
+        return robot.data.body_pos_w[:, body_idx, :]
+
+    def _ee_quat_w(self, robot) -> torch.Tensor:
+        body_idx = self._ee_body_idx if self._ee_body_idx >= 0 else -1
+        return robot.data.body_quat_w[:, body_idx, :]
+
+    def _joint_position_franka_action(
+        self,
+        env,
+        target_pos_w: torch.Tensor,
+        target_quat_w: torch.Tensor,
+        gripper_cmd: torch.Tensor,
+    ) -> torch.Tensor:
+        robot = env.scene["robot"]
+        root_pos_w = robot.data.root_pos_w
+        root_quat_w = robot.data.root_quat_w
+        root_quat_inv = quat_inv(root_quat_w)
+
+        target_pos_root = quat_apply(root_quat_inv, target_pos_w - root_pos_w)
+        ee_pos_root = quat_apply(root_quat_inv, self._ee_pos_w(robot) - root_pos_w)
+        delta_pos_root = _clamp_delta(target_pos_root - ee_pos_root)
+
+        delta_quat_w = _shortest_quat(quat_mul(target_quat_w, quat_inv(self._ee_quat_w(robot))))
+        delta_rot_w = axis_angle_from_quat(delta_quat_w)
+        delta_rot_root = _clamp_delta(quat_apply(root_quat_inv, delta_rot_w), _MAX_ROT_DELTA)
+
+        pose_delta_root = torch.cat([delta_pos_root, delta_rot_root], dim=-1)
+        joint_pos_target = self._arm_joint_pos(robot) + self._compute_delta_joint_pos(
+            pose_delta_root, self._ee_jacobian_root(robot)
+        )
+        joint_pos_target = self._clamp_arm_joint_pos(robot, joint_pos_target)
+        return torch.cat([joint_pos_target, gripper_cmd], dim=-1)
+
+    def _arm_joint_pos(self, robot) -> torch.Tensor:
+        if not self._arm_joint_ids:
+            raise RuntimeError("setup() must run before requesting actions.")
+        return robot.data.joint_pos[:, self._arm_joint_ids]
+
+    def _ee_jacobian_root(self, robot) -> torch.Tensor:
+        if self._jacobi_body_idx < 0 or not self._jacobi_joint_ids:
+            raise RuntimeError("setup() must run before requesting actions.")
+
+        jacobian = robot.root_physx_view.get_jacobians()[
+            :, self._jacobi_body_idx, :, self._jacobi_joint_ids
+        ].clone()
+        root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_quat_w))
+        jacobian[:, :3, :] = torch.bmm(root_rot_matrix, jacobian[:, :3, :])
+        jacobian[:, 3:, :] = torch.bmm(root_rot_matrix, jacobian[:, 3:, :])
+        return jacobian
+
+    def _compute_delta_joint_pos(self, pose_delta: torch.Tensor, jacobian: torch.Tensor) -> torch.Tensor:
+        jacobian_t = torch.transpose(jacobian, dim0=1, dim1=2)
+        lambda_matrix = (_IK_DLS_LAMBDA**2) * torch.eye(
+            jacobian.shape[1], device=jacobian.device, dtype=jacobian.dtype
+        )
+        delta_joint_pos = (
+            jacobian_t @ torch.inverse(jacobian @ jacobian_t + lambda_matrix) @ pose_delta.unsqueeze(-1)
+        )
+        return delta_joint_pos.squeeze(-1)
+
+    def _clamp_arm_joint_pos(self, robot, joint_pos: torch.Tensor) -> torch.Tensor:
+        joint_pos_limits = getattr(robot.data, "soft_joint_pos_limits", None)
+        if joint_pos_limits is None:
+            joint_pos_limits = getattr(robot.data, "joint_pos_limits", None)
+        if joint_pos_limits is None:
+            return joint_pos
+        arm_joint_pos_limits = joint_pos_limits[:, self._arm_joint_ids, :]
+        return torch.clamp(joint_pos, arm_joint_pos_limits[..., 0], arm_joint_pos_limits[..., 1])
+
+    def _gripper_down_quat_w(
+        self,
+        obj_quat_w: torch.Tensor,
+        obj_name: str,
+        num_envs: int,
+        device: torch.device,
+        dtype: torch.dtype,
+        yaw_offset: float = 0.0,
+    ) -> torch.Tensor:
+        if self._gripper_down_yaw_w is None or self._gripper_down_yaw_w.shape[0] != num_envs:
+            base_yaw = _yaw_from_quat_wxyz(obj_quat_w).to(device=device, dtype=dtype) # gripper aligned with the orientation of the object
+            self._gripper_down_yaw_offset_w = torch.empty(num_envs, device=device, dtype=dtype).uniform_(
+                _GRIPPER_DOWN_YAW_OFFSET_RANGE[0],
+                _GRIPPER_DOWN_YAW_OFFSET_RANGE[1],
+            )
+            if obj_name == _KNIFE_NAME:
+                base_yaw = torch.zeros_like(base_yaw) # fixed direction
+            self._gripper_down_yaw_w = (
+                base_yaw + yaw_offset + self._gripper_down_yaw_offset_w
+            ).clone()
+
+        roll = torch.full((num_envs,), _GRIPPER_DOWN_ROLL_W, device=device, dtype=dtype)
+        pitch = torch.full((num_envs,), _GRIPPER_DOWN_PITCH_W, device=device, dtype=dtype)
+        yaw = self._gripper_down_yaw_w.to(device=device, dtype=dtype)
+        return quat_from_euler_xyz(roll, pitch, yaw)
+
+    # ------------------------------------------------------------------
+    # Properties
+    # ------------------------------------------------------------------
+
+    @property
+    def is_episode_done(self) -> bool:
+        return self._episode_done
+
+    @property
+    def step_count(self) -> int:
+        return self._step_count
+
+    @property
+    def task_object_names(self) -> tuple[str, ...]:
+        return (_FORK_NAME, _KNIFE_NAME, _PLATE_NAME)

exp_all_changes/datagen.log

@@ -0,0 +1,257 @@
+=== Running exp_all_changes ===
+Mon May 25 03:26:24 PM PST 2026
+
+==========
+== CUDA ==
+==========
+
+CUDA Version 12.8.1
+
+Container image Copyright (c) 2016-2023, NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+
+This container image and its contents are governed by the NVIDIA Deep Learning Container License.
+By pulling and using the container, you accept the terms and conditions of this license:
+https://developer.nvidia.com/ngc/nvidia-deep-learning-container-license
+
+A copy of this license is made available in this container at /NGC-DL-CONTAINER-LICENSE for your convenience.
+
+debconf: delaying package configuration, since apt-utils is not installed
+[INFO][AppLauncher]: Using device: cuda
+[INFO][AppLauncher]: Loading experience file: /workspace/aicapstone/dependencies/IsaacLab/apps/isaaclab.python.headless.rendering.kit
+[Info] [carb] Logging to file: /usr/local/lib/python3.11/dist-packages/isaacsim/kit/logs/Kit/Isaac-Sim/5.1/kit_20260525_072748.log
+2026-05-25T07:27:48Z [301ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T07:27:48Z [301ms] [Warning] [carb] Failed to startup plugin carb.windowing-glfw.plugin (interfaces: [carb::windowing::IGLContext v1.0],[carb::windowing::IWindowing v1.5]) (impl: carb.windowing-glfw.plugin)
+2026-05-25T07:27:48Z [311ms] [Warning] [omni.platforminfo.plugin] failed to open the default display.  Can't verify X Server version.
+
+|---------------------------------------------------------------------------------------------|
+| Driver Version: 580.159.03    | Graphics API: Vulkan
+|=============================================================================================|
+| GPU | Name                             | Active | LDA | GPU Memory | Vendor-ID | LUID       |
+|     |                                  |        |     |            | Device-ID | UUID       |
+|     |                                  |        |     |            | Bus-ID    |            |
+|---------------------------------------------------------------------------------------------|
+| 0   | NVIDIA GeForce RTX 3060          | Yes: 0 |     | 12534   MB | 10de      | 0          |
+|     |                                  |        |     |            | 2504      | 032c9579.. |
+|     |                                  |        |     |            | 1         |            |
+|=============================================================================================|
+| OS: 22.04.5 LTS (Jammy Jellyfish) ubuntu, Version: 22.04.5, Kernel: 6.17.0-29-generic
+| Processor: Intel(R) Core(TM) i5-10500 CPU @ 3.10GHz
+| Cores: 6 | Logical Cores: 12
+|---------------------------------------------------------------------------------------------|
+| Total Memory (MB): 31934 | Free Memory: 29463
+| Total Page/Swap (MB): 4095 | Free Page/Swap: 0
+|---------------------------------------------------------------------------------------------|
+2026-05-25T07:27:49Z [1,228ms] [Warning] [gpu.foundation.plugin] CPU performance profile is set to powersave. This profile sets the CPU to the lowest frequency reducing performance.
+2026-05-25T07:27:50Z [2,213ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T07:27:50Z [2,213ms] [Warning] [carb] Failed to startup plugin carb.windowing-glfw.plugin (interfaces: [carb::windowing::IGLContext v1.0],[carb::windowing::IWindowing v1.5]) (impl: carb.windowing-glfw.plugin)
+2026-05-25T07:27:50Z [2,343ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T07:27:50Z [2,343ms] [Warning] [carb] Failed to startup plugin carb.windowing-glfw.plugin (interfaces: [carb::windowing::IGLContext v1.0],[carb::windowing::IWindowing v1.5]) (impl: carb.windowing-glfw.plugin)
+2026-05-25T07:27:50Z [2,353ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T07:27:50Z [2,353ms] [Warning] [carb] Failed to startup plugin carb.windowing-glfw.plugin (interfaces: [carb::windowing::IGLContext v1.0],[carb::windowing::IWindowing v1.5]) (impl: carb.windowing-glfw.plugin)
+2026-05-25T07:27:50Z [2,360ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T07:27:50Z [2,360ms] [Warning] [carb] Failed to startup plugin carb.windowing-glfw.plugin (interfaces: [carb::windowing::IGLContext v1.0],[carb::windowing::IWindowing v1.5]) (impl: carb.windowing-glfw.plugin)
+2026-05-25T07:27:50Z [2,363ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T07:27:50Z [2,363ms] [Warning] [carb] Failed to startup plugin carb.windowing-glfw.plugin (interfaces: [carb::windowing::IGLContext v1.0],[carb::windowing::IWindowing v1.5]) (impl: carb.windowing-glfw.plugin)
+2026-05-25T07:27:50Z [2,364ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T07:27:50Z [2,364ms] [Warning] [carb] Failed to startup plugin carb.windowing-glfw.plugin (interfaces: [carb::windowing::IGLContext v1.0],[carb::windowing::IWindowing v1.5]) (impl: carb.windowing-glfw.plugin)
+2026-05-25T07:27:50Z [2,558ms] [Warning] [omni.log] Source: omni.hydra was already registered.
+2026-05-25T07:27:51Z [2,866ms] [Warning] [carb] Acquiring non optional plugin interface which is not listed as dependency: [omni::physx::IPhysxBenchmarks v1.0] (plugin: <default plugin>), by client: omni.physics.physx.plugin. Add it to CARB_PLUGIN_IMPL_DEPS() macro of a client.
+2026-05-25T07:27:51Z [2,877ms] [Warning] [omni.isaac.dynamic_control] omni.isaac.dynamic_control is deprecated as of Isaac Sim 4.5. No action is needed from end-users.
+2026-05-25T07:27:53Z [5,291ms] [Warning] [pxr.Semantics] pxr.Semantics is deprecated - please use Semantics instead
+2026-05-25T07:27:53Z [5,460ms] [Warning] [omni.graph.core.plugin] Found duplicate of category 'Replicator' - was 'Annotators', adding 'Fabric Reader'
+2026-05-25T07:27:53Z [5,460ms] [Warning] [omni.graph.core.plugin] Category 'Replicator' not accepted on node type 'omni.replicator.core.FabricReader' in extension 'omni.replicator.core'
+2026-05-25T07:27:53Z [5,460ms] [Warning] [omni.replicator.core.scripts.extension] No material configuration file, adding configuration to material settings directly.
+2026-05-25T07:28:43Z [55,340ms] [Warning] [omni.fabric.plugin] Warning: attribute viewportHandle not found for bucket id 7
+
+[INFO]: Parsing configuration from: simulator.tasks.cutlery_arrangement.cutlery_arrangement_env_cfg:CutleryArrangementEnvCfg
+Loaded 10 replay episodes from data/umi/test_fsm_v3/object_poses_10_shared.json
+
+======================================================================================
+[INFO][IsaacLab]: Logging to file: /tmp/isaaclab/logs/isaaclab_2026-05-25_07-30-26.log
+======================================================================================
+
+07:30:27 [simulation_context.py] WARNING: The `enable_external_forces_every_iteration` parameter in the PhysxCfg is set to False. If you are experiencing noisy velocities, consider enabling this flag. You may need to slightly increase the number of velocity iterations (setting it to 1 or 2 rather than 0), together with this flag, to improve the accuracy of velocity updates.
+[INFO]: Base environment:
+	Environment device    : cuda:0
+	Environment seed      : 1779694226
+	Physics step-size     : 0.016666666666666666
+	Rendering step-size   : 0.016666666666666666
+	Environment step-size : 0.016666666666666666
+07:30:29 [prims.py] WARNING: Could not perform 'modify_mass_properties' on any prims under: '/World/envs/env_0/Scene/plate'. This might be because of the following reasons:
+	(1) The desired attribute does not exist on any of the prims.
+	(2) The desired attribute exists on an instanced prim.
+		Discovered list of instanced prim paths: []
+07:30:29 [prims.py] WARNING: Could not perform 'modify_mass_properties' on any prims under: '/World/envs/env_0/Scene/knife'. This might be because of the following reasons:
+	(1) The desired attribute does not exist on any of the prims.
+	(2) The desired attribute exists on an instanced prim.
+		Discovered list of instanced prim paths: []
+07:30:29 [prims.py] WARNING: Could not perform 'modify_mass_properties' on any prims under: '/World/envs/env_0/Scene/fork'. This might be because of the following reasons:
+	(1) The desired attribute does not exist on any of the prims.
+	(2) The desired attribute exists on an instanced prim.
+		Discovered list of instanced prim paths: []
+[INFO]: Time taken for scene creation : 1.798379 seconds
+[INFO]: Scene manager:  <class InteractiveScene>
+	Number of environments: 1
+	Environment spacing   : 8.0
+	Source prim name      : /World/envs/env_0
+	Global prim paths     : []
+	Replicate physics     : True
+[INFO]: Starting the simulation. This may take a few seconds. Please wait...
+2026-05-25T07:30:35Z [167,452ms] [Warning] [rtx.postprocessing.plugin] DLSS increasing input dimensions: Render resolution of (371, 278) is below minimal input resolution of 300.
+[INFO]: Time taken for simulation start : 6.544522 seconds
+[INFO] Command Manager:  <CommandManager> contains 0 active terms.
++------------------------+
+|  Active Command Terms  |
++--------+-------+-------+
+| Index  | Name  |  Type |
++--------+-------+-------+
++--------+-------+-------+
+
+[INFO] Event Manager:  <EventManager> contains 1 active terms.
++---------------------------------+
+| Active Event Terms in Mode: 'reset' |
++------------+--------------------+
+|   Index    | Name               |
++------------+--------------------+
+|     0      | reset_all          |
++------------+--------------------+
+
+[INFO] Recorder Manager:  <RecorderManager> contains 5 active terms.
++--------------------------------------------------+
+|              Active Recorder Terms               |
++-------+------------------------------------------+
+| Index | Name                                     |
++-------+------------------------------------------+
+|   0   | record_initial_state                     |
+|   1   | record_post_step_states                  |
+|   2   | record_pre_step_actions                  |
+|   3   | record_pre_step_flat_policy_observations |
+|   4   | record_post_step_processed_actions       |
++-------+------------------------------------------+
+
+[INFO] Action Manager:  <ActionManager> contains 2 active terms.
++------------------------------------+
+|   Active Action Terms (shape: 8)   |
++-------+----------------+-----------+
+| Index | Name           | Dimension |
++-------+----------------+-----------+
+|   0   | arm_action     |         7 |
+|   1   | gripper_action |         1 |
++-------+----------------+-----------+
+
+[INFO] Observation Manager: <ObservationManager> contains 1 groups.
++-----------------------------------------------+
+|  Active Observation Terms in Group: 'policy'  |
++--------+--------------------+-----------------+
+| Index  | Name               |      Shape      |
++--------+--------------------+-----------------+
+|   0    | joint_pos          |       (9,)      |
+|   1    | joint_vel          |       (9,)      |
+|   2    | joint_pos_rel      |       (9,)      |
+|   3    | joint_vel_rel      |       (9,)      |
+|   4    | actions            |       (8,)      |
+|   5    | wrist              |  (480, 640, 3)  |
+|   6    | front              |  (480, 640, 3)  |
+|   7    | joint_pos_target   |       (9,)      |
++--------+--------------------+-----------------+
+
+[INFO] Termination Manager:  <TerminationManager> contains 1 active terms.
++----------------------------+
+|  Active Termination Terms  |
++-------+---------+----------+
+| Index | Name    | Time Out |
++-------+---------+----------+
+|   0   | success |  False   |
++-------+---------+----------+
+
+[INFO] Reward Manager:  <RewardManager> contains 0 active terms.
++-----------------------+
+|  Active Reward Terms  |
++-------+------+--------+
+| Index | Name | Weight |
++-------+------+--------+
++-------+------+--------+
+
+[INFO] Curriculum Manager:  <CurriculumManager> contains 0 active terms.
++----------------------+
+| Active Curriculum Terms |
++-----------+----------+
+|   Index   | Name     |
++-----------+----------+
++-----------+----------+
+
+[INFO]: Completed setting up the environment...
+  [pose] fork: pos=(0.599, -0.172, 0.050) yaw=  -0.0°
+  [pose] knife: pos=(0.503, -0.261, 0.050) yaw=+180.0°
+Start Recording!!!
+2026-05-25T07:30:40Z [171,832ms] [Warning] [carb] Client gpu.foundation.plugin has acquired [gpu::unstable::IMemoryBudgetManagerFactory v0.1] 100 times. Consider accessing this interface with carb::getCachedInterface() (Performance warning)
+Episode failed!
+Stop Recording!!!
+  [pose] fork: pos=(0.348, -0.264, 0.050) yaw=  -0.0°
+  [pose] knife: pos=(0.475, -0.230, 0.050) yaw=+180.0°
+[video] saved /workspace/aicapstone/fsm_experiments/exp_all_changes/videos/ep_01_failed.mp4 (133 frames)
+[Data Usage]1/10 fail.
+Start Recording!!!
+Episode failed!
+Stop Recording!!!
+  [pose] fork: pos=(0.408, -0.010, 0.050) yaw=  -0.0°
+  [pose] knife: pos=(0.626, -0.034, 0.050) yaw=+180.0°
+[video] saved /workspace/aicapstone/fsm_experiments/exp_all_changes/videos/ep_02_failed.mp4 (133 frames)
+[Data Usage]1/10 fail.
+Start Recording!!!
+Episode failed!
+Stop Recording!!!
+  [pose] knife: pos=(0.136, -0.188, 0.050) yaw=+180.0°
+  [pose] fork: pos=(0.310, -0.261, 0.050) yaw=  -0.0°
+[video] saved /workspace/aicapstone/fsm_experiments/exp_all_changes/videos/ep_03_failed.mp4 (133 frames)
+[Data Usage]1/10 fail.
+Start Recording!!!
+Episode failed!
+Stop Recording!!!
+  [pose] knife: pos=(0.138, -0.101, 0.050) yaw=+180.0°
+  [pose] fork: pos=(0.273, -0.355, 0.050) yaw=  -0.0°
+[video] saved /workspace/aicapstone/fsm_experiments/exp_all_changes/videos/ep_04_failed.mp4 (133 frames)
+[Data Usage]1/10 fail.
+Start Recording!!!
+Episode failed!
+Stop Recording!!!
+  [pose] knife: pos=(0.113, -0.284, 0.050) yaw=+180.0°
+  [pose] fork: pos=(0.137, -0.081, 0.050) yaw=  -0.0°
+[video] saved /workspace/aicapstone/fsm_experiments/exp_all_changes/videos/ep_05_failed.mp4 (133 frames)
+[Data Usage]1/10 fail.
+Start Recording!!!
+Episode failed!
+Stop Recording!!!
+  [pose] fork: pos=(0.171, -0.498, 0.050) yaw=  -0.0°
+  [pose] knife: pos=(0.452, -0.561, 0.050) yaw=+180.0°
+[video] saved /workspace/aicapstone/fsm_experiments/exp_all_changes/videos/ep_06_failed.mp4 (133 frames)
+[Data Usage]1/10 fail.
+Start Recording!!!
+Episode failed!
+Stop Recording!!!
+  [pose] fork: pos=(0.385, -0.486, 0.050) yaw=  -0.0°
+  [pose] knife: pos=(0.314, -0.241, 0.050) yaw=+180.0°
+[video] saved /workspace/aicapstone/fsm_experiments/exp_all_changes/videos/ep_07_failed.mp4 (133 frames)
+[Data Usage]1/10 fail.
+Start Recording!!!
+Episode failed!
+Stop Recording!!!
+  [pose] fork: pos=(0.133, -0.028, 0.050) yaw=  -0.0°
+  [pose] knife: pos=(0.130, -0.237, 0.050) yaw=+180.0°
+[video] saved /workspace/aicapstone/fsm_experiments/exp_all_changes/videos/ep_08_failed.mp4 (133 frames)
+[Data Usage]1/10 fail.
+Start Recording!!!
+Episode failed!
+Stop Recording!!!
+  [pose] knife: pos=(0.350, -0.499, 0.050) yaw=+180.0°
+  [pose] fork: pos=(0.383, -0.364, 0.050) yaw=  -0.0°
+[video] saved /workspace/aicapstone/fsm_experiments/exp_all_changes/videos/ep_09_failed.mp4 (133 frames)
+[Data Usage]1/10 fail.
+Start Recording!!!
+Episode failed!
+Stop Recording!!!
+Replayed all 10 episodes. Exiting the app.
+[video] saved /workspace/aicapstone/fsm_experiments/exp_all_changes/videos/ep_10_failed.mp4 (133 frames)
+[Data Usage]1/10 fail.
+=== exp_all_changes done. S=0
+0 F=10 ===

exp_all_changes/videos/ep_01_failed.mp4

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

exp_all_changes/videos/ep_02_failed.mp4

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

exp_all_changes/videos/ep_03_failed.mp4

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

exp_all_changes/videos/ep_04_failed.mp4

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

exp_all_changes/videos/ep_05_failed.mp4

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

exp_all_changes/videos/ep_06_failed.mp4

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

exp_all_changes/videos/ep_07_failed.mp4

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

exp_all_changes/videos/ep_08_failed.mp4

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

exp_all_changes/videos/ep_09_failed.mp4

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

exp_all_changes/videos/ep_10_failed.mp4

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