code-GMR / scripts /phmr_to_robot.py
Tinghong-Ye22's picture
Add files using upload-large-folder tool
12ea01b verified
Raw History Blame Contribute Delete
5.49 kB
import argparse
import pathlib
import os
import time
import numpy as np
from general_motion_retargeting import GeneralMotionRetargeting as GMR
from general_motion_retargeting import RobotMotionViewer
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
if __name__ == "__main__":
HERE = pathlib.Path(__file__).parent
parser = argparse.ArgumentParser()
parser.add_argument(
"--phmr_pred_file",
help="SMPLX motion file to load.",
type=str,
# required=True,
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.",
)
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)
# 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=f"videos/{args.robot}_{args.phmr_pred_file.split('/')[-1].split('.')[0]}.mp4",)
curr_frame = 0
# FPS measurement variables
fps_counter = 0
fps_start_time = time.time()
fps_display_interval = 2.0 # Display FPS every 2 seconds
if args.save_path is not None:
save_dir = os.path.dirname(args.save_path)
if 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:
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 not args.headless:
robot_motion_viewer.close()