356 lines
14 KiB
Python
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()
|