Download scripts/viz/viz_episode_simple.py from Tinghong-Ye22/code-Psi0-yth-v21: direct link, hf CLI and curl.
- Browser
- Download file 12.5 kB
-
https://huggingface.co/Tinghong-Ye22/code-Psi0-yth-v21/resolve/main/scripts/viz/viz_episode_simple.py
- Command line
-
hf download hf://Tinghong-Ye22/code-Psi0-yth-v21/scripts/viz/viz_episode_simple.py
-
curl -L -o viz_episode_simple.py https://huggingface.co/Tinghong-Ye22/code-Psi0-yth-v21/resolve/main/scripts/viz/viz_episode_simple.py
12.5 kB
| from __future__ import annotations | |
| import os | |
| import time | |
| from typing import Literal | |
| import dataclasses | |
| from pathlib import Path | |
| import numpy as np | |
| import tyro | |
| import yourdfpy | |
| import viser | |
| from viser.extras import ViserUrdf | |
| from typing import Any | |
| import pinocchio as pin | |
| from fk import G1FK | |
| from g1 import * | |
| from psi.utils import resolve_data_path | |
| # copy-pasted from simple | |
| joints_names=[ | |
| "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint", "left_knee_joint", "left_ankle_pitch_joint", "left_ankle_roll_joint", | |
| "right_hip_pitch_joint", "right_hip_roll_joint", "right_hip_yaw_joint", "right_knee_joint", "right_ankle_pitch_joint", "right_ankle_roll_joint", | |
| "waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint", | |
| "left_shoulder_pitch_joint", "left_shoulder_roll_joint", "left_shoulder_yaw_joint", "left_elbow_joint","left_wrist_roll_joint","left_wrist_pitch_joint","left_wrist_yaw_joint", | |
| "right_shoulder_pitch_joint", "right_shoulder_roll_joint", "right_shoulder_yaw_joint", "right_elbow_joint","right_wrist_roll_joint","right_wrist_pitch_joint","right_wrist_yaw_joint", | |
| "left_hand_thumb_0_joint", "left_hand_thumb_1_joint", "left_hand_thumb_2_joint", "left_hand_index_0_joint", "left_hand_index_1_joint", "left_hand_middle_0_joint", "left_hand_middle_1_joint", | |
| "right_hand_thumb_0_joint", "right_hand_thumb_1_joint", "right_hand_thumb_2_joint", "right_hand_index_0_joint", "right_hand_index_1_joint", "right_hand_middle_0_joint", "right_hand_middle_1_joint" | |
| ] | |
| class Args: | |
| urdf: str = "assets/robots/g1/g1_body29_hand14.urdf" | |
| data_dir: str = "simple/G1WholebodyBendPick-v0-psi0" | |
| episode_idx: int = 0 | |
| host: str = "0.0.0.0" | |
| port: int = 9000 | |
| def __post_init__(self) -> None: | |
| if not Path(self.data_dir).is_absolute(): | |
| self.data_dir = str(resolve_data_path(self.data_dir)) | |
| if not Path(self.urdf).is_absolute(): | |
| self.urdf = str(resolve_data_path(self.urdf)) | |
| def create_robot_control_sliders( | |
| server: viser.ViserServer, viser_urdf: ViserUrdf, joint_cfg: dict[str, float] | None = None | |
| ) -> tuple[dict[str, viser.GuiInputHandle[float]], dict[str, float]]: | |
| slider_handles: dict[str, viser.GuiInputHandle[float]] = {} | |
| initial_config: dict[str, float] = {} | |
| for joint_name, ( | |
| lower, | |
| upper, | |
| ) in viser_urdf.get_actuated_joint_limits().items(): | |
| lower = lower if lower is not None else -np.pi | |
| upper = upper if upper is not None else np.pi | |
| initial_pos = joint_cfg[joint_name] if joint_cfg and joint_name in joint_cfg else 0.0 | |
| if initial_pos < lower or initial_pos > upper: | |
| print(f"Warning: Initial position {initial_pos} for joint {joint_name} is out of bounds [{lower}, {upper}].") | |
| slider = server.gui.add_slider( | |
| label=joint_name, | |
| min=lower,#min(lower,initial_pos), | |
| max=upper, | |
| step=1e-3, | |
| initial_value=np.clip(initial_pos, lower, upper), | |
| ) | |
| def _on_slider_update(_): | |
| global suppress_joint_slider_events | |
| if suppress_joint_slider_events: | |
| return | |
| viser_urdf.update_cfg({name: s.value for name, s in slider_handles.items()}) # type: ignore | |
| slider.on_update(_on_slider_update) | |
| slider_handles[joint_name] = slider | |
| initial_config[joint_name] = initial_pos | |
| return slider_handles, initial_config | |
| def extract_proprio_state(frame: dict[str, Any]) -> dict[str, float]: | |
| # global joints_names | |
| # return dict(zip(joints_names, frame["observation.proprio_joint_positions"].numpy().tolist())) | |
| proprio_state = {} | |
| proprio_state.update(dict(zip(HAND_JOINT_NAMES, frame["observation.hand_joints"].numpy().tolist()))) | |
| proprio_state.update(dict(zip(ARM_JOINT_NAMES, frame["observation.arm_joints"].numpy().tolist()))) | |
| proprio_state.update(dict(zip(LEG_JOINT_NAMES, frame["observation.leg_joints"].numpy().tolist()))) | |
| return proprio_state | |
| def extract_action_joints(frame: dict[str, Any]) -> dict[str, float]: | |
| action = {} | |
| # global joints_names | |
| # action.update(dict(zip(joints_names, frame["action"].numpy().tolist()[:len(joints_names)]))) | |
| action_np = frame["action"].numpy().tolist() | |
| action.update(dict(zip(HAND_JOINT_NAMES, action_np[0:14]))) | |
| # action.update(dict(zip(LEG_JOINT_NAMES, [0]*len(LEG_JOINT_NAMES)))) | |
| action.update(dict(zip(LEG_JOINT_NAMES, frame["observation.leg_joints"].numpy().tolist()))) # use leg joints from observation | |
| action.update(dict(zip(ARM_JOINT_NAMES, action_np[14:28]))) | |
| return action | |
| def _matrix_to_quat(rot_matrix): | |
| """Convert 3x3 rotation matrix to quaternion (w, x, y, z)""" | |
| from scipy.spatial.transform import Rotation | |
| r = Rotation.from_matrix(rot_matrix) | |
| quat_xyzw = r.as_quat() # returns [x, y, z, w] | |
| return np.array([quat_xyzw[3], quat_xyzw[0], quat_xyzw[1], quat_xyzw[2]]) # convert to [w, x, y, z] | |
| is_playing = False | |
| current_frame = 0 | |
| init_base_height = 0.75 | |
| play_speed = 10 # jump frames per step | |
| get_base_height = lambda result: -0.5 * (result["l_foot"]["position"][2] + result["r_foot"]["position"][2]) # frame["observation.prev_height"] | |
| suppress_joint_slider_events = False | |
| def main(args: Args) -> None: | |
| global current_frame | |
| server = viser.ViserServer(args.host, args.port) | |
| # Load the LeRobot dataset | |
| from psi.data.lerobot.compat import LeRobotDataset | |
| dataset = LeRobotDataset(args.data_dir, episodes=[args.episode_idx]) | |
| def _(client: viser.ClientHandle) -> None: | |
| client.add_notification( | |
| f"Loaded {len(dataset)} frames.", | |
| f"from {os.path.basename(args.data_dir)}, episode {args.episode_idx}", | |
| with_close_button=True, | |
| ) | |
| # Create GUI controls | |
| with server.gui.add_folder("Dataset Playback"): | |
| play_button = server.gui.add_button("Play/Pause") | |
| frame_slider = server.gui.add_slider( | |
| "Frame", | |
| min=0, # Start from frame 1 since we need prev_row | |
| max=len(dataset) - 1, | |
| step=1, | |
| initial_value=0, | |
| ) | |
| speed_slider = server.gui.add_slider( | |
| "Speed", | |
| min=1, | |
| max=50, | |
| step=1, | |
| initial_value=10, | |
| ) | |
| def _(_): | |
| global play_speed | |
| play_speed = int(speed_slider.value) | |
| def _(_): | |
| global current_frame, suppress_joint_slider_events | |
| current_frame = int(frame_slider.value) | |
| # update robot qpos | |
| frame = dataset[current_frame] | |
| joint_states = extract_proprio_state(frame) | |
| action_qpos = extract_action_joints(frame) | |
| # NOTE: use finger joints from action | |
| for jname in joints_names[-14:]: | |
| joint_states[jname] = action_qpos[jname] | |
| # batch-update sliders without triggering their callbacks | |
| suppress_joint_slider_events = True | |
| try: | |
| for jname, q in joint_states.items(): | |
| slider_handles[jname].value = q | |
| finally: | |
| suppress_joint_slider_events = False | |
| # joint_states["right_hand_thumb_2_joint"] -= 0.3 # TEST | |
| # single robot update after batching slider changes | |
| viser_urdf.update_cfg(joint_states) # type: ignore | |
| # update kinmatics frames | |
| result = g1.fk(joint_states) | |
| # update z | |
| viser_urdf._visual_root_frame.position = (0, 0, get_base_height(result) - init_base_height) # type: ignore | |
| l_ee_pose = result["l_ee"]["position"] | |
| l_ee_pose[2] += get_base_height(result) - init_base_height | |
| l_ee_frame_handle.position = tuple(l_ee_pose) | |
| r_ee_pose = result["r_ee"]["position"] | |
| r_ee_pose[2] += get_base_height(result) - init_base_height | |
| r_ee_frame_handle.position = tuple(r_ee_pose) | |
| action = g1.fk(action_qpos) | |
| l_ee_pose_action = action["l_ee"]["position"] | |
| l_ee_pose_action[2] += get_base_height(result) - init_base_height | |
| l_ee_handle.position = tuple(l_ee_pose_action) | |
| r_ee_pose_action = action["r_ee"]["position"] | |
| r_ee_pose_action[2] += get_base_height(result) - init_base_height | |
| r_ee_handle.position = tuple(r_ee_pose_action) | |
| def _(event: viser.GuiEvent): | |
| global is_playing | |
| is_playing = not is_playing | |
| client = event.client | |
| assert client is not None | |
| client.add_notification( | |
| f"Playing" if is_playing else "Paused", | |
| f"total frames: {len(dataset)}" if is_playing else "Paused", | |
| with_close_button=True, | |
| ) | |
| urdf = yourdfpy.URDF.load(args.urdf) | |
| viser_urdf = ViserUrdf( | |
| server, | |
| urdf_or_path=urdf, | |
| load_meshes=True, | |
| load_collision_meshes=False, | |
| collision_mesh_color_override=(1.0, 0.0, 0.0, 0.5), | |
| # root_node_name="/world/robot", | |
| ) | |
| frame = dataset[current_frame] | |
| joint_cfg = extract_proprio_state(frame) | |
| # init_base_height = frame["observation.odometry.position"][2] | |
| g1 = G1FK(args.urdf, mode="default") | |
| action = g1.fk(extract_action_joints(frame)) | |
| result = g1.fk(extract_proprio_state(frame)) | |
| init_base_height = get_base_height(result) #frame["observation.prev_height"] | |
| # actions | |
| l_ee_handle = server.scene.add_icosphere( | |
| "/action/l_ee", | |
| radius=0.05, | |
| color=(1.0, 0.0, 0.0), | |
| position=tuple(action["l_ee"]["position"]) | |
| ) | |
| r_ee_handle = server.scene.add_icosphere( | |
| "/action/r_ee", | |
| radius=0.05, | |
| color=(1.0, 0.0, 0.0), | |
| position=tuple(action["r_ee"]["position"]) | |
| ) | |
| states = g1.fk(joint_cfg) | |
| # states | |
| l_ee_frame_handle = server.scene.add_frame( | |
| "/states/l_ee_frame", | |
| wxyz=_matrix_to_quat(np.asarray(states["l_ee"]["matrix"])[:3, :3]), | |
| position=tuple(states["l_ee"]["position"]), | |
| axes_length=0.1, | |
| axes_radius=0.005 | |
| ) | |
| r_ee_frame_handle = server.scene.add_frame( | |
| "/states/r_ee_frame", | |
| wxyz=_matrix_to_quat(np.asarray(states["r_ee"]["matrix"])[:3, :3]), | |
| position=tuple(states["r_ee"]["position"]), | |
| axes_length=0.1, | |
| axes_radius=0.005 | |
| ) | |
| # Create sliders in GUI that help us move the robot joints. | |
| with server.gui.add_folder("Joint position control"): | |
| (slider_handles, initial_config) = create_robot_control_sliders( | |
| server, viser_urdf, joint_cfg | |
| ) | |
| # Add visibility checkboxes. | |
| with server.gui.add_folder("Visibility"): | |
| show_meshes_cb = server.gui.add_checkbox( | |
| "Show meshes", | |
| viser_urdf.show_visual, | |
| ) | |
| show_collision_meshes_cb = server.gui.add_checkbox( | |
| "Show collision meshes", viser_urdf.show_collision | |
| ) | |
| def _(_): | |
| viser_urdf.show_visual = show_meshes_cb.value | |
| def _(_): | |
| viser_urdf.show_collision = show_collision_meshes_cb.value | |
| # Hide checkboxes if meshes are not loaded. | |
| show_meshes_cb.visible = True | |
| show_collision_meshes_cb.visible = False # doesn't work | |
| # Set initial robot configuration. | |
| viser_urdf.update_cfg(initial_config) # type: ignore | |
| # Create grid. | |
| trimesh_scene = viser_urdf._urdf.scene or viser_urdf._urdf.collision_scene | |
| server.scene.add_grid( | |
| "/grid", | |
| width=2, | |
| height=2, | |
| position=( | |
| 0.0, | |
| 0.0, | |
| # Get the minimum z value of the trimesh scene. | |
| trimesh_scene.bounds[0, 2] if trimesh_scene is not None else 0.0, | |
| ), | |
| ) | |
| # Create joint reset button. | |
| reset_button = server.gui.add_button("Reset") | |
| def _(_): | |
| global suppress_joint_slider_events | |
| suppress_joint_slider_events = True | |
| try: | |
| for jname, init_q in initial_config.items(): | |
| slider_handles[jname].value = init_q | |
| finally: | |
| suppress_joint_slider_events = False | |
| viser_urdf.update_cfg(initial_config) # type: ignore | |
| # Sleep forever. | |
| while True: | |
| if is_playing: | |
| current_frame = (current_frame + 1*play_speed) % len(dataset) | |
| frame_slider.value = current_frame | |
| time.sleep(0.01) | |
| if __name__ == "__main__": | |
| from dotenv import load_dotenv | |
| load_dotenv() | |
| tyro.cli(main) | |