Upload folder using huggingface_hub

.gitattributes

@@ -56,3 +56,13 @@ exp_heights_only/videos/ep_10_failed.mp4 filter=lfs diff=lfs merge=lfs -text
 exp_all_changes/all_failed.mp4 filter=lfs diff=lfs merge=lfs -text
 exp_heights_only/all_failed.mp4 filter=lfs diff=lfs merge=lfs -text
 exp_heights_only/all_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_fix/videos/ep_01_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_fix/videos/ep_02_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_fix/videos/ep_03_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_fix/videos/ep_04_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_fix/videos/ep_05_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_fix/videos/ep_06_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_fix/videos/ep_07_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_lift_fix/videos/ep_08_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+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/CHANGES.md

@@ -0,0 +1,28 @@
+# exp_lift_fix — structural fixes on top of heights-only
+
+This version keeps the height changes from `exp_heights_only` but **fixes two FSM
+logic bugs** that previous experiments couldn't address with constants alone.
+
+## What changed vs main
+
+| Change | Original | This version | Why |
+|---|---|---|---|
+| `_phase_lift` target z | `obj.z + LIFT_Z_OFFSET` (relative) | `plate.z + LIFT_Z_OFFSET` (absolute) | Original chased the held object's moving z; arm rose until joint limits. |
+| `_phase_retreat` gripper | Opens unconditionally | **Stays CLOSED until EE within `_RELEASE_TOL_M` of release_z** | Old code opened the gripper at start of phase 6 even if descent wasn't done. |
+| `_LIFT_Z_OFFSET` | 0.20 | 0.15 | Lower transit altitude (carried over from heights-only). |
+| `_RELEASE_Z_OFFSET` | 0.09 | 0.06 | Gentler placement. |
+| `_MAX_CARTESIAN_DELTA` | 0.018 | **0.018** (unchanged) | Per user request: no speed changes. |
+| `_PHASE_DURATIONS_PER_OBJECT` | `(180,130,20,160,170,15,30)` | **same** (unchanged) | Original timing kept for correctness; will re-time after this works. |
+
+## New constant
+`_RELEASE_TOL_M = 0.015` — phase 6 won't open the gripper until the EE z is within 1.5 cm
+of the release altitude.
+
+## Behavior expected vs prior runs
+- Arm should rise only to ~0.20 m (plate.z + 0.15) during lift, NOT to joint limits
+- Phase 4 transition should look like the place→pickup move that already worked
+- Phase 6 should hold gripper closed until descent confirmed, THEN open
+
+## What this still doesn't fix
+Episode length stays at ~25 s (we kept original phase durations). Eval cap is 20 s.
+Once we confirm the FSM behaves correctly, the next iteration will tune phase durations.

exp_lift_fix/SUMMARY.md

@@ -0,0 +1,33 @@
+# FSM Experiment: exp_lift_fix
+
+- Date: Mon May 25 05:32:28 PM PST 2026
+- Poses: `data/umi/test_fsm_v3/object_poses_10_shared.json`
+- Successes: **5** / Failures: **5**
+
+```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.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)
+_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
+        # `obj.z + _LIFT_Z_OFFSET` made the target always sit LIFT_Z_OFFSET above the
+        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 explanation.

exp_lift_fix/cutlery_arrangement.py

@@ -0,0 +1,510 @@
+"""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.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)
+_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)
+
+    # ------------------------------------------------------------------
+    # 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:
+            # Pass plate_pos_w so lift targets a FIXED altitude (plate.z + LIFT_Z_OFFSET)
+            # instead of chasing the held object's moving z (which causes unbounded rise).
+            target_pos_w, gripper_cmd = self._phase_lift(obj_pos_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, obj_pos_w, plate_pos_w, num_envs, device):
+        # FIX: target a FIXED altitude (plate.z + LIFT_Z_OFFSET), not relative to the
+        # currently-held object. The held object's z tracks the gripper, so the old
+        # `obj.z + _LIFT_Z_OFFSET` made the target always sit LIFT_Z_OFFSET above the
+        # gripper itself — the arm chased it upward until it hit joint limits.
+        target = obj_pos_w.clone()
+        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
+
+    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_lift_fix/datagen.log

@@ -0,0 +1,471 @@
+=== Running exp_lift_fix ===
+Mon May 25 05:12:22 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_091347.log
+2026-05-25T09:13:47Z [301ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T09:13:47Z [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-25T09:13:47Z [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: 29442
+| Total Page/Swap (MB): 4095 | Free Page/Swap: 0
+|---------------------------------------------------------------------------------------------|
+2026-05-25T09:13:48Z [1,295ms] [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:13:49Z [2,194ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T09:13:49Z [2,194ms] [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:13:49Z [2,324ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T09:13:49Z [2,324ms] [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:13:49Z [2,333ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T09:13:49Z [2,333ms] [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:13:49Z [2,340ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T09:13:49Z [2,340ms] [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:13:49Z [2,343ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T09:13:49Z [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-25T09:13:49Z [2,344ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T09:13:49Z [2,344ms] [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:13:49Z [2,542ms] [Warning] [omni.log] Source: omni.hydra was already registered.
+2026-05-25T09:13:50Z [2,891ms] [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:13:50Z [2,902ms] [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:13:52Z [5,449ms] [Warning] [pxr.Semantics] pxr.Semantics is deprecated - please use Semantics instead
+2026-05-25T09:13:52Z [5,627ms] [Warning] [omni.graph.core.plugin] Found duplicate of category 'Replicator' - was 'Annotators', adding 'Fabric Reader'
+2026-05-25T09:13:52Z [5,627ms] [Warning] [omni.graph.core.plugin] Category 'Replicator' not accepted on node type 'omni.replicator.core.FabricReader' in extension 'omni.replicator.core'
+2026-05-25T09:13:52Z [5,628ms] [Warning] [omni.replicator.core.scripts.extension] No material configuration file, adding configuration to material settings directly.
+2026-05-25T09:14:44Z [57,494ms] [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_09-16-28.log
+======================================================================================
+
+09:16:29 [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      : 1779700588
+	Physics step-size     : 0.016666666666666666
+	Rendering step-size   : 0.016666666666666666
+	Environment step-size : 0.016666666666666666
+09:16:30 [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: []
+09:16:30 [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: []
+09:16:30 [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.563058 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-25T09:16:37Z [169,886ms] [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.600648 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-25T09:16:41Z [174,378ms] [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_fix/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 @ 0x74cbfc4cdd40] 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 @ 0x74cbfcb8dd40] 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_fix/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 @ 0x74cbfc3dad40] 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 @ 0x74cbfd2d9d40] 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_fix/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 @ 0x74cbf3fffd40] 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 @ 0x74cbfc103d40] 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_fix/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 @ 0x74cbf3fffd40] 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 @ 0x74cbf3fffd40] 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_fix/videos/ep_05_success.mp4 (235 frames)
+[Data Usage]5/10 success.
+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_lift_fix/videos/ep_06_failed.mp4 (235 frames)
+[Data Usage]6/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_lift_fix/videos/ep_07_failed.mp4 (235 frames)
+[Data Usage]6/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_fix/videos/ep_08_failed.mp4 (235 frames)
+[Data Usage]6/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_fix/videos/ep_09_failed.mp4 (235 frames)
+[Data Usage]6/10 fail.
+Start Recording!!!
+Episode failed!
+Stop Recording!!!
+Replayed all 10 episodes. Exiting the app.
+[video] saved /workspace/aicapstone/fsm_experiments/exp_lift_fix/videos/ep_10_failed.mp4 (235 frames)
+[Data Usage]6/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 '

exp_lift_fix/videos/ep_01_success.mp4

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

exp_lift_fix/videos/ep_02_success.mp4

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

exp_lift_fix/videos/ep_03_success.mp4

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

exp_lift_fix/videos/ep_04_success.mp4

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

exp_lift_fix/videos/ep_05_success.mp4

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

exp_lift_fix/videos/ep_06_failed.mp4

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

exp_lift_fix/videos/ep_07_failed.mp4

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

exp_lift_fix/videos/ep_08_failed.mp4

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

exp_lift_fix/videos/ep_09_failed.mp4

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

exp_lift_fix/videos/ep_10_failed.mp4

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