Download real/deploy/psi-inference_rtc.py from Tinghong-Ye22/code-Psi0-yth-v21: direct link, hf CLI and curl.
- Browser
- Download file 14 kB
-
https://huggingface.co/Tinghong-Ye22/code-Psi0-yth-v21/resolve/main/real/deploy/psi-inference_rtc.py
- Command line
-
hf download hf://Tinghong-Ye22/code-Psi0-yth-v21/real/deploy/psi-inference_rtc.py
-
curl -L -o psi-inference_rtc.py https://huggingface.co/Tinghong-Ye22/code-Psi0-yth-v21/resolve/main/real/deploy/psi-inference_rtc.py
14 kB
| import os | |
| import time | |
| import threading | |
| import json | |
| import cv2 | |
| import numpy as np | |
| import requests | |
| import json_numpy | |
| from multiprocessing import Array, Event | |
| from teleop.master_whole_body import RobotTaskmaster | |
| from teleop.robot_control.compute_tau import GetTauer | |
| import zmq | |
| from websocket import WebSocketApp | |
| TASK_INSTRUCTION = "Spray the bowl and wipe it and stack it up." | |
| FREQ_CTRL = 60 | |
| OBS_SEND_INTERVAL = 0.01 | |
| json_numpy.patch() | |
| from base64 import b64encode, b64decode | |
| from numpy.lib.format import dtype_to_descr, descr_to_dtype | |
| class RSCamera: | |
| def __init__(self): | |
| self.context = zmq.Context() | |
| self.socket = self.context.socket(zmq.REQ) | |
| self.socket.connect("tcp://192.168.123.164:5556") | |
| def get_frame(self): | |
| self.socket.send(b"get_frame") | |
| rgb_bytes, _, _ = self.socket.recv_multipart() | |
| rgb_array = np.frombuffer(rgb_bytes, np.uint8) | |
| rgb_image = cv2.imdecode(rgb_array, cv2.IMREAD_COLOR) | |
| return rgb_image | |
| def get_observation(camera, state): | |
| frame = camera.get_frame() | |
| # frame = cv2.resize(frame, (224, 224), interpolation=cv2.INTER_AREA) | |
| frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) | |
| img = frame.astype(np.uint8) | |
| img_obs = { | |
| "observation.images.egocentric": frame, | |
| } | |
| state_obs = { | |
| "arm_joints": np.array(state["arm_joints"]), | |
| "hand_joints": np.array(state["hand_joints"]), | |
| } | |
| return img_obs, state_obs | |
| 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 = RSCamera() | |
| pred_action_buffer = {"actions": None} | |
| pred_action_lock = threading.Lock() | |
| # state_lock = threading.Lock() | |
| # shared_robot_state = { | |
| # "motor": None, | |
| # "hand": None, | |
| # } | |
| running = Event() | |
| running.set() | |
| # ============ Serialization utilities ============ | |
| def numpy_serialize(o): | |
| if isinstance(o, (np.ndarray, np.generic)): | |
| data = o.data if o.flags["C_CONTIGUOUS"] else o.tobytes() | |
| return { | |
| "__numpy__": b64encode(data).decode(), | |
| "dtype": dtype_to_descr(o.dtype), | |
| "shape": o.shape, | |
| } | |
| raise TypeError(f"Object of type {o.__class__.__name__} is not JSON serializable") | |
| def numpy_deserialize(dct): | |
| if "__numpy__" in dct: | |
| np_obj = np.frombuffer(b64decode(dct["__numpy__"]), descr_to_dtype(dct["dtype"])) | |
| return np_obj.reshape(dct["shape"]) if dct["shape"] else np_obj[0] | |
| return dct | |
| def convert_numpy_in_dict(data, func): | |
| if isinstance(data, dict): | |
| if "__numpy__" in data: | |
| return func(data) | |
| return {key: convert_numpy_in_dict(value, func) for key, value in data.items()} | |
| elif isinstance(data, list): | |
| return [convert_numpy_in_dict(item, func) for item in data] | |
| elif isinstance(data, (np.ndarray, np.generic)): | |
| return func(data) | |
| else: | |
| return data | |
| # ============ RTCClient ============ | |
| class RTCWebSocketClient: | |
| def __init__(self, server_url: str): | |
| self.server_url = server_url | |
| self._running = True | |
| self._connected = threading.Event() | |
| self._ws = None | |
| self._send_lock = threading.Lock() | |
| self.start_time = time.time() | |
| def execute_action(self, action: np.ndarray): | |
| """ | |
| Execute the action on the robot. | |
| TODO: Replace with your actual robot control code. | |
| """ | |
| with pred_action_lock: | |
| pred_action_buffer["actions"] = action | |
| # print(f"[client] Executing action: shape={action.shape}, first_3={action.flatten()[:3]}") | |
| def _on_open(self, ws): | |
| """WebSocket connection opened""" | |
| print("[client] Connected!") | |
| self._connected.set() | |
| def _on_message(self, ws, message): | |
| """Receive message from server (runs in WebSocketApp thread)""" | |
| interval = time.time() - self.start_time | |
| self.start_time = time.time() | |
| print(f"[client] recv_action interval: {interval} seconds") | |
| try: | |
| data = json.loads(message) | |
| action_data = data.get("action") | |
| version = data.get("version", -1) | |
| if action_data is not None: | |
| action = convert_numpy_in_dict(action_data, numpy_deserialize) | |
| if isinstance(action, np.ndarray): | |
| self.execute_action(action) | |
| print(f"[client] Received action, version={version}") | |
| except Exception as e: | |
| print(f"[client] Message processing error: {e}") | |
| def _on_error(self, ws, error): | |
| """WebSocket error handler""" | |
| print(f"[client] WebSocket error: {error}") | |
| def _on_close(self, ws, close_status_code, close_msg): | |
| """WebSocket connection closed""" | |
| print(f"[client] Connection closed: {close_status_code} - {close_msg}") | |
| self._running = False | |
| running.clear() | |
| def _send_thread(self): | |
| """Send observations at high frequency""" | |
| print("[client] Send thread started") | |
| # Wait for connection | |
| self._connected.wait() | |
| prev_tick = time.perf_counter() | |
| while self._running and running.is_set(): | |
| start = time.time() | |
| try: | |
| # Get observation | |
| state = { | |
| "arm_joints": master.motorstate[15:29], | |
| "hand_joints": master.handstate, | |
| } | |
| img_obs, state_obs = get_observation(camera, state) | |
| payload = { | |
| "image": img_obs, | |
| "state": state_obs, | |
| "gt_action": None, | |
| "dataset_name": None, | |
| "instruction": TASK_INSTRUCTION, | |
| "history": None, | |
| "condition": None, | |
| "timestamp": None, | |
| } | |
| payload = convert_numpy_in_dict(payload, numpy_serialize) | |
| message = json.dumps(payload) | |
| # Send (thread-safe) | |
| with self._send_lock: | |
| if self._ws and self._ws.sock and self._ws.sock.connected: | |
| self._ws.send(message) | |
| else: | |
| print("[client] WebSocket not connected, skipping send") | |
| break | |
| except Exception as e: | |
| print(f"[client] Send error: {e}") | |
| break | |
| # # Sleep for the remaining time | |
| # elapsed = time.time() - start | |
| # sleep_time = max(0, OBS_SEND_INTERVAL - elapsed) | |
| # print("sleep time:", sleep_time) | |
| # time.sleep(sleep_time) | |
| now = time.perf_counter() | |
| interval = now - prev_tick | |
| prev_tick = now | |
| print(f"send thread interval: {interval} seconds") | |
| print("[client] Send thread stopped") | |
| def run(self): | |
| """Main client loop - runs WebSocketApp in current thread""" | |
| print(f"[client] Connecting to {self.server_url}") | |
| # Create WebSocketApp | |
| self._ws = WebSocketApp( | |
| self.server_url, | |
| on_open=self._on_open, | |
| on_message=self._on_message, | |
| on_error=self._on_error, | |
| on_close=self._on_close | |
| ) | |
| # Start send thread | |
| send_thread = threading.Thread(target=self._send_thread, daemon=True) | |
| send_thread.start() | |
| # Run WebSocketApp (blocks until connection closes) | |
| self._ws.run_forever() | |
| # Wait for send thread to finish | |
| self._running = False | |
| send_thread.join(timeout=2.0) | |
| print("[client] Client stopped") | |
| def stop(self): | |
| """Stop the client""" | |
| self._running = False | |
| if self._ws: | |
| self._ws.close() | |
| def main(server_url): | |
| master.reset_yaw_offset = True | |
| _last_target_yaw = None | |
| def apply_action_from_buffer(last_pd_target): | |
| current_lr_arm_q, current_lr_arm_dq = master.get_robot_data() | |
| have_vla = False | |
| with pred_action_lock: | |
| action = pred_action_buffer["actions"] | |
| if action is not None: | |
| have_vla = True | |
| action = action[0] | |
| arm_cmd = None | |
| hand_cmd = None | |
| if have_vla: | |
| if action.shape[0] < 36: | |
| print("[CTRL] Invalid action shape:", action.shape) | |
| else: | |
| vx = action[32] | |
| vy = action[33] | |
| vyaw = action[34] | |
| # dyaw = action[35] | |
| target_yaw = action[35] | |
| vx = 0.6 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[:14] | |
| master.torso_roll = rpyh[0] | |
| master.torso_pitch = rpyh[1] | |
| master.torso_yaw = rpyh[2] | |
| master.torso_height = rpyh[3] | |
| print("torso_roll, pitch, yaw, height:", master.torso_roll, master.torso_pitch, master.torso_yaw, master.torso_height) | |
| 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_vla: | |
| 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 | |
| master.vx = master.prev_vx | |
| 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(): | |
| 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 e: | |
| print("[CTRL] loop error:", e) | |
| time.sleep(dt) | |
| print("[CTRL] Control loop stopped.") | |
| def websocket_thread(): | |
| client = RTCWebSocketClient(server_url=server_url) | |
| client.run() | |
| print("[WS] WebSocket thread 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_ctrl = threading.Thread(target=control_loop_thread, daemon=True) | |
| t_ctrl.start() | |
| t_ws = threading.Thread(target=websocket_thread, daemon=True) | |
| t_ws.start() | |
| print("[MAIN] Running. Ctrl+C to stop.") | |
| while not kill_event.is_set() and running.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__": | |
| import argparse | |
| parser = argparse.ArgumentParser() | |
| parser.add_argument("--host", type=str, default="localhost") | |
| parser.add_argument("--port", type=int, default=8014) | |
| parser.add_argument("--task", type=str, default=TASK_INSTRUCTION) | |
| args = parser.parse_args() | |
| TASK_INSTRUCTION = args.task | |
| server_url = f"ws://{args.host}:{args.port}/ws" | |
| main(server_url) | |