2026-10-09 16:41:13 +08:00

356 lines
14 KiB
Python

import argparse
import os
import cv2
from client_server.tcp.model_client import ModelClient
import numpy as np
from XPolicyLab.utils.process_data import get_robot_action_dim_info
Batch_Size = 10
class TestEnv:
def __init__(self, deploy_cfg):
self.success_num, self.episode_num = 0, 0
self._stop_check = None
self.deploy_cfg = deploy_cfg
self.episode_step_limit = 20
self.obs_encoded = deploy_cfg.get('obs_encoded', False)
env_cfg_type = deploy_cfg['env_cfg_type']
self.robot_action_dim_info = get_robot_action_dim_info(env_cfg_type)
if deploy_cfg.get("protocol", "ws") == "ws":
from client_server.ws import WsModelClient
policy_server_url = deploy_cfg["policy_server_url"]
if policy_server_url is None:
policy_server_url = f"ws://{deploy_cfg['host']}:{deploy_cfg['port']}"
self.model_client = WsModelClient(
url=policy_server_url,
evaluation_id=deploy_cfg["evaluation_id"],
trial_id=deploy_cfg["trial_id"],
action_case_id=deploy_cfg["action_case_id"],
repeat_index=deploy_cfg["repeat_index"],
ws_ping_interval_s=deploy_cfg.get("ws_ping_interval_s", 20.0),
ws_ping_timeout_s=deploy_cfg.get("ws_ping_timeout_s", 20.0),
# Omitted keys keep the client defaults; a slow-loading policy
# server mainly needs max_connect_attempts / request_timeout_s.
connect_timeout_s=deploy_cfg.get("connect_timeout_s"),
handshake_timeout_s=deploy_cfg.get("handshake_timeout_s"),
request_timeout_s=deploy_cfg.get("request_timeout_s"),
max_connect_attempts=deploy_cfg.get("max_connect_attempts"),
connect_retry_delay_s=deploy_cfg.get("connect_retry_delay_s"),
max_connect_seconds=deploy_cfg.get("max_connect_seconds"),
close_timeout_s=deploy_cfg.get("close_timeout_s"),
)
else:
self.model_client = ModelClient(host=deploy_cfg['host'], port=deploy_cfg['port'])
def set_stop_check(self, stop_check):
self._stop_check = stop_check
def get_obs(self, env_idx=0):
# v1.0
demo_obs = { # aloha
"vision": {
"cam_head": {
"color": np.zeros((480, 640, 3), dtype=np.uint8),
"depth": np.zeros((480, 640, 3), dtype=np.uint8),
"intrinsic_matrix": [
[615.0, 0.0, 320.0],
[0.0, 615.0, 240.0],
[0.0, 0.0, 1.0],
],
"extrinsics_matrix": [
[1.0, 0.0, 0.0, 0.10],
[0.0, 1.0, 0.0, 1.20],
[0.0, 0.0, 1.0, 1.50],
[0.0, 0.0, 0.0, 1.0],
],
"shape": (480, 640),
},
"cam_left_wrist": {
"color": np.zeros((480, 640, 3), dtype=np.uint8),
"depth": np.zeros((480, 640, 3), dtype=np.uint8),
"intrinsic_matrix": [
[615.0, 0.0, 320.0],
[0.0, 615.0, 240.0],
[0.0, 0.0, 1.0],
],
"extrinsics_matrix": [
[1.0, 0.0, 0.0, 0.10],
[0.0, 1.0, 0.0, 1.20],
[0.0, 0.0, 1.0, 1.50],
[0.0, 0.0, 0.0, 1.0],
],
"shape": (480, 640),
},
"cam_right_wrist": {
"color": np.zeros((480, 640, 3), dtype=np.uint8),
"depth": np.zeros((480, 640, 3), dtype=np.uint8),
"intrinsic_matrix": [
[615.0, 0.0, 320.0],
[0.0, 615.0, 240.0],
[0.0, 0.0, 1.0],
],
"extrinsics_matrix": [
[1.0, 0.0, 0.0, 0.10],
[0.0, 1.0, 0.0, 1.20],
[0.0, 0.0, 1.0, 1.50],
[0.0, 0.0, 0.0, 1.0],
],
"shape": (480, 640),
},
},
"instruction": "language instruction",
"state": {
"mobile": {
"base_pose": [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], # x,y,z + quat
"base_twist": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], # vx,vy,vz, wx,wy,wz
},
},
"additional_info": {
"frequency": 30, # Hz
},
"data_format_version": "v1.0",
"env_idx": env_idx
}
state = demo_obs.setdefault("state", {})
arm_dims = self.robot_action_dim_info["arm_dim"]
ee_dims = self.robot_action_dim_info["ee_dim"]
if len(arm_dims) == 1:
prefixes = [""]
elif len(arm_dims) == 2:
prefixes = ["left_", "right_"]
else:
raise ValueError(f"Unsupported arm count: {len(arm_dims)}")
for i, prefix in enumerate(prefixes):
state[f"{prefix}arm_joint_state"] = np.zeros(arm_dims[i], dtype=np.float32)
state[f"{prefix}ee_joint_state"] = np.zeros(ee_dims[i], dtype=np.float32)
state[f"{prefix}ee_pose"] = np.ones(7, dtype=np.float32)
state[f"{prefix}tcp_pose"] = np.zeros(7, dtype=np.float32)
state[f"{prefix}delta_ee_pose"] = np.zeros(7, dtype=np.float32)
if self.obs_encoded:
self._encode_obs_colors(demo_obs)
return demo_obs
@staticmethod
def _encode_obs_colors(obs):
"""Send encoded colors so the server-side decode path gets exercised."""
vision = obs["vision"]
buffer = cv2.imencode(".jpg", vision["cam_head"]["color"])[1].reshape(-1)
vision["cam_head"]["color"] = buffer # 1-D uint8 buffer
vision["cam_left_wrist"]["color"] = buffer.tobytes() # raw bytes
# cam_right_wrist stays a plain array, covering the passthrough branch.
def get_obs_batch(self, env_idx_list):
demo_obs_list = [self.get_obs(env_idx) for env_idx in env_idx_list]
return demo_obs_list
def eval_one_episode(self):
policy_name = self.deploy_cfg['policy_name']
try:
eval_module = __import__(f'XPolicyLab.policy.{policy_name}.deploy', fromlist=['eval_one_episode'])
except ImportError as e:
print("[TestEnv]", f"Failed to import policy module: XPolicyLab.policy.{policy_name}.deploy. Error: {e}", "ERROR")
raise e
if not hasattr(eval_module, 'eval_one_episode'):
print("[TestEnv]", f"Module '.{policy_name}.deploy' does not have 'eval_one_episode' function", "ERROR")
raise AttributeError(f"Missing eval_one_episode in policy module")
eval_module.eval_one_episode(TASK_ENV=self, model_client=self.model_client)
def eval_one_episode_batch(self):
policy_name = self.deploy_cfg['policy_name']
try:
eval_module = __import__(f'XPolicyLab.policy.{policy_name}.deploy', fromlist=['eval_one_episode_batch'])
except ImportError as e:
print("[TestEnv]", f"Failed to import policy module: XPolicyLab.policy.{policy_name}.deploy. Error: {e}", "ERROR")
raise e
if not hasattr(eval_module, 'eval_one_episode_batch'):
print("[TestEnv]", f"Module '.{policy_name}.deploy' does not have 'eval_one_episode_batch' function", "ERROR")
raise AttributeError(f"Missing eval_one_episode_batch in policy module")
eval_module.eval_one_episode_batch(TASK_ENV=self, model_client=self.model_client)
def reset(self):
self.model_client.call(func_name="reset")
self.episode_step = 0
def take_action(self, action):
print(f"[TestEnv] Action Step: {self.episode_step} / {self.episode_step_limit} (step_limit)")
self.episode_step += 1
validate_robot_state_dict(action, self.robot_action_dim_info)
def take_action_batch(self, action_list, env_idx_list):
print(f"[TestEnv] Action Step: {self.episode_step} / {self.episode_step_limit} (step_limit)")
self.episode_step += 1
assert len(action_list) == len(env_idx_list), f"action num != env num: {len(action_list)} != {len(env_idx_list)}"
for action in action_list:
validate_robot_state_dict(action, self.robot_action_dim_info)
def is_episode_end(self):
if self._stop_check is not None and self._stop_check():
print("[TestEnv] Check Episode End: stop requested")
return True
print("[TestEnv] Check Episode End:", self.episode_step >= self.episode_step_limit)
return self.episode_step >= self.episode_step_limit
def finish_episode(self):
print("[TestEnv] Episode finished")
def get_running_env_idx_list(self):
# For demonstration, we assume all envs are running. Replace with actual logic if needed.
return list(range(Batch_Size))
def validate_robot_state_dict(state_dict: dict, robot_action_dim_info: dict) -> None:
"""
Validate whether the state_dict keys use the correct prefixes and dimensions.
Args:
state_dict: e.g. demo_obs["state"]
robot_action_dim_info: {
"arm_dim": [...],
"ee_dim": [...],
}
Raises:
KeyError: if required keys are missing
ValueError: if unexpected keys or wrong dimensions are found
TypeError: if values are not array-like
"""
arm_dims = robot_action_dim_info["arm_dim"]
ee_dims = robot_action_dim_info["ee_dim"]
if len(arm_dims) != len(ee_dims):
raise ValueError(
f"robot_action_dim_info mismatch: len(arm_dim)={len(arm_dims)} "
f"!= len(ee_dim)={len(ee_dims)}"
)
arm_count = len(arm_dims)
if arm_count == 1:
expected = {
"arm_joint_state": arm_dims[0],
"ee_joint_state": ee_dims[0],
"ee_pose": 7,
"tcp_pose": 7,
"delta_ee_pose": 7,
}
forbidden_prefixes = ("left_", "right_")
elif arm_count == 2:
expected = {
"left_arm_joint_state": arm_dims[0],
"left_ee_joint_state": ee_dims[0],
"left_ee_pose": 7,
"left_tcp_pose": 7,
"left_delta_ee_pose": 7,
"right_arm_joint_state": arm_dims[1],
"right_ee_joint_state": ee_dims[1],
"right_ee_pose": 7,
"right_tcp_pose": 7,
"right_delta_ee_pose": 7,
}
forbidden_prefixes = ()
else:
raise ValueError(f"Unsupported arm count: {arm_count}")
if forbidden_prefixes:
bad_prefixed_keys = [
k for k in state_dict.keys()
if k.startswith(forbidden_prefixes)
]
if bad_prefixed_keys:
raise ValueError(
f"Single-arm robot should not contain prefixed keys, "
f"but got: {bad_prefixed_keys}"
)
unexpected_keys = [k for k in state_dict if k not in expected]
if unexpected_keys:
raise ValueError(f"Unexpected state keys: {unexpected_keys}")
for key, expected_dim in expected.items():
if not key in state_dict.keys():
continue
value = state_dict[key]
if not isinstance(value, (np.ndarray, list, tuple)):
raise TypeError(
f"state_dict['{key}'] must be array-like, got {type(value)}"
)
arr = np.asarray(value)
if arr.ndim != 1:
raise ValueError(
f"state_dict['{key}'] must be 1D, got shape {arr.shape}"
)
if arr.shape[0] != expected_dim:
raise ValueError(
f"state_dict['{key}'] dim mismatch: expected {expected_dim}, "
f"got shape {arr.shape}"
)
def str2bool(v):
if isinstance(v, bool):
return v
if v.lower() in ("yes", "true", "t", "1"):
return True
if v.lower() in ("no", "false", "f", "0"):
return False
raise argparse.ArgumentTypeError("Boolean value expected.")
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--bench_name", required=True, type=str)
parser.add_argument("--task_name", required=True, type=str)
parser.add_argument("--env_cfg_type", type=str, required=True)
parser.add_argument("--policy_name", type=str, required=True, help="XPolicyLab module name for deployment")
parser.add_argument("--protocol", choices=("legacy_tcp", "ws"), default="ws")
parser.add_argument("--host", type=str, default="localhost", help="server host")
parser.add_argument("--port", type=int, required=True, help="server port")
parser.add_argument("--policy_server_url", type=str)
parser.add_argument("--evaluation_id", type=str, default="debug-eval")
parser.add_argument("--action_case_id", type=str)
parser.add_argument("--trial_id", type=str, default="debug-trial")
parser.add_argument("--repeat_index", type=int)
parser.add_argument("--eval_episode_num", type=int, default=10, help="number of evaluation episodes")
parser.add_argument("--eval_batch", type=str2bool, default=False, help="whether to run batch evaluation")
parser.add_argument("--obs_encoded", type=str2bool, default=os.environ.get("DEBUG_OBS_ENCODED", "0"),
help="send encoded camera colors to exercise the server-side decode path")
args_cli = parser.parse_args()
deploy_cfg = vars(args_cli)
test_env = TestEnv(deploy_cfg)
eval_batch = deploy_cfg['eval_batch']
try:
# Load XPolicyLab
for idx in range(deploy_cfg["eval_episode_num"]):
print(f"\033[94m🚀 Running Episode {idx}\033[0m")
test_env.reset() # reset model, robot, and environment
if not eval_batch:
test_env.eval_one_episode()
else:
test_env.eval_one_episode_batch()
test_env.finish_episode()
finally:
close = getattr(test_env.model_client, "close", None)
if callable(close):
close()