code-Psi0-yth-v21 / real /deploy /psi-inference_rtc.py
Tinghong-Ye22's picture
Add files using upload-large-folder tool
211ae03 verified
Raw History Blame Contribute Delete
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)