Get current MuJoCo body poses as a dictionary. Returns dict with structure: {clean_body_name: {"xpos": [x,y,z], "xquat": [w,x,y,z]}}
(self)
| 3198 | } |
| 3199 | |
| 3200 | def _get_mujoco_poses(self) -> Dict[str, Dict[str, Any]]: |
| 3201 | """Get current MuJoCo body poses as a dictionary. |
| 3202 | |
| 3203 | Returns dict with structure: {clean_body_name: {"xpos": [x,y,z], "xquat": [w,x,y,z]}} |
| 3204 | """ |
| 3205 | body_dict = {} |
| 3206 | data = self._mujoco_data |
| 3207 | clean_names = self._mujoco_clean_names # Use cached clean names |
| 3208 | |
| 3209 | for body_name, body_id in self._mujoco_bodies.items(): |
| 3210 | if "world" in body_name.lower(): |
| 3211 | continue # Skip world body |
| 3212 | |
| 3213 | # Get pose data - .tolist() creates a copy (safe for threading) |
| 3214 | xpos = data.body(body_id).xpos.tolist() |
| 3215 | xquat = data.body(body_id).xquat.tolist() |
| 3216 | |
| 3217 | # Use pre-computed clean name (no string ops per frame) |
| 3218 | clean_name = clean_names.get(body_name, body_name) |
| 3219 | body_dict[clean_name] = { |
| 3220 | "xpos": xpos, |
| 3221 | "xquat": xquat, |
| 3222 | } |
| 3223 | |
| 3224 | return body_dict |
| 3225 | |
| 3226 | _isaac_pose_debug_counter = 0 # Class-level counter for debug |
| 3227 |
no test coverage detected