Skip to content

SceneMosaic: Efficient and Diverse Simulation-Ready Scene Generation via Hybrid Agentic Layout Evolution

Python
43.8 KB
Raw Blame
import argparse
import json
from pathlib import Path

import numpy as np

try:
    import sapien
except ImportError as e:
    try:
        import sapien.core as sapien
    except ImportError as inner_e:
        raise ImportError(
            "sapien is required for simulation. Install it with: pip install sapien"
        ) from inner_e

try:
    from sapien.utils.viewer import Viewer as SapienViewer
except Exception:
    SapienViewer = None


def as_vector(value, expected_sizes):
    arr = np.asarray(value, dtype=np.float32).reshape(-1)
    if arr.size not in expected_sizes:
        raise ValueError(f"Expected sizes {expected_sizes}, but got shape {arr.shape}")
    return arr


def parse_scale(value):
    arr = as_vector(value, {1, 3})
    if arr.size == 1:
        return np.repeat(arr, 3)
    return arr


def parse_gravity_vector(value):
    gravity = as_vector(value, {1, 3})
    if gravity.size == 1:
        gravity = np.array([0.0, -float(gravity[0]), 0.0], dtype=np.float32)
    norm = np.linalg.norm(gravity)
    if norm <= 1e-8:
        raise ValueError("Gravity vector norm is zero.")
    return gravity.astype(np.float32)


def parse_translation(value):
    return as_vector(value, {3})


def parse_rotation_wxyz(value):
    quat = as_vector(value, {4})
    norm = np.linalg.norm(quat)
    if norm <= 1e-8:
        raise ValueError("Quaternion norm is zero.")
    return quat / norm


def quat_wxyz_to_rotmat(quat_wxyz):
    q = parse_rotation_wxyz(quat_wxyz)
    w, x, y, z = q.tolist()
    xx, yy, zz = x * x, y * y, z * z
    xy, xz, yz = x * y, x * z, y * z
    wx, wy, wz = w * x, w * y, w * z
    return np.array(
        [
            [1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy)],
            [2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx)],
            [2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy)],
        ],
        dtype=np.float32,
    )


def quat_wxyz_conjugate(quat_wxyz):
    q = parse_rotation_wxyz(quat_wxyz)
    return np.array([q[0], -q[1], -q[2], -q[3]], dtype=np.float32)


def quat_wxyz_multiply(lhs_wxyz, rhs_wxyz):
    lw, lx, ly, lz = parse_rotation_wxyz(lhs_wxyz).tolist()
    rw, rx, ry, rz = parse_rotation_wxyz(rhs_wxyz).tolist()
    out = np.array(
        [
            lw * rw - lx * rx - ly * ry - lz * rz,
            lw * rx + lx * rw + ly * rz - lz * ry,
            lw * ry - lx * rz + ly * rw + lz * rx,
            lw * rz + lx * ry - ly * rx + lz * rw,
        ],
        dtype=np.float32,
    )
    return parse_rotation_wxyz(out)


def quat_delta_wxyz(prev_wxyz, curr_wxyz):
    return quat_wxyz_multiply(curr_wxyz, quat_wxyz_conjugate(prev_wxyz))


def quat_delta_angle_deg(prev_wxyz, curr_wxyz):
    delta = quat_delta_wxyz(prev_wxyz, curr_wxyz)
    angle_rad = 2.0 * np.arctan2(np.linalg.norm(delta[1:]), max(float(delta[0]), 1e-8))
    return float(np.degrees(abs(angle_rad)))


def rotmat_to_quat_wxyz(rotmat):
    """Convert a 3x3 rotation matrix to a normalized wxyz quaternion."""
    m = np.asarray(rotmat, dtype=np.float32).reshape(3, 3)
    trace = float(np.trace(m))
    if trace > 0.0:
        s = np.sqrt(trace + 1.0) * 2.0
        w = 0.25 * s
        x = (m[2, 1] - m[1, 2]) / s
        y = (m[0, 2] - m[2, 0]) / s
        z = (m[1, 0] - m[0, 1]) / s
    elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]:
        s = np.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) * 2.0
        w = (m[2, 1] - m[1, 2]) / s
        x = 0.25 * s
        y = (m[0, 1] + m[1, 0]) / s
        z = (m[0, 2] + m[2, 0]) / s
    elif m[1, 1] > m[2, 2]:
        s = np.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) * 2.0
        w = (m[0, 2] - m[2, 0]) / s
        x = (m[0, 1] + m[1, 0]) / s
        y = 0.25 * s
        z = (m[1, 2] + m[2, 1]) / s
    else:
        s = np.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) * 2.0
        w = (m[1, 0] - m[0, 1]) / s
        x = (m[0, 2] + m[2, 0]) / s
        y = (m[1, 2] + m[2, 1]) / s
        z = 0.25 * s

    return parse_rotation_wxyz([w, x, y, z])


def normalized_vector(value):
    vec = np.asarray(value, dtype=np.float32).reshape(3)
    norm = np.linalg.norm(vec)
    if norm <= 1e-8:
        raise ValueError("Vector norm is zero.")
    return vec / norm


def normalize_object_id(object_id):
    object_id = str(object_id)
    if object_id and object_id[0].isdigit():
        return f"object_{object_id}"
    return object_id


def resolve_requested_object_ids(requested_object_ids):
    if requested_object_ids is None:
        return None
    if isinstance(requested_object_ids, (str, int)):
        requested_object_ids = [requested_object_ids]
    return [normalize_object_id(object_id) for object_id in requested_object_ids]


def layout_pose_to_sapien(translation, rotation_wxyz):
    """Return a SAPIEN pose in the same P3D frame used by layout JSON files."""
    return translation.tolist(), rotation_wxyz.tolist()


def sapien_pose_to_layout(translation_xyz, rotation_wxyz):
    """Convert a SAPIEN pose back to P3D layout JSON fields.

    SAPIEN runs in the same world frame as the layout data here, so fields can
    be written back directly in translation + wxyz format.
    """
    quat = parse_rotation_wxyz(rotation_wxyz)
    return (
        [list(map(float, translation_xyz))],
        [list(map(float, quat))],
    )


def load_object_ids(layout_dir: Path, hull_dir: Path, requested_object_ids=None):
    layout_ids = {p.stem for p in layout_dir.glob("*.json")}
    hull_ids = {p.stem for p in hull_dir.glob("*.obj")}
    matched_ids = sorted(layout_ids & hull_ids)

    normalized_requested_ids = resolve_requested_object_ids(requested_object_ids)
    if normalized_requested_ids is None:
        return matched_ids

    missing_ids = [object_id for object_id in normalized_requested_ids if object_id not in matched_ids]
    if missing_ids:
        normalized_requested_ids = [obj_id for obj_id in normalized_requested_ids if obj_id not in missing_ids]
        if not normalized_requested_ids:
            raise FileNotFoundError(
                f"Requested object ids not found in both layout and unposed directories: {missing_ids}. Matched ids: {matched_ids}"
            )
        else:
            print(f"Warning: Requested object ids not found in both layout and unposed directories: {missing_ids}. Matched ids: {matched_ids}")
    return normalized_requested_ids


def load_layout(layout_path: Path):
    with layout_path.open("r", encoding="utf-8") as f:
        raw = json.load(f)

    translation = parse_translation(raw["translation"])
    rotation_wxyz = parse_rotation_wxyz(raw["rotation"])
    scale = parse_scale(raw["scale"])

    return raw, translation, rotation_wxyz, scale


def load_obj_vertices(obj_path: Path):
    vertices = []
    with obj_path.open("r", encoding="utf-8") as f:
        for line in f:
            if line.startswith("v "):
                parts = line.strip().split()
                if len(parts) >= 4:
                    vertices.append([float(parts[1]), float(parts[2]), float(parts[3])])

    if not vertices:
        raise ValueError(f"No vertex lines found in OBJ: {obj_path}")

    return np.asarray(vertices, dtype=np.float32)


def compute_scene_ground_plane_from_hulls(layout_dir: Path, hull_dir: Path, object_ids, gravity_xyz):
    up_direction = normalized_vector(-np.asarray(gravity_xyz, dtype=np.float32).reshape(3))
    surface_level = None

    for object_id in object_ids:
        layout_path = layout_dir / f"{normalize_object_id(object_id)}.json"
        hull_path = hull_dir / f"{normalize_object_id(object_id)}.obj"

        _, translation, rotation_wxyz, scale = load_layout(layout_path)
        local_vertices = load_obj_vertices(hull_path)

        rotation = quat_wxyz_to_rotmat(rotation_wxyz)
        scaled_vertices = local_vertices * scale.reshape(1, 3)
        world_vertices = scaled_vertices @ rotation.T + translation.reshape(1, 3)

        obj_min_level = float(np.min(world_vertices @ up_direction))
        if surface_level is None or obj_min_level < surface_level:
            surface_level = obj_min_level

    if surface_level is None:
        raise RuntimeError("Failed to compute scene ground plane from hulls.")

    return up_direction, float(surface_level)


def compute_initial_y_offset(layout_dir: Path, hull_dir: Path, object_ids, selected_ground_object_ids):
    max_extent = None
    selected_ground_object_ids = set(selected_ground_object_ids or [])

    for object_id in object_ids:
        if object_id in selected_ground_object_ids:
            continue

        layout_path = layout_dir / f"{normalize_object_id(object_id)}.json"
        hull_path = hull_dir / f"{normalize_object_id(object_id)}.obj"

        _, _, _, scale = load_layout(layout_path)
        local_vertices = load_obj_vertices(hull_path)
        bbox_extents = np.ptp(local_vertices, axis=0)
        scaled_extents = bbox_extents * scale.reshape(3)
        object_max_extent = float(np.max(scaled_extents))
        if max_extent is None or object_max_extent > max_extent:
            max_extent = object_max_extent

    if max_extent is None:
        return 0.0

    return float(min(max_extent / 2.0, 0.4))


def choose_ground_object_id(
    layout_dir: Path,
    object_ids,
    use_ground_object: bool,
    ground_object_ids=None,
):
    if not use_ground_object:
        return set()

    if ground_object_ids:
        normalized_ids = []
        for object_id in ground_object_ids:
            normalized_id = normalize_object_id(object_id)
            if normalized_id not in object_ids:
                raise ValueError(
                    f"Requested --ground_object_id={normalized_id}, "
                    f"but it is not in matched objects: {object_ids}"
                )
            normalized_ids.append(normalized_id)
        return set(normalized_ids)

    min_y = None
    selected_id = None
    for object_id in object_ids:
        layout_path = layout_dir / f"{normalize_object_id(object_id)}.json"
        _, translation, _, _ = load_layout(layout_path)
        y = float(translation[1])
        if min_y is None or y < min_y:
            min_y = y
            selected_id = object_id
    return {selected_id} if selected_id is not None else set()


def create_scene_compatible(engine, scene_config, cpu_only: bool):
    """Create a SAPIEN scene with optional CPU-only mode.

    Newer SAPIEN wrappers build a render system by default when using
    Engine.create_scene(), which fails on machines without a rendering device.
    This helper provides a pure-physics fallback scene.
    """
    if cpu_only:
        if hasattr(sapien, "physx") and hasattr(sapien.physx, "set_scene_config"):
            sapien.physx.set_scene_config(scene_config)
        if hasattr(sapien, "Scene") and hasattr(sapien.physx, "PhysxCpuSystem"):
            return sapien.Scene([sapien.physx.PhysxCpuSystem()])
        raise RuntimeError(
            "--cpu_only was set, but this SAPIEN build does not expose "
            "sapien.Scene([sapien.physx.PhysxCpuSystem()])."
        )

    try:
        return engine.create_scene(scene_config)
    except RuntimeError as e:
        msg = str(e).lower()
        if "render" in msg and "device" in msg:
            print(
                "[WARN] Failed to create scene with renderer (no rendering device). "
                "Falling back to CPU-only PhysX scene."
            )
            if hasattr(sapien, "physx") and hasattr(sapien.physx, "set_scene_config"):
                sapien.physx.set_scene_config(scene_config)
            if hasattr(sapien, "Scene") and hasattr(sapien.physx, "PhysxCpuSystem"):
                return sapien.Scene([sapien.physx.PhysxCpuSystem()])
            raise RuntimeError(
                "SAPIEN cannot create a render scene, and CPU-only scene API is unavailable."
            ) from e
        raise


def create_renderer_compatible():
    """Create a renderer across SAPIEN API variants."""
    if hasattr(sapien, "SapienRenderer"):
        return sapien.SapienRenderer()
    if hasattr(sapien, "VulkanRenderer"):
        return sapien.VulkanRenderer()
    raise RuntimeError(
        "No supported SAPIEN renderer found (SapienRenderer/VulkanRenderer)."
    )


def add_visual_mesh_compatible(builder, mesh_path: Path, scale):
    """Attach visual mesh if the SAPIEN API supports it."""
    if not hasattr(builder, "add_visual_from_file"):
        return False

    try:
        builder.add_visual_from_file(
            filename=str(mesh_path),
            pose=sapien.Pose(),
            scale=scale.tolist(),
        )
        return True
    except TypeError:
        try:
            builder.add_visual_from_file(str(mesh_path))
            return True
        except Exception:
            return False
    except Exception:
        return False


def setup_scene_lighting(scene):
    """Add robust default lights for GUI rendering."""
    if hasattr(scene, "set_ambient_light"):
        scene.set_ambient_light([0.35, 0.35, 0.35])
    if hasattr(scene, "add_directional_light"):
        scene.add_directional_light(
            direction=[0.5, -1.0, -0.3],
            color=[1.0, 1.0, 1.0],
            shadow=True,
        )
    if hasattr(scene, "add_point_light"):
        scene.add_point_light([3.0, 3.0, 3.0], [1.0, 1.0, 1.0], shadow=False)
        scene.add_point_light([-3.0, 3.0, -3.0], [0.7, 0.7, 0.7], shadow=False)


def _poll_viewer_key(window, key_name: str):
    """Best-effort key polling across viewer API variants."""
    candidates = ["key_down", "is_key_down", "key_press", "is_key_pressed"]
    key_variants = [key_name, key_name.lower(), key_name.upper()]
    for attr in candidates:
        if not hasattr(window, attr):
            continue
        fn = getattr(window, attr)
        for key in key_variants:
            try:
                if bool(fn(key)):
                    return True
            except Exception:
                continue
    return False


def quaternion_wxyz_to_rpy(quat_wxyz):
    """Convert a wxyz quaternion to roll/pitch/yaw in radians."""
    w, x, y, z = parse_rotation_wxyz(quat_wxyz).tolist()

    sinr_cosp = 2.0 * (w * x + y * z)
    cosr_cosp = 1.0 - 2.0 * (x * x + y * y)
    roll = float(np.arctan2(sinr_cosp, cosr_cosp))

    sinp = 2.0 * (w * y - z * x)
    if abs(sinp) >= 1.0:
        pitch = float(np.sign(sinp) * (np.pi / 2.0))
    else:
        pitch = float(np.arcsin(sinp))

    siny_cosp = 2.0 * (w * z + x * y)
    cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
    yaw = float(np.arctan2(siny_cosp, cosy_cosp))

    return roll, pitch, yaw


def limit_dynamic_actor_velocities(
    actor_to_data,
    selected_ground_object_ids,
    max_abs_velocity_xyz,
    max_abs_angular_velocity_xyz,
):
    """Clamp dynamic object linear/angular velocities component-wise in world XYZ."""
    max_abs_velocity_xyz = np.asarray(max_abs_velocity_xyz, dtype=np.float32).reshape(3)
    max_abs_angular_velocity_xyz = np.asarray(
        max_abs_angular_velocity_xyz, dtype=np.float32
    ).reshape(3)

    def _get_set_velocity_accessors(entity):
        # Old-style Actor API
        linear_getter = None
        linear_setter = None
        angular_getter = None
        angular_setter = None

        if hasattr(entity, "get_velocity") and hasattr(entity, "set_velocity"):
            linear_getter = entity.get_velocity
            linear_setter = entity.set_velocity
        if hasattr(entity, "get_angular_velocity") and hasattr(entity, "set_angular_velocity"):
            angular_getter = entity.get_angular_velocity
            angular_setter = entity.set_angular_velocity

        if (
            linear_getter is not None
            and linear_setter is not None
            and angular_getter is not None
            and angular_setter is not None
        ):
            return linear_getter, linear_setter, angular_getter, angular_setter

        # Newer Entity + PhysX component API
        physx = getattr(sapien, "physx", None)
        dyn_type = getattr(physx, "PhysxRigidDynamicComponent", None)
        if dyn_type is not None and hasattr(entity, "find_component_by_type"):
            try:
                comp = entity.find_component_by_type(dyn_type)
            except Exception:
                comp = None
            if comp is not None:
                if hasattr(comp, "get_linear_velocity") and hasattr(comp, "set_linear_velocity"):
                    linear_getter = comp.get_linear_velocity
                    linear_setter = comp.set_linear_velocity
                elif hasattr(comp, "linear_velocity"):

                    def _linear_getter():
                        return comp.linear_velocity

                    def _linear_setter(v):
                        comp.linear_velocity = v

                    linear_getter = _linear_getter
                    linear_setter = _linear_setter

                if hasattr(comp, "get_angular_velocity") and hasattr(comp, "set_angular_velocity"):
                    angular_getter = comp.get_angular_velocity
                    angular_setter = comp.set_angular_velocity
                elif hasattr(comp, "angular_velocity"):

                    def _angular_getter():
                        return comp.angular_velocity

                    def _angular_setter(v):
                        comp.angular_velocity = v

                    angular_getter = _angular_getter
                    angular_setter = _angular_setter

        return linear_getter, linear_setter, angular_getter, angular_setter

    for actor, data in actor_to_data.items():
        if data["object_id"] in selected_ground_object_ids:
            continue

        (
            velocity_getter,
            velocity_setter,
            angular_velocity_getter,
            angular_velocity_setter,
        ) = _get_set_velocity_accessors(actor)
        if (
            velocity_getter is None
            or velocity_setter is None
            or angular_velocity_getter is None
            or angular_velocity_setter is None
        ):
            continue

        velocity = np.asarray(velocity_getter(), dtype=np.float32)
        clipped_velocity = np.clip(velocity, -max_abs_velocity_xyz, max_abs_velocity_xyz)
        if not np.allclose(velocity, clipped_velocity):
            velocity_setter(clipped_velocity)

        angular_velocity = np.asarray(angular_velocity_getter(), dtype=np.float32)
        clipped_angular_velocity = np.clip(
            angular_velocity,
            -max_abs_angular_velocity_xyz,
            max_abs_angular_velocity_xyz,
        )
        if not np.allclose(angular_velocity, clipped_angular_velocity):
            angular_velocity_setter(clipped_angular_velocity)


def run_simulation(
    layout_dir: Path,
    hull_dir: Path,
    output_layout_dir: Path,
    gravity,
    timestep: float,
    depenetration_steps: int,
    simulation_steps: int,
    mass: float,
    linear_damping: float,
    angular_damping: float,
    solver_iterations: int,
    use_ground_object: bool,
    ground_object_ids,
    ground_plane_mode: str,
    gui: bool,
    gui_manual_control: bool,
    pose_delta_output: Path | None,
    object_ids=None,
    verify_only: bool = False,
    lock_rotation_all: bool = False,
    cpu_only: bool = False,
    init_non_ground_y_offset: bool = False,
):
    gravity_xyz = parse_gravity_vector(gravity)
    requested_object_ids = resolve_requested_object_ids(object_ids)
    object_ids = load_object_ids(layout_dir, hull_dir, requested_object_ids=requested_object_ids)
    if not object_ids:
        raise FileNotFoundError(
            f"No matching object files in {layout_dir} and {hull_dir}."
        )

    if verify_only:
        print("[INFO] Running in verify-only mode (no simulation)")
        print(f"[INFO] Found {len(object_ids)} matched objects: {object_ids}")
        for object_id in object_ids:
            layout_path = layout_dir / f"{normalize_object_id(object_id)}.json"
            hull_path = hull_dir / f"{normalize_object_id(object_id)}.obj"
            raw_layout, translation, rotation_wxyz, scale = load_layout(layout_path)
            print(f"[OK] object_{object_id}:")
            print(f"  Layout file: {layout_path}")
            print(f"  Hull file: {hull_path} (exists: {hull_path.exists()})")
            print(f"  Translation: {translation}")
            print(f"  Rotation (wxyz): {rotation_wxyz}")
            print(f"  Scale: {scale}")
            print("[INFO] All objects loaded successfully!")
        return

    selected_ground_object_ids = choose_ground_object_id(
        layout_dir=layout_dir,
        object_ids=object_ids,
        use_ground_object=use_ground_object,
        ground_object_ids=ground_object_ids,
    )
    if selected_ground_object_ids:
        selected_ground_object_ids = sorted(selected_ground_object_ids)
        print(
            "[INFO] Using static ground objects: "
            + ", ".join(normalize_object_id(object_id) for object_id in selected_ground_object_ids)
        )

    initial_y_offset = 0.0
    if init_non_ground_y_offset:
        initial_y_offset = compute_initial_y_offset(
            layout_dir=layout_dir,
            hull_dir=hull_dir,
            object_ids=object_ids,
            selected_ground_object_ids=selected_ground_object_ids,
        )
        print(f"[INFO] Initial non-ground Y offset enabled: {initial_y_offset:.6f}")

    if cpu_only:
        print("[INFO] CPU-only mode enabled: scene will be created without RenderSystem.")
        if gui:
            print("[WARN] --gui is incompatible with --cpu_only. Falling back to headless mode.")

    engine = sapien.Engine()
    renderer = None
    gui_enabled = bool(gui and (not cpu_only))
    gui_manual_enabled = False

    if not cpu_only:
        try:
            renderer = create_renderer_compatible()
            if hasattr(engine, "set_renderer"):
                engine.set_renderer(renderer)
        except Exception as e:
            print(f"[WARN] Failed to initialize SAPIEN renderer: {e}")
            if gui_enabled:
                print("[WARN] GUI requested, but renderer init failed. Falling back to headless mode.")
            gui_enabled = False

    viewer = None
    if gui_enabled:
        if SapienViewer is None:
            print(
                "[WARN] sapien.utils.viewer.Viewer is unavailable in this environment. "
                "Falling back to headless mode."
            )
            gui_enabled = False
        else:
            try:
                if renderer is not None:
                    viewer = SapienViewer(renderer)
                else:
                    viewer = SapienViewer()
            except TypeError:
                try:
                    viewer = SapienViewer()
                except Exception as e:
                    print(f"[WARN] Failed to initialize Viewer: {e}")
                    gui_enabled = False
            except Exception as e:
                print(f"[WARN] Failed to initialize Viewer: {e}")
                gui_enabled = False

            if viewer is not None:
                print("[INFO] GUI mode enabled.")
                gui_manual_enabled = bool(gui_manual_control)
                if gui_manual_enabled:
                    print(
                        "[INFO] GUI manual mode enabled: starts paused at first frame. "
                        "Controls: [N] single-step, [Space] run/pause, [R] reset pause, [Q/Esc] quit."
                    )

    def bind_viewer_scene(scene):
        if viewer is None:
            return
        if hasattr(viewer, "set_scene"):
            viewer.set_scene(scene)
        camera_translation = [-0.02, 0.05, -0.68]

        if hasattr(viewer, "set_camera_xyz"):
            viewer.set_camera_xyz(*camera_translation)

    if ground_plane_mode == "bbox":
        ground_up_direction, ground_surface_level = compute_scene_ground_plane_from_hulls(
            layout_dir,
            hull_dir,
            object_ids,
            gravity_xyz,
        )
        print(
            f"[INFO] Ground plane mode=bbox, up_direction={ground_up_direction.tolist()}, "
            f"surface_level={ground_surface_level:.6f}"
        )
    else:
        ground_up_direction = np.array([0.0, 1.0, 0.0], dtype=np.float32)
        ground_surface_level = 0.0
        print("[INFO] Ground plane mode=y0, top surface at y=0.0")

    def build_ground_plane_pose(up_direction, surface_level, half_thickness=0.01):
        up_direction = normalized_vector(up_direction)
        reference = np.array([1.0, 0.0, 0.0], dtype=np.float32)
        if abs(float(np.dot(up_direction, reference))) > 0.9:
            reference = np.array([0.0, 0.0, 1.0], dtype=np.float32)
        x_axis = normalized_vector(np.cross(reference, up_direction))
        z_axis = normalized_vector(np.cross(x_axis, up_direction))
        rotation = np.column_stack([x_axis, up_direction, z_axis])
        return up_direction * (float(surface_level) - float(half_thickness)), rotmat_to_quat_wxyz(rotation)

    # Freeze ground plane pose at initialization so it stays fixed for all steps/phases.
    fixed_ground_pose_p, fixed_ground_pose_q = build_ground_plane_pose(
        ground_up_direction,
        ground_surface_level,
    )

    def build_scene_with_actors(gravity_xyz, pose_overrides=None, lock_rotation=False):
        scene_config = sapien.SceneConfig()
        scene_config.gravity = list(map(float, np.asarray(gravity_xyz, dtype=np.float32).tolist()))
        scene = create_scene_compatible(engine=engine, scene_config=scene_config, cpu_only=cpu_only)
        scene.set_timestep(float(timestep))
        if viewer is not None:
            setup_scene_lighting(scene)
        bind_viewer_scene(scene)

        # Keep contacts stable and make the behavior closer to the old backend.
        if hasattr(scene, "set_solver_iterations"):
            scene.set_solver_iterations(int(solver_iterations))

        physical_material = scene.create_physical_material(
            static_friction=0.3,
            dynamic_friction=0.3,
            restitution=0.0,
        )

        # Ground plane aligned with the chosen up direction using a thin kinematic box.
        ground_builder = scene.create_actor_builder()
        ground_builder.add_box_collision(
            half_size=[1000.0, 0.01, 1000.0],
            pose=sapien.Pose(p=fixed_ground_pose_p.tolist(), q=fixed_ground_pose_q.tolist()),
            material=physical_material,
        )
        if viewer is not None and hasattr(ground_builder, "add_box_visual"):
            try:
                ground_builder.add_box_visual(
                    half_size=[1000.0, 0.01, 1000.0],
                    pose=sapien.Pose(p=fixed_ground_pose_p.tolist(), q=fixed_ground_pose_q.tolist()),
                    color=[0.2, 0.24, 0.28],
                )
            except TypeError:
                try:
                    ground_builder.add_box_visual(
                        half_size=[1000.0, 0.01, 1000.0],
                        pose=sapien.Pose(p=fixed_ground_pose_p.tolist(), q=fixed_ground_pose_q.tolist()),
                    )
                except TypeError:
                    ground_builder.add_box_visual([1000.0, 0.01, 1000.0])
        ground_builder.build_kinematic(name="ground_plane")

        actor_to_data = {}
        prev_pose_by_actor = {}
        for object_id in object_ids:
            layout_path = layout_dir / f"{normalize_object_id(object_id)}.json"
            hull_path = hull_dir / f"{normalize_object_id(object_id)}.obj"

            raw_layout, translation, rotation_wxyz, scale = load_layout(layout_path)
            if pose_overrides is not None and object_id in pose_overrides:
                translation = np.asarray(pose_overrides[object_id]["p"], dtype=np.float32)
                rotation_wxyz = parse_rotation_wxyz(pose_overrides[object_id]["q"])

            position_xyz, orientation_wxyz = layout_pose_to_sapien(
                translation=translation,
                rotation_wxyz=rotation_wxyz,
            )
            if init_non_ground_y_offset and object_id not in selected_ground_object_ids:
                position_xyz = np.asarray(position_xyz, dtype=np.float32)
                position_xyz[1] += initial_y_offset

            builder = scene.create_actor_builder()
            if viewer is not None:
                add_visual_mesh_compatible(builder, hull_path, scale)
            if hasattr(builder, "add_multiple_convex_collisions_from_file"):
                builder.add_multiple_convex_collisions_from_file(
                    filename=str(hull_path),
                    pose=sapien.Pose(),
                    scale=scale.tolist(),
                    material=physical_material,
                    density=max(float(mass), 1e-6),
                )
            else:
                raise RuntimeError(
                    "No supported mesh collision API found on ActorBuilder."
                )

            if object_id in selected_ground_object_ids:
                actor = builder.build_kinematic(name=normalize_object_id(object_id))
            else:
                actor = builder.build(name=normalize_object_id(object_id))
                if hasattr(actor, "set_damping"):
                    actor.set_damping(float(linear_damping), float(angular_damping))

            actor.set_pose(sapien.Pose(p=position_xyz, q=orientation_wxyz))
            if lock_rotation:
                physx_component = actor.find_component_by_type(sapien.physx.PhysxRigidDynamicComponent)
                if physx_component:
                    physx_component.set_locked_motion_axes([False, False, False, True, True, True])

            actor_to_data[actor] = {
                "object_id": object_id,
                "layout": raw_layout,
                "scale": scale,
            }
            prev_pose_by_actor[actor] = {
                "p": np.asarray(position_xyz, dtype=np.float32),
                "q": parse_rotation_wxyz(orientation_wxyz),
            }

        return scene, actor_to_data, prev_pose_by_actor

    pose_delta_records = []

    def record_pose_deltas(phase: str, step_index: int, actor_to_data, prev_pose_by_actor):
        if pose_delta_output is None:
            return

        frame_records = []
        for actor, data in actor_to_data.items():
            pose = actor.get_pose()
            curr_p = np.asarray(pose.p, dtype=np.float32)
            curr_q = parse_rotation_wxyz(pose.q)
            prev_state = prev_pose_by_actor[actor]

            delta_p = curr_p - prev_state["p"]
            delta_q = quat_delta_wxyz(prev_state["q"], curr_q)
            delta_angle_deg = quat_delta_angle_deg(prev_state["q"], curr_q)

            frame_records.append(
                {
                    "object_id": data["object_id"],
                    "translation_xyz": list(map(float, curr_p.tolist())),
                    "rotation_wxyz": list(map(float, curr_q.tolist())),
                    "delta_translation_xyz": list(map(float, delta_p.tolist())),
                    "delta_rotation_wxyz": list(map(float, delta_q.tolist())),
                    "delta_rotation_angle_deg": delta_angle_deg,
                }
            )

            prev_state["p"] = curr_p
            prev_state["q"] = curr_q

        pose_delta_records.append(
            {
                "phase": phase,
                "step": int(step_index),
                "objects": frame_records,
            }
        )

    zero_gravity = np.array([0.0, 0.0, 0.0], dtype=np.float32)
    simulation_gravity = np.asarray(gravity_xyz, dtype=np.float32).reshape(3)
    # Clamp velocity across the whole pipeline. Y is allowed to be faster than X/Z in the legacy default,
    # but the clip is component-wise so custom gravity directions still work.
    max_abs_velocity_xyz = np.array([5e-5, 2e-2, 5e-5], dtype=np.float32)
    # Clamp angular velocity (rad/s) across the whole pipeline.
    max_abs_angular_velocity_xyz = np.array([5e-5, 5e-5, 5e-5], dtype=np.float32)

    actor_to_data = {}
    prev_pose_by_actor = {}
    stop_requested = False
    run_continuous = not gui_manual_enabled
    manual_key_supported = False

    if viewer is not None and gui_manual_enabled and hasattr(viewer, "window"):
        window = viewer.window
        manual_key_supported = any(
            hasattr(window, name)
            for name in ["key_down", "is_key_down", "key_press", "is_key_pressed"]
        )
        if not manual_key_supported:
            print(
                "[WARN] Viewer key polling API is unavailable in this SAPIEN build; "
                "falling back to continuous stepping."
            )
            run_continuous = True

    def after_step_render(scene):
        nonlocal stop_requested
        if viewer is None:
            return
        if hasattr(scene, "update_render"):
            scene.update_render()
        if hasattr(viewer, "render"):
            viewer.render()
        if hasattr(viewer, "closed") and viewer.closed:
            stop_requested = True

    def process_manual_controls():
        nonlocal run_continuous, stop_requested
        if (viewer is None) or (not gui_manual_enabled) or (not manual_key_supported):
            return False

        window = viewer.window
        if _poll_viewer_key(window, "q") or _poll_viewer_key(window, "esc"):
            stop_requested = True
            return False

        if _poll_viewer_key(window, "space"):
            run_continuous = not run_continuous

        if _poll_viewer_key(window, "r"):
            run_continuous = False

        if run_continuous:
            return True

        return _poll_viewer_key(window, "n")

    def step_scene_with_gui(scene, phase: str, steps: int, actor_to_data, prev_pose_by_actor):
        nonlocal stop_requested
        if steps / 50 == 0:
            print(f"[INFO] {phase} step 0/{max(steps, 0)}")
        record_pose_deltas(phase, 0, actor_to_data, prev_pose_by_actor)

        applied_steps = 0
        while applied_steps < max(steps, 0):
            do_step = True
            if gui_manual_enabled:
                do_step = process_manual_controls()
            if stop_requested:
                break

            if do_step:
                scene.step()
                limit_dynamic_actor_velocities(
                    actor_to_data=actor_to_data,
                    selected_ground_object_ids=selected_ground_object_ids,
                    max_abs_velocity_xyz=max_abs_velocity_xyz,
                    max_abs_angular_velocity_xyz=max_abs_angular_velocity_xyz,
                )
                applied_steps += 1
                print(f"[INFO] {phase} step {applied_steps}/{max(steps, 0)}")
                record_pose_deltas(
                    phase,
                    applied_steps,
                    actor_to_data,
                    prev_pose_by_actor,
                )

            after_step_render(scene)

            if stop_requested:
                break

        return applied_steps

    if depenetration_steps > 0:
        print(f"[INFO] Depenetration gravity = {zero_gravity.tolist()}")
        scene, actor_to_data, prev_pose_by_actor = build_scene_with_actors(
            zero_gravity,
            lock_rotation=True,
        )
        step_scene_with_gui(
            scene,
            phase="depenetration",
            steps=depenetration_steps,
            actor_to_data=actor_to_data,
            prev_pose_by_actor=prev_pose_by_actor,
        )
        if stop_requested:
            print("[INFO] Viewer closed by user during depenetration.")

        if (simulation_steps > 0) and (not stop_requested):
            start_poses = {}
            for actor, data in actor_to_data.items():
                pose = actor.get_pose()
                start_poses[data["object_id"]] = {
                    "p": np.asarray(pose.p, dtype=np.float32),
                    "q": parse_rotation_wxyz(pose.q),
                }

            print(f"[INFO] Simulation gravity = {simulation_gravity.tolist()}")
            scene, actor_to_data, prev_pose_by_actor = build_scene_with_actors(
                simulation_gravity,
                pose_overrides=start_poses,
                lock_rotation=lock_rotation_all,
            )
            step_scene_with_gui(
                scene,
                phase="simulation",
                steps=simulation_steps,
                actor_to_data=actor_to_data,
                prev_pose_by_actor=prev_pose_by_actor,
            )
            if stop_requested:
                print("[INFO] Viewer closed by user during simulation.")
    else:
        print(f"[INFO] Simulation gravity = {simulation_gravity.tolist()}")
        scene, actor_to_data, prev_pose_by_actor = build_scene_with_actors(
            simulation_gravity,
            lock_rotation=lock_rotation_all,
        )
        step_scene_with_gui(
            scene,
            phase="simulation",
            steps=simulation_steps,
            actor_to_data=actor_to_data,
            prev_pose_by_actor=prev_pose_by_actor,
        )
        if stop_requested:
            print("[INFO] Viewer closed by user during simulation.")

    output_layout_dir.mkdir(parents=True, exist_ok=True)
    for actor, data in actor_to_data.items():
        pose = actor.get_pose()

        updated_layout = dict(data["layout"])
        translation_p3d, rotation_p3d = sapien_pose_to_layout(
            translation_xyz=pose.p,
            rotation_wxyz=pose.q,
        )
        updated_layout["translation"] = translation_p3d
        if lock_rotation_all:
            updated_layout["rotation"] = data["layout"]["rotation"]
        else:
            updated_layout["rotation"] = rotation_p3d
        updated_layout["scale"] = [list(map(float, data["scale"]))]

        out_path = output_layout_dir / f"{normalize_object_id(data['object_id'])}.json"
        with out_path.open("w", encoding="utf-8") as f:
            json.dump(updated_layout, f, ensure_ascii=False, indent=2)
        print(f"[OK] Saved simulated pose: {out_path}")

    if pose_delta_output is not None:
        pose_delta_output.parent.mkdir(parents=True, exist_ok=True)
        with pose_delta_output.open("w", encoding="utf-8") as f:
            json.dump(pose_delta_records, f, ensure_ascii=False, indent=2)
        print(f"[OK] Saved per-step pose deltas: {pose_delta_output}")


def main():
    parser = argparse.ArgumentParser(
        description=(
            "Run simple rigid-body simulation with collision hulls and save updated "
            "object_XXX pose JSON files."
        )
    )
    parser.add_argument(
        "--layout_dir",
        type=Path,
        default=Path("outputs/layout_params"),
        help="Directory containing object_XXX.json layout files.",
    )
    parser.add_argument(
        "--hull_dir",
        type=Path,
        default=Path("outputs/collision_hulls"),
        help="Directory containing object_XXX.obj collision hull files.",
    )
    parser.add_argument(
        "--output_layout_dir",
        type=Path,
        default=Path("outputs/layout_params_simulated"),
        help="Directory to save updated object_XXX.json files.",
    )
    parser.add_argument(
        "--gravity",
        type=float,
        nargs="+",
        default=[2.0],
        help=(
            "Gravity magnitude or vector. Pass one value for legacy -Y gravity, "
            "or three values for an arbitrary gravity vector in world XYZ."
        ),
    )
    parser.add_argument(
        "--timestep",
        type=float,
        default=1.0 / 100.0,
        help="Simulation timestep in seconds.",
    )
    parser.add_argument(
        "--depenetration_steps",
        type=int,
        default=200,
        help="Steps with zero gravity to resolve initial penetrations.",
    )
    parser.add_argument(
        "--simulation_steps",
        type=int,
        default=2000,
        help="Steps with gravity enabled.",
    )
    parser.add_argument(
        "--mass",
        type=float,
        default=10.0,
        help="Density proxy for each object body in SAPIEN collision loading.",
    )
    parser.add_argument(
        "--linear_damping",
        type=float,
        default=5.0,
        help="Linear damping for each object.",
    )
    parser.add_argument(
        "--angular_damping",
        type=float,
        default=5.0,
        help="Angular damping for each object.",
    )
    parser.add_argument(
        "--solver_iterations",
        type=int,
        default=100,
        help="Constraint solver iterations.",
    )
    parser.add_argument(
        "--use_ground_object",
        action="store_true",
        help=(
            "If set, use a static object as ground anchor to reduce global drift. "
            "By default, the lowest-Y object is chosen (PyTorch3D coordinate system)."
        ),
    )
    parser.add_argument(
        "--ground_object_id",
        type=str,
        nargs="+",
        default=None,
        help=(
            "One or more object ids (XXX in object_XXX) to use as static ground objects. "
            "Only valid when --use_ground_object is enabled."
        ),
    )
    parser.add_argument(
        "--ground_plane_mode",
        type=str,
        choices=["bbox", "y0"],
        default="bbox",
        help=(
            "Ground plane placement mode: bbox aligns the plane to the gravity direction "
            "and uses the transformed collision-hull bbox; y0 keeps the legacy fixed y=0 plane."
        ),
    )
    parser.add_argument(
        "--cpu_only",
        action="store_true",
        help=(
            "Create a pure-physics SAPIEN scene without RenderSystem. "
            "Useful on machines without graphics/rendering devices."
        ),
    )
    parser.add_argument(
        "--gui",
        action="store_true",
        help=(
            "Enable SAPIEN GUI viewer when renderer/device is available; "
            "otherwise automatically fall back to headless mode."
        ),
    )
    parser.add_argument(
        "--gui_manual_control",
        action="store_true",
        help=(
            "GUI only: start paused on first frame and manually control stepping "
            "with keyboard (N single-step, Space run/pause, R pause, Q/Esc quit)."
        ),
    )
    parser.add_argument(
        "--pose_delta_output",
        type=Path,
        default=None,
        help=(
            "If set, record per-step translation/rotation changes for each object "
            "and save to this JSON file."
        ),
    )
    parser.add_argument(
        "--lock_rotation_all",
        action="store_true",
        help="Lock rotation for all objects and only update translations in output.",
    )
    parser.add_argument(
        "--object_ids",
        nargs="+",
        default=None,
        help=(
            "Optional subset of object ids to simulate (XXX in object_XXX). "
            "Defaults to all matched objects."
        ),
    )
    parser.add_argument(
        "--verify_only",
        action="store_true",
        help="Only verify loading of layout and hull files, without running simulation.",
    )
    parser.add_argument(
        "--init_non_ground_y_offset",
        action="store_true",
        help="Shift non-ground objects upward before simulation by a hull-size-based offset.",
    )
    args = parser.parse_args()

    if not args.layout_dir.exists():
        raise FileNotFoundError(f"Layout directory not found: {args.layout_dir}")
    if not args.hull_dir.exists():
        raise FileNotFoundError(f"Hull directory not found: {args.hull_dir}")
    if args.mass <= 0:
        raise ValueError("--mass must be positive.")
    if args.timestep <= 0:
        raise ValueError("--timestep must be positive.")
    if args.ground_object_id is not None and not args.use_ground_object:
        raise ValueError("--ground_object_id requires --use_ground_object.")

    run_simulation(
        layout_dir=args.layout_dir,
        hull_dir=args.hull_dir,
        output_layout_dir=args.output_layout_dir,
        gravity=args.gravity,
        timestep=args.timestep,
        depenetration_steps=args.depenetration_steps,
        simulation_steps=args.simulation_steps,
        mass=args.mass,
        object_ids=args.object_ids,
        linear_damping=args.linear_damping,
        angular_damping=args.angular_damping,
        solver_iterations=args.solver_iterations,
        use_ground_object=args.use_ground_object,
        ground_object_ids=args.ground_object_id,
        ground_plane_mode=args.ground_plane_mode,
        gui=args.gui,
        gui_manual_control=args.gui_manual_control,
        pose_delta_output=args.pose_delta_output,
        verify_only=args.verify_only,
        lock_rotation_all=args.lock_rotation_all,
        cpu_only=args.cpu_only,
        init_non_ground_y_offset=args.init_non_ground_y_offset,
    )


if __name__ == "__main__":
    main()