55
66import asyncio
77import contextlib
8- import os
98import sys
109import traceback
1110from typing import Any
1211
1312import torch
14- import warp as wp
1513
1614from isaaclab .envs import ManagerBasedRLMimicEnv
1715from isaaclab .envs .mdp .recorders .recorders_cfg import ActionStateRecorderManagerCfg
1816from isaaclab .managers import DatasetExportMode , TerminationTermCfg
1917from isaaclab .managers .recorder_manager import RecorderManagerBaseCfg
20- from isaaclab .utils .math import subtract_frame_transforms
2118
2219from isaaclab_mimic .datagen .data_generator import DataGenerator
2320from isaaclab_mimic .datagen .datagen_info_pool import DataGenInfoPool
3027num_attempts = 0
3128
3229
33- def _debug_wrist_cam_pose (env : ManagerBasedRLMimicEnv , env_id : int , tag : str , hand_body_idx : int | None ):
34- """Print wrist camera and hand-link transforms for debugging camera alignment."""
35- try :
36- if "wrist_cam" not in env .scene .keys () or "robot" not in env .scene .keys ():
37- print (f"[WRIST_CAM_DEBUG] { tag } env={ env_id } missing wrist_cam or robot in scene" )
38- return
39-
40- wrist_cam = env .scene ["wrist_cam" ]
41- robot = env .scene ["robot" ]
42- cam_pos = wrist_cam .data .pos_w [env_id ].detach ().cpu ()
43- cam_quat = wrist_cam .data .quat_w_world [env_id ].detach ().cpu ()
44- cam_frame = int (wrist_cam .frame [env_id ].item ()) if wrist_cam .frame .numel () > env_id else - 1
45- live_pos_all , live_quat_all = wrist_cam ._view .get_world_poses ([env_id ])
46- live_cam_pos = live_pos_all [0 ].detach ().cpu ()
47- live_cam_quat = live_quat_all [0 ].detach ().cpu ()
48-
49- if hand_body_idx is None :
50- print (
51- f"[WRIST_CAM_DEBUG] { tag } env={ env_id } cam_frame={ cam_frame } "
52- f"cam_pos={ cam_pos .tolist ()} cam_quat_xyzw={ cam_quat .tolist ()} "
53- f"live_cam_pos={ live_cam_pos .tolist ()} live_cam_quat_xyzw={ live_cam_quat .tolist ()} "
54- f"hand_body_idx=None"
55- )
56- return
57-
58- hand_pos = wp .to_torch (robot .data .body_pos_w )[env_id , hand_body_idx ].detach ().cpu ()
59- hand_quat = wp .to_torch (robot .data .body_quat_w )[env_id , hand_body_idx ].detach ().cpu ()
60- rel_pos , rel_quat = subtract_frame_transforms (
61- hand_pos .unsqueeze (0 ),
62- hand_quat .unsqueeze (0 ),
63- cam_pos .unsqueeze (0 ),
64- cam_quat .unsqueeze (0 ),
65- )
66- live_rel_pos , live_rel_quat = subtract_frame_transforms (
67- hand_pos .unsqueeze (0 ),
68- hand_quat .unsqueeze (0 ),
69- live_cam_pos .unsqueeze (0 ),
70- live_cam_quat .unsqueeze (0 ),
71- )
72- print (
73- f"[WRIST_CAM_DEBUG] { tag } env={ env_id } cam_frame={ cam_frame } "
74- f"cam_pos={ cam_pos .tolist ()} cam_quat_xyzw={ cam_quat .tolist ()} "
75- f"live_cam_pos={ live_cam_pos .tolist ()} live_cam_quat_xyzw={ live_cam_quat .tolist ()} "
76- f"hand_pos={ hand_pos .tolist ()} hand_quat_xyzw={ hand_quat .tolist ()} "
77- f"cam_rel_hand_pos={ rel_pos [0 ].detach ().cpu ().tolist ()} "
78- f"cam_rel_hand_quat_xyzw={ rel_quat [0 ].detach ().cpu ().tolist ()} "
79- f"live_cam_rel_hand_pos={ live_rel_pos [0 ].detach ().cpu ().tolist ()} "
80- f"live_cam_rel_hand_quat_xyzw={ live_rel_quat [0 ].detach ().cpu ().tolist ()} "
81- )
82- except Exception as exc :
83- print (f"[WRIST_CAM_DEBUG] { tag } env={ env_id } logging_failed={ exc } " )
84-
85-
8630async def run_data_generator (
8731 env : ManagerBasedRLMimicEnv ,
8832 env_id : int ,
@@ -150,18 +94,6 @@ def env_loop(
15094 global num_success , num_failures , num_attempts
15195 env_id_tensor = torch .tensor ([0 ], dtype = torch .int64 , device = env .device )
15296 prev_num_attempts = 0
153- debug_wrist_cam = os .environ .get ("ISAACLAB_DEBUG_WRIST_CAM" , "0" ) == "1"
154- post_reset_step_counters = [0 for _ in range (env .num_envs )]
155- hand_body_idx = None
156- if debug_wrist_cam :
157- try :
158- if "robot" in env .scene .keys ():
159- body_ids , _ = env .scene ["robot" ].find_bodies ("panda_hand" )
160- hand_body_idx = int (body_ids [0 ]) if len (body_ids ) > 0 else None
161- print (f"[WRIST_CAM_DEBUG] enabled=1 hand_body_idx={ hand_body_idx } " )
162- except Exception as exc :
163- print (f"[WRIST_CAM_DEBUG] enabled=1 hand_body_idx_lookup_failed={ exc } " )
164-
16597 # simulate environment -- run everything in inference mode
16698 with contextlib .suppress (KeyboardInterrupt ) and torch .inference_mode ():
16799 while True :
@@ -176,11 +108,6 @@ def env_loop(
176108 while not env_reset_queue .empty ():
177109 env_id_tensor [0 ] = env_reset_queue .get_nowait ()
178110 env .reset (env_ids = env_id_tensor )
179- reset_env_id = int (env_id_tensor [0 ].item ())
180- if 0 <= reset_env_id < env .num_envs :
181- post_reset_step_counters [reset_env_id ] = 0
182- if debug_wrist_cam :
183- _debug_wrist_cam_pose (env , reset_env_id , tag = "after_reset" , hand_body_idx = hand_body_idx )
184111 env_reset_queue .task_done ()
185112
186113 actions = torch .zeros (env .action_space .shape )
@@ -191,16 +118,6 @@ def env_loop(
191118
192119 # perform action on environment
193120 env .step (actions )
194- if debug_wrist_cam :
195- for env_id in range (env .num_envs ):
196- if post_reset_step_counters [env_id ] <= 1 :
197- _debug_wrist_cam_pose (
198- env ,
199- env_id ,
200- tag = f"after_step_{ post_reset_step_counters [env_id ]} " ,
201- hand_body_idx = hand_body_idx ,
202- )
203- post_reset_step_counters [env_id ] += 1
204121
205122 # mark done so the data generators can continue with the step results
206123 for i in range (env .num_envs ):
0 commit comments