From eded84176f6f3aecb075b88cb8f129f53bc8cd95 Mon Sep 17 00:00:00 2001 From: EverNorif <1320346985@qq.com> Date: Tue, 7 Apr 2026 18:54:38 +0800 Subject: [PATCH 1/2] curobo world obstacles v0. --- .../visualization/curobo_world_obstacles.py | 231 ++++++++++++++++++ 1 file changed, 231 insertions(+) create mode 100644 examples/visualization/curobo_world_obstacles.py diff --git a/examples/visualization/curobo_world_obstacles.py b/examples/visualization/curobo_world_obstacles.py new file mode 100644 index 0000000..69634da --- /dev/null +++ b/examples/visualization/curobo_world_obstacles.py @@ -0,0 +1,231 @@ +"""Visualize cuRobo world obstacles in the Isaac Sim viewport. + +This script reads the static obstacle world that cuRobo uses for collision checking +(as built inside `autosim`'s `CuroboPlanner._initialize_static_world()`), converts +all obstacle poses from the planner's reference frame (robot root) to the world frame, +and draws simple wireframes. + +Usage +----- +Run with Isaac Sim UI enabled (do NOT use ``--headless`` if you want to see the viewport): + + python examples/visualization/curobo_world_obstacles.py --pipeline_id + +Notes +----- +* cuRobo's `UsdHelper.get_obstacles_from_stage(..., reference_prim_path=robot_prim_path)` + applies an inverse reference transform, so obstacle poses are returned in the + **robot root frame** (not world). We therefore compose with the robot root pose + in world to get world-frame obstacle poses for visualization. +* Wireframe drawing uses debug lines; keep the app running to inspect the scene. +""" + +from __future__ import annotations + +import argparse +from collections.abc import Sequence +from dataclasses import dataclass + +import torch +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="Visualize cuRobo collision world obstacles.") +parser.add_argument("--pipeline_id", type=str, default=None, help="Name of the autosim pipeline.") + +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() + +app_launcher = AppLauncher(vars(args_cli)) +simulation_app = app_launcher.app + + +import isaaclab.utils.math as PoseUtils + +import autosim_examples # noqa: F401 +from autosim import make_pipeline +from autosim.utils.debug_util import clear_debug_drawing, draw_line + + +@dataclass(frozen=True) +class _Pose7: + pos: torch.Tensor # (3,) + quat: torch.Tensor # (4,) wxyz + + +def _as_pose7(pose7: Sequence[float] | torch.Tensor, *, device: torch.device, dtype: torch.dtype) -> _Pose7: + t = torch.as_tensor(pose7, device=device, dtype=dtype).view(7) + return _Pose7(pos=t[:3], quat=t[3:]) + + +def _quat_to_rotmat_wxyz(q: torch.Tensor) -> torch.Tensor: + """Convert quaternion (wxyz) to rotation matrix. q shape (4,). Returns (3,3).""" + q = q / (q.norm(p=2) + 1e-12) + w, x, y, z = q + ww = w * w + xx = x * x + yy = y * y + zz = z * z + wx = w * x + wy = w * y + wz = w * z + xy = x * y + xz = x * z + yz = y * z + return torch.stack( + [ + torch.stack([ww + xx - yy - zz, 2.0 * (xy - wz), 2.0 * (xz + wy)], dim=0), + torch.stack([2.0 * (xy + wz), ww - xx + yy - zz, 2.0 * (yz - wx)], dim=0), + torch.stack([2.0 * (xz - wy), 2.0 * (yz + wx), ww - xx - yy + zz], dim=0), + ], + dim=0, + ) + + +def _transform_points(pose: _Pose7, pts: torch.Tensor) -> torch.Tensor: + """Apply pose (pos, quat) to points. pts shape (...,3) in local frame.""" + r = _quat_to_rotmat_wxyz(pose.quat) + return (pts @ r.T) + pose.pos + + +def _draw_oriented_box(*, pose_w: _Pose7, half_dims_xyz: torch.Tensor, color, thickness: float, z_lift: float) -> None: + hx, hy, hz = float(half_dims_xyz[0]), float(half_dims_xyz[1]), float(half_dims_xyz[2]) + corners_l = torch.tensor( + [ + [-hx, -hy, -hz], + [-hx, -hy, +hz], + [-hx, +hy, -hz], + [-hx, +hy, +hz], + [+hx, -hy, -hz], + [+hx, -hy, +hz], + [+hx, +hy, -hz], + [+hx, +hy, +hz], + ], + device=pose_w.pos.device, + dtype=pose_w.pos.dtype, + ) + corners_w = _transform_points(pose_w, corners_l).detach().cpu() + if z_lift != 0.0: + corners_w[:, 2] += float(z_lift) + + # 12 edges by index pairs + edges = [ + (0, 1), + (0, 2), + (1, 3), + (2, 3), + (4, 5), + (4, 6), + (5, 7), + (6, 7), + (0, 4), + (1, 5), + (2, 6), + (3, 7), + ] + for a, b in edges: + pa = tuple(float(v) for v in corners_w[a].tolist()) + pb = tuple(float(v) for v in corners_w[b].tolist()) + draw_line(pa, pb, color=color, size=thickness) + + +def _compose_robot_to_world(*, robot_root_pose_w: torch.Tensor, pose_r: _Pose7) -> _Pose7: + """Compose pose in robot root frame to world frame.""" + rr_pos_w = robot_root_pose_w[:3].view(1, 3) + rr_quat_w = robot_root_pose_w[3:].view(1, 4) + pos_r = pose_r.pos.view(1, 3) + quat_r = pose_r.quat.view(1, 4) + pos_w, quat_w = PoseUtils.combine_frame_transforms(rr_pos_w, rr_quat_w, pos_r, quat_r) + return _Pose7(pos=pos_w.view(3), quat=quat_w.view(4)) + + +def _visualize_world_obstacles( + *, + pipeline, + env_id: int = 0, + color=(0.95, 0.2, 0.25, 1.0), + thickness: float = 2.0, + z_lift: float = 0.0, + max_mesh_vertices: int = 200000, +): + planner = pipeline._motion_planner + if not hasattr(planner, "_static_world_config"): + raise RuntimeError( + "CuroboPlanner has no `_static_world_config`. Ensure planner initialized and warmup complete." + ) + world_r = planner._static_world_config # WorldConfig in robot root frame + + robot_root_pose_w = pipeline._robot.data.root_pose_w[env_id].detach() + device = robot_root_pose_w.device + dtype = robot_root_pose_w.dtype + + clear_debug_drawing() + + # Cuboids (OBB pose + dims) + for cub in world_r.cuboid: + pose_r = _as_pose7(cub.pose, device=device, dtype=dtype) + pose_w = _compose_robot_to_world(robot_root_pose_w=robot_root_pose_w, pose_r=pose_r) + dims = torch.as_tensor(cub.dims, device=device, dtype=dtype).view(3) + half_dims = dims * 0.5 + _draw_oriented_box(pose_w=pose_w, half_dims_xyz=half_dims, color=color, thickness=thickness, z_lift=z_lift) + + # Spheres (pose center + radius) -> draw as cube approximation + for sph in world_r.sphere: + pose_r = _as_pose7(sph.pose, device=device, dtype=dtype) + pose_w = _compose_robot_to_world(robot_root_pose_w=robot_root_pose_w, pose_r=pose_r) + r = float(sph.radius) + half_dims = torch.tensor([r, r, r], device=device, dtype=dtype) + _draw_oriented_box(pose_w=pose_w, half_dims_xyz=half_dims, color=color, thickness=thickness, z_lift=z_lift) + + # Cylinders (pose + radius + height) -> draw as oriented bounding box approximation + for cyl in world_r.cylinder: + pose_r = _as_pose7(cyl.pose, device=device, dtype=dtype) + pose_w = _compose_robot_to_world(robot_root_pose_w=robot_root_pose_w, pose_r=pose_r) + half_dims = torch.tensor( + [float(cyl.radius), float(cyl.radius), float(cyl.height) * 0.5], device=device, dtype=dtype + ) + _draw_oriented_box(pose_w=pose_w, half_dims_xyz=half_dims, color=color, thickness=thickness, z_lift=z_lift) + + # Capsules (pose + radius + base/tip) -> draw as oriented bounding box approximation + for cap in world_r.capsule: + pose_r = _as_pose7(cap.pose, device=device, dtype=dtype) + pose_w = _compose_robot_to_world(robot_root_pose_w=robot_root_pose_w, pose_r=pose_r) + base = torch.as_tensor(cap.base, device=device, dtype=dtype).view(3) + tip = torch.as_tensor(cap.tip, device=device, dtype=dtype).view(3) + height = float((tip - base).norm().item()) + half_dims = torch.tensor([float(cap.radius), float(cap.radius), height * 0.5], device=device, dtype=dtype) + _draw_oriented_box(pose_w=pose_w, half_dims_xyz=half_dims, color=color, thickness=thickness, z_lift=z_lift) + + # Mesh obstacles: prefer a tight OBB via cuRobo's `Mesh.get_cuboid()`. + for mesh in world_r.mesh: + verts = getattr(mesh, "vertices", None) + if verts is not None and len(verts) > int(max_mesh_vertices): + continue + + try: + cub = mesh.get_cuboid() + except Exception: + # Fallback: if OBB conversion fails, skip visualization for this mesh. + print(f"Failed to convert mesh {mesh.name} to OBB") + continue + + pose_r = _as_pose7(cub.pose, device=device, dtype=dtype) + pose_w = _compose_robot_to_world(robot_root_pose_w=robot_root_pose_w, pose_r=pose_r) + dims = torch.as_tensor(cub.dims, device=device, dtype=dtype).view(3) + half_dims = dims * 0.5 + _draw_oriented_box(pose_w=pose_w, half_dims_xyz=half_dims, color=color, thickness=thickness, z_lift=z_lift) + + +def main(): + pipeline = make_pipeline(args_cli.pipeline_id) + pipeline.initialize() + pipeline.reset_env() + + _visualize_world_obstacles(pipeline=pipeline) + + while simulation_app.is_running(): + pipeline._env.sim.render() + + +if __name__ == "__main__": + main() + simulation_app.close() From 51f9a4e263b6626b9e2f6ae0a49a7d6085b5816c Mon Sep 17 00:00:00 2001 From: EverNorif <1320346985@qq.com> Date: Wed, 8 Apr 2026 11:10:03 +0800 Subject: [PATCH 2/2] draw line util. --- source/autosim/autosim/utils/debug_util.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/source/autosim/autosim/utils/debug_util.py b/source/autosim/autosim/utils/debug_util.py index b1293b5..bafeaaa 100644 --- a/source/autosim/autosim/utils/debug_util.py +++ b/source/autosim/autosim/utils/debug_util.py @@ -199,3 +199,23 @@ def debug_visualize_goal_sampling( fig.canvas.draw_idle() fig.canvas.flush_events() plt.pause(0.02) + + +def draw_line(start, end, color=(1.0, 0.0, 0.0, 1.0), size=1.0): + """ + Draws a single line between two points. + """ + import isaacsim.util.debug_draw._debug_draw as omni_debug_draw + + draw = omni_debug_draw.acquire_debug_draw_interface() + draw.draw_lines([start], [end], [color], [size]) + + +def clear_debug_drawing(): + """ + Clears all debug drawings. + """ + import isaacsim.util.debug_draw._debug_draw as omni_debug_draw + + draw = omni_debug_draw.acquire_debug_draw_interface() + draw.clear_lines()