Download scripts/phmr_to_robot_centered.py from Tinghong-Ye22/code-GMR: direct link, hf CLI and curl.
- Browser
- Download file 8.99 kB
-
https://huggingface.co/Tinghong-Ye22/code-GMR/resolve/main/scripts/phmr_to_robot_centered.py
- Command line
-
hf download hf://Tinghong-Ye22/code-GMR/scripts/phmr_to_robot_centered.py
-
curl -L -o phmr_to_robot_centered.py https://huggingface.co/Tinghong-Ye22/code-GMR/resolve/main/scripts/phmr_to_robot_centered.py
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() | |