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