Merge pull request PCrnjak#27 from PCrnjak/feat/mcp-server

Unhomed motion gate, fast-path home, stop/estop/reset

Author: Jepson2k

parol6/ack_policy.py

@@ -4,12 +4,13 @@
 
 # System command types (always require ACK)
 SYSTEM_CMD_TYPES: set[CmdType] = {
-    CmdType.RESUME,
-    CmdType.HALT,
+    CmdType.RESET,
+    CmdType.ESTOP,
+    CmdType.STOP,
     CmdType.CONNECT_HARDWARE,
     CmdType.SIMULATOR,
     CmdType.SELECT_PROFILE,
-    CmdType.RESET,
+    CmdType.RESET_STATE,
     CmdType.WRITE_IO,
     CmdType.SET_TCP_OFFSET,
     CmdType.SET_SHAPES,

parol6/client/async_client.py

@@ -41,7 +41,7 @@
     IOResultStruct,
     JointSpeedsCmd,
     LoopStatsCmd,
-    HaltCmd,
+    EstopCmd,
     HomeCmd,
     JogJCmd,
     JogLCmd,
@@ -64,8 +64,9 @@
     ReachableCmd,
     ResetCmd,
     ResetLoopStatsCmd,
-    ResumeCmd,
+    ResetStateCmd,
     Response,
+    StopCmd,
     ResponseMsg,
     SelectProfileCmd,
     SelectToolCmd,
@@ -660,6 +661,10 @@ async def home(
     ) -> int:
         """Home the robot to its home position.
 
+        Unhomed, this runs the full referencing sequence (each joint seeks
+        its limit switch, then moves to standby). Already homed, it returns
+        to standby with a normal planned, collision-checked joint move.
+
         Returns the command index (≥ 0) on success, -1 on failure.
 
         Category: Motion
@@ -679,31 +684,48 @@ async def home(
                 raise TimeoutError(f"home() timed out after {timeout}s")
         return index
 
-    async def resume(self) -> int:
-        """Re-enable the robot controller, allowing motion commands.
+    async def stop(self) -> int:
+        """Stop all motion — cancel the active move and clear the queue.
+
+        The controller stays enabled and holding position; the next motion
+        command is accepted immediately.
 
         Category: Control
 
         Example:
-            rbt.resume()
+            rbt.stop()
 
         Returns:
             1 if acknowledged, 0 on failure.
         """
-        return await self._send(ResumeCmd())
+        return await self._send(StopCmd())
 
-    async def halt(self) -> int:
-        """Halt the robot — stop all motion and disable.
+    async def estop(self) -> int:
+        """Protective stop: stop all motion and latch the controller
+        disabled until ``reset()``.
 
         Category: Control
 
         Example:
-            rbt.halt()
+            rbt.estop()
 
         Returns:
             1 if acknowledged, 0 on failure.
         """
-        return await self._send(HaltCmd())
+        return await self._send(EstopCmd())
+
+    async def reset(self) -> int:
+        """Clear a latched protective stop, re-enabling motion.
+
+        Category: Control
+
+        Example:
+            rbt.reset()
+
+        Returns:
+            1 if acknowledged, 0 on failure.
+        """
+        return await self._send(ResetCmd())
 
     async def simulator(self, enabled: bool) -> int:
         """Enable or disable simulator mode.
@@ -764,7 +786,7 @@ async def connect_hardware(self, port_str: str) -> int:
             raise ValueError("No port provided")
         return await self._send(ConnectHardwareCmd(port_str=port_str))
 
-    async def reset(self) -> int:
+    async def reset_state(self) -> int:
         """Reset controller state to initial values.
 
         Instantly resets positions to home, clears queues, resets tool/errors.
@@ -773,9 +795,9 @@ async def reset(self) -> int:
         Category: Control
 
         Example:
-            rbt.reset()
+            rbt.reset_state()
         """
-        return await self._send(ResetCmd())
+        return await self._send(ResetStateCmd())
 
     # --------------- Status / Queries ---------------
     async def ping(self) -> PingResult | None:

parol6/client/dry_run_client.py

@@ -167,6 +167,7 @@ def __init__(
         self,
         initial_joints_deg: list[float] | None = None,
         max_snapshot_points: int = 200,
+        initial_homed: bool = True,
     ) -> None:
         # Reset tool transform — process pool workers persist across
         # invocations, so a previous run's select_tool() leaves a stale
@@ -190,6 +191,10 @@ def __init__(
 
         self._planner = TrajectoryPlanner(diagnostic=True)
         self._planner.state.Position_in[:] = self._state.Position_in
+        # Mirror the live gate: seeded from an unhomed robot, planned moves
+        # are refused until the script homes (home()/teleport() establish
+        # references — see _snap_to_angles).
+        self._planner.state.Homed_in.fill(1 if initial_homed else 0)
 
         self._registry = CommandRegistry()
         self._q_rad_buf = np.zeros(6, dtype=np.float64)
@@ -230,18 +235,25 @@ def flush(self) -> list[DryRunResult]:
         return results
 
     def _snap_to_angles(self, angles_deg: list[float]) -> DryRunResult:
-        """Snap to angles instantly (no trajectory) — used by Home and Teleport."""
+        """Snap to angles instantly (no trajectory) — used by Home and Teleport.
+
+        Both establish position references, so subsequent planned moves pass
+        the homed gate."""
         self._planner.flush()
         deg = np.asarray(angles_deg, dtype=np.float64)
         deg_to_steps(deg, self._state.Position_in)
         self._planner.state.Position_in[:] = self._state.Position_in
+        self._planner.state.Homed_in.fill(1)
         rad = np.radians(deg).reshape(1, -1)
         return _build_result(rad, duration=0.0)
 
     def _dispatch(self, params: Any) -> DryRunResult | None:
         """Route a command struct through the trajectory planner."""
         if isinstance(params, HomeCmd):
-            return self._snap_to_angles(HOME_ANGLES_DEG)
+            if not self._planner.state.Homed_in[:6].all():
+                return self._snap_to_angles(HOME_ANGLES_DEG)
+            # Already referenced → fall through: the planner fast-paths HOME
+            # into a planned return move, so the preview renders the path.
         if isinstance(params, TeleportCmd):
             return self._snap_to_angles(params.angles)
         if isinstance(params, SelectToolCmd):

parol6/client/sync_client.py

@@ -112,7 +112,7 @@ class RobotClient:
     Can be used as a context manager to ensure proper cleanup:
 
         with RobotClient() as client:
-            client.resume()
+            client.home()
             ...
     """
 
@@ -178,6 +178,10 @@ def port(self) -> int:
     def home(self, wait: bool = False, timeout: float = 60.0) -> int:
         """Home the robot to its home position.
 
+        Unhomed, this runs the full referencing sequence (each joint seeks
+        its limit switch, then moves to standby). Already homed, it returns
+        to standby with a normal planned, collision-checked joint move.
+
         Returns the command index (≥ 0) on success, -1 on failure.
 
         Args:
@@ -194,21 +198,33 @@ def teleport(
         """Instantly set joint angles and optional tool positions (simulator only)."""
         return _run(self._inner.teleport(angles_deg, tool_positions=tool_positions))
 
-    def resume(self) -> int:
-        """Re-enable the robot controller, allowing motion commands.
+    def stop(self) -> int:
+        """Stop all motion — cancel the active move and clear the queue.
+
+        The controller stays enabled and holding position; the next motion
+        command is accepted immediately.
 
         Returns:
             1 if acknowledged, 0 on failure.
         """
-        return _run(self._inner.resume())
+        return _run(self._inner.stop())
 
-    def halt(self) -> int:
-        """Halt the robot — stop all motion and disable.
+    def estop(self) -> int:
+        """Protective stop: stop all motion and latch the controller
+        disabled until ``reset()``.
 
         Returns:
             1 if acknowledged, 0 on failure.
         """
-        return _run(self._inner.halt())
+        return _run(self._inner.estop())
+
+    def reset(self) -> int:
+        """Clear a latched protective stop, re-enabling motion.
+
+        Returns:
+            1 if acknowledged, 0 on failure.
+        """
+        return _run(self._inner.reset())
 
     def simulator(self, enabled: bool) -> int:
         """Enable or disable simulator mode."""
@@ -229,9 +245,9 @@ def connect_hardware(self, port_str: str) -> int:
         """
         return _run(self._inner.connect_hardware(port_str))
 
-    def reset(self) -> int:
+    def reset_state(self) -> int:
         """Reset controller state to initial values."""
-        return _run(self._inner.reset())
+        return _run(self._inner.reset_state())
 
     # ---------- status / queries ----------
     def ping(self) -> PingResult | None:

parol6/commands/base.py

@@ -13,12 +13,28 @@
 from parol6.config import TRACE
 from parol6.protocol.wire import CmdType, Command, CommandCode, QueryType
 from parol6.server.state import ControllerState
-from parol6.utils.error_catalog import RobotError, extract_robot_error
+from parol6.utils.error_catalog import RobotError, extract_robot_error, make_error
 from parol6.utils.error_codes import ErrorCode
+from parol6.utils.errors import TrajectoryPlanningError
 
 logger = logging.getLogger(__name__)
 
 
+def guard_homed(state: ControllerState) -> None:
+    """Refuse planned motion while the robot is not homed.
+
+    Reported joint positions are unreferenced until homing (the boot state is
+    all-zeros steps — outside J2/J3's limits), so building or collision-checking
+    a trajectory from them is meaningless. Called at the top of every planned
+    command's ``do_setup``, like ``guard_joint_path``. Jog/servo/home are
+    deliberately not gated: they don't plan a path from the reported pose, and
+    an unhomed arm may need to be jogged clear of an obstruction before homing.
+    """
+    for i in range(6):
+        if not state.Homed_in[i]:
+            raise TrajectoryPlanningError(make_error(ErrorCode.MOTN_NOT_HOMED))
+
+
 class ExecutionStatusCode(Enum):
     """Enumeration for command execution status codes."""
 

parol6/commands/basic_commands.py

@@ -77,7 +77,8 @@ class HomeState(Enum):
 class HomeCommand(MotionCommand[HomeCmd]):
     """
     A non-blocking command that tells the robot to perform its internal homing sequence.
-    This version uses a state machine to allow re-homing even if the robot is already homed.
+    Reached only while the robot is unhomed — the planner routes HOME from an
+    already-referenced robot to a planned return move instead.
     """
 
     PARAMS_TYPE = HomeCmd

parol6/commands/cartesian_commands.py

@@ -38,6 +38,7 @@
     ExecutionStatusCode,
     MotionCommand,
     TrajectoryMoveCommandBase,
+    guard_homed,
 )
 
 logger = logging.getLogger(__name__)
@@ -293,6 +294,7 @@ def __init__(self, p: MoveLCmd):
 
     def do_setup(self, state: "ControllerState") -> None:
         """Set up the move - compute target pose and pre-compute trajectory."""
+        guard_homed(state)
         self.initial_pose = get_fkine_se3(state)
         self._compute_target_pose(state)
         self._precompute_trajectory(state)
@@ -408,6 +410,7 @@ def do_setup_with_blend(
         next_cmds: "list[TrajectoryMoveCommandBase]",
     ) -> int:
         """Build composite Cartesian trajectory with blend zones."""
+        guard_homed(state)
         if self.blend_radius <= 0 or not next_cmds:
             self.do_setup(state)
             return 0

parol6/commands/curved_commands.py

@@ -12,7 +12,7 @@
 import numpy as np
 
 from parol6.commands._collision_guard import guard_joint_path
-from parol6.commands.base import TrajectoryMoveCommandBase
+from parol6.commands.base import TrajectoryMoveCommandBase, guard_homed
 from parol6.config import INTERVAL_S, LIMITS, steps_to_rad
 from parol6.motion import CircularMotion, JointPath, SplineMotion, TrajectoryBuilder
 from parol6.protocol.wire import (
@@ -115,6 +115,7 @@ def get_current_pose(self, state: "ControllerState") -> np.ndarray:
 
     def do_setup(self, state: "ControllerState") -> None:
         """Pre-compute trajectory from current position."""
+        guard_homed(state)
         self.log_debug("  -> Preparing %s...", self.name)
 
         current_pose = self.get_current_pose(state)

parol6/commands/joint_commands.py

@@ -17,7 +17,7 @@
 
 import parol6.PAROL6_ROBOT as PAROL6_ROBOT
 from parol6.commands._collision_guard import guard_joint_path
-from parol6.commands.base import TrajectoryMoveCommandBase
+from parol6.commands.base import TrajectoryMoveCommandBase, guard_homed
 from parol6.config import (
     INTERVAL_S,
     MAX_BLEND_LOOKAHEAD,
@@ -79,6 +79,7 @@ def _get_target_rad(
 
     def do_setup(self, state: ControllerState) -> None:
         """Build trajectory from current position to target using unified motion pipeline."""
+        guard_homed(state)
         steps_to_rad(state.Position_in, self._q_rad_buf)
         target_rad = self._get_target_rad(state, self._q_rad_buf)
         current_rad = self._q_rad_buf
@@ -116,6 +117,7 @@ def do_setup_with_blend(
         next_cmds: "list[TrajectoryMoveCommandBase]",
     ) -> int:
         """Build composite joint-space trajectory with blend zones."""
+        guard_homed(state)
         if self.blend_radius <= 0 or not next_cmds:
             self.do_setup(state)
             return 0