|
10 | 10 | import warp as wp |
11 | 11 |
|
12 | 12 | import isaaclab.utils.math as math_utils |
| 13 | +from isaaclab.sim.views import XformPrimView |
13 | 14 |
|
14 | 15 | from .occupancy_map_utils import OccupancyMap, intersect_occupancy_maps |
15 | 16 | from .transform_utils import transform_mul |
@@ -100,16 +101,25 @@ def __init__(self, scene, entity_name: str): |
100 | 101 | self.scene = scene |
101 | 102 | self.entity_name = entity_name |
102 | 103 |
|
| 104 | + def _get_xform_view(self) -> XformPrimView: |
| 105 | + """Return the XformPrimView for this asset, refreshing it if prims were not yet cloned.""" |
| 106 | + xform_prim = self.scene[self.entity_name] |
| 107 | + if xform_prim.count == 0: |
| 108 | + # The view was created before environment cloning; rebuild it now that prims exist. |
| 109 | + xform_prim = XformPrimView(xform_prim._prim_path, device=xform_prim.device) |
| 110 | + self.scene.extras[self.entity_name] = xform_prim |
| 111 | + return xform_prim |
| 112 | + |
103 | 113 | def get_pose(self): |
104 | 114 | """Get the 3D pose of the entity.""" |
105 | | - xform_prim = self.scene[self.entity_name] |
| 115 | + xform_prim = self._get_xform_view() |
106 | 116 | position, orientation = xform_prim.get_world_poses() |
107 | 117 | pose = torch.cat([position, orientation], dim=-1) |
108 | 118 | return pose |
109 | 119 |
|
110 | 120 | def set_pose(self, pose: torch.Tensor): |
111 | 121 | """Set the 3D pose of the entity.""" |
112 | | - xform_prim = self.scene[self.entity_name] |
| 122 | + xform_prim = self._get_xform_view() |
113 | 123 | position = pose[..., :3] |
114 | 124 | orientation = pose[..., 3:] |
115 | 125 | xform_prim.set_world_poses(position, orientation, None) |
@@ -149,6 +159,10 @@ def get_occupancy_map(self): |
149 | 159 | ) |
150 | 160 |
|
151 | 161 | transform = self.get_transform_2d().detach().cpu().numpy() |
| 162 | + # get_world_poses() may return a batched (num_envs, 3, 3) or empty (0, 3, 3) tensor. |
| 163 | + # For a fixed background asset placed at the world origin, fall back to identity when empty. |
| 164 | + if transform.ndim == 3: |
| 165 | + transform = transform[0] if transform.shape[0] > 0 else np.eye(3) |
152 | 166 |
|
153 | 167 | occupancy_map = local_occupancy_map.transformed(transform) |
154 | 168 |
|
|
0 commit comments