Download scripts/xsens_live_streaming.py from Tinghong-Ye22/code-GMR: direct link, hf CLI and curl.
- Browser
- Download file 6.52 kB
-
https://huggingface.co/Tinghong-Ye22/code-GMR/resolve/main/scripts/xsens_live_streaming.py
- Command line
-
hf download hf://Tinghong-Ye22/code-GMR/scripts/xsens_live_streaming.py
-
curl -L -o xsens_live_streaming.py https://huggingface.co/Tinghong-Ye22/code-GMR/resolve/main/scripts/xsens_live_streaming.py
6.52 kB
| #!/usr/bin/env python3 | |
| """ | |
| Xsens MVN Live Streaming to Unitree G1 Retargeting. | |
| Real-time motion capture retargeting following bvh_xsens_to_robot.py format. | |
| """ | |
| import argparse | |
| import os | |
| import signal | |
| import sys | |
| import time | |
| import numpy as np | |
| from rich import print | |
| from general_motion_retargeting.utils.xsens_vendor.xsens_to_gmr_adapter import XsensToGMR | |
| from general_motion_retargeting import GeneralMotionRetargeting as GMR | |
| from general_motion_retargeting import RobotMotionViewer | |
| # Global flag for graceful shutdown | |
| g_running = True | |
| def signal_handler(signum, frame): | |
| global g_running | |
| print(f"\nReceived signal {signum}, shutting down...") | |
| g_running = False | |
| if __name__ == "__main__": | |
| signal.signal(signal.SIGINT, signal_handler) | |
| signal.signal(signal.SIGTERM, signal_handler) | |
| parser = argparse.ArgumentParser( | |
| description="Xsens MVN Live Streaming to Robot Retargeting", | |
| formatter_class=argparse.ArgumentDefaultsHelpFormatter, | |
| ) | |
| parser.add_argument( | |
| "--port", | |
| type=int, | |
| default=9763, | |
| help="UDP port for Xsens MVN streaming", | |
| ) | |
| parser.add_argument( | |
| "--robot", | |
| choices=["unitree_g1"], | |
| default="unitree_g1", | |
| ) | |
| parser.add_argument( | |
| "--human_height", | |
| type=float, | |
| default=None, | |
| help="Actual height of human in meters (optional, for better scaling)", | |
| ) | |
| parser.add_argument( | |
| "--record_video", | |
| action="store_true", | |
| default=False, | |
| ) | |
| parser.add_argument( | |
| "--video_path", | |
| type=str, | |
| default="videos/xsens_live.mp4", | |
| ) | |
| parser.add_argument( | |
| "--rate_limit", | |
| action="store_true", | |
| default=True, | |
| ) | |
| parser.add_argument( | |
| "--save_dir", | |
| default=None, | |
| help="Directory to save the robot motion CSV. If not set, no file is saved.", | |
| ) | |
| args = parser.parse_args() | |
| target_fps = 60 # Xsens MVN typical streaming rate | |
| if args.save_dir is not None: | |
| os.makedirs(args.save_dir, exist_ok=True) | |
| qpos_list = [] | |
| # ---- Initialize Xsens adapter ---- | |
| print("[1/3] Initializing Xsens adapter...") | |
| xsens = XsensToGMR(port=args.port, verbose=True) | |
| if not xsens.initialize(): | |
| print("Failed to initialize Xsens adapter") | |
| sys.exit(1) | |
| # ---- Initialize retargeter ---- | |
| print("[2/3] Initializing retargeter...") | |
| retargeter = GMR( | |
| src_human="xsens_mvn", | |
| tgt_robot=args.robot, | |
| actual_human_height=args.human_height, | |
| solver="daqp", | |
| damping=1.0, | |
| use_velocity_limit=True, | |
| ) | |
| # ---- Initialize viewer ---- | |
| print("[3/3] Initializing viewer...") | |
| robot_motion_viewer = RobotMotionViewer( | |
| robot_type=args.robot, | |
| motion_fps=target_fps, | |
| transparent_robot=0, | |
| record_video=args.record_video, | |
| video_path=args.video_path, | |
| ) | |
| # ---- Start streaming ---- | |
| xsens.start() | |
| time.sleep(1.0) # Wait for stream to stabilize | |
| print(f"mocap_frame_rate: {target_fps}") | |
| print("Starting live retargeting... Press Ctrl+C to stop\n") | |
| # FPS measurement | |
| fps_counter = 0 | |
| fps_start_time = time.time() | |
| fps_display_interval = 2.0 | |
| total_frames = 0 | |
| dropped_frames = 0 | |
| last_valid_qpos = None | |
| last_valid_human_frame = None | |
| try: | |
| while g_running: | |
| human_frame = xsens.get_human_frame() | |
| if human_frame is None: | |
| dropped_frames += 1 | |
| # Show last valid pose if available | |
| if last_valid_qpos is not None: | |
| robot_motion_viewer.step( | |
| root_pos=last_valid_qpos[:3], | |
| root_rot=last_valid_qpos[3:7], | |
| dof_pos=last_valid_qpos[7:], | |
| human_motion_data=last_valid_human_frame, | |
| rate_limit=args.rate_limit, | |
| ) | |
| time.sleep(0.001) | |
| continue | |
| total_frames += 1 | |
| # Retarget | |
| try: | |
| qpos = retargeter.retarget(human_frame, offset_to_ground=True) | |
| except Exception as e: | |
| print(f"Retargeting failed: {e}") | |
| dropped_frames += 1 | |
| continue | |
| last_valid_qpos = qpos.copy() | |
| last_valid_human_frame = retargeter.scaled_human_data | |
| # Visualize | |
| robot_motion_viewer.step( | |
| root_pos=qpos[:3], | |
| root_rot=qpos[3:7], | |
| dof_pos=qpos[7:], | |
| human_motion_data=retargeter.scaled_human_data, | |
| rate_limit=args.rate_limit, | |
| follow_camera=False, | |
| ) | |
| if args.save_dir is not None: | |
| qpos_list.append(qpos) | |
| # 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"FPS: {actual_fps:.1f} | " | |
| f"Retargeted: {total_frames} | " | |
| f"Dropped: {dropped_frames}" | |
| ) | |
| fps_counter = 0 | |
| fps_start_time = current_time | |
| except KeyboardInterrupt: | |
| print("\nInterrupted by user") | |
| finally: | |
| # Stop streaming | |
| xsens.stop() | |
| # Save trajectory | |
| if args.save_dir is not None and qpos_list: | |
| 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": target_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}") | |
| # Print final stats | |
| print(f"\nTotal retargeted: {total_frames}") | |
| print(f"Total dropped: {dropped_frames}") | |
| robot_motion_viewer.close() |