Upload folder using huggingface_hub

.gitattributes

@@ -68,3 +68,15 @@ exp_lift_fix/videos/ep_09_failed.mp4 filter=lfs diff=lfs merge=lfs -text
 exp_lift_fix/videos/ep_10_failed.mp4 filter=lfs diff=lfs merge=lfs -text
 exp_lift_fix/all_failed.mp4 filter=lfs diff=lfs merge=lfs -text
 exp_lift_fix/all_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_v2/all_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_v2/all_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_v2/videos/ep_01_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_v2/videos/ep_02_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_v2/videos/ep_03_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_v2/videos/ep_04_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_v2/videos/ep_05_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_v2/videos/ep_06_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_v2/videos/ep_07_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_v2/videos/ep_08_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_v2/videos/ep_09_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_v2/videos/ep_10_failed.mp4 filter=lfs diff=lfs merge=lfs -text

exp_lift_v2/CHANGES.md

@@ -0,0 +1,49 @@
+# exp_lift_v2 — fixes the "extends outward then comes back" bug
+
+Builds on `exp_lift_fix` (which already fixed unbounded vertical chase and the
+premature gripper-open). Two new fixes here:
+
+## 1. Phase 3 (lift) — eliminate xy drift
+
+**Bug:** After grasp, the held object's xy has a fixed +offset (≈ 2.5 cm in the
+away-from-robot direction) relative to the gripper, because the grasp anchor was
+pulled inward via `_retreat_xy_toward(obj, robot, 0.025)`. The previous lift
+code targeted `obj.xy`, which always sat at `gripper.xy + offset`. Each step the
+IK moved the gripper ~1.8 cm toward that target — but the object (and thus the
+target) moved with the gripper. The chase never converged: the arm drifted
+continuously *away from the robot*, which is usually also *away from the plate*.
+That's the wasted 1-2 seconds of "extends outward, then comes back" before
+phase 4 starts pulling the arm toward the plate.
+
+**Fix:** Capture the EE xy at the FIRST step of phase 3 and use that fixed xy
+as the target throughout the phase. The arm now lifts straight up from the grasp
+point. New state field `_lift_start_ee_xy_w`, reset on object boundary and at
+`reset()`.
+
+## 2. `_LIFT_Z_OFFSET` raised back from 0.15 → 0.20
+
+The 0.15 m clearance was too tight — the lifted cutlery and the arm itself were
+brushing/clipping the *other* cutlery on the table. Back to the original 0.20 m
+(1.33× the previous value).
+
+## Carried over from `exp_lift_fix`
+
+- Phase 3 uses absolute target z = `plate.z + LIFT_Z_OFFSET` (not relative to
+  the held object's chasing z).
+- Phase 6 (retreat) keeps gripper CLOSED until EE is within
+  `_RELEASE_TOL_M = 0.015` of the release altitude.
+
+## Unchanged (per request)
+
+- `_MAX_CARTESIAN_DELTA = 0.018` — no speed changes.
+- `_PHASE_DURATIONS_PER_OBJECT = (180, 130, 20, 160, 170, 15, 30)` — original
+  timing; we'll re-tune phase durations once correctness is locked in.
+
+## Compared to earlier experiments
+
+| Experiment | Success / Failure | Key change |
+|---|---|---|
+| `exp_all_changes` | 0 / 10 | Aggressive everything |
+| `exp_heights_only` | 4 / 6 | Heights only |
+| `exp_lift_fix` | 5 / 5 | + fixed-z lift + release-z check |
+| `exp_lift_v2` (this) | **?** / **?** | + fixed-xy lift + raised LIFT to 0.20 |

exp_lift_v2/SUMMARY.md

@@ -0,0 +1,32 @@
+# FSM Experiment: exp_lift_v2
+
+- Date: Mon May 25 06:16:42 PM PST 2026
+- Poses: `data/umi/test_fsm_v3/object_poses_10_shared.json`
+- Successes: **6** / Failures: **4**
+
+```python
+_MAX_CARTESIAN_DELTA = 0.018  # original; per-step IK clamp not changed in this version
+_HOVER_Z_OFFSET = 0.15
+_GRASP_Z_OFFSET = 0.08
+_LIFT_Z_OFFSET = 0.20        # raised back to 0.20 (0.15 was too low — held cutlery and arm clipped neighbouring objects)
+_RELEASE_Z_OFFSET = 0.06     # was 0.09; 0.04 was too low (gripper fingers hit plate/table)
+_RELEASE_TOL_M = 0.015       # phase 6 keeps gripper CLOSED until EE is within this band of release_z
+_PHASE_DURATIONS_PER_OBJECT = (180, 130, 20, 160, 170, 15, 30)  # original durations; correctness fix first, re-time later
+_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] = plate_pos_w[:, 2] + _LIFT_Z_OFFSET
+        target[:, 2] += _LIFT_Z_OFFSET
+        target[:, 2] += _RELEASE_Z_OFFSET
+        # Only once the EE is within `_RELEASE_TOL_M` of the release altitude do
+            release_z = place_pos_w[:, 2] + _RELEASE_Z_OFFSET
+            descent_complete = bool((ee_z <= release_z + _RELEASE_TOL_M).all().item())
+            target[:, 2] += _RELEASE_Z_OFFSET
+        target[:, 2] += _LIFT_Z_OFFSET
+```
+
+See `CHANGES.md` for the changes in this version vs the previous `exp_lift_fix`.

exp_lift_v2/all_failed.mp4

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

exp_lift_v2/all_success.mp4

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

exp_lift_v2/cutlery_arrangement.py

@@ -0,0 +1,520 @@
+"""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.018  # original; per-step IK clamp not changed in this version
+_MAX_ROT_DELTA = 0.08
+_IK_DLS_LAMBDA = 0.01
+
+_HOVER_Z_OFFSET = 0.15
+_GRASP_Z_OFFSET = 0.08
+_LIFT_Z_OFFSET = 0.20        # raised back to 0.20 (0.15 was too low — held cutlery and arm clipped neighbouring objects)
+_RELEASE_Z_OFFSET = 0.06     # was 0.09; 0.04 was too low (gripper fingers hit plate/table)
+_RELEASE_TOL_M = 0.015       # phase 6 keeps gripper CLOSED until EE is within this band of release_z
+_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 = (180, 130, 20, 160, 170, 15, 30)  # original durations; correctness fix first, re-time later
+_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)
+        # Captured EE xy at start of phase 3 (lift). Prevents target drift caused
+        # by tracking the held object's moving position.
+        self._lift_start_ee_xy_w: torch.Tensor | None = None
+
+    # ------------------------------------------------------------------
+    # 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:
+            # Lift: capture EE xy on first call so the target stays fixed (avoids
+            # drifting with the held object's xy). Target z is plate.z + LIFT_Z.
+            ee_pos_now_w = self._ee_pos_w(robot)
+            target_pos_w, gripper_cmd = self._phase_lift(ee_pos_now_w, plate_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:
+            # Phase 6: only open gripper once descent is verified complete.
+            target_pos_w, gripper_cmd = self._phase_retreat(
+                place_target_w, num_envs, device, robot=robot
+            )
+
+        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, ee_pos_w, plate_pos_w, num_envs, device):
+        # Capture EE xy on the FIRST step of this phase so the target stays fixed.
+        # If we used obj.xy (or refreshed ee.xy every step), the target would drift
+        # with the held object's offset relative to the gripper — causing the arm
+        # to extend outward instead of just lifting straight up.
+        if self._step_count == 0 or self._lift_start_ee_xy_w is None:
+            self._lift_start_ee_xy_w = ee_pos_w[:, :2].clone()
+        target = ee_pos_w.clone()
+        target[:, :2] = self._lift_start_ee_xy_w
+        # Fixed altitude: plate.z + LIFT_Z_OFFSET (no unbounded chase upward).
+        target[:, 2] = plate_pos_w[:, 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, robot=None):
+        # EXPLICIT DESCENT-COMPLETE CHECK: if the EE has not actually reached the
+        # release altitude yet (because phase 5 ran out of steps or IK was slow),
+        # we keep gripper CLOSED and continue targeting the release point.
+        # Only once the EE is within `_RELEASE_TOL_M` of the release altitude do
+        # we transition to the actual retreat (gripper open + target up).
+        if robot is not None and self._ee_body_idx >= 0:
+            ee_z = robot.data.body_pos_w[:, self._ee_body_idx, 2]
+            release_z = place_pos_w[:, 2] + _RELEASE_Z_OFFSET
+            descent_complete = bool((ee_z <= release_z + _RELEASE_TOL_M).all().item())
+        else:
+            descent_complete = True
+
+        if not descent_complete:
+            # Still descending — hold release-point target, keep gripper closed.
+            target = place_pos_w.clone()
+            target[:, 2] += _RELEASE_Z_OFFSET
+            return target, _constant_gripper(num_envs, device, _GRIPPER_CLOSE)
+
+        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
+            self._lift_start_ee_xy_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
+        self._lift_start_ee_xy_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_lift_v2/datagen.log

@@ -0,0 +1,520 @@
+=== Running exp_lift_v2 ===
+Mon May 25 05:55:59 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_095730.log
+2026-05-25T09:57:30Z [176ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T09:57:30Z [176ms] [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-25T09:57:30Z [186ms] [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: 29391
+| Total Page/Swap (MB): 4095 | Free Page/Swap: 0
+|---------------------------------------------------------------------------------------------|
+2026-05-25T09:57:31Z [894ms] [Warning] [gpu.foundation.plugin] CPU performance profile is set to powersave. This profile sets the CPU to the lowest frequency reducing performance.
+2026-05-25T09:57:31Z [1,494ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T09:57:31Z [1,494ms] [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-25T09:57:31Z [1,621ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T09:57:31Z [1,621ms] [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-25T09:57:31Z [1,629ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T09:57:31Z [1,629ms] [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-25T09:57:31Z [1,637ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T09:57:31Z [1,637ms] [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-25T09:57:31Z [1,639ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T09:57:31Z [1,639ms] [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-25T09:57:31Z [1,640ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T09:57:31Z [1,640ms] [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-25T09:57:31Z [1,833ms] [Warning] [omni.log] Source: omni.hydra was already registered.
+2026-05-25T09:57:32Z [2,148ms] [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-25T09:57:32Z [2,159ms] [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-25T09:57:34Z [4,561ms] [Warning] [pxr.Semantics] pxr.Semantics is deprecated - please use Semantics instead
+2026-05-25T09:57:34Z [4,727ms] [Warning] [omni.graph.core.plugin] Found duplicate of category 'Replicator' - was 'Annotators', adding 'Fabric Reader'
+2026-05-25T09:57:34Z [4,727ms] [Warning] [omni.graph.core.plugin] Category 'Replicator' not accepted on node type 'omni.replicator.core.FabricReader' in extension 'omni.replicator.core'
+2026-05-25T09:57:34Z [4,728ms] [Warning] [omni.replicator.core.scripts.extension] No material configuration file, adding configuration to material settings directly.
+2026-05-25T09:58:24Z [54,019ms] [Warning] [omni.fabric.plugin] Warning: attribute viewPickingId 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_10-00-09.log
+======================================================================================
+
+10:00:10 [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      : 1779703209
+	Physics step-size     : 0.016666666666666666
+	Rendering step-size   : 0.016666666666666666
+	Environment step-size : 0.016666666666666666
+10:00:11 [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: []
+10:00:12 [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: []
+10:00:12 [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.481702 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-25T10:00:18Z [167,953ms] [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.297369 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-25T10:00:22Z [172,299ms] [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 success!
+Stop Recording!!!
+
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [version]:	SVT-AV1 Encoder Lib v3.0.0
+Svt[info]: SVT [build]  :	GCC 14.2.1 20250110 (Red Hat 14.2.1-7)	 64 bit
+Svt[info]: LIB Build date: Jul  3 2025 03:14:07
+Svt[info]: -------------------------------------------
+Svt[warn]: Preset M12 is mapped to M10.
+Svt[info]: Level of Parallelism: 5
+Svt[info]: Number of PPCS 76
+Svt[info]: [asm level on system : up to avx2]
+Svt[info]: [asm level selected : up to avx2]
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [config]: main profile	tier (auto)	level (auto)
+Svt[info]: SVT [config]: width / height / fps numerator / fps denominator 		: 640 / 480 / 30 / 1
+Svt[info]: SVT [config]: bit-depth / color format 					: 8 / YUV420
+Svt[info]: SVT [config]: preset / tune / pred struct 					: 10 / PSNR / random access
+Svt[info]: SVT [config]: gop size / mini-gop size / key-frame type 			: 2 / 16 / key frame
+Svt[info]: SVT [config]: BRC mode / rate factor 					: CRF / 30 
+Svt[info]: SVT [config]: AQ mode / variance boost 					: 2 / 0
+Svt[info]: SVT [config]: sharpness / luminance-based QP bias 			: 0 / 0
+Svt[info]: Svt[info]: -------------------------------------------
+Svt[warn]: Failed to set thread priority: Invalid argument
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [version]:	SVT-AV1 Encoder Lib v3.0.0
+Svt[info]: SVT [build]  :	GCC 14.2.1 20250110 (Red Hat 14.2.1-7)	 64 bit
+Svt[info]: LIB Build date: Jul  3 2025 03:14:07
+Svt[info]: -------------------------------------------
+Svt[warn]: Preset M12 is mapped to M10.
+Svt[info]: Level of Parallelism: 5
+Svt[info]: Number of PPCS 76
+Svt[info]: [asm level on system : up to avx2]
+Svt[info]: [asm level selected : up to avx2]
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [config]: main profile	tier (auto)	level (auto)
+Svt[info]: SVT [config]: width / height / fps numerator / fps denominator 		: 640 / 480 / 30 / 1
+Svt[info]: SVT [config]: bit-depth / color format 					: 8 / YUV420
+Svt[info]: SVT [config]: preset / tune / pred struct 					: 10 / PSNR / random access
+Svt[info]: SVT [config]: gop size / mini-gop size / key-frame type 			: 2 / 16 / key frame
+Svt[info]: SVT [config]: BRC mode / rate factor 					: CRF / 30 
+Svt[info]: SVT [config]: AQ mode / variance boost 					: 2 / 0
+Svt[info]: SVT [config]: sharpness / luminance-based QP bias 			: 0 / 0
+Svt[info]: Svt[info]: -------------------------------------------
+  [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_lift_v2/videos/ep_01_success.mp4 (235 frames)
+[Data Usage]1/10 success.
+Start Recording!!!
+Episode success!
+Stop Recording!!!
+
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [version]:	SVT-AV1 Encoder Lib v3.0.0
+Svt[info]: SVT [build]  :	GCC 14.2.1 20250110 (Red Hat 14.2.1-7)	 64 bit
+Svt[info]: LIB Build date: Jul  3 2025 03:14:07
+Svt[info]: -------------------------------------------
+Svt[warn]: Preset M12 is mapped to M10.
+Svt[info]: Level of Parallelism: 5
+Svt[info]: Number of PPCS 76
+Svt[info]: [asm level on system : up to avx2]
+Svt[info]: [asm level selected : up to avx2]
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [config]: main profile	tier (auto)	level (auto)
+Svt[info]: SVT [config]: width / height / fps numerator / fps denominator 		: 640 / 480 / 30 / 1
+Svt[info]: SVT [config]: bit-depth / color format 					: 8 / YUV420
+Svt[info]: SVT [config]: preset / tune / pred struct 					: 10 / PSNR / random access
+Svt[info]: SVT [config]: gop size / mini-gop size / key-frame type 			: 2 / 16 / key frame
+Svt[info]: SVT [config]: BRC mode / rate factor 					: CRF / 30 
+Svt[info]: SVT [config]: AQ mode / variance boost 					: 2 / 0
+Svt[info]: SVT [config]: sharpness / luminance-based QP bias 			: 0 / 0
+Svt[info]: Svt[info]: -------------------------------------------
+[mp4 @ 0x7c6357008580] Starting second pass: moving the moov atom to the beginning of the file
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [version]:	SVT-AV1 Encoder Lib v3.0.0
+Svt[info]: SVT [build]  :	GCC 14.2.1 20250110 (Red Hat 14.2.1-7)	 64 bit
+Svt[info]: LIB Build date: Jul  3 2025 03:14:07
+Svt[info]: -------------------------------------------
+Svt[warn]: Preset M12 is mapped to M10.
+Svt[info]: Level of Parallelism: 5
+Svt[info]: Number of PPCS 76
+Svt[info]: [asm level on system : up to avx2]
+Svt[info]: [asm level selected : up to avx2]
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [config]: main profile	tier (auto)	level (auto)
+Svt[info]: SVT [config]: width / height / fps numerator / fps denominator 		: 640 / 480 / 30 / 1
+Svt[info]: SVT [config]: bit-depth / color format 					: 8 / YUV420
+Svt[info]: SVT [config]: preset / tune / pred struct 					: 10 / PSNR / random access
+Svt[info]: SVT [config]: gop size / mini-gop size / key-frame type 			: 2 / 16 / key frame
+Svt[info]: SVT [config]: BRC mode / rate factor 					: CRF / 30 
+Svt[info]: SVT [config]: AQ mode / variance boost 					: 2 / 0
+Svt[info]: SVT [config]: sharpness / luminance-based QP bias 			: 0 / 0
+Svt[info]: Svt[info]: -------------------------------------------
+[mp4 @ 0x7c639add8d40] Starting second pass: moving the moov atom to the beginning of the file
+  [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_lift_v2/videos/ep_02_success.mp4 (235 frames)
+[Data Usage]2/10 success.
+Start Recording!!!
+Episode success!
+Stop Recording!!!
+
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [version]:	SVT-AV1 Encoder Lib v3.0.0
+Svt[info]: SVT [build]  :	GCC 14.2.1 20250110 (Red Hat 14.2.1-7)	 64 bit
+Svt[info]: LIB Build date: Jul  3 2025 03:14:07
+Svt[info]: -------------------------------------------
+Svt[warn]: Preset M12 is mapped to M10.
+Svt[info]: Level of Parallelism: 5
+Svt[info]: Number of PPCS 76
+Svt[info]: [asm level on system : up to avx2]
+Svt[info]: [asm level selected : up to avx2]
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [config]: main profile	tier (auto)	level (auto)
+Svt[info]: SVT [config]: width / height / fps numerator / fps denominator 		: 640 / 480 / 30 / 1
+Svt[info]: SVT [config]: bit-depth / color format 					: 8 / YUV420
+Svt[info]: SVT [config]: preset / tune / pred struct 					: 10 / PSNR / random access
+Svt[info]: SVT [config]: gop size / mini-gop size / key-frame type 			: 2 / 16 / key frame
+Svt[info]: SVT [config]: BRC mode / rate factor 					: CRF / 30 
+Svt[info]: SVT [config]: AQ mode / variance boost 					: 2 / 0
+Svt[info]: SVT [config]: sharpness / luminance-based QP bias 			: 0 / 0
+Svt[info]: Svt[info]: -------------------------------------------
+[mp4 @ 0x7c639b90dd40] Starting second pass: moving the moov atom to the beginning of the file
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [version]:	SVT-AV1 Encoder Lib v3.0.0
+Svt[info]: SVT [build]  :	GCC 14.2.1 20250110 (Red Hat 14.2.1-7)	 64 bit
+Svt[info]: LIB Build date: Jul  3 2025 03:14:07
+Svt[info]: -------------------------------------------
+Svt[warn]: Preset M12 is mapped to M10.
+Svt[info]: Level of Parallelism: 5
+Svt[info]: Number of PPCS 76
+Svt[info]: [asm level on system : up to avx2]
+Svt[info]: [asm level selected : up to avx2]
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [config]: main profile	tier (auto)	level (auto)
+Svt[info]: SVT [config]: width / height / fps numerator / fps denominator 		: 640 / 480 / 30 / 1
+Svt[info]: SVT [config]: bit-depth / color format 					: 8 / YUV420
+Svt[info]: SVT [config]: preset / tune / pred struct 					: 10 / PSNR / random access
+Svt[info]: SVT [config]: gop size / mini-gop size / key-frame type 			: 2 / 16 / key frame
+Svt[info]: SVT [config]: BRC mode / rate factor 					: CRF / 30 
+Svt[info]: SVT [config]: AQ mode / variance boost 					: 2 / 0
+Svt[info]: SVT [config]: sharpness / luminance-based QP bias 			: 0 / 0
+Svt[info]: Svt[info]: -------------------------------------------
+[mp4 @ 0x7c639b408d40] Starting second pass: moving the moov atom to the beginning of the file
+  [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_lift_v2/videos/ep_03_success.mp4 (235 frames)
+[Data Usage]3/10 success.
+Start Recording!!!
+Episode success!
+Stop Recording!!!
+
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [version]:	SVT-AV1 Encoder Lib v3.0.0
+Svt[info]: SVT [build]  :	GCC 14.2.1 20250110 (Red Hat 14.2.1-7)	 64 bit
+Svt[info]: LIB Build date: Jul  3 2025 03:14:07
+Svt[info]: -------------------------------------------
+Svt[warn]: Preset M12 is mapped to M10.
+Svt[info]: Level of Parallelism: 5
+Svt[info]: Number of PPCS 76
+Svt[info]: [asm level on system : up to avx2]
+Svt[info]: [asm level selected : up to avx2]
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [config]: main profile	tier (auto)	level (auto)
+Svt[info]: SVT [config]: width / height / fps numerator / fps denominator 		: 640 / 480 / 30 / 1
+Svt[info]: SVT [config]: bit-depth / color format 					: 8 / YUV420
+Svt[info]: SVT [config]: preset / tune / pred struct 					: 10 / PSNR / random access
+Svt[info]: SVT [config]: gop size / mini-gop size / key-frame type 			: 2 / 16 / key frame
+Svt[info]: SVT [config]: BRC mode / rate factor 					: CRF / 30 
+Svt[info]: SVT [config]: AQ mode / variance boost 					: 2 / 0
+Svt[info]: SVT [config]: sharpness / luminance-based QP bias 			: 0 / 0
+Svt[info]: Svt[info]: -------------------------------------------
+[mp4 @ 0x7c639b408d40] Starting second pass: moving the moov atom to the beginning of the file
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [version]:	SVT-AV1 Encoder Lib v3.0.0
+Svt[info]: SVT [build]  :	GCC 14.2.1 20250110 (Red Hat 14.2.1-7)	 64 bit
+Svt[info]: LIB Build date: Jul  3 2025 03:14:07
+Svt[info]: -------------------------------------------
+Svt[warn]: Preset M12 is mapped to M10.
+Svt[info]: Level of Parallelism: 5
+Svt[info]: Number of PPCS 76
+Svt[info]: [asm level on system : up to avx2]
+Svt[info]: [asm level selected : up to avx2]
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [config]: main profile	tier (auto)	level (auto)
+Svt[info]: SVT [config]: width / height / fps numerator / fps denominator 		: 640 / 480 / 30 / 1
+Svt[info]: SVT [config]: bit-depth / color format 					: 8 / YUV420
+Svt[info]: SVT [config]: preset / tune / pred struct 					: 10 / PSNR / random access
+Svt[info]: SVT [config]: gop size / mini-gop size / key-frame type 			: 2 / 16 / key frame
+Svt[info]: SVT [config]: BRC mode / rate factor 					: CRF / 30 
+Svt[info]: SVT [config]: AQ mode / variance boost 					: 2 / 0
+Svt[info]: SVT [config]: sharpness / luminance-based QP bias 			: 0 / 0
+Svt[info]: Svt[info]: -------------------------------------------
+[mp4 @ 0x7c639b90dd40] Starting second pass: moving the moov atom to the beginning of the file
+  [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_lift_v2/videos/ep_04_success.mp4 (235 frames)
+[Data Usage]4/10 success.
+Start Recording!!!
+Episode success!
+Stop Recording!!!
+
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [version]:	SVT-AV1 Encoder Lib v3.0.0
+Svt[info]: SVT [build]  :	GCC 14.2.1 20250110 (Red Hat 14.2.1-7)	 64 bit
+Svt[info]: LIB Build date: Jul  3 2025 03:14:07
+Svt[info]: -------------------------------------------
+Svt[warn]: Preset M12 is mapped to M10.
+Svt[info]: Level of Parallelism: 5
+Svt[info]: Number of PPCS 76
+Svt[info]: [asm level on system : up to avx2]
+Svt[info]: [asm level selected : up to avx2]
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [config]: main profile	tier (auto)	level (auto)
+Svt[info]: SVT [config]: width / height / fps numerator / fps denominator 		: 640 / 480 / 30 / 1
+Svt[info]: SVT [config]: bit-depth / color format 					: 8 / YUV420
+Svt[info]: SVT [config]: preset / tune / pred struct 					: 10 / PSNR / random access
+Svt[info]: SVT [config]: gop size / mini-gop size / key-frame type 			: 2 / 16 / key frame
+Svt[info]: SVT [config]: BRC mode / rate factor 					: CRF / 30 
+Svt[info]: SVT [config]: AQ mode / variance boost 					: 2 / 0
+Svt[info]: SVT [config]: sharpness / luminance-based QP bias 			: 0 / 0
+Svt[info]: Svt[info]: -------------------------------------------
+[mp4 @ 0x7c639b58ad40] Starting second pass: moving the moov atom to the beginning of the file
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [version]:	SVT-AV1 Encoder Lib v3.0.0
+Svt[info]: SVT [build]  :	GCC 14.2.1 20250110 (Red Hat 14.2.1-7)	 64 bit
+Svt[info]: LIB Build date: Jul  3 2025 03:14:07
+Svt[info]: -------------------------------------------
+Svt[warn]: Preset M12 is mapped to M10.
+Svt[info]: Level of Parallelism: 5
+Svt[info]: Number of PPCS 76
+Svt[info]: [asm level on system : up to avx2]
+Svt[info]: [asm level selected : up to avx2]
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [config]: main profile	tier (auto)	level (auto)
+Svt[info]: SVT [config]: width / height / fps numerator / fps denominator 		: 640 / 480 / 30 / 1
+Svt[info]: SVT [config]: bit-depth / color format 					: 8 / YUV420
+Svt[info]: SVT [config]: preset / tune / pred struct 					: 10 / PSNR / random access
+Svt[info]: SVT [config]: gop size / mini-gop size / key-frame type 			: 2 / 16 / key frame
+Svt[info]: SVT [config]: BRC mode / rate factor 					: CRF / 30 
+Svt[info]: SVT [config]: AQ mode / variance boost 					: 2 / 0
+Svt[info]: SVT [config]: sharpness / luminance-based QP bias 			: 0 / 0
+Svt[info]: Svt[info]: -------------------------------------------
+[mp4 @ 0x7c639add8d40] Starting second pass: moving the moov atom to the beginning of the file
+  [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_lift_v2/videos/ep_05_success.mp4 (235 frames)
+[Data Usage]5/10 success.
+Start Recording!!!
+Episode success!
+Stop Recording!!!
+
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [version]:	SVT-AV1 Encoder Lib v3.0.0
+Svt[info]: SVT [build]  :	GCC 14.2.1 20250110 (Red Hat 14.2.1-7)	 64 bit
+Svt[info]: LIB Build date: Jul  3 2025 03:14:07
+Svt[info]: -------------------------------------------
+Svt[warn]: Preset M12 is mapped to M10.
+Svt[info]: Level of Parallelism: 5
+Svt[info]: Number of PPCS 76
+Svt[info]: [asm level on system : up to avx2]
+Svt[info]: [asm level selected : up to avx2]
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [config]: main profile	tier (auto)	level (auto)
+Svt[info]: SVT [config]: width / height / fps numerator / fps denominator 		: 640 / 480 / 30 / 1
+Svt[info]: SVT [config]: bit-depth / color format 					: 8 / YUV420
+Svt[info]: SVT [config]: preset / tune / pred struct 					: 10 / PSNR / random access
+Svt[info]: SVT [config]: gop size / mini-gop size / key-frame type 			: 2 / 16 / key frame
+Svt[info]: SVT [config]: BRC mode / rate factor 					: CRF / 30 
+Svt[info]: SVT [config]: AQ mode / variance boost 					: 2 / 0
+Svt[info]: SVT [config]: sharpness / luminance-based QP bias 			: 0 / 0
+Svt[info]: Svt[info]: -------------------------------------------
+[mp4 @ 0x7c639b58ad40] Starting second pass: moving the moov atom to the beginning of the file
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [version]:	SVT-AV1 Encoder Lib v3.0.0
+Svt[info]: SVT [build]  :	GCC 14.2.1 20250110 (Red Hat 14.2.1-7)	 64 bit
+Svt[info]: LIB Build date: Jul  3 2025 03:14:07
+Svt[info]: -------------------------------------------
+Svt[warn]: Preset M12 is mapped to M10.
+Svt[info]: Level of Parallelism: 5
+Svt[info]: Number of PPCS 76
+Svt[info]: [asm level on system : up to avx2]
+Svt[info]: [asm level selected : up to avx2]
+Svt[info]: -------------------------------------------
+Svt[info]: SVT [config]: main profile	tier (auto)	level (auto)
+Svt[info]: SVT [config]: width / height / fps numerator / fps denominator 		: 640 / 480 / 30 / 1
+Svt[info]: SVT [config]: bit-depth / color format 					: 8 / YUV420
+Svt[info]: SVT [config]: preset / tune / pred struct 					: 10 / PSNR / random access
+Svt[info]: SVT [config]: gop size / mini-gop size / key-frame type 			: 2 / 16 / key frame
+Svt[info]: SVT [config]: BRC mode / rate factor 					: CRF / 30 
+Svt[info]: SVT [config]: AQ mode / variance boost 					: 2 / 0
+Svt[info]: SVT [config]: sharpness / luminance-based QP bias 			: 0 / 0
+Svt[info]: Svt[info]: -------------------------------------------
+[mp4 @ 0x7c639ae40d40] Starting second pass: moving the moov atom to the beginning of the file
+  [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_lift_v2/videos/ep_06_success.mp4 (235 frames)
+[Data Usage]6/10 success.
+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_lift_v2/videos/ep_07_failed.mp4 (235 frames)
+[Data Usage]7/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_lift_v2/videos/ep_08_failed.mp4 (235 frames)
+[Data Usage]7/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_lift_v2/videos/ep_09_failed.mp4 (235 frames)
+[Data Usage]7/10 fail.
+Start Recording!!!
+Episode failed!
+Stop Recording!!!
+Replayed all 10 episodes. Exiting the app.
+[video] saved /workspace/aicapstone/fsm_experiments/exp_lift_v2/videos/ep_10_failed.mp4 (235 frames)
+[Data Usage]7/10 fail.
+/usr/lib/python3.11/multiprocessing/resource_tracker.py:254: UserWarning: resource_tracker: There appear to be 1 leaked semaphore objects to clean up at shutdown
+  warnings.warn('resource_tracker: There appear to be %d '
+Processing /home/threedavatar/Fran/ai-capstone26/aicapstone/fsm_experiments/exp_lift_v2/videos/...
+  concatenating 6 success videos → /home/threedavatar/Fran/ai-capstone26/aicapstone/fsm_experiments/exp_lift_v2/all_success.mp4
+  ✓ wrote /home/threedavatar/Fran/ai-capstone26/aicapstone/fsm_experiments/exp_lift_v2/all_success.mp4 (13M)
+  concatenating 4 failed videos → /home/threedavatar/Fran/ai-capstone26/aicapstone/fsm_experiments/exp_lift_v2/all_failed.mp4
+  ✓ wrote /home/threedavatar/Fran/ai-capstone26/aicapstone/fsm_experiments/exp_lift_v2/all_failed.mp4 (7.3M)
+Done.

exp_lift_v2/videos/ep_01_success.mp4

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

exp_lift_v2/videos/ep_02_success.mp4

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

exp_lift_v2/videos/ep_03_success.mp4

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

exp_lift_v2/videos/ep_04_success.mp4

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

exp_lift_v2/videos/ep_05_success.mp4

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

exp_lift_v2/videos/ep_06_success.mp4

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

exp_lift_v2/videos/ep_07_failed.mp4

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

exp_lift_v2/videos/ep_08_failed.mp4

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

exp_lift_v2/videos/ep_09_failed.mp4

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

exp_lift_v2/videos/ep_10_failed.mp4

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