Upload folder using huggingface_hub

.gitattributes

@@ -43,3 +43,13 @@ exp_all_changes/videos/ep_07_failed.mp4 filter=lfs diff=lfs merge=lfs -text
 exp_all_changes/videos/ep_08_failed.mp4 filter=lfs diff=lfs merge=lfs -text
 exp_all_changes/videos/ep_09_failed.mp4 filter=lfs diff=lfs merge=lfs -text
 exp_all_changes/videos/ep_10_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_heights_only/videos/ep_01_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_heights_only/videos/ep_02_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_heights_only/videos/ep_03_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_heights_only/videos/ep_04_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_heights_only/videos/ep_05_success.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_heights_only/videos/ep_06_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_heights_only/videos/ep_07_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_heights_only/videos/ep_08_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_heights_only/videos/ep_09_failed.mp4 filter=lfs diff=lfs merge=lfs -text
+exp_heights_only/videos/ep_10_failed.mp4 filter=lfs diff=lfs merge=lfs -text

exp_heights_only/CHANGES.md

@@ -0,0 +1,27 @@
+# exp_heights_only — what changed vs. main-branch FSM
+
+This is the **diagnostic comparison run** for `exp_all_changes`. Phase durations
+and IK speed are reverted to the original values, isolating the effect of the
+**height changes alone**.
+
+| Constant | Original (main) | This version | Why |
+|---|---|---|---|
+| `_LIFT_Z_OFFSET` | `0.20` | `0.15` | Lower transit altitude. |
+| `_RELEASE_Z_OFFSET` | `0.09` | `0.06` | Gentler placement (cutlery drops from 6 cm vs 9 cm). |
+| `_MAX_CARTESIAN_DELTA` | `0.018` | `0.018` | **Reverted** to original. |
+| `_PHASE_DURATIONS_PER_OBJECT` | `(180, 130, 20, 160, 170, 15, 30)` | **`(180, 130, 20, 160, 170, 15, 30)`** | **Reverted** to original. |
+
+## Purpose
+If `exp_all_changes` has high failure rate but this one doesn't, the timing
+(phase durations × IK speed) is the culprit and we should be more conservative.
+If both fail, the height changes themselves are bad.
+If neither fails, all four changes can be applied safely.
+
+## Important caveat
+Because phase durations are reverted, **this version will not fit in the 20s eval cap**
+(each episode runs ~25s). That's expected. We're only testing whether the FSM **physically
+completes the task** with the new heights.
+
+## What the videos should show
+- Slower execution (~25s per episode, original timing)
+- Same lower placement heights as `exp_all_changes`

exp_heights_only/SUMMARY.md

@@ -0,0 +1,31 @@
+# FSM Experiment: exp_heights_only
+
+- Date: Mon May 25 03:58:25 PM PST 2026
+- Poses file: `data/umi/test_fsm_v3/object_poses_10_shared.json`
+- Successes: **4** / Failures: **6**
+
+## FSM constants used in this run
+```python
+_MAX_CARTESIAN_DELTA = 0.018  # heights-only experiment: reverted to original
+_HOVER_Z_OFFSET = 0.15
+_GRASP_Z_OFFSET = 0.08
+_LIFT_Z_OFFSET = 0.15        # was 0.20; 0.10 was too low (held cutlery clipped plate edge during transport)
+_RELEASE_Z_OFFSET = 0.06     # was 0.09; 0.04 was too low (gripper fingers hit plate/table)
+_PHASE_DURATIONS_PER_OBJECT = (180, 130, 20, 160, 170, 15, 30)  # heights-only experiment: reverted to original
+_PHASES_PER_OBJECT = len(_PHASE_DURATIONS_PER_OBJECT)
+def _clamp_delta(delta: torch.Tensor, max_norm: float = _MAX_CARTESIAN_DELTA) -> torch.Tensor:
+    MAX_STEPS: int = len(_PICK_ORDER) * sum(_PHASE_DURATIONS_PER_OBJECT) + 100
+        self._events_dt: list[int] = list(_PHASE_DURATIONS_PER_OBJECT) * len(_PICK_ORDER)
+        target[:, 2] += _HOVER_Z_OFFSET
+        target[:, 2] += _GRASP_Z_OFFSET
+        target[:, 2] += _GRASP_Z_OFFSET
+        target[:, 2] += _LIFT_Z_OFFSET
+        target[:, 2] += _LIFT_Z_OFFSET
+        target[:, 2] += _RELEASE_Z_OFFSET
+        target[:, 2] += _LIFT_Z_OFFSET
+```
+
+## Videos
+Per-episode mp4 in `videos/` named `ep_NN_<success|failed>.mp4`. Side-by-side (wrist | front, 1280x480 @ 10 fps).
+
+See `CHANGES.md` for what was modified in this version and why.

exp_heights_only/cutlery_arrangement.py

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

exp_heights_only/datagen.log

@@ -0,0 +1,429 @@
+=== Running exp_heights_only ===
+Mon May 25 03:38:37 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_074002.log
+2026-05-25T07:40:02Z [172ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T07:40:02Z [172ms] [Warning] [carb] Failed to startup plugin carb.windowing-glfw.plugin (interfaces: [carb::windowing::IGLContext v1.0],[carb::windowing::IWindowing v1.5]) (impl: carb.windowing-glfw.plugin)
+2026-05-25T07:40:02Z [182ms] [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: 29452
+| Total Page/Swap (MB): 4095 | Free Page/Swap: 0
+|---------------------------------------------------------------------------------------------|
+2026-05-25T07:40:03Z [816ms] [Warning] [gpu.foundation.plugin] CPU performance profile is set to powersave. This profile sets the CPU to the lowest frequency reducing performance.
+2026-05-25T07:40:03Z [1,407ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T07:40:03Z [1,407ms] [Warning] [carb] Failed to startup plugin carb.windowing-glfw.plugin (interfaces: [carb::windowing::IGLContext v1.0],[carb::windowing::IWindowing v1.5]) (impl: carb.windowing-glfw.plugin)
+2026-05-25T07:40:03Z [1,536ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T07:40:03Z [1,536ms] [Warning] [carb] Failed to startup plugin carb.windowing-glfw.plugin (interfaces: [carb::windowing::IGLContext v1.0],[carb::windowing::IWindowing v1.5]) (impl: carb.windowing-glfw.plugin)
+2026-05-25T07:40:03Z [1,544ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T07:40:03Z [1,544ms] [Warning] [carb] Failed to startup plugin carb.windowing-glfw.plugin (interfaces: [carb::windowing::IGLContext v1.0],[carb::windowing::IWindowing v1.5]) (impl: carb.windowing-glfw.plugin)
+2026-05-25T07:40:03Z [1,552ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T07:40:03Z [1,552ms] [Warning] [carb] Failed to startup plugin carb.windowing-glfw.plugin (interfaces: [carb::windowing::IGLContext v1.0],[carb::windowing::IWindowing v1.5]) (impl: carb.windowing-glfw.plugin)
+2026-05-25T07:40:03Z [1,554ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T07:40:03Z [1,554ms] [Warning] [carb] Failed to startup plugin carb.windowing-glfw.plugin (interfaces: [carb::windowing::IGLContext v1.0],[carb::windowing::IWindowing v1.5]) (impl: carb.windowing-glfw.plugin)
+2026-05-25T07:40:03Z [1,555ms] [Warning] [carb.windowing-glfw.plugin] GLFW initialization failed.
+2026-05-25T07:40:03Z [1,555ms] [Warning] [carb] Failed to startup plugin carb.windowing-glfw.plugin (interfaces: [carb::windowing::IGLContext v1.0],[carb::windowing::IWindowing v1.5]) (impl: carb.windowing-glfw.plugin)
+2026-05-25T07:40:04Z [1,742ms] [Warning] [omni.log] Source: omni.hydra was already registered.
+2026-05-25T07:40:04Z [2,113ms] [Warning] [carb] Acquiring non optional plugin interface which is not listed as dependency: [omni::physx::IPhysxBenchmarks v1.0] (plugin: <default plugin>), by client: omni.physics.physx.plugin. Add it to CARB_PLUGIN_IMPL_DEPS() macro of a client.
+2026-05-25T07:40:04Z [2,125ms] [Warning] [omni.isaac.dynamic_control] omni.isaac.dynamic_control is deprecated as of Isaac Sim 4.5. No action is needed from end-users.
+2026-05-25T07:40:06Z [4,486ms] [Warning] [pxr.Semantics] pxr.Semantics is deprecated - please use Semantics instead
+2026-05-25T07:40:06Z [4,663ms] [Warning] [omni.graph.core.plugin] Found duplicate of category 'Replicator' - was 'Annotators', adding 'Fabric Reader'
+2026-05-25T07:40:06Z [4,663ms] [Warning] [omni.graph.core.plugin] Category 'Replicator' not accepted on node type 'omni.replicator.core.FabricReader' in extension 'omni.replicator.core'
+2026-05-25T07:40:06Z [4,663ms] [Warning] [omni.replicator.core.scripts.extension] No material configuration file, adding configuration to material settings directly.
+2026-05-25T07:40:55Z [53,211ms] [Warning] [omni.fabric.plugin] Warning: attribute viewportHandle not found for bucket id 7
+
+[INFO]: Parsing configuration from: simulator.tasks.cutlery_arrangement.cutlery_arrangement_env_cfg:CutleryArrangementEnvCfg
+Loaded 10 replay episodes from data/umi/test_fsm_v3/object_poses_10_shared.json
+
+======================================================================================
+[INFO][IsaacLab]: Logging to file: /tmp/isaaclab/logs/isaaclab_2026-05-25_07-42-40.log
+======================================================================================
+
+07:42:41 [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      : 1779694960
+	Physics step-size     : 0.016666666666666666
+	Rendering step-size   : 0.016666666666666666
+	Environment step-size : 0.016666666666666666
+07:42:43 [prims.py] WARNING: Could not perform 'modify_mass_properties' on any prims under: '/World/envs/env_0/Scene/plate'. This might be because of the following reasons:
+	(1) The desired attribute does not exist on any of the prims.
+	(2) The desired attribute exists on an instanced prim.
+		Discovered list of instanced prim paths: []
+07:42:43 [prims.py] WARNING: Could not perform 'modify_mass_properties' on any prims under: '/World/envs/env_0/Scene/knife'. This might be because of the following reasons:
+	(1) The desired attribute does not exist on any of the prims.
+	(2) The desired attribute exists on an instanced prim.
+		Discovered list of instanced prim paths: []
+07:42:43 [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.596632 seconds
+[INFO]: Scene manager:  <class InteractiveScene>
+	Number of environments: 1
+	Environment spacing   : 8.0
+	Source prim name      : /World/envs/env_0
+	Global prim paths     : []
+	Replicate physics     : True
+[INFO]: Starting the simulation. This may take a few seconds. Please wait...
+2026-05-25T07:42:49Z [167,004ms] [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.438627 seconds
+[INFO] Command Manager:  <CommandManager> contains 0 active terms.
++------------------------+
+|  Active Command Terms  |
++--------+-------+-------+
+| Index  | Name  |  Type |
++--------+-------+-------+
++--------+-------+-------+
+
+[INFO] Event Manager:  <EventManager> contains 1 active terms.
++---------------------------------+
+| Active Event Terms in Mode: 'reset' |
++------------+--------------------+
+|   Index    | Name               |
++------------+--------------------+
+|     0      | reset_all          |
++------------+--------------------+
+
+[INFO] Recorder Manager:  <RecorderManager> contains 5 active terms.
++--------------------------------------------------+
+|              Active Recorder Terms               |
++-------+------------------------------------------+
+| Index | Name                                     |
++-------+------------------------------------------+
+|   0   | record_initial_state                     |
+|   1   | record_post_step_states                  |
+|   2   | record_pre_step_actions                  |
+|   3   | record_pre_step_flat_policy_observations |
+|   4   | record_post_step_processed_actions       |
++-------+------------------------------------------+
+
+[INFO] Action Manager:  <ActionManager> contains 2 active terms.
++------------------------------------+
+|   Active Action Terms (shape: 8)   |
++-------+----------------+-----------+
+| Index | Name           | Dimension |
++-------+----------------+-----------+
+|   0   | arm_action     |         7 |
+|   1   | gripper_action |         1 |
++-------+----------------+-----------+
+
+[INFO] Observation Manager: <ObservationManager> contains 1 groups.
++-----------------------------------------------+
+|  Active Observation Terms in Group: 'policy'  |
++--------+--------------------+-----------------+
+| Index  | Name               |      Shape      |
++--------+--------------------+-----------------+
+|   0    | joint_pos          |       (9,)      |
+|   1    | joint_vel          |       (9,)      |
+|   2    | joint_pos_rel      |       (9,)      |
+|   3    | joint_vel_rel      |       (9,)      |
+|   4    | actions            |       (8,)      |
+|   5    | wrist              |  (480, 640, 3)  |
+|   6    | front              |  (480, 640, 3)  |
+|   7    | joint_pos_target   |       (9,)      |
++--------+--------------------+-----------------+
+
+[INFO] Termination Manager:  <TerminationManager> contains 1 active terms.
++----------------------------+
+|  Active Termination Terms  |
++-------+---------+----------+
+| Index | Name    | Time Out |
++-------+---------+----------+
+|   0   | success |  False   |
++-------+---------+----------+
+
+[INFO] Reward Manager:  <RewardManager> contains 0 active terms.
++-----------------------+
+|  Active Reward Terms  |
++-------+------+--------+
+| Index | Name | Weight |
++-------+------+--------+
++-------+------+--------+
+
+[INFO] Curriculum Manager:  <CurriculumManager> contains 0 active terms.
++----------------------+
+| Active Curriculum Terms |
++-----------+----------+
+|   Index   | Name     |
++-----------+----------+
++-----------+----------+
+
+[INFO]: Completed setting up the environment...
+  [pose] fork: pos=(0.599, -0.172, 0.050) yaw=  -0.0°
+  [pose] knife: pos=(0.503, -0.261, 0.050) yaw=+180.0°
+Start Recording!!!
+2026-05-25T07:42:53Z [171,430ms] [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_heights_only/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 @ 0x74ee75d0fd40] 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 @ 0x74ee75f87d40] 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_heights_only/videos/ep_02_success.mp4 (235 frames)
+[Data Usage]2/10 success.
+Start Recording!!!
+Episode failed!
+Stop Recording!!!
+  [pose] knife: pos=(0.136, -0.188, 0.050) yaw=+180.0°
+  [pose] fork: pos=(0.310, -0.261, 0.050) yaw=  -0.0°
+[video] saved /workspace/aicapstone/fsm_experiments/exp_heights_only/videos/ep_03_failed.mp4 (235 frames)
+[Data Usage]3/10 fail.
+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 @ 0x74ee76075d40] 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 @ 0x74ee3f51f080] 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_heights_only/videos/ep_04_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 @ 0x74ee76075d40] 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 @ 0x74ee75cc9d40] 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_heights_only/videos/ep_05_success.mp4 (235 frames)
+[Data Usage]4/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_heights_only/videos/ep_06_failed.mp4 (235 frames)
+[Data Usage]5/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_heights_only/videos/ep_07_failed.mp4 (235 frames)
+[Data Usage]5/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_heights_only/videos/ep_08_failed.mp4 (235 frames)
+[Data Usage]5/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_heights_only/videos/ep_09_failed.mp4 (235 frames)
+[Data Usage]5/10 fail.
+Start Recording!!!
+Episode failed!
+Stop Recording!!!
+Replayed all 10 episodes. Exiting the app.
+[video] saved /workspace/aicapstone/fsm_experiments/exp_heights_only/videos/ep_10_failed.mp4 (235 frames)
+[Data Usage]5/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_heights_only done. S=4 F=6 ===

exp_heights_only/videos/ep_01_success.mp4

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

exp_heights_only/videos/ep_02_success.mp4

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

exp_heights_only/videos/ep_03_failed.mp4

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

exp_heights_only/videos/ep_04_success.mp4

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

exp_heights_only/videos/ep_05_success.mp4

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

exp_heights_only/videos/ep_06_failed.mp4

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

exp_heights_only/videos/ep_07_failed.mp4

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

exp_heights_only/videos/ep_08_failed.mp4

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

exp_heights_only/videos/ep_09_failed.mp4

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

exp_heights_only/videos/ep_10_failed.mp4

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