From ad29dc862502ba4ab06cece95939df93fed8c446 Mon Sep 17 00:00:00 2001 From: yangheqing Date: Sat, 29 Aug 2026 02:49:13 +0800 Subject: [PATCH 1/2] feat: add MCP robot agent control --- .gitignore | 6 + pyproject.toml | 4 +- src/core/app/agent_control.py | 549 ++++++++++++++++++++++++++++++ src/core/app/control_channel.py | 42 ++- src/core/app/run.py | 10 + src/core/app/state.py | 5 + tests/app/test_agent_control.py | 203 +++++++++++ tests/app/test_control_channel.py | 23 ++ tests/tools/test_mcp.py | 73 ++++ tools/mcp/__init__.py | 1 + tools/mcp/server.py | 231 +++++++++++++ 11 files changed, 1145 insertions(+), 2 deletions(-) create mode 100644 src/core/app/agent_control.py create mode 100644 tests/app/test_agent_control.py create mode 100644 tests/tools/test_mcp.py create mode 100644 tools/mcp/__init__.py create mode 100644 tools/mcp/server.py diff --git a/.gitignore b/.gitignore index f197422..dbf78e0 100644 --- a/.gitignore +++ b/.gitignore @@ -33,6 +33,12 @@ tools/* !tools/__init__.py !tools/conversion/ !tools/conversion/** +!tools/mcp/ +!tools/mcp/** +tools/mcp/.venv/ +tools/mcp/__pycache__/ +tools/mcp/*.egg-info/ +tools/mcp/uv.lock !tools/skills/ !tools/skills/** tools/conversion/__pycache__/ diff --git a/pyproject.toml b/pyproject.toml index ddbe4e0..b2a2daa 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -14,6 +14,7 @@ dependencies = [ "jax==0.6.2", "jaxlib==0.6.2", "jaxls @ git+https://github.com/brentyi/jaxls.git@50a58be88c5ef74532f09e3f55268b4f02c490e3", + "mcp==2.0.0", "msgpack==1.1.2", "msgpack-numpy==0.4.8", "mcap==1.4.0", @@ -24,7 +25,7 @@ dependencies = [ "psutil==7.1.0", "pyarrow==23.0.0", "pycollada==0.9.3", - "pydantic==2.11.7", + "pydantic==2.13.5", "pyroki @ git+https://github.com/chungmin99/pyroki.git@388e43e1fc0d0ee382968d3dd72970fd62a0450c", "pyyaml==6.0.3", "pyzmq>=25.0.0", @@ -51,6 +52,7 @@ viz = [ [project.scripts] eva = "main:main" +eva-mcp = "tools.mcp.server:main" [tool.uv] environments = [ diff --git a/src/core/app/agent_control.py b/src/core/app/agent_control.py new file mode 100644 index 0000000..2349b07 --- /dev/null +++ b/src/core/app/agent_control.py @@ -0,0 +1,549 @@ +"""Agent-facing robot operations executed by the EVA main loop. + +The MCP server is a separate process. It submits typed operations through the +existing ZMQ control channel, while this module owns the small amount of runtime +logic that must stay in-process with the robot transport: FK/IK, joint-space +interpolation, publication, cancellation, and capability/state snapshots. +""" + +from __future__ import annotations + +import base64 +import dataclasses +import math +import uuid +from typing import Any + +import cv2 +import numpy as np + +from core.app.handlers.control import publish_action_chunk +from core.app.handlers.imaging import build_linear_trajectory +from core.app.handlers.io import ensure_ik_solver +from core.app.handlers.utils import reset_infer_strategy +from core.app.state import ( + OutputTarget, + RuntimeState, + SessionMode, + SessionState, + SessionStatus, + set_phase, + set_status, +) +from core.config import ConfigDict +from robots.base import Robot + +_DIRECT_ACTIONS = frozenset({"move_eef", "move_joints", "set_gripper", "solve_ik"}) +_ACTIVE_OPERATION_STATES = frozenset({"queued", "running", "cancel_requested"}) +_POSITION_TOLERANCE_M = 0.02 +_ORIENTATION_TOLERANCE_RAD = 0.2 +_DEFAULT_MAX_QPOS_STEP = 0.02 + + +@dataclasses.dataclass(frozen=True) +class AgentCommand: + """One validated command queued by the control channel for the main loop.""" + + operation_id: str + action: str + arguments: dict[str, Any] + + +class MotionCancelled(RuntimeError): + """The active agent motion was interrupted before its trajectory completed.""" + + +def _operation_snapshot(runtime: RuntimeState) -> dict[str, Any] | None: + with runtime.agent_operation_lock: + operation = runtime.agent_operation + return None if operation is None else dict(operation) + + +def _update_operation(runtime: RuntimeState, operation_id: str, **updates: Any) -> bool: + with runtime.agent_operation_lock: + operation = runtime.agent_operation + if operation is None or operation["operation_id"] != operation_id: + return False + runtime.agent_operation = {**operation, **updates} + return True + + +def queue_agent_command(runtime: RuntimeState, payload: object) -> dict[str, Any]: + """Validate and enqueue one direct robot operation without blocking ZMQ.""" + if not isinstance(payload, dict): + return {"ok": False, "error": "agent_command must be a JSON object"} + action = str(payload.get("action", "")).strip() + if action not in _DIRECT_ACTIONS: + return {"ok": False, "error": f"unknown agent action: {action!r}"} + arguments = payload.get("arguments", {}) + if not isinstance(arguments, dict): + return {"ok": False, "error": "agent command arguments must be a JSON object"} + + operation_id = uuid.uuid4().hex + with runtime.agent_operation_lock: + active = runtime.agent_operation + if active is not None and active["status"] in _ACTIVE_OPERATION_STATES: + return { + "ok": False, + "error": "another agent operation is active", + "operation": dict(active), + } + runtime.agent_operation = { + "operation_id": operation_id, + "action": action, + "status": "queued", + "result": None, + "error": "", + } + runtime.agent_command_queue.put(AgentCommand(operation_id, action, dict(arguments))) + return {"ok": True, "operation": _operation_snapshot(runtime)} + + +def request_agent_stop(runtime: RuntimeState) -> dict[str, Any]: + """Request immediate cancellation of direct motion and any policy rollout.""" + operation = _operation_snapshot(runtime) + if operation is not None and operation["status"] in _ACTIVE_OPERATION_STATES: + _update_operation(runtime, operation["operation_id"], status="cancel_requested") + if runtime.console_ctx is not None: + runtime.console_ctx.session.interrupt_requested = True + if runtime.command_queue is not None: + runtime.command_queue.put("web:halt") + return { + "ok": True, + "operation": _operation_snapshot(runtime), + "direct_control_capable": True, + "direct_control_available": ( + runtime.transport.get_latest_qpos() is not None and not runtime.transport.is_offline() + ), + } + + +def serialize_agent_operation(runtime: RuntimeState, operation_id: str | None = None) -> dict: + """Return the latest direct-control operation and reject stale ids explicitly.""" + operation = _operation_snapshot(runtime) + if operation_id and (operation is None or operation["operation_id"] != operation_id): + return {"ok": False, "error": f"unknown operation_id: {operation_id}"} + return {"ok": True, "operation": operation} + + +def _active_config(runtime: RuntimeState) -> ConfigDict: + if runtime.active_config is not None: + return runtime.active_config + if runtime.console_ctx is None: + raise RuntimeError("EVA runtime context is not ready") + return runtime.console_ctx.config + + +def _latest_qpos(runtime: RuntimeState) -> np.ndarray: + qpos = runtime.transport.get_latest_qpos() + if qpos is None: + raise RuntimeError("robot joint feedback is unavailable") + vector = np.asarray(qpos, dtype=np.float32).reshape(-1) + expected = runtime.robot.total_action_dim + if vector.shape != (expected,): + raise RuntimeError(f"robot qpos has shape {vector.shape}; expected ({expected},)") + if not np.all(np.isfinite(vector)): + raise RuntimeError("robot qpos contains non-finite values") + return vector + + +def _group_slice(runtime: RuntimeState, group_name: str) -> tuple[slice, Any]: + offset = 0 + for group in runtime.robot.actuator_groups: + group_slice = slice(offset, offset + group.dof) + if group.name == group_name: + return group_slice, group + offset += group.dof + valid = [group.name for group in runtime.robot.actuator_groups] + raise ValueError(f"unknown actuator group {group_name!r}; expected one of {valid}") + + +def _arm_eef_slice(runtime: RuntimeState, group_name: str) -> slice: + names = [group.name for group in runtime.robot.arm_groups] + if group_name not in names: + raise ValueError(f"group {group_name!r} has no end effector; expected one of {names}") + index = names.index(group_name) + return slice(index * 8, (index + 1) * 8) + + +def _vector_norm(vector: np.ndarray) -> float: + return math.sqrt(float(np.dot(vector, vector))) + + +def _quaternion_error(left: np.ndarray, right: np.ndarray) -> float: + left_norm = _vector_norm(left) + right_norm = _vector_norm(right) + if left_norm < 1e-8 or right_norm < 1e-8: + return math.inf + left = left / left_norm + right = right / right_norm + return 2.0 * math.acos(float(np.clip(abs(np.dot(left, right)), 0.0, 1.0))) + + +def _solve_eef_target( + config: ConfigDict, + runtime: RuntimeState, + group_name: str, + position: Any, + quaternion_wxyz: Any, + gripper: Any = None, + frame: Any = None, +) -> tuple[np.ndarray, dict[str, float]]: + current_qpos = _latest_qpos(runtime) + configured_frame = str(getattr(config.robot, "eef_reference_frame", "")) + if frame not in (None, "", configured_frame): + raise ValueError(f"frame must be the configured EEF frame {configured_frame!r}") + target_position = np.asarray(position, dtype=np.float64) + target_quaternion = np.asarray(quaternion_wxyz, dtype=np.float64) + if target_position.shape != (3,): + raise ValueError(f"position must contain 3 values, got {target_position.shape}") + if not np.all(np.isfinite(target_position)): + raise ValueError("position must contain only finite values") + if ( + target_quaternion.shape != (4,) + or not np.all(np.isfinite(target_quaternion)) + or _vector_norm(target_quaternion) < 1e-8 + ): + raise ValueError("quaternion_wxyz must contain a non-zero 4D quaternion") + target_quaternion /= _vector_norm(target_quaternion) + + solver = ensure_ik_solver(config, runtime) + current_eef = np.asarray(solver.fk_chunk(current_qpos)[0], dtype=np.float64) + eef_slice = _arm_eef_slice(runtime, group_name) + target_eef = current_eef.copy() + target_eef[eef_slice.start : eef_slice.start + 3] = target_position + target_eef[eef_slice.start + 3 : eef_slice.start + 7] = target_quaternion + if gripper is not None: + target_gripper = float(gripper) + if not math.isfinite(target_gripper): + raise ValueError("gripper must be finite") + target_eef[eef_slice.start + 7] = target_gripper + + solved = np.asarray( + solver.solve_chunk(np.stack([current_eef, target_eef]), seed_qpos=current_qpos)[-1], + dtype=np.float32, + ) + if solved.shape != current_qpos.shape or not np.all(np.isfinite(solved)): + raise RuntimeError("IK returned an invalid qpos vector") + group_slice, _ = _group_slice(runtime, group_name) + keep = np.ones(len(current_qpos), dtype=bool) + keep[group_slice] = False + solved[keep] = current_qpos[keep] + + reached = np.asarray(solver.fk_chunk(solved)[0], dtype=np.float64)[eef_slice] + position_error = _vector_norm(reached[:3] - target_position) + orientation_error = _quaternion_error(reached[3:7], target_quaternion) + if position_error > _POSITION_TOLERANCE_M: + raise RuntimeError( + f"IK position error {position_error:.4f} m exceeds {_POSITION_TOLERANCE_M:.4f} m" + ) + if orientation_error > _ORIENTATION_TOLERANCE_RAD: + raise RuntimeError( + f"IK orientation error {orientation_error:.4f} rad exceeds " + f"{_ORIENTATION_TOLERANCE_RAD:.4f} rad" + ) + return solved, { + "position_error_m": position_error, + "orientation_error_rad": orientation_error, + } + + +def _execute_target( + config: ConfigDict, + runtime: RuntimeState, + session: SessionState, + target_qpos: np.ndarray, + duration_s: float | None, +) -> dict[str, Any]: + current_qpos = _latest_qpos(runtime) + if not np.all(np.isfinite(target_qpos)): + raise ValueError("target qpos must contain only finite values") + max_step = float(config.inference_cfg.get("manual_max_qpos_step", _DEFAULT_MAX_QPOS_STEP)) + if not math.isfinite(max_step) or max_step <= 0: + raise ValueError("inference_cfg.manual_max_qpos_step must be positive") + gripper_mask = np.asarray(runtime.robot.gripper_mask, dtype=bool) + joint_delta = np.abs(target_qpos - current_qpos)[~gripper_mask] + delta_steps = max(1, int(np.ceil(float(np.max(joint_delta)) / max_step))) + rate_hz = int(config.inference_cfg.publish_rate) + if rate_hz <= 0: + raise ValueError("inference_cfg.publish_rate must be positive") + if duration_s is not None and (not math.isfinite(duration_s) or duration_s < 0): + raise ValueError("duration_s must be a finite non-negative value") + duration_steps = 0 if duration_s is None else int(np.ceil(duration_s * rate_hz)) + steps = max(delta_steps, duration_steps, 1) + trajectory = build_linear_trajectory(current_qpos, target_qpos, steps + 1)[1:] + trajectory[:, gripper_mask] = target_qpos[gripper_mask] + completed = publish_action_chunk( + config, + runtime, + trajectory, + OutputTarget.REAL, + session=session, + ) + if not completed: + raise MotionCancelled("agent motion was interrupted") + return { + "target_qpos": target_qpos.astype(float).tolist(), + "steps": steps, + "duration_s": steps / max(rate_hz, 1), + "motion_mode": "ik_joint_interpolation", + "collision_aware": False, + } + + +def _move_eef( + config: ConfigDict, + runtime: RuntimeState, + session: SessionState, + arguments: dict[str, Any], + execute: bool, +) -> dict[str, Any]: + target_qpos, residual = _solve_eef_target( + config, + runtime, + str(arguments.get("group", "")), + arguments.get("position"), + arguments.get("quaternion_wxyz"), + arguments.get("gripper"), + arguments.get("frame"), + ) + result: dict[str, Any] = { + "target_qpos": target_qpos.astype(float).tolist(), + "residual": residual, + "motion_mode": "ik_joint_interpolation", + "collision_aware": False, + } + if execute: + result.update( + _execute_target(config, runtime, session, target_qpos, arguments.get("duration_s")) + ) + return result + + +def _move_joints( + config: ConfigDict, + runtime: RuntimeState, + session: SessionState, + arguments: dict[str, Any], +) -> dict[str, Any]: + group_slice, group = _group_slice(runtime, str(arguments.get("group", ""))) + positions = np.asarray(arguments.get("positions"), dtype=np.float32) + if positions.shape != (group.dof,): + raise ValueError(f"positions for {group.name!r} must have {group.dof} values") + if not np.all(np.isfinite(positions)): + raise ValueError("positions must contain only finite values") + target = _latest_qpos(runtime).copy() + target[group_slice] = positions + return _execute_target(config, runtime, session, target, arguments.get("duration_s")) + + +def _set_gripper( + config: ConfigDict, + runtime: RuntimeState, + session: SessionState, + arguments: dict[str, Any], +) -> dict[str, Any]: + group_slice, group = _group_slice(runtime, str(arguments.get("group", ""))) + if group.gripper_index is None: + raise ValueError(f"actuator group {group.name!r} has no gripper") + state = str(arguments.get("state", "")).lower() + if state not in {"open", "close"}: + raise ValueError("gripper state must be 'open' or 'close'") + target = _latest_qpos(runtime).copy() + value = config.robot.gripper_open if state == "open" else config.robot.gripper_close + target[group_slice.start + group.gripper_index] = float(value) + result = _execute_target(config, runtime, session, target, duration_s=1.0) + result.update({"group": group.name, "gripper_state": state, "gripper_value": float(value)}) + return result + + +def handle_agent_command( + command: AgentCommand, + config: ConfigDict, + runtime: RuntimeState, + session: SessionState, +) -> None: + """Execute one queued operation on the EVA main thread and publish its result.""" + operation = _operation_snapshot(runtime) + if operation is None or operation["operation_id"] != command.operation_id: + return + if operation["status"] == "cancel_requested": + _update_operation(runtime, command.operation_id, status="cancelled") + session.interrupt_requested = False + return + teleop_execution = runtime.teleop_execution + if ( + runtime.collection_teleop_armed + or runtime.collection_teleop_active + or runtime.rollout_intervention_active + or bool(getattr(teleop_execution, "active", False)) + ): + _update_operation( + runtime, + command.operation_id, + status="failed", + error="operator teleoperation currently owns robot control", + ) + return + + # Direct agent control always preempts policy execution but never teleoperation. + if session.status is SessionStatus.RUNNING and session.mode in { + SessionMode.REAL, + SessionMode.SIM, + }: + set_status(session, SessionStatus.READY, reason="coding agent direct-control takeover") + set_phase(runtime, "ready") + reset_infer_strategy(runtime) + session.pending_real_chunk = None + session.follow_human_gripper = False + session.gripper_locks.clear() + session.interrupt_requested = False + _update_operation(runtime, command.operation_id, status="running") + + try: + if command.action == "move_eef": + result = _move_eef(config, runtime, session, command.arguments, execute=True) + elif command.action == "solve_ik": + result = _move_eef(config, runtime, session, command.arguments, execute=False) + elif command.action == "move_joints": + result = _move_joints(config, runtime, session, command.arguments) + else: + result = _set_gripper(config, runtime, session, command.arguments) + except MotionCancelled as error: + _update_operation( + runtime, + command.operation_id, + status="cancelled", + error=str(error), + ) + except Exception as error: + _update_operation( + runtime, + command.operation_id, + status="failed", + error=str(error), + ) + else: + _update_operation( + runtime, + command.operation_id, + status="succeeded", + result=result, + error="", + ) + + +def serialize_agent_status(runtime: RuntimeState) -> dict[str, Any]: + """Describe direct-control and optional-policy capabilities for a coding agent.""" + config = _active_config(runtime) + qpos = runtime.transport.get_latest_qpos() + eef_capable = ( + runtime.ik_solver is not None + or type(runtime.robot).build_kinematics is not Robot.build_kinematics + ) + direct_ready = qpos is not None and not runtime.transport.is_offline() + modes = ["direct"] if direct_ready else [] + if runtime.policy is not None: + modes.append("policy") + disabled_cameras = set(config.transport.get("disabled_cameras", [])) + return { + "ok": True, + "robot": runtime.robot.name, + "transport": config.transport.type, + "direct_control_capable": True, + "direct_control_available": direct_ready, + "eef_control_capable": eef_capable, + "eef_control_available": direct_ready and eef_capable, + "actuator_groups": [ + { + "name": group.name, + "dof": group.dof, + "eef": group in runtime.robot.arm_groups, + "gripper": group.gripper_index is not None, + } + for group in runtime.robot.actuator_groups + ], + "cameras": [ + camera.observation_key + for camera in runtime.robot.observation_schema.cameras + if camera.observation_key not in disabled_cameras + ], + "direct_control_tools": [ + "robot_get_state", + "camera_capture", + "robot_solve_ik", + "robot_move_eef", + "robot_move_joints", + "robot_set_gripper", + "robot_stop", + ], + "control_modes": modes, + "motion": {"mode": "ik_joint_interpolation", "collision_aware": False}, + "policy": { + "configured_type": config.policy.type, + "connected": runtime.policy is not None, + "metadata": runtime.policy_metadata or {}, + "error": runtime.last_policy_error, + }, + "operation": _operation_snapshot(runtime), + "guidance": ( + "The coding agent can control the robot directly without a policy model. " + "Use direct tools whenever the model is offline or direct correction is useful." + ), + } + + +def serialize_robot_state(runtime: RuntimeState) -> dict[str, Any]: + """Return current joint feedback and EEF state when the IK solver is initialized.""" + qpos = _latest_qpos(runtime) + groups = {} + offset = 0 + for group in runtime.robot.actuator_groups: + groups[group.name] = { + "qpos": qpos[offset : offset + group.dof].astype(float).tolist(), + "dof": group.dof, + "has_gripper": group.gripper_index is not None, + } + offset += group.dof + eef = None + if runtime.ik_solver is not None: + eef = np.asarray(runtime.ik_solver.fk_chunk(qpos)[0], dtype=float).tolist() + return { + "ok": True, + "qpos": qpos.astype(float).tolist(), + "groups": groups, + "eef": eef, + "eef_layout": "per arm: xyz + quaternion_wxyz + gripper", + "direct_control_available": not runtime.transport.is_offline(), + } + + +def serialize_camera(runtime: RuntimeState, camera_name: str) -> dict[str, Any]: + """Capture one camera frame and return a JPEG payload for the MCP adapter.""" + config = _active_config(runtime) + ctx = runtime.console_ctx + if ctx is None: + raise RuntimeError("EVA runtime context is not ready") + reader = ctx.obs_reader or runtime.transport + frame = reader.get_frame() + if frame is None: + raise RuntimeError("camera frame is unavailable") + image = frame.images.get(camera_name) + if image is None: + raise ValueError(f"unknown camera {camera_name!r}; expected one of {sorted(frame.images)}") + array = np.asarray(image) + if array.dtype != np.uint8: + array = np.clip(array, 0, 255).astype(np.uint8) + if array.ndim == 2: + array = np.repeat(array[..., None], 3, axis=2) + if not config.transport.convert_bgr_to_rgb and array.shape[2] == 3: + array = cv2.cvtColor(array, cv2.COLOR_RGB2BGR) + ok, encoded = cv2.imencode(".jpg", array, [int(cv2.IMWRITE_JPEG_QUALITY), 80]) + if not ok: + raise RuntimeError(f"failed to encode camera {camera_name!r}") + return { + "ok": True, + "camera": camera_name, + "mime_type": "image/jpeg", + "data": base64.b64encode(encoded.tobytes()).decode("ascii"), + } diff --git a/src/core/app/control_channel.py b/src/core/app/control_channel.py index dd33f1e..7413c75 100644 --- a/src/core/app/control_channel.py +++ b/src/core/app/control_channel.py @@ -104,14 +104,54 @@ def _handle_command(runtime: RuntimeState, message: dict) -> dict: return {"ok": True, "cmd": command} +def _handle_agent_query(runtime: RuntimeState, payload: object) -> dict: + """Serve MCP-oriented capability, state, camera, and operation queries.""" + from core.app.agent_control import ( + serialize_agent_operation, + serialize_agent_status, + serialize_camera, + serialize_robot_state, + ) + + if not isinstance(payload, dict): + return _reject("agent_query must be a JSON object") + action = str(payload.get("action", "")).strip() + try: + if action == "status": + return serialize_agent_status(runtime) + if action == "robot_state": + return serialize_robot_state(runtime) + if action == "operation": + operation_id = payload.get("operation_id") + return serialize_agent_operation( + runtime, + None if operation_id is None else str(operation_id), + ) + if action == "camera_capture": + return serialize_camera(runtime, str(payload.get("camera", ""))) + except Exception as error: + return _reject(str(error)) + return _reject(f"unknown agent query: {action!r}") + + def _handle_message(runtime: RuntimeState, message: object) -> dict: if not isinstance(message, dict): return _reject("message must be a JSON object") + if "agent_query" in message: + return _handle_agent_query(runtime, message["agent_query"]) + if "agent_command" in message: + from core.app.agent_control import queue_agent_command + + return queue_agent_command(runtime, message["agent_command"]) + if "agent_stop" in message: + from core.app.agent_control import request_agent_stop + + return request_agent_stop(runtime) if "query" in message: return _handle_query(runtime, str(message.get("query", "")).strip()) if "cmd" in message: return _handle_command(runtime, message) - return _reject("message must carry 'cmd' or 'query'") + return _reject("message must carry cmd, query, agent_query, agent_command, or agent_stop") def maybe_start_control_channel(config: ConfigDict, runtime: RuntimeState) -> None: diff --git a/src/core/app/run.py b/src/core/app/run.py index 5c46837..a7aa159 100644 --- a/src/core/app/run.py +++ b/src/core/app/run.py @@ -14,6 +14,7 @@ import numpy as np +from core.app.agent_control import handle_agent_command from core.app.cli import maybe_start_inference_cli from core.app.console.server import build_console_context, start_console_server from core.app.control_channel import maybe_start_control_channel @@ -1450,6 +1451,15 @@ def log_gc_timing(phase: str, info: dict[str, int]) -> None: effective = runtime.active_config or config prompt_ready.set() + while True: + try: + agent_command = runtime.agent_command_queue.get_nowait() + except queue.Empty: + break + handle_agent_command(agent_command, effective, runtime, session) + effective = runtime.active_config or config + prompt_ready.set() + if runtime.teleop_client is not None: for event in drain_teleop_events(runtime): handle_teleop_operator_event( diff --git a/src/core/app/state.py b/src/core/app/state.py index c250f9d..813ce25 100644 --- a/src/core/app/state.py +++ b/src/core/app/state.py @@ -148,6 +148,8 @@ class RuntimeState: ik_solver: IK solver instance used for EEF-to-qpos conversion. selected_inference_strategy_key: Key of the active inference strategy. command_queue: Inter-thread queue carrying web/CLI commands to the main loop. + agent_command_queue: Typed coding-agent operations executed by the main loop. + agent_operation: Latest direct-control operation exposed over the control channel. prompt_ready: Event signaling that a prompt has been selected. infer_strategy: Active inference strategy instance. episode_logger: Logger writing teleop/collection episodes; None when not recording. @@ -233,6 +235,9 @@ class RuntimeState: ik_solver: Any | None = None selected_inference_strategy_key: str | None = None command_queue: queue.Queue[str] | None = None + agent_command_queue: queue.Queue[Any] = dataclasses.field(default_factory=queue.Queue) + agent_operation: dict[str, Any] | None = None + agent_operation_lock: threading.Lock = dataclasses.field(default_factory=threading.Lock) prompt_ready: threading.Event | None = None infer_strategy: BaseInferStrategy | None = None # pyright: ignore[reportGeneralTypeIssues] episode_logger: EpisodeLogger | None = None diff --git a/tests/app/test_agent_control.py b/tests/app/test_agent_control.py new file mode 100644 index 0000000..3bc362e --- /dev/null +++ b/tests/app/test_agent_control.py @@ -0,0 +1,203 @@ +from __future__ import annotations + +import queue +from types import SimpleNamespace + +import numpy as np + +from core.app.agent_control import ( + AgentCommand, + handle_agent_command, + queue_agent_command, + request_agent_stop, + serialize_agent_status, +) +from core.app.state import RuntimeState, SessionState +from core.config import ConfigDict +from core.types import Observation +from robots.base import ActuatorGroup, ObservationSchema, Robot +from transport.base import TransportBridge + + +class FakeTransport(TransportBridge): + def __init__(self, qpos: np.ndarray) -> None: + self.qpos = qpos.copy() + self.published: list[np.ndarray] = [] + + def get_frame(self) -> Observation | None: + return Observation(images={}, state_qpos=self.qpos.copy()) + + def publish_action(self, action: np.ndarray, target: str = "real") -> None: + assert target == "real" + self.qpos = np.asarray(action, dtype=np.float32).copy() + self.published.append(self.qpos) + + def get_latest_qpos(self) -> np.ndarray | None: + return self.qpos.copy() + + def close(self) -> None: + return + + +class FakeSolver: + def fk_chunk(self, qpos_chunk: np.ndarray) -> np.ndarray: + qpos = np.asarray(qpos_chunk, dtype=np.float32) + if qpos.ndim == 1: + qpos = qpos[None, :] + rows = [] + for row in qpos: + rows.append( + np.array( + [row[0], row[1], row[2], 1, 0, 0, 0, row[3], 0, 0, 0, 1, 0, 0, 0, row[7]], + dtype=np.float32, + ) + ) + return np.asarray(rows) + + def solve_chunk(self, eef_chunk: np.ndarray, seed_qpos: np.ndarray | None = None) -> np.ndarray: + assert seed_qpos is not None + result = np.tile(np.asarray(seed_qpos, dtype=np.float32), (len(eef_chunk), 1)) + result[-1, :3] = eef_chunk[-1, :3] + result[-1, 3] = eef_chunk[-1, 7] + result[-1, 4:7] = 9.0 + return result + + +def _runtime() -> tuple[ConfigDict, RuntimeState, SessionState, FakeTransport]: + groups = ( + ActuatorGroup("left_arm", 4, ("l0", "l1", "l2", "lg"), gripper_index=3), + ActuatorGroup("right_arm", 4, ("r0", "r1", "r2", "rg"), gripper_index=3), + ) + robot = Robot( + "fake", + groups, + np.zeros(8, dtype=np.float32), + ObservationSchema(cameras=(), state_composition=("left_arm", "right_arm")), + ) + transport = FakeTransport(np.array([0.0, 0.0, 0.0, 0.2, 1.0, 1.0, 1.0, 0.8])) + runtime = RuntimeState(robot=robot, transport=transport, ik_solver=FakeSolver()) + runtime.command_queue = queue.Queue() + config = ConfigDict( + { + "robot": { + "eef_reference_frame": "base", + "gripper_open": 1.0, + "gripper_close": 0.0, + }, + "inference_cfg": {"publish_rate": 20, "manual_max_qpos_step": 0.1}, + "transport": {"type": "fake", "convert_bgr_to_rgb": True}, + "policy": {"type": "mock"}, + } + ) + session = SessionState() + runtime.console_ctx = SimpleNamespace(config=config, session=session, obs_reader=None) + return config, runtime, session, transport + + +def test_move_eef_preserves_other_actuator_groups() -> None: + config, runtime, session, transport = _runtime() + reply = queue_agent_command( + runtime, + { + "action": "move_eef", + "arguments": { + "group": "left_arm", + "position": [0.3, 0.2, 0.1], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + "frame": "base", + }, + }, + ) + command = runtime.agent_command_queue.get_nowait() + assert isinstance(command, AgentCommand) + + handle_agent_command(command, config, runtime, session) + + assert runtime.agent_operation is not None + assert runtime.agent_operation["status"] == "succeeded" + np.testing.assert_allclose(transport.published[-1][:3], [0.3, 0.2, 0.1]) + np.testing.assert_allclose(transport.published[-1][4:], [1.0, 1.0, 1.0, 0.8]) + assert reply["operation"]["operation_id"] == runtime.agent_operation["operation_id"] + + +def test_stop_cancels_queued_operation() -> None: + config, runtime, session, transport = _runtime() + queue_agent_command( + runtime, + { + "action": "move_joints", + "arguments": {"group": "left_arm", "positions": [0.1, 0.2, 0.3, 0.4]}, + }, + ) + command = runtime.agent_command_queue.get_nowait() + + reply = request_agent_stop(runtime) + handle_agent_command(command, config, runtime, session) + + assert reply["operation"]["status"] == "cancel_requested" + assert runtime.agent_operation is not None + assert runtime.agent_operation["status"] == "cancelled" + assert transport.published == [] + + +def test_stop_without_direct_operation_does_not_leave_interrupt_pending() -> None: + _, runtime, session, _ = _runtime() + + reply = request_agent_stop(runtime) + + assert reply["operation"] is None + assert session.interrupt_requested is False + assert runtime.command_queue.get_nowait() == "web:halt" + + +def test_status_keeps_joint_control_available_without_ik_capability() -> None: + _, runtime, _, _ = _runtime() + runtime.ik_solver = None + + status = serialize_agent_status(runtime) + + assert status["direct_control_available"] is True + assert status["eef_control_available"] is False + assert [group["name"] for group in status["actuator_groups"]] == [ + "left_arm", + "right_arm", + ] + + +def test_move_joints_rejects_non_finite_positions() -> None: + config, runtime, session, transport = _runtime() + queue_agent_command( + runtime, + { + "action": "move_joints", + "arguments": {"group": "left_arm", "positions": [0.1, float("nan"), 0.3, 0.4]}, + }, + ) + command = runtime.agent_command_queue.get_nowait() + + handle_agent_command(command, config, runtime, session) + + assert runtime.agent_operation is not None + assert runtime.agent_operation["status"] == "failed" + assert "finite" in runtime.agent_operation["error"] + assert transport.published == [] + + +def test_direct_motion_defers_to_armed_operator_teleop() -> None: + config, runtime, session, transport = _runtime() + runtime.collection_teleop_armed = True + queue_agent_command( + runtime, + { + "action": "move_joints", + "arguments": {"group": "left_arm", "positions": [0.1, 0.2, 0.3, 0.4]}, + }, + ) + command = runtime.agent_command_queue.get_nowait() + + handle_agent_command(command, config, runtime, session) + + assert runtime.agent_operation is not None + assert runtime.agent_operation["status"] == "failed" + assert "teleoperation" in runtime.agent_operation["error"] + assert transport.published == [] diff --git a/tests/app/test_control_channel.py b/tests/app/test_control_channel.py index 0771ed3..b63d5aa 100644 --- a/tests/app/test_control_channel.py +++ b/tests/app/test_control_channel.py @@ -2,6 +2,7 @@ import inspect import queue +import threading from types import SimpleNamespace from core.app import run @@ -76,3 +77,25 @@ def test_channel_handles_rl_and_collect_commands() -> None: reply = _handle_message(runtime, {"cmd": "web:collect_arm:on"}) assert reply == {"ok": True, "cmd": "web:collect_arm:on"} assert runtime.command_queue.get_nowait() == "web:collect_arm:on" + + +def test_channel_queues_typed_agent_command() -> None: + runtime = _runtime() + runtime.agent_command_queue = queue.Queue() + runtime.agent_operation = None + runtime.agent_operation_lock = threading.Lock() + + reply = _handle_message( + runtime, + { + "agent_command": { + "action": "move_joints", + "arguments": {"group": "left_arm", "positions": [0.1, 0.2]}, + } + }, + ) + + assert reply["ok"] is True + command = runtime.agent_command_queue.get_nowait() + assert command.action == "move_joints" + assert command.operation_id == reply["operation"]["operation_id"] diff --git a/tests/tools/test_mcp.py b/tests/tools/test_mcp.py new file mode 100644 index 0000000..793d6e2 --- /dev/null +++ b/tests/tools/test_mcp.py @@ -0,0 +1,73 @@ +from __future__ import annotations + +import pytest + +pytest.importorskip("mcp") + +import anyio +from mcp import Client + +from tools.mcp.server import build_server + + +class FakeControlClient: + def __init__(self) -> None: + self.messages: list[dict] = [] + + def require_ok(self, message: dict) -> dict: + self.messages.append(message) + if "agent_query" in message and message["agent_query"]["action"] == "status": + return { + "ok": True, + "direct_control_available": True, + "control_modes": ["direct"], + "policy": {"connected": False}, + } + return {"ok": True, "operation": {"operation_id": "op-1", "status": "queued"}} + + +def test_server_exposes_direct_and_policy_tools() -> None: + async def check() -> None: + fake = FakeControlClient() + async with Client(build_server(fake)) as client: + result = await client.list_tools() + names = {tool.name for tool in result.tools} + assert { + "eva_status", + "robot_get_state", + "camera_capture", + "robot_solve_ik", + "robot_move_eef", + "robot_move_joints", + "robot_set_gripper", + "robot_get_operation", + "robot_stop", + "policy_run", + "policy_stop", + } <= names + + result = await client.call_tool("eva_status", {}) + assert result.structured_content is not None + assert result.structured_content["direct_control_available"] is True + assert result.structured_content["policy"]["connected"] is False + + anyio.run(check) + + +def test_move_eef_maps_to_typed_agent_command() -> None: + async def check() -> None: + fake = FakeControlClient() + async with Client(build_server(fake)) as client: + await client.call_tool( + "robot_move_eef", + { + "group": "left_arm", + "position": [0.1, 0.2, 0.3], + "quaternion_wxyz": [1.0, 0.0, 0.0, 0.0], + }, + ) + message = fake.messages[-1] + assert message["agent_command"]["action"] == "move_eef" + assert message["agent_command"]["arguments"]["group"] == "left_arm" + + anyio.run(check) diff --git a/tools/mcp/__init__.py b/tools/mcp/__init__.py new file mode 100644 index 0000000..7d4606e --- /dev/null +++ b/tools/mcp/__init__.py @@ -0,0 +1 @@ +"""MCP adapter for coding-agent access to a running EVA Client.""" diff --git a/tools/mcp/server.py b/tools/mcp/server.py new file mode 100644 index 0000000..913f5a0 --- /dev/null +++ b/tools/mcp/server.py @@ -0,0 +1,231 @@ +"""Expose a running EVA Client to coding agents over MCP stdio. + +The MCP process owns no robot resources. Each tool sends a bounded JSON request +to EVA Client's existing ZMQ control channel. Direct robot motions are queued for +the EVA main loop and return an operation id immediately, while status and camera +queries remain read-only. Configure the coding-agent MCP host to launch +``eva-mcp --eva-endpoint tcp://127.0.0.1:5757``. +""" + +from __future__ import annotations + +import argparse +import base64 +from typing import Any + +import zmq +from mcp.server.mcpserver import Image, MCPServer + +_INSTRUCTIONS = """ +EVA Client exposes two peer control modes. Direct tools do not depend on a policy +model and remain available whenever EVA has live robot feedback; a policy model +is optional and may be offline. Use eva_status first. Policy failure never removes +direct-control capability. Direct EEF motion uses IK plus joint interpolation and +is not collision-aware, so prefer small observable steps and use robot_stop +whenever the scene or state is uncertain. +""".strip() + + +class EvaControlClient: + """Short-lived ZMQ request client for the EVA control channel.""" + + def __init__(self, endpoint: str, timeout_ms: int = 5000) -> None: + self.endpoint = endpoint + self.timeout_ms = timeout_ms + + def request(self, message: dict[str, Any]) -> dict[str, Any]: + context = zmq.Context.instance() + socket = context.socket(zmq.REQ) + socket.linger = 0 + socket.connect(self.endpoint) + try: + socket.send_json(message) + if socket.poll(self.timeout_ms) == 0: + raise TimeoutError(f"EVA control channel timed out at {self.endpoint}") + response = socket.recv_json() + finally: + socket.close() + if not isinstance(response, dict): + raise RuntimeError("EVA control channel returned a non-object response") + return response + + def require_ok(self, message: dict[str, Any]) -> dict[str, Any]: + response = self.request(message) + if not response.get("ok", False): + raise RuntimeError(str(response.get("error", "EVA request failed"))) + return response + + +def build_server(client: EvaControlClient) -> MCPServer: + """Build the stdio MCP server around one EVA control-channel client.""" + server = MCPServer("EVA Client", instructions=_INSTRUCTIONS) + + @server.tool() + def eva_status() -> dict[str, Any]: + """Discover robot, direct-control, model, motion, and operation status.""" + return client.require_ok({"agent_query": {"action": "status"}}) + + @server.tool() + def robot_get_state() -> dict[str, Any]: + """Read current joint groups and EEF state when kinematics is initialized.""" + return client.require_ok({"agent_query": {"action": "robot_state"}}) + + @server.tool() + def camera_capture(camera: str) -> Image: + """Capture the named EVA camera and return one JPEG image to the agent.""" + response = client.require_ok( + {"agent_query": {"action": "camera_capture", "camera": camera}} + ) + return Image(data=base64.b64decode(response["data"]), format="jpeg") + + @server.tool() + def robot_solve_ik( + group: str, + position: list[float], + quaternion_wxyz: list[float], + frame: str | None = None, + gripper: float | None = None, + ) -> dict[str, Any]: + """Solve an EEF target without moving; poll robot_get_operation for the result.""" + return client.require_ok( + { + "agent_command": { + "action": "solve_ik", + "arguments": { + "group": group, + "position": position, + "quaternion_wxyz": quaternion_wxyz, + "frame": frame, + "gripper": gripper, + }, + } + } + ) + + @server.tool() + def robot_move_eef( + group: str, + position: list[float], + quaternion_wxyz: list[float], + duration_s: float | None = None, + frame: str | None = None, + gripper: float | None = None, + ) -> dict[str, Any]: + """Move one EEF using IK plus joint interpolation; this is not collision-aware.""" + return client.require_ok( + { + "agent_command": { + "action": "move_eef", + "arguments": { + "group": group, + "position": position, + "quaternion_wxyz": quaternion_wxyz, + "duration_s": duration_s, + "frame": frame, + "gripper": gripper, + }, + } + } + ) + + @server.tool() + def robot_move_joints( + group: str, + positions: list[float], + duration_s: float | None = None, + ) -> dict[str, Any]: + """Move one actuator group in joint space while all other groups hold position.""" + return client.require_ok( + { + "agent_command": { + "action": "move_joints", + "arguments": { + "group": group, + "positions": positions, + "duration_s": duration_s, + }, + } + } + ) + + @server.tool() + def robot_set_gripper(group: str, state: str) -> dict[str, Any]: + """Open or close the named actuator group's gripper directly.""" + return client.require_ok( + { + "agent_command": { + "action": "set_gripper", + "arguments": {"group": group, "state": state}, + } + } + ) + + @server.tool() + def robot_get_operation(operation_id: str | None = None) -> dict[str, Any]: + """Read the latest direct-control operation state and result.""" + return client.require_ok( + { + "agent_query": { + "action": "operation", + "operation_id": operation_id, + } + } + ) + + @server.tool() + def robot_stop() -> dict[str, Any]: + """Cancel queued/running direct motion and halt policy execution immediately.""" + return client.require_ok({"agent_stop": True}) + + @server.tool() + def policy_run(instruction: str) -> dict[str, Any]: + """Ask the configured policy model to run a task; direct control remains available.""" + for command in ( + "web:select_mode:real", + f"web:switch_task:{instruction}", + "web:setup", + "web:run", + ): + client.require_ok({"cmd": command}) + return { + "ok": True, + "status": "queued", + "instruction": instruction, + "direct_control_capable": True, + "next": "Poll eva_status; use robot_stop then direct tools to take over.", + } + + @server.tool() + def policy_stop() -> dict[str, Any]: + """Stop policy execution without disabling the robot's direct-control tools.""" + response = client.require_ok({"cmd": "web:halt"}) + response["direct_control_capable"] = True + return response + + return server + + +def _parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="EVA Client MCP server") + parser.add_argument( + "--eva-endpoint", + default="tcp://127.0.0.1:5757", + help="Running EVA Client ZMQ control endpoint", + ) + parser.add_argument( + "--timeout-ms", + type=int, + default=5000, + help="Per-request EVA control timeout in milliseconds", + ) + return parser.parse_args() + + +def main() -> None: + """Run the local MCP server over stdio for a coding-agent host.""" + args = _parse_args() + build_server(EvaControlClient(args.eva_endpoint, args.timeout_ms)).run() + + +if __name__ == "__main__": + main() From d6f40c09e8cbdc0d480439662b87e06befa76bac Mon Sep 17 00:00:00 2001 From: yangheqing Date: Sat, 29 Aug 2026 10:37:31 +0800 Subject: [PATCH 2/2] test: align rollout teleop reset expectations --- tests/config/test_control_state.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/config/test_control_state.py b/tests/config/test_control_state.py index 550e38a..e825309 100644 --- a/tests/config/test_control_state.py +++ b/tests/config/test_control_state.py @@ -527,7 +527,7 @@ def test_start_rollout_intervention_activates_teleop_client_source() -> None: assert recording.start_rollout_intervention(config, runtime, session) is True assert runtime.rollout_intervention_active is True assert runtime.teleop_client.starts == 1 - assert runtime.teleop_client.resets == [True] + assert runtime.teleop_client.resets == [False] assert runtime.transport.hil_modes == [] @@ -800,7 +800,7 @@ def test_start_rollout_intervention_uses_teleop_client_path() -> None: assert runtime.transport.hil_resets == 0 assert runtime.transport.hil_relay_enabled == [] assert runtime.teleop_client.starts == 1 - assert runtime.teleop_client.resets == [True] + assert runtime.teleop_client.resets == [False] def test_rollout_hil_status_tracks_teleop_client_connection() -> None: