code-Psi0-yth-v21 / scripts /viz /viz_episode_simple.py
Tinghong-Ye22's picture
Add files using upload-large-folder tool
c4f8e4a verified
Raw History Blame Contribute Delete
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"
]
@dataclasses.dataclass
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])
@server.on_client_connect
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,
)
@speed_slider.on_update
def _(_):
global play_speed
play_speed = int(speed_slider.value)
@frame_slider.on_update
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)
@play_button.on_click
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
)
@show_meshes_cb.on_update
def _(_):
viser_urdf.show_visual = show_meshes_cb.value
@show_collision_meshes_cb.on_update
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")
@reset_button.on_click
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)