From a7b7f83c13d99b5a85ec73323a26e76441093e0f Mon Sep 17 00:00:00 2001 From: ruziniuuuuu Date: Fri, 11 Sep 2026 22:49:06 +0800 Subject: [PATCH 1/2] fix: align Galbot wrist camera mesh and calibration Signed-off-by: ruziniuuuuu --- .../galbot_one_golf_description/README.md | 23 +++++++ .../usd/payloads/base.usda | 2 +- .../usd/payloads/geometries.usd | 4 +- .../usd/payloads/instances.usda | 4 +- robolab/robots/galbot_golf.py | 68 +++++++++++++++---- tests/test_galbot_configs.py | 51 ++++++++++++-- tests/test_galbot_wrist_camera.py | 47 +++++++++++++ 7 files changed, 176 insertions(+), 23 deletions(-) create mode 100644 tests/test_galbot_wrist_camera.py diff --git a/assets/robots/galbot_one_golf_description/README.md b/assets/robots/galbot_one_golf_description/README.md index 27feb4e7..1b4c3f6d 100644 --- a/assets/robots/galbot_one_golf_description/README.md +++ b/assets/robots/galbot_one_golf_description/README.md @@ -25,6 +25,29 @@ textures are stored under `usd/`. Variant sets on the root prim (defaults in bold): Physics (**physx**, physics, none, mujoco), Robot (**none**, robot), Sensor (**none**, sensors). +### Wrist camera visual and calibration + +The existing `d405` visual prims use the forward `d405_sz073` assembly from +the description repository at revision `69cdfde0178f2eed10c4b2e612c0c3d3b2263c7a` +(`usd/components/camera/camera_d405_sz073/payloads/instances.usda`, +`/Instances/mesh`). Its visual meshes and materials are embedded in the existing +`geometries.usd` entry `/Geometries/mesh_49`. The assembly includes its stand, +so the original `d405_stand` visuals are hidden. Link names, transforms, +collision geometry, and physical properties are unchanged. + +RoboLab's `left_wrist_cam` and `right_wrist_cam` use SynthNova's Yundonghui +policy calibration (`extensions/yundonghui/src/synthnova_yundonghui/cameras.py`, +SynthNova revision `429f5a4`). Images are resized from 400 x 224 to 640 x 360, +scaling each intrinsic-matrix row by its corresponding image dimension: +`fx=323.2`, `fy=324.64285714285717`, `cx=320`, `cy=180`. +The wrist sensor applies Isaac Sim's OpenCV pinhole lens schema (zero distortion) +and reports the same matrix, preserving the unequal focal lengths that the +default RTX pinhole and IsaacLab square-pixel assumption would otherwise lose. +The source ROS optical extrinsics are relative to each +`arm_end_effector_mount_link`. The camera config composes that mount's fixed +transform into `arm_link7` and converts XYZW quaternions to WXYZ, allowing +the sensors to follow the moving rigid bodies directly. + ## LICENSE This software is licensed under the Apache License 2.0. See `LICENSE` for details. diff --git a/assets/robots/galbot_one_golf_description/usd/payloads/base.usda b/assets/robots/galbot_one_golf_description/usd/payloads/base.usda index a8cae7f3..c3bff6bb 100644 --- a/assets/robots/galbot_one_golf_description/usd/payloads/base.usda +++ b/assets/robots/galbot_one_golf_description/usd/payloads/base.usda @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:e8b875d66fbc490581e480766dc2f5d4ad223503031903e22050e5d9f6eeabbd +oid sha256:a1f8db0babad4db0827936f689cfe6cbc72a7c9268f6181b1c3d6831704930ce size 159611 diff --git a/assets/robots/galbot_one_golf_description/usd/payloads/geometries.usd b/assets/robots/galbot_one_golf_description/usd/payloads/geometries.usd index e31484ee..fe062a50 100644 --- a/assets/robots/galbot_one_golf_description/usd/payloads/geometries.usd +++ b/assets/robots/galbot_one_golf_description/usd/payloads/geometries.usd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:9b27cec215224033c055c896d2bb4bf2e7c83abbbc689b99a1075dcaca536d46 -size 6126521 +oid sha256:c7e7786a5e628a72e862525b1d3980c528280eda8c9541f822e357e831e8021c +size 7040765 diff --git a/assets/robots/galbot_one_golf_description/usd/payloads/instances.usda b/assets/robots/galbot_one_golf_description/usd/payloads/instances.usda index 55141b76..ce5451b0 100644 --- a/assets/robots/galbot_one_golf_description/usd/payloads/instances.usda +++ b/assets/robots/galbot_one_golf_description/usd/payloads/instances.usda @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:51ee22994b8f655c8c02101ded61b0d9429f54b27336b255a5e156dc3114d5bd -size 68312 +oid sha256:ba2762c5e82283a0820973a89d905951ef26177cb8e0815e6678861153b01409 +size 67782 diff --git a/robolab/robots/galbot_golf.py b/robolab/robots/galbot_golf.py index e9747d82..62ea5de9 100644 --- a/robolab/robots/galbot_golf.py +++ b/robolab/robots/galbot_golf.py @@ -4,12 +4,14 @@ """Fixed-base ``galbot_one_golf`` dual-arm robot configuration.""" import os +from collections.abc import Sequence import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg -from isaaclab.sensors import TiledCameraCfg +from isaaclab.sensors import TiledCamera, TiledCameraCfg from isaaclab.utils import configclass +from pxr import Gf, Sdf from robolab.constants import ROBOTS_DIR from robolab.robots.galbot_golf_definitions import * # noqa @@ -70,28 +72,70 @@ def _source_replay_camera( ) +class _WristCamera(TiledCamera): + """Render and report the independently resized wrist focal lengths.""" + + def __init__(self, cfg: TiledCameraCfg) -> None: + super().__init__(cfg) + # RTX's default pinhole and IsaacLab's intrinsic reporting assume + # square pixels. OpenCV pinhole preserves fx != fy after resizing. + for prim in sim_utils.find_matching_prims(cfg.prim_path): + # Author the schema and attributes explicitly: headless Kit apps + # need not have registered the optional schema's USD fallbacks. + prim.AddAppliedSchema("OmniLensDistortionOpenCvPinholeAPI") + prim.CreateAttribute("omni:lensdistortion:model", Sdf.ValueTypeNames.Token).Set("opencvPinhole") + parameters = { + "fx": cfg.width * cfg.spawn.focal_length / cfg.spawn.horizontal_aperture, + "fy": cfg.height * cfg.spawn.focal_length / cfg.spawn.vertical_aperture, + "cx": cfg.width / 2, + "cy": cfg.height / 2, + **dict.fromkeys(("k1", "k2", "k3", "k4", "k5", "k6", "p1", "p2", "s1", "s2", "s3", "s4"), 0.0), + } + for name, value in parameters.items(): + prim.CreateAttribute(f"omni:lensdistortion:opencvPinhole:{name}", Sdf.ValueTypeNames.Float).Set(value) + prim.CreateAttribute("omni:lensdistortion:opencvPinhole:imageSize", Sdf.ValueTypeNames.Int2).Set( + Gf.Vec2i(cfg.width, cfg.height) + ) + + def _update_intrinsic_matrices(self, env_ids: Sequence[int]) -> None: + super()._update_intrinsic_matrices(env_ids) + for index in env_ids: + prim = self._sensor_prims[index].GetPrim() + self._data.intrinsic_matrices[index, 1, 1] = prim.GetAttribute( + "omni:lensdistortion:opencvPinhole:fy" + ).Get() + + def _wrist_camera(side: str) -> TiledCameraCfg: - """Camera parented to Golf's URDF camera frame, using its +Z optical axis.""" + """Yundonghui policy calibration resized from 400x224 to 640x360.""" + # SynthNova extensions/yundonghui/src/synthnova_yundonghui/cameras.py: + # ROS optical pose relative to the arm end-effector mount, quaternion XYZW. + mount_pos = (0.07089459385344977, 0.011084636915734618, 0.0475356786953811) + mount_quat = (-0.5921350168801781, 0.5860286987303605, -0.38397145780165504, 0.3981361647004496) + if side == "right": + mount_quat = tuple(-value for value in mount_quat) + x, y, z, w = mount_quat + # arm_link7 -> mount: translation (-0.10926, 0, 0), rotation Ry(pi). + # Attach directly to the moving rigid body so Fabric updates the camera; + # compose the mount transform and convert XYZW to IsaacLab's WXYZ order. return TiledCameraCfg( - prim_path=( - f"{{ENV_REGEX_NS}}/robot/{side}_arm_link7/" - f"{side}_arm_wrist_camera_stand/{side}_wrist_camera_link/{side}_wrist_cam" - ), - height=224, - width=400, + class_type=_WristCamera, + prim_path=f"{{ENV_REGEX_NS}}/robot/{side}_arm_link7/{side}_wrist_cam", + height=360, + width=640, data_types=["rgb"], spawn=sim_utils.PinholeCameraCfg( + # Keep the source film gate: fx=202*640/400, fy=202*360/224, + # with centered principal point (320, 180) after resizing. focal_length=202 * 0.03, focus_distance=0.0, horizontal_aperture=400 * 0.03, vertical_aperture=224 * 0.03, clipping_range=(0.03, 10.0), ), - # The URDF camera link supplies the optical +Z direction, but its image - # axes are rolled 90 degrees relative to the upright D405 recording. offset=TiledCameraCfg.OffsetCfg( - pos=(0.0, 0.0, 0.0), - rot=(0.7071067812, 0.0, 0.0, 0.7071067812), + pos=(-0.10926 - mount_pos[0], mount_pos[1], -mount_pos[2]), + rot=(-y, z, w, -x), convention="ros", ), ) diff --git a/tests/test_galbot_configs.py b/tests/test_galbot_configs.py index 5403b586..522cd58a 100644 --- a/tests/test_galbot_configs.py +++ b/tests/test_galbot_configs.py @@ -4,7 +4,7 @@ """Focused contracts for the fixed-base Galbot One Golf embodiment.""" import pytest -from pxr import Usd +from pxr import Gf, Usd, UsdGeom, UsdShade from robolab.constants import TASK_DIR from robolab.core.environments.config import generate_scene_env_cfg @@ -71,15 +71,54 @@ def test_left_ego_camera_uses_policy_calibration(): ) -def test_wrist_cameras_use_golf_sensor_calibration(): +def test_wrist_cameras_use_resized_yundonghui_calibration(): left = GalbotGolfLeftWristCameraCfg().left_wrist_cam right = GalbotGolfRightWristCameraCfg().right_wrist_cam - assert (left.width, left.height) == (400, 224) - assert (right.width, right.height) == (400, 224) + assert (left.width, left.height) == (640, 360) + assert (right.width, right.height) == (640, 360) for camera in (left, right): - assert camera.width * camera.spawn.focal_length / camera.spawn.horizontal_aperture == pytest.approx(202) - assert camera.height * camera.spawn.focal_length / camera.spawn.vertical_aperture == pytest.approx(202) + assert camera.width * camera.spawn.focal_length / camera.spawn.horizontal_aperture == pytest.approx(323.2) + assert camera.height * camera.spawn.focal_length / camera.spawn.vertical_aperture == pytest.approx( + 324.64285714285717 + ) + assert camera.spawn.horizontal_aperture_offset == 0.0 + assert camera.spawn.vertical_aperture_offset == 0.0 assert camera.spawn.clipping_range == pytest.approx((0.03, 10.0)) + assert camera.offset.convention == "ros" + + # Compare the composed optical frame with the source mount-relative pose, + # using the actual USD mount transform rather than repeating its conversion. + stage = Usd.Stage.Open(GALBOT_GOLF_USD_PATH) + cache = UsdGeom.XformCache() + source_quat = Gf.Quatd(0.3981361647004496, -0.5921350168801781, 0.5860286987303605, -0.38397145780165504) + source_pose = Gf.Matrix4d().SetRotate(source_quat) + source_pose.SetTranslateOnly(Gf.Vec3d(0.07089459385344977, 0.011084636915734618, 0.0475356786953811)) + for side, camera in (("left", left), ("right", right)): + assert camera.prim_path == f"{{ENV_REGEX_NS}}/robot/{side}_arm_link7/{side}_wrist_cam" + body = stage.GetPrimAtPath(f"/galbot_one_golf/{side}_arm_link7") + mount = body.GetChild(f"{side}_arm_end_effector_mount_link") + expected = source_pose * cache.GetLocalToWorldTransform(mount) + actual = Gf.Matrix4d().SetRotate(Gf.Quatd(camera.offset.rot[0], *camera.offset.rot[1:])) + actual.SetTranslateOnly(Gf.Vec3d(*camera.offset.pos)) + actual *= cache.GetLocalToWorldTransform(body) + assert Gf.IsClose(actual, expected, 1e-6) + + +def test_wrist_visuals_use_forward_assembly_with_bound_materials(): + stage = Usd.Stage.Open(GALBOT_GOLF_USD_PATH) + for side in ("left", "right"): + visuals = stage.GetPrimAtPath(f"/galbot_one_golf/{side}_arm_link7/visuals") + assert UsdGeom.Imageable(visuals.GetChild("d405_stand")).ComputeVisibility() == "invisible" + camera = visuals.GetChild("d405") + assert UsdGeom.Imageable(camera).ComputeVisibility() == "inherited" + meshes = [prim for prim in Usd.PrimRange(camera, Usd.TraverseInstanceProxies()) if prim.IsA(UsdGeom.Mesh)] + assert {prim.GetName() for prim in meshes} == { + "d405_sz073_body", "d405_sz073_black", "d405_sz073_lens" + } + for mesh in meshes: + material = UsdShade.MaterialBindingAPI(mesh).ComputeBoundMaterial()[0] + assert material + assert material.ComputeSurfaceSource()[0] def test_usd_contains_split_fingertip_collision_meshes(): diff --git a/tests/test_galbot_wrist_camera.py b/tests/test_galbot_wrist_camera.py new file mode 100644 index 00000000..73d57329 --- /dev/null +++ b/tests/test_galbot_wrist_camera.py @@ -0,0 +1,47 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Runtime projection contract for resized Galbot wrist observations.""" + +import isaaclab.sim as sim_utils +import pytest +from isaaclab.sim import SimulationCfg, SimulationContext +from isaacsim.core.utils.stage import create_new_stage, get_current_stage + +from robolab.robots.galbot_golf import GalbotGolfLeftWristCameraCfg + + +def test_wrist_renderer_and_observation_preserve_independent_focal_lengths(): + create_new_stage() + sim = SimulationContext(SimulationCfg(dt=0.01, device="cuda:0")) + camera = None + try: + stage = get_current_stage() + stage.DefinePrim("/World/wrist", "Xform") + light = sim_utils.DomeLightCfg(intensity=1000.0) + light.func("/World/light", light) + box = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + box.func("/World/box", box) + cfg = GalbotGolfLeftWristCameraCfg().left_wrist_cam.replace( + prim_path="/World/wrist/left_wrist_cam" + ) + camera = cfg.class_type(cfg) + sim.reset() + for _ in range(3): + sim.step() + camera.update(sim.get_physics_dt()) + assert tuple(camera.data.output["rgb"].shape) == (1, 360, 640, 3) + matrix = camera.data.intrinsic_matrices[0].cpu().numpy() + assert matrix[0].tolist() == pytest.approx([323.2, 0.0, 320.0]) + assert matrix[1].tolist() == pytest.approx([0.0, 324.64285714285717, 180.0]) + assert matrix[2].tolist() == pytest.approx([0.0, 0.0, 1.0]) + prim = stage.GetPrimAtPath(cfg.prim_path) + assert prim.GetAttribute("omni:lensdistortion:model").Get() == "opencvPinhole" + for name, value in (("fx", matrix[0, 0]), ("fy", matrix[1, 1])): + assert prim.GetAttribute(f"omni:lensdistortion:opencvPinhole:{name}").Get() == pytest.approx(value) + finally: + if camera is not None: + del camera + sim.clear_all_callbacks() + sim.clear_instance() + sim.stop() From dab80f31eb5e2397c12c21cccb88c8d0003f80be Mon Sep 17 00:00:00 2001 From: ruziniuuuuu Date: Fri, 11 Sep 2026 22:53:59 +0800 Subject: [PATCH 2/2] refactor: keep Galbot wrist camera changes minimal Signed-off-by: ruziniuuuuu --- .../galbot_one_golf_description/README.md | 28 ++------ robolab/robots/galbot_golf.py | 64 +++---------------- tests/test_galbot_configs.py | 45 +------------ tests/test_galbot_wrist_camera.py | 47 -------------- 4 files changed, 18 insertions(+), 166 deletions(-) delete mode 100644 tests/test_galbot_wrist_camera.py diff --git a/assets/robots/galbot_one_golf_description/README.md b/assets/robots/galbot_one_golf_description/README.md index 1b4c3f6d..b6f53639 100644 --- a/assets/robots/galbot_one_golf_description/README.md +++ b/assets/robots/galbot_one_golf_description/README.md @@ -25,28 +25,12 @@ textures are stored under `usd/`. Variant sets on the root prim (defaults in bold): Physics (**physx**, physics, none, mujoco), Robot (**none**, robot), Sensor (**none**, sensors). -### Wrist camera visual and calibration - -The existing `d405` visual prims use the forward `d405_sz073` assembly from -the description repository at revision `69cdfde0178f2eed10c4b2e612c0c3d3b2263c7a` -(`usd/components/camera/camera_d405_sz073/payloads/instances.usda`, -`/Instances/mesh`). Its visual meshes and materials are embedded in the existing -`geometries.usd` entry `/Geometries/mesh_49`. The assembly includes its stand, -so the original `d405_stand` visuals are hidden. Link names, transforms, -collision geometry, and physical properties are unchanged. - -RoboLab's `left_wrist_cam` and `right_wrist_cam` use SynthNova's Yundonghui -policy calibration (`extensions/yundonghui/src/synthnova_yundonghui/cameras.py`, -SynthNova revision `429f5a4`). Images are resized from 400 x 224 to 640 x 360, -scaling each intrinsic-matrix row by its corresponding image dimension: -`fx=323.2`, `fy=324.64285714285717`, `cx=320`, `cy=180`. -The wrist sensor applies Isaac Sim's OpenCV pinhole lens schema (zero distortion) -and reports the same matrix, preserving the unequal focal lengths that the -default RTX pinhole and IsaacLab square-pixel assumption would otherwise lose. -The source ROS optical extrinsics are relative to each -`arm_end_effector_mount_link`. The camera config composes that mount's fixed -transform into `arm_link7` and converts XYZW quaternions to WXYZ, allowing -the sensors to follow the moving rigid bodies directly. +The `d405` visuals use the forward `d405_sz073` assembly from description +revision `69cdfde0178f2eed10c4b2e612c0c3d3b2263c7a`. Its built-in stand replaces +the hidden original stand; collision geometry and physics are unchanged. +Wrist camera parameters match SynthNova's Yundonghui calibration at 640 x 360. +The standard IsaacLab camera implementation is retained, including its +runtime square-pixel assumption (`fy=fx`). ## LICENSE diff --git a/robolab/robots/galbot_golf.py b/robolab/robots/galbot_golf.py index 62ea5de9..47c56147 100644 --- a/robolab/robots/galbot_golf.py +++ b/robolab/robots/galbot_golf.py @@ -4,14 +4,12 @@ """Fixed-base ``galbot_one_golf`` dual-arm robot configuration.""" import os -from collections.abc import Sequence import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg -from isaaclab.sensors import TiledCamera, TiledCameraCfg +from isaaclab.sensors import TiledCameraCfg from isaaclab.utils import configclass -from pxr import Gf, Sdf from robolab.constants import ROBOTS_DIR from robolab.robots.galbot_golf_definitions import * # noqa @@ -72,70 +70,26 @@ def _source_replay_camera( ) -class _WristCamera(TiledCamera): - """Render and report the independently resized wrist focal lengths.""" - - def __init__(self, cfg: TiledCameraCfg) -> None: - super().__init__(cfg) - # RTX's default pinhole and IsaacLab's intrinsic reporting assume - # square pixels. OpenCV pinhole preserves fx != fy after resizing. - for prim in sim_utils.find_matching_prims(cfg.prim_path): - # Author the schema and attributes explicitly: headless Kit apps - # need not have registered the optional schema's USD fallbacks. - prim.AddAppliedSchema("OmniLensDistortionOpenCvPinholeAPI") - prim.CreateAttribute("omni:lensdistortion:model", Sdf.ValueTypeNames.Token).Set("opencvPinhole") - parameters = { - "fx": cfg.width * cfg.spawn.focal_length / cfg.spawn.horizontal_aperture, - "fy": cfg.height * cfg.spawn.focal_length / cfg.spawn.vertical_aperture, - "cx": cfg.width / 2, - "cy": cfg.height / 2, - **dict.fromkeys(("k1", "k2", "k3", "k4", "k5", "k6", "p1", "p2", "s1", "s2", "s3", "s4"), 0.0), - } - for name, value in parameters.items(): - prim.CreateAttribute(f"omni:lensdistortion:opencvPinhole:{name}", Sdf.ValueTypeNames.Float).Set(value) - prim.CreateAttribute("omni:lensdistortion:opencvPinhole:imageSize", Sdf.ValueTypeNames.Int2).Set( - Gf.Vec2i(cfg.width, cfg.height) - ) - - def _update_intrinsic_matrices(self, env_ids: Sequence[int]) -> None: - super()._update_intrinsic_matrices(env_ids) - for index in env_ids: - prim = self._sensor_prims[index].GetPrim() - self._data.intrinsic_matrices[index, 1, 1] = prim.GetAttribute( - "omni:lensdistortion:opencvPinhole:fy" - ).Get() - - def _wrist_camera(side: str) -> TiledCameraCfg: """Yundonghui policy calibration resized from 400x224 to 640x360.""" - # SynthNova extensions/yundonghui/src/synthnova_yundonghui/cameras.py: - # ROS optical pose relative to the arm end-effector mount, quaternion XYZW. - mount_pos = (0.07089459385344977, 0.011084636915734618, 0.0475356786953811) - mount_quat = (-0.5921350168801781, 0.5860286987303605, -0.38397145780165504, 0.3981361647004496) - if side == "right": - mount_quat = tuple(-value for value in mount_quat) - x, y, z, w = mount_quat - # arm_link7 -> mount: translation (-0.10926, 0, 0), rotation Ry(pi). - # Attach directly to the moving rigid body so Fabric updates the camera; - # compose the mount transform and convert XYZW to IsaacLab's WXYZ order. return TiledCameraCfg( - class_type=_WristCamera, prim_path=f"{{ENV_REGEX_NS}}/robot/{side}_arm_link7/{side}_wrist_cam", height=360, width=640, data_types=["rgb"], spawn=sim_utils.PinholeCameraCfg( - # Keep the source film gate: fx=202*640/400, fy=202*360/224, - # with centered principal point (320, 180) after resizing. - focal_length=202 * 0.03, + # Scaled intrinsics: fx=323.2, fy=324.642857, cx=320, cy=180. + focal_length=6.06, focus_distance=0.0, - horizontal_aperture=400 * 0.03, - vertical_aperture=224 * 0.03, + horizontal_aperture=12.0, + vertical_aperture=6.72, clipping_range=(0.03, 10.0), ), + # Mount-relative calibration expressed in arm_link7, quaternion WXYZ. + # Both wrists have the same rotation (the source quaternions differ only in sign). offset=TiledCameraCfg.OffsetCfg( - pos=(-0.10926 - mount_pos[0], mount_pos[1], -mount_pos[2]), - rot=(-y, z, w, -x), + pos=(-0.18015459385344976, 0.011084636915734618, -0.0475356786953811), + rot=(-0.5860286987303605, -0.38397145780165504, 0.3981361647004496, 0.5921350168801781), convention="ros", ), ) diff --git a/tests/test_galbot_configs.py b/tests/test_galbot_configs.py index 522cd58a..61bde49e 100644 --- a/tests/test_galbot_configs.py +++ b/tests/test_galbot_configs.py @@ -4,7 +4,7 @@ """Focused contracts for the fixed-base Galbot One Golf embodiment.""" import pytest -from pxr import Gf, Usd, UsdGeom, UsdShade +from pxr import Usd from robolab.constants import TASK_DIR from robolab.core.environments.config import generate_scene_env_cfg @@ -71,54 +71,15 @@ def test_left_ego_camera_uses_policy_calibration(): ) -def test_wrist_cameras_use_resized_yundonghui_calibration(): +def test_wrist_cameras_use_golf_sensor_calibration(): left = GalbotGolfLeftWristCameraCfg().left_wrist_cam right = GalbotGolfRightWristCameraCfg().right_wrist_cam assert (left.width, left.height) == (640, 360) assert (right.width, right.height) == (640, 360) for camera in (left, right): assert camera.width * camera.spawn.focal_length / camera.spawn.horizontal_aperture == pytest.approx(323.2) - assert camera.height * camera.spawn.focal_length / camera.spawn.vertical_aperture == pytest.approx( - 324.64285714285717 - ) - assert camera.spawn.horizontal_aperture_offset == 0.0 - assert camera.spawn.vertical_aperture_offset == 0.0 + assert camera.height * camera.spawn.focal_length / camera.spawn.vertical_aperture == pytest.approx(324.642857) assert camera.spawn.clipping_range == pytest.approx((0.03, 10.0)) - assert camera.offset.convention == "ros" - - # Compare the composed optical frame with the source mount-relative pose, - # using the actual USD mount transform rather than repeating its conversion. - stage = Usd.Stage.Open(GALBOT_GOLF_USD_PATH) - cache = UsdGeom.XformCache() - source_quat = Gf.Quatd(0.3981361647004496, -0.5921350168801781, 0.5860286987303605, -0.38397145780165504) - source_pose = Gf.Matrix4d().SetRotate(source_quat) - source_pose.SetTranslateOnly(Gf.Vec3d(0.07089459385344977, 0.011084636915734618, 0.0475356786953811)) - for side, camera in (("left", left), ("right", right)): - assert camera.prim_path == f"{{ENV_REGEX_NS}}/robot/{side}_arm_link7/{side}_wrist_cam" - body = stage.GetPrimAtPath(f"/galbot_one_golf/{side}_arm_link7") - mount = body.GetChild(f"{side}_arm_end_effector_mount_link") - expected = source_pose * cache.GetLocalToWorldTransform(mount) - actual = Gf.Matrix4d().SetRotate(Gf.Quatd(camera.offset.rot[0], *camera.offset.rot[1:])) - actual.SetTranslateOnly(Gf.Vec3d(*camera.offset.pos)) - actual *= cache.GetLocalToWorldTransform(body) - assert Gf.IsClose(actual, expected, 1e-6) - - -def test_wrist_visuals_use_forward_assembly_with_bound_materials(): - stage = Usd.Stage.Open(GALBOT_GOLF_USD_PATH) - for side in ("left", "right"): - visuals = stage.GetPrimAtPath(f"/galbot_one_golf/{side}_arm_link7/visuals") - assert UsdGeom.Imageable(visuals.GetChild("d405_stand")).ComputeVisibility() == "invisible" - camera = visuals.GetChild("d405") - assert UsdGeom.Imageable(camera).ComputeVisibility() == "inherited" - meshes = [prim for prim in Usd.PrimRange(camera, Usd.TraverseInstanceProxies()) if prim.IsA(UsdGeom.Mesh)] - assert {prim.GetName() for prim in meshes} == { - "d405_sz073_body", "d405_sz073_black", "d405_sz073_lens" - } - for mesh in meshes: - material = UsdShade.MaterialBindingAPI(mesh).ComputeBoundMaterial()[0] - assert material - assert material.ComputeSurfaceSource()[0] def test_usd_contains_split_fingertip_collision_meshes(): diff --git a/tests/test_galbot_wrist_camera.py b/tests/test_galbot_wrist_camera.py deleted file mode 100644 index 73d57329..00000000 --- a/tests/test_galbot_wrist_camera.py +++ /dev/null @@ -1,47 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -"""Runtime projection contract for resized Galbot wrist observations.""" - -import isaaclab.sim as sim_utils -import pytest -from isaaclab.sim import SimulationCfg, SimulationContext -from isaacsim.core.utils.stage import create_new_stage, get_current_stage - -from robolab.robots.galbot_golf import GalbotGolfLeftWristCameraCfg - - -def test_wrist_renderer_and_observation_preserve_independent_focal_lengths(): - create_new_stage() - sim = SimulationContext(SimulationCfg(dt=0.01, device="cuda:0")) - camera = None - try: - stage = get_current_stage() - stage.DefinePrim("/World/wrist", "Xform") - light = sim_utils.DomeLightCfg(intensity=1000.0) - light.func("/World/light", light) - box = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) - box.func("/World/box", box) - cfg = GalbotGolfLeftWristCameraCfg().left_wrist_cam.replace( - prim_path="/World/wrist/left_wrist_cam" - ) - camera = cfg.class_type(cfg) - sim.reset() - for _ in range(3): - sim.step() - camera.update(sim.get_physics_dt()) - assert tuple(camera.data.output["rgb"].shape) == (1, 360, 640, 3) - matrix = camera.data.intrinsic_matrices[0].cpu().numpy() - assert matrix[0].tolist() == pytest.approx([323.2, 0.0, 320.0]) - assert matrix[1].tolist() == pytest.approx([0.0, 324.64285714285717, 180.0]) - assert matrix[2].tolist() == pytest.approx([0.0, 0.0, 1.0]) - prim = stage.GetPrimAtPath(cfg.prim_path) - assert prim.GetAttribute("omni:lensdistortion:model").Get() == "opencvPinhole" - for name, value in (("fx", matrix[0, 0]), ("fy", matrix[1, 1])): - assert prim.GetAttribute(f"omni:lensdistortion:opencvPinhole:{name}").Get() == pytest.approx(value) - finally: - if camera is not None: - del camera - sim.clear_all_callbacks() - sim.clear_instance() - sim.stop()