Download real/deploy/hrdt_inference.py from Tinghong-Ye22/code-Psi0-yth-v21: direct link, hf CLI and curl.
- Browser
- Download file 12.8 kB
-
https://huggingface.co/Tinghong-Ye22/code-Psi0-yth-v21/resolve/main/real/deploy/hrdt_inference.py
- Command line
-
hf download hf://Tinghong-Ye22/code-Psi0-yth-v21/real/deploy/hrdt_inference.py
-
curl -L -o hrdt_inference.py https://huggingface.co/Tinghong-Ye22/code-Psi0-yth-v21/resolve/main/real/deploy/hrdt_inference.py
12.8 kB
| import argparse | |
| import threading | |
| import time | |
| from datetime import datetime | |
| import cv2 | |
| import numpy as np | |
| import requests | |
| import json_numpy | |
| import zmq | |
| from multiprocessing import Array, Event | |
| from teleop.master_whole_body import RobotTaskmaster | |
| from teleop.robot_control.compute_tau import GetTauer | |
| json_numpy.patch() | |
| class RSCamera: | |
| def __init__(self, endpoint: str): | |
| self.context = zmq.Context() | |
| self.socket = self.context.socket(zmq.REQ) | |
| self.socket.connect(endpoint) | |
| def get_frame(self) -> np.ndarray: | |
| self.socket.send(b"get_frame") | |
| rgb_bytes, _, _ = self.socket.recv_multipart() | |
| rgb_array = np.frombuffer(rgb_bytes, np.uint8) | |
| return cv2.imdecode(rgb_array, cv2.IMREAD_COLOR) | |
| def get_observation_with_gt(data_dir: str, idx: int) -> tuple[dict, dict]: | |
| episode_dir = f"{data_dir}/episode_{idx:02d}" | |
| img_name = f"{episode_dir}/color/frame_{idx:06d}.jpg" | |
| frame = cv2.imread(img_name, cv2.IMREAD_COLOR) | |
| if frame is None: | |
| raise FileNotFoundError(f"Image not found: {img_name}") | |
| frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) | |
| img = frame.astype(np.uint8) | |
| state_vec = np.zeros(32, dtype=np.float32) | |
| return {"egocentric": img}, {"state": state_vec} | |
| def get_observation(camera: RSCamera, state: dict) -> tuple[dict, dict]: | |
| frame = camera.get_frame() | |
| frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) | |
| img = frame.astype(np.uint8) | |
| img_obs = {"egocentric": img} | |
| state_vec = np.concatenate( | |
| [ | |
| state["left_hand"], | |
| state["right_hand"], | |
| state["left_arm"], | |
| state["right_arm"], | |
| state["rpy"], | |
| state["height"], | |
| ] | |
| ).astype(np.float32) | |
| return img_obs, {"state": state_vec} | |
| def build_request( | |
| image_obs: dict, | |
| state_obs: dict, | |
| instruction: str, | |
| ) -> dict: | |
| return { | |
| "image": image_obs, | |
| "instruction": instruction, | |
| "history": {}, | |
| "state": state_obs, | |
| "condition": {}, | |
| "gt_action": None, | |
| "dataset_name": "lerobot", | |
| "timestamp": datetime.now().isoformat(), | |
| } | |
| def main() -> None: | |
| parser = argparse.ArgumentParser(description="HRDT real-world whole-body client") | |
| parser.add_argument("--host", default="127.0.0.1") | |
| parser.add_argument("--port", type=int, default=8014) | |
| parser.add_argument("--camera-endpoint", default="tcp://192.168.123.164:5556") | |
| parser.add_argument("--instruction", default="Rotate to pour ham into plate and push the cart forward.") | |
| parser.add_argument("--freq-vla", type=int, default=30) | |
| parser.add_argument("--freq-ctrl", type=int, default=60) | |
| parser.add_argument("--max-steps", type=int, default=500) | |
| parser.add_argument("--use-gt", action="store_true", help="Use dataset observations instead of live camera.") | |
| parser.add_argument("--gt-dir", default=None, help="Path to a dataset folder with color/frame_*.jpg.") | |
| parser.add_argument("--gt-index", type=int, default=0) | |
| args = parser.parse_args() | |
| url = f"http://{args.host}:{args.port}/act" | |
| freq_vla = args.freq_vla | |
| freq_ctrl = args.freq_ctrl | |
| action_repeat = max(1, int(round(freq_ctrl / freq_vla))) | |
| shared_data = { | |
| "kill_event": Event(), | |
| "session_start_event": Event(), | |
| "failure_event": Event(), | |
| "end_event": Event(), | |
| "dirname": None, | |
| } | |
| kill_event = shared_data["kill_event"] | |
| robot_shm_array = Array("d", 512, lock=False) | |
| teleop_shm_array = Array("d", 64, lock=False) | |
| master = RobotTaskmaster( | |
| task_name="inference", | |
| shared_data=shared_data, | |
| robot_shm_array=robot_shm_array, | |
| teleop_shm_array=teleop_shm_array, | |
| robot="g1", | |
| ) | |
| get_tauer = GetTauer() | |
| camera = None if args.use_gt else RSCamera(args.camera_endpoint) | |
| pred_action_buffer = {"actions": None, "idx": 0} | |
| pred_action_lock = threading.Lock() | |
| state_lock = threading.Lock() | |
| shared_robot_state = {"motor": None, "hand": None} | |
| running = Event() | |
| running.set() | |
| sequence_done_event = Event() | |
| sequence_done_event.set() | |
| def action_request_thread() -> None: | |
| session = requests.Session() | |
| for step in range(args.max_steps): | |
| if not running.is_set(): | |
| break | |
| sequence_done_event.wait() | |
| try: | |
| with state_lock: | |
| motor = shared_robot_state["motor"].copy() if shared_robot_state["motor"] is not None else None | |
| hand = shared_robot_state["hand"].copy() if shared_robot_state["hand"] is not None else None | |
| if motor is None or hand is None: | |
| print("[HRDT] Waiting for robot state...") | |
| time.sleep(0.01) | |
| continue | |
| arm_joints = motor[15:29].astype(np.float32) | |
| hand_joints = np.array(hand, dtype=np.float32) | |
| state = { | |
| "left_hand": hand_joints[:7], | |
| "right_hand": hand_joints[7:14], | |
| "left_arm": arm_joints[:7], | |
| "right_arm": arm_joints[7:14], | |
| "rpy": np.array( | |
| [master.torso_roll, master.torso_pitch, master.torso_yaw], | |
| dtype=np.float32, | |
| ), | |
| "height": np.array([master.torso_height], dtype=np.float32), | |
| } | |
| if args.use_gt: | |
| if args.gt_dir is None: | |
| raise ValueError("--gt-dir is required when --use-gt is set.") | |
| img_obs, state_obs = get_observation_with_gt(args.gt_dir, args.gt_index) | |
| else: | |
| img_obs, state_obs = get_observation(camera, state) | |
| request = build_request(img_obs, state_obs, args.instruction) | |
| resp = session.post(url, json=request, timeout=120.0) | |
| resp.raise_for_status() | |
| payload = resp.json() | |
| actions = np.asarray(payload["action"], dtype=np.float32) | |
| if actions.ndim != 2 or actions.shape[1] != 36: | |
| print(f"[HRDT] Invalid action shape: {actions.shape}") | |
| continue | |
| with pred_action_lock: | |
| pred_action_buffer["actions"] = actions | |
| pred_action_buffer["idx"] = 0 | |
| print(f"[HRDT] Got sequence of {len(actions)} actions.") | |
| sequence_done_event.clear() | |
| except Exception as exc: | |
| print(f"[HRDT] step {step} failed: {exc}") | |
| time.sleep(1.0 / freq_vla) | |
| print("[HRDT] Finished or stopped. Signaling kill_event.") | |
| kill_event.set() | |
| def apply_action_from_buffer(last_pd_target): | |
| current_lr_arm_q, current_lr_arm_dq = master.get_robot_data() | |
| with state_lock: | |
| shared_robot_state["motor"] = master.motorstate.copy() | |
| shared_robot_state["hand"] = master.handstate.copy() | |
| with pred_action_lock: | |
| actions = pred_action_buffer["actions"] | |
| idx = pred_action_buffer["idx"] | |
| action = None | |
| have_action = False | |
| if actions is not None: | |
| real_idx = idx // action_repeat | |
| if real_idx < len(actions): | |
| action = actions[real_idx] | |
| have_action = True | |
| pred_action_buffer["idx"] += 1 | |
| next_real_idx = pred_action_buffer["idx"] // action_repeat | |
| if next_real_idx >= len(actions): | |
| pred_action_buffer["actions"] = None | |
| pred_action_buffer["idx"] = 0 | |
| sequence_done_event.set() | |
| else: | |
| pred_action_buffer["actions"] = None | |
| pred_action_buffer["idx"] = 0 | |
| sequence_done_event.set() | |
| arm_cmd = None | |
| hand_cmd = None | |
| if have_action: | |
| if action.shape[0] < 36: | |
| print("[CTRL] Invalid action shape:", action.shape) | |
| else: | |
| vx = action[32] | |
| vy = action[33] | |
| vyaw = action[34] | |
| target_yaw = action[35] | |
| vx = 0.35 if vx > 0.25 else 0 | |
| vy = 0 if abs(vy) < 0.3 else 0.5 * (1 if vy > 0 else -1) | |
| rpyh = action[28:32] | |
| arm_cmd = action[14:28] | |
| hand_cmd = action[0:14] | |
| master.torso_roll = rpyh[0] | |
| master.torso_pitch = rpyh[1] | |
| master.torso_yaw = rpyh[2] | |
| master.torso_height = rpyh[3] | |
| print("height rpy:", master.torso_height, master.torso_roll, master.torso_pitch, master.torso_yaw) | |
| master.vx = vx | |
| master.vy = vy | |
| master.vyaw = vyaw | |
| master.target_yaw = target_yaw | |
| master.prev_torso_roll = master.torso_roll | |
| master.prev_torso_pitch = master.torso_pitch | |
| master.prev_torso_yaw = master.torso_yaw | |
| master.prev_torso_height = master.torso_height | |
| master.prev_vx = master.vx | |
| master.prev_vy = master.vy | |
| master.prev_vyaw = master.vyaw | |
| master.prev_target_yaw = master.target_yaw | |
| master.prev_arm = arm_cmd | |
| master.prev_hand = hand_cmd | |
| if not have_action: | |
| master.torso_roll = master.prev_torso_roll | |
| master.torso_pitch = master.prev_torso_pitch | |
| master.torso_yaw = master.prev_torso_yaw | |
| master.torso_height = master.prev_torso_height | |
| print("height rpy:", master.torso_height, master.torso_roll, master.torso_pitch, master.torso_yaw) | |
| arm_cmd = master.prev_arm | |
| hand_cmd = master.prev_hand | |
| master.vx = master.prev_vx | |
| # master.vx = 0 | |
| master.vy = 0 | |
| master.vyaw = master.prev_vyaw | |
| master.target_yaw = master.prev_target_yaw | |
| master.get_ik_observation(record=False) | |
| pd_target, pd_tauff, raw_action = master.body_ik.solve_whole_body_ik( | |
| left_wrist=None, | |
| right_wrist=None, | |
| current_lr_arm_q=current_lr_arm_q, | |
| current_lr_arm_dq=current_lr_arm_dq, | |
| observation=master.observation, | |
| extra_hist=master.extra_hist, | |
| is_teleop=False, | |
| ) | |
| master.last_action = np.concatenate( | |
| [raw_action.copy(), (master.motorstate - master.default_dof_pos)[15:] / master.action_scale] | |
| ) | |
| if arm_cmd is not None: | |
| pd_target[15:] = arm_cmd | |
| tau_arm = np.asarray(get_tauer(arm_cmd), dtype=np.float64).reshape(-1) | |
| pd_tauff[15:] = tau_arm | |
| if hand_cmd is not None: | |
| with master.dual_hand_data_lock: | |
| master.hand_shm_array[:] = hand_cmd | |
| master.body_ctrl.ctrl_whole_body( | |
| pd_target[15:], pd_tauff[15:], pd_target[:15], pd_tauff[:15] | |
| ) | |
| return pd_target | |
| def control_loop_thread() -> None: | |
| dt = 1.0 / freq_ctrl | |
| last_pd_target = None | |
| while running.is_set() and not kill_event.is_set(): | |
| try: | |
| last_pd_target = apply_action_from_buffer(last_pd_target) | |
| except Exception as exc: | |
| print("[CTRL] loop error:", exc) | |
| time.sleep(dt) | |
| print("[CTRL] Control loop stopped.") | |
| try: | |
| stabilize_thread = threading.Thread(target=master.maintain_standing, daemon=True) | |
| stabilize_thread.start() | |
| master.episode_kill_event.set() | |
| print("[MAIN] Initialize with standing pose...") | |
| time.sleep(30) | |
| master.episode_kill_event.clear() | |
| master.reset_yaw_offset = True | |
| t_req = threading.Thread(target=action_request_thread, daemon=True) | |
| t_ctrl = threading.Thread(target=control_loop_thread, daemon=True) | |
| t_req.start() | |
| t_ctrl.start() | |
| print("[MAIN] Running. Ctrl+C to stop.") | |
| while not kill_event.is_set(): | |
| time.sleep(0.5) | |
| print("[MAIN] kill_event set, preparing to stop...") | |
| running.clear() | |
| time.sleep(0.5) | |
| master.episode_kill_event.set() | |
| print("[MAIN] Returning to standing pose for 5s...") | |
| time.sleep(5) | |
| master.episode_kill_event.clear() | |
| except KeyboardInterrupt: | |
| print("[MAIN] Caught Ctrl+C, exiting...") | |
| running.clear() | |
| kill_event.set() | |
| finally: | |
| shared_data["end_event"].set() | |
| master.stop() | |
| print("[MAIN] Shutdown complete.") | |
| if __name__ == "__main__": | |
| main() | |