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()