code-GMR / scripts /phmr_to_robot_centered.py
Tinghong-Ye22's picture
Add files using upload-large-folder tool
12ea01b verified
Raw History Blame Contribute Delete
8.99 kB
import argparse
import pathlib
import os
import time
import sys
import numpy as np
import imageio
os.environ.setdefault("MUJOCO_GL", "egl")
os.environ.setdefault("PYOPENGL_PLATFORM", "egl")
import mujoco as mj
HERE = pathlib.Path(__file__).parent
sys.path.insert(0, str(HERE.parent))
from general_motion_retargeting import GeneralMotionRetargeting as GMR
from general_motion_retargeting import RobotMotionViewer
from general_motion_retargeting import ROBOT_XML_DICT, ROBOT_BASE_DICT, VIEWER_CAM_DISTANCE_DICT
from general_motion_retargeting.utils.smpl import load_phmr_pred_file
from general_motion_retargeting.utils.smpl import get_phmr_data_offline_fast
from rich import print
def find_root_name(smplx_data_frame):
for name in ["pelvis", "Pelvis", "Hips", "hips"]:
if name in smplx_data_frame:
return name
return list(smplx_data_frame.keys())[0]
def center_motion_to_origin(smplx_data_frames, keep_height=True):
if len(smplx_data_frames) == 0:
return smplx_data_frames, np.zeros(3)
root_name = find_root_name(smplx_data_frames[0])
offset = np.asarray(smplx_data_frames[0][root_name][0]).copy()
if keep_height:
offset[2] = 0.0
centered_frames = []
for smplx_data in smplx_data_frames:
centered_smplx_data = {}
for joint_name, (pos, quat) in smplx_data.items():
centered_smplx_data[joint_name] = (np.asarray(pos) - offset, quat)
centered_frames.append(centered_smplx_data)
return centered_frames, offset
def render_qpos_video_headless(robot_type, qpos_list, motion_fps, video_path, video_width=640, video_height=480):
if len(qpos_list) == 0:
return
video_dir = os.path.dirname(video_path)
if video_dir and not os.path.exists(video_dir):
os.makedirs(video_dir, exist_ok=True)
model = mj.MjModel.from_xml_path(str(ROBOT_XML_DICT[robot_type]))
data = mj.MjData(model)
renderer = mj.Renderer(model, height=video_height, width=video_width)
camera = mj.MjvCamera()
robot_base = ROBOT_BASE_DICT[robot_type]
viewer_cam_distance = VIEWER_CAM_DISTANCE_DICT[robot_type]
with imageio.get_writer(video_path, fps=motion_fps) as writer:
for qpos in qpos_list:
data.qpos[:] = qpos
mj.mj_forward(model, data)
camera.lookat[:] = data.xpos[model.body(robot_base).id]
camera.distance = viewer_cam_distance
camera.elevation = -10
renderer.update_scene(data, camera=camera)
writer.append_data(renderer.render())
print(f"Video saved to {video_path}")
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument(
"--phmr_pred_file",
help="SMPLX motion file to load.",
type=str,
default="/home/yanjieze/projects/g1_wbc/GMR/GVHMR/outputs/demo/tennis/hmr4d_results.pt",
)
parser.add_argument(
"--robot",
choices=["unitree_g1", "unitree_g1_with_hands", "unitree_h1", "unitree_h1_2",
"booster_t1", "booster_t1_29dof","stanford_toddy", "fourier_n1",
"engineai_pm01", "kuavo_s45", "hightorque_hi", "galaxea_r1pro", "berkeley_humanoid_lite", "booster_k1",
"pnd_adam_lite", "openloong", "tienkung"],
default="unitree_g1",
)
parser.add_argument(
"--save_path",
default=None,
help="Path to save the robot motion.",
)
parser.add_argument(
"--loop",
default=False,
action="store_true",
help="Loop the motion.",
)
parser.add_argument(
"--headless",
default=False,
action="store_true",
help="Loop the motion.",
)
parser.add_argument(
"--record_video",
default=False,
action="store_true",
help="Record the video.",
)
parser.add_argument(
"--rate_limit",
default=False,
action="store_true",
help="Limit the rate of the retargeted robot motion to keep the same as the human motion.",
)
parser.add_argument(
"--full_origin",
default=False,
action="store_true",
help="Shift xyz all together. Default only shifts horizontal axes and keeps height.",
)
parser.add_argument(
"--video_path",
default=None,
help="Path to save the video. Default saves to yetinghong output folder.",
)
args = parser.parse_args()
SMPLX_FOLDER = HERE / ".." / "assets" / "body_models"
# Load SMPLX trajectory
smplx_data, body_model, smplx_output, actual_human_height = load_phmr_pred_file(
args.phmr_pred_file, SMPLX_FOLDER
)
# align fps
tgt_fps = 30
smplx_data_frames, aligned_fps = get_phmr_data_offline_fast(smplx_data, body_model, smplx_output, tgt_fps=tgt_fps)
smplx_data_frames, origin_offset = center_motion_to_origin(
smplx_data_frames, keep_height=not args.full_origin
)
print(f"Centered motion by offset: {origin_offset}")
default_video_path = (
f"/cpfs01/thscc/sharestorage/AIGC-06-01/Hithink_VideoGen_Group/user_workspace/yetinghong/code/GR00T-WholeBodyControl/outputs/gmr_videos/"
f"{args.robot}_{pathlib.Path(args.phmr_pred_file).stem}.mp4"
)
video_path = args.video_path if args.video_path is not None else default_video_path
# Initialize the retargeting system
retarget = GMR(
actual_human_height=actual_human_height,
src_human="smplx",
tgt_robot=args.robot,
)
if not args.headless:
robot_motion_viewer = RobotMotionViewer(robot_type=args.robot,
motion_fps=aligned_fps,
transparent_robot=0,
record_video=args.record_video,
video_path=video_path,)
if not args.loop and not args.rate_limit:
print("Viewer is enabled, but motion will play as fast as possible and may close immediately. Use --loop --rate_limit for interactive visualization.")
curr_frame = 0
# FPS measurement variables
fps_counter = 0
fps_start_time = time.time()
fps_display_interval = 2.0 # Display FPS every 2 seconds
qpos_list = None
if args.save_path is not None or (args.headless and args.record_video):
save_dir = os.path.dirname(args.save_path) if args.save_path is not None else None
if args.save_path is not None and save_dir: # Only create directory if it's not empty
os.makedirs(save_dir, exist_ok=True)
qpos_list = []
# Start the viewer
i = 0
while True:
if args.loop:
i = (i + 1) % len(smplx_data_frames)
else:
i += 1
if i >= len(smplx_data_frames):
break
# FPS measurement
fps_counter += 1
current_time = time.time()
if current_time - fps_start_time >= fps_display_interval:
actual_fps = fps_counter / (current_time - fps_start_time)
print(f"Actual rendering FPS: {actual_fps:.2f}")
fps_counter = 0
fps_start_time = current_time
# Update task targets.
smplx_data = smplx_data_frames[i]
# retarget
qpos = retarget.retarget(smplx_data)
# visualize
if not args.headless:
robot_motion_viewer.step(
root_pos=qpos[:3],
root_rot=qpos[3:7],
dof_pos=qpos[7:],
human_motion_data=retarget.scaled_human_data,
# human_motion_data=smplx_data,
human_pos_offset=np.array([0.0, 0.0, 0.0]),
show_human_body_name=False,
rate_limit=args.rate_limit,
)
if args.save_path is not None or (args.headless and args.record_video):
qpos_list.append(qpos)
if args.save_path is not None:
import pickle
root_pos = np.array([qpos[:3] for qpos in qpos_list])
# save from wxyz to xyzw
root_rot = np.array([qpos[3:7][[1,2,3,0]] for qpos in qpos_list])
dof_pos = np.array([qpos[7:] for qpos in qpos_list])
local_body_pos = None
body_names = None
motion_data = {
"fps": aligned_fps,
"root_pos": root_pos,
"root_rot": root_rot,
"dof_pos": dof_pos,
"local_body_pos": local_body_pos,
"link_body_list": body_names,
}
with open(args.save_path, "wb") as f:
pickle.dump(motion_data, f)
print(f"Saved to {args.save_path}")
if args.headless and args.record_video:
render_qpos_video_headless(args.robot, qpos_list, aligned_fps, video_path)
if not args.headless:
robot_motion_viewer.close()