88from __future__ import annotations
99
1010import logging
11- import os
12- import re
1311from collections .abc import Sequence
1412from typing import TYPE_CHECKING
1513
2826
2927# import logger
3028logger = logging .getLogger (__name__ )
31- _VIS_DEBUG_ENABLED = os .getenv ("ISAACLAB_VIS_DEBUG" , "0" ).lower () in {"1" , "true" , "yes" , "on" }
3229
3330
3431class UniformVelocityCommand (CommandTerm ):
@@ -87,9 +84,6 @@ def __init__(self, cfg: UniformVelocityCommandCfg, env: ManagerBasedEnv):
8784 self .heading_target = torch .zeros (self .num_envs , device = self .device )
8885 self .is_heading_env = torch .zeros (self .num_envs , dtype = torch .bool , device = self .device )
8986 self .is_standing_env = torch .zeros_like (self .is_heading_env )
90- self ._debug_vis_callback_count = 0
91- self ._debug_env0_probe_body_path : str | None = None
92- self ._debug_env0_probe_status : str = "uninitialized"
9387 # -- metrics
9488 self .metrics ["error_vel_xy" ] = torch .zeros (self .num_envs , device = self .device )
9589 self .metrics ["error_vel_yaw" ] = torch .zeros (self .num_envs , device = self .device )
@@ -207,83 +201,6 @@ def _debug_vis_callback(self, event):
207201 # display markers
208202 self .goal_vel_visualizer .visualize (base_pos_w , vel_des_arrow_quat , vel_des_arrow_scale )
209203 self .current_vel_visualizer .visualize (base_pos_w , vel_arrow_quat , vel_arrow_scale )
210- self ._debug_vis_callback_count += 1
211- if _VIS_DEBUG_ENABLED and (
212- self ._debug_vis_callback_count <= 3 or self ._debug_vis_callback_count % 120 == 0
213- ):
214- mean_cmd = float (torch .linalg .norm (self .command [:, :2 ], dim = - 1 ).mean ().item ())
215- mean_vel = float (torch .linalg .norm (wp .to_torch (self .robot .data .root_lin_vel_b )[:, :2 ], dim = - 1 ).mean ().item ())
216- tensor_root_pos = wp .to_torch (self .robot .data .root_pos_w )[0 ].detach ().cpu ()
217- fabric_root_pos = self ._get_env0_fabric_probe_position ()
218- root_view_pos = self ._get_env0_root_view_position ()
219- robot_root = self ._resolve_env0_robot_root_path ()
220- fabric_base_pos = self ._get_fabric_position_for_path (f"{ robot_root } /base" ) if robot_root else None
221- fabric_trunk_pos = self ._get_fabric_position_for_path (f"{ robot_root } /trunk" ) if robot_root else None
222- if fabric_root_pos is not None :
223- fabric_root_pos_t = torch .tensor (fabric_root_pos , dtype = tensor_root_pos .dtype )
224- delta = torch .linalg .norm (tensor_root_pos - fabric_root_pos_t ).item ()
225- root_view_delta = None
226- if root_view_pos is not None :
227- root_view_pos_t = torch .tensor (root_view_pos , dtype = tensor_root_pos .dtype )
228- root_view_delta = torch .linalg .norm (tensor_root_pos - root_view_pos_t ).item ()
229- logger .warning (
230- "[VIS-DEBUG][UniformVelocityCommand._debug_vis_callback] count=%d mean_cmd_xy=%.4f "
231- "mean_vel_xy=%.4f env0_tensor_root=(%.3f, %.3f, %.3f) env0_fabric_root=(%.3f, %.3f, %.3f) "
232- "tensor_fabric_delta=%.5f root_view_delta=%s fabric_base=%s fabric_trunk=%s "
233- "probe_path=%s probe_status=%s" ,
234- self ._debug_vis_callback_count ,
235- mean_cmd ,
236- mean_vel ,
237- float (tensor_root_pos [0 ]),
238- float (tensor_root_pos [1 ]),
239- float (tensor_root_pos [2 ]),
240- float (fabric_root_pos_t [0 ]),
241- float (fabric_root_pos_t [1 ]),
242- float (fabric_root_pos_t [2 ]),
243- float (delta ),
244- f"{ root_view_delta :.5f} " if root_view_delta is not None else "<unavailable>" ,
245- (
246- f"({ fabric_base_pos [0 ]:.3f} , { fabric_base_pos [1 ]:.3f} , { fabric_base_pos [2 ]:.3f} )"
247- if fabric_base_pos is not None
248- else "<unavailable>"
249- ),
250- (
251- f"({ fabric_trunk_pos [0 ]:.3f} , { fabric_trunk_pos [1 ]:.3f} , { fabric_trunk_pos [2 ]:.3f} )"
252- if fabric_trunk_pos is not None
253- else "<unavailable>"
254- ),
255- self ._debug_env0_probe_body_path or "<unresolved>" ,
256- self ._debug_env0_probe_status ,
257- )
258- else :
259- logger .warning (
260- "[VIS-DEBUG][UniformVelocityCommand._debug_vis_callback] count=%d mean_cmd_xy=%.4f "
261- "mean_vel_xy=%.4f env0_tensor_root=(%.3f, %.3f, %.3f) env0_fabric_root=<unavailable> "
262- "root_view=%s fabric_base=%s fabric_trunk=%s probe_path=%s probe_status=%s" ,
263- self ._debug_vis_callback_count ,
264- mean_cmd ,
265- mean_vel ,
266- float (tensor_root_pos [0 ]),
267- float (tensor_root_pos [1 ]),
268- float (tensor_root_pos [2 ]),
269- (
270- f"({ root_view_pos [0 ]:.3f} , { root_view_pos [1 ]:.3f} , { root_view_pos [2 ]:.3f} )"
271- if root_view_pos is not None
272- else "<unavailable>"
273- ),
274- (
275- f"({ fabric_base_pos [0 ]:.3f} , { fabric_base_pos [1 ]:.3f} , { fabric_base_pos [2 ]:.3f} )"
276- if fabric_base_pos is not None
277- else "<unavailable>"
278- ),
279- (
280- f"({ fabric_trunk_pos [0 ]:.3f} , { fabric_trunk_pos [1 ]:.3f} , { fabric_trunk_pos [2 ]:.3f} )"
281- if fabric_trunk_pos is not None
282- else "<unavailable>"
283- ),
284- self ._debug_env0_probe_body_path or "<unresolved>" ,
285- self ._debug_env0_probe_status ,
286- )
287204
288205 """
289206 Internal helpers.
@@ -306,110 +223,6 @@ def _resolve_xy_velocity_to_arrow(self, xy_velocity: torch.Tensor) -> tuple[torc
306223
307224 return arrow_scale , arrow_quat
308225
309- def _resolve_env0_robot_root_path (self ) -> str | None :
310- """Best-effort resolve env_0 robot root path from articulation cfg prim path."""
311- prim_path = getattr (self .robot .cfg , "prim_path" , None )
312- if not isinstance (prim_path , str ) or not prim_path :
313- return None
314- path = prim_path
315- if "{ENV_REGEX_NS}" in path :
316- path = path .replace ("{ENV_REGEX_NS}" , "/World/envs/env_0" )
317- path = path .replace ("env_.*" , "env_0" )
318- path = re .sub (r"\.\*$" , "0" , path )
319- return path
320-
321- def _resolve_env0_fabric_probe_body_path (self ) -> str | None :
322- """Resolve a rigid-body prim path for env_0 suitable for Fabric world-matrix probing."""
323- if self ._debug_env0_probe_body_path is not None :
324- return self ._debug_env0_probe_body_path
325- try :
326- from pxr import UsdPhysics
327-
328- stage = self ._env .sim .stage
329- if stage is None :
330- self ._debug_env0_probe_status = "no_stage"
331- return None
332- robot_root = self ._resolve_env0_robot_root_path ()
333- if not robot_root :
334- self ._debug_env0_probe_status = "no_robot_root_path"
335- return None
336- root_prim = stage .GetPrimAtPath (robot_root )
337- if not root_prim .IsValid ():
338- self ._debug_env0_probe_status = f"invalid_robot_root:{ robot_root } "
339- return None
340-
341- # First try common body names directly under Robot to avoid brittle traversal assumptions.
342- fallback_candidates = [
343- f"{ robot_root } /base" ,
344- f"{ robot_root } /base_link" ,
345- f"{ robot_root } /trunk" ,
346- f"{ robot_root } /pelvis" ,
347- ]
348- for candidate in fallback_candidates :
349- prim = stage .GetPrimAtPath (candidate )
350- if prim .IsValid () and prim .HasAPI (UsdPhysics .RigidBodyAPI ):
351- self ._debug_env0_probe_body_path = candidate
352- self ._debug_env0_probe_status = "resolved_fallback_candidate"
353- return self ._debug_env0_probe_body_path
354-
355- preferred_names = {"base" , "base_link" , "trunk" , "pelvis" }
356- for prim in root_prim .Traverse ():
357- if not prim .IsValid () or not prim .HasAPI (UsdPhysics .RigidBodyAPI ):
358- continue
359- if prim .GetName () in preferred_names :
360- self ._debug_env0_probe_body_path = prim .GetPath ().pathString
361- self ._debug_env0_probe_status = "resolved_traverse_preferred"
362- return self ._debug_env0_probe_body_path
363- for prim in root_prim .Traverse ():
364- if prim .IsValid () and prim .HasAPI (UsdPhysics .RigidBodyAPI ):
365- self ._debug_env0_probe_body_path = prim .GetPath ().pathString
366- self ._debug_env0_probe_status = "resolved_traverse_first_rigidbody"
367- return self ._debug_env0_probe_body_path
368- self ._debug_env0_probe_status = "no_rigidbody_under_robot_root"
369- except Exception :
370- self ._debug_env0_probe_status = "resolve_exception"
371- return None
372- return None
373-
374- def _get_env0_fabric_probe_position (self ) -> tuple [float , float , float ] | None :
375- """Read env_0 probe body world translation from Fabric backend."""
376- body_path = self ._resolve_env0_fabric_probe_body_path ()
377- if not body_path :
378- return None
379- return self ._get_fabric_position_for_path (body_path )
380-
381- def _get_fabric_position_for_path (self , body_path : str ) -> tuple [float , float , float ] | None :
382- """Read a body world translation from Fabric backend."""
383- try :
384- import isaacsim .core .experimental .utils .backend as backend_utils
385- import isaacsim .core .experimental .utils .prim as prim_utils
386- import numpy as np
387-
388- with backend_utils .use_backend ("fabric" ):
389- world_matrix = prim_utils .get_prim_attribute_value (body_path , "omni:fabric:worldMatrix" )
390- translation = np .array (world_matrix .ExtractTranslation (), dtype = float )
391- self ._debug_env0_probe_status = "fabric_read_ok"
392- return (float (translation [0 ]), float (translation [1 ]), float (translation [2 ]))
393- except Exception :
394- self ._debug_env0_probe_status = "fabric_read_exception"
395- return None
396-
397- def _get_env0_root_view_position (self ) -> tuple [float , float , float ] | None :
398- """Read env_0 articulation root position directly from PhysX root_view."""
399- try :
400- root_transforms = self .robot .root_view .get_root_transforms ()
401- root_transforms_t = wp .to_torch (root_transforms )
402- if root_transforms_t .ndim >= 2 and root_transforms_t .shape [1 ] >= 3 and root_transforms_t .shape [0 ] > 0 :
403- return (
404- float (root_transforms_t [0 , 0 ].item ()),
405- float (root_transforms_t [0 , 1 ].item ()),
406- float (root_transforms_t [0 , 2 ].item ()),
407- )
408- except Exception :
409- return None
410- return None
411-
412-
413226class NormalVelocityCommand (UniformVelocityCommand ):
414227 """Command generator that generates a velocity command in SE(2) from a normal distribution.
415228
0 commit comments