add tool variants and concurrent tool commands, tune motion limits, clean up executor threading

Author: Jepson2k

.gitignore

@@ -124,4 +124,5 @@ cython_debug/
 # VS Code
 .vscode/
 
-serial_port.txt
+serial_port.txt
+test-results.xml

CLAUDE.md

@@ -132,7 +132,7 @@ For streamable commands (`streamable = True`), `do_setup()` also runs at high fr
 ## Code Style
 
 - **Comments**: Describe the final code state, not what changed. Avoid "changed X to Y" or "added this because..." comments.
-- **Git commits/PRs**: Keep messages concise and factual. No emoji, no "Generated by..." footers, no co-author boilerplate.
+- **Git commits/PRs**: No emoji, no "Generated by..." footers, no co-author boilerplate.
 - **Type annotations**: Fix type errors properly instead of using `# type: ignore`. Prefer:
   - `@overload` decorators for functions with different return types based on input
   - `assert` statements to narrow types after None checks
@@ -145,5 +145,6 @@ For streamable commands (`streamable = True`), `do_setup()` also runs at high fr
 **When CI tests fail, fix them.** Don't waste time analyzing whether failures are "related to your changes" — just fix all failing tests. The goal is a green CI, not attribution.
 
 Prefer fewer, comprehensive integration tests that mimic manual testing over a large number of unit tests. We have no code coverage requirements—the goal is working features, not metrics.
-- **NEVER truncate test output.** Do not pipe pytest through `tail`, `head`, `grep`, or any other filter. Always let the full output come through so failures and tracebacks are visible. This applies to both foreground and background test runs.
+- **Test results are in `test-results.xml`.** Pytest writes JUnit XML to `test-results.xml` automatically. When diagnosing failures, read this file — it contains test names, durations, failure messages, and captured output. This is more reliable than parsing console output.
 - **NEVER run parol6 and web commander test suites in parallel** — no proper isolation, they share resources and have timing issues when resource-constrained. Always run sequentially.
+- **NEVER allow subagents to run tests.** Many tests are timing-sensitive and the system doesn't have enough resources for agents and tests to run simultaneously. Only the main conversation should run tests, and only after all agents have completed.

parol6/PAROL6_ROBOT.py

@@ -61,23 +61,26 @@
 robot: Robot = Robot(_urdf_path)
 
 
-def apply_tool(tool_name: str) -> None:
+def apply_tool(tool_name: str, variant_key: str = "") -> None:
     """
     Apply tool transform to the robot model.
 
     Parameters
     ----------
     tool_name : str
         Name of the tool from the tool registry
+    variant_key : str
+        Optional variant key for the tool
     """
-    T_tool = get_tool_transform(tool_name)
+    T_tool = get_tool_transform(tool_name, variant_key=variant_key or None)
 
+    label = f"'{tool_name}:{variant_key}'" if variant_key else f"'{tool_name}'"
     if tool_name != "NONE" and not np.allclose(T_tool, np.eye(4)):
         robot.set_tool_transform(T_tool)
-        logger.info(f"Applied tool '{tool_name}' to robot model")
+        logger.info(f"Applied tool {label} to robot model")
     else:
         robot.clear_tool_transform()
-        logger.info(f"Applied tool '{tool_name}' (identity)")
+        logger.info(f"Applied tool {label} (identity)")
 
 
 # Initialize with no tool
@@ -98,14 +101,14 @@ def _cleanup_robot() -> None:
     [6.4, 20.0, 20.0 * (38.0 / 42.0), 4.0, 4.0, 10.0], dtype=np.float64
 )
 
-# Joint speeds (steps/s) - hardware limits
+# Joint speeds (steps/s)
 _joint_max_speed_hw: Vec6i = np.array(
-    [9750, 27000, 30000, 30000, 33000, 33000], dtype=np.int32
+    [16000, 27000, 32000, 10000, 10000, 32000], dtype=np.int32
 )
 _joint_min_speed: Vec6i = np.array([100, 100, 100, 100, 100, 100], dtype=np.int32)
 
 # Effective max speeds with scaling applied
-_joint_max_speed: Vec6i = _joint_max_speed_hw.astype(np.int32)
+_joint_max_speed: Vec6i = _joint_max_speed_hw.copy()
 
 # Jog speeds (steps/s) - 80% of scaled max for safety margin during jogging
 _joint_max_jog_speed: Vec6i = (_joint_max_speed * 0.8).astype(np.int32)
@@ -137,11 +140,11 @@ def _cleanup_robot() -> None:
 # Linear units: mm/s, mm/s^2, mm/s^3
 # Angular units: deg/s, deg/s^2, deg/s^3
 _cart_linear_velocity_max: float = 200
-_cart_angular_velocity_max: float = 280
+_cart_angular_velocity_max: float = 100
 _cart_linear_acc_max: float = 550
-_cart_angular_acc_max: float = 835
+_cart_angular_acc_max: float = 275
 _cart_linear_jerk_max: float = 5500
-_cart_angular_jerk_max: float = 8350
+_cart_angular_jerk_max: float = 2750
 
 # Min values as 1% of max
 _cart_linear_velocity_min: float = _cart_linear_velocity_max * 0.01
@@ -323,32 +326,55 @@ def split_2_bitfield(var_in: int) -> list[int]:
     def _compute_tcp_velocity_at_config(
         q: NDArray, direction: int, v_max_joint: NDArray
     ) -> float | None:
-        """Max TCP velocity in one direction while maintaining orientation."""
+        """Max TCP velocity in one Cartesian direction.
+
+        For linear directions (0-2), rejects samples that cause orientation change.
+        For angular directions (3-5), rejects samples that cause linear translation.
+        """
         try:
             J = robot.jacob0(q)
             if np.linalg.cond(J) > 1e6:
                 return None
             desired = np.zeros(6)
             desired[direction] = 1.0
             q_dot = np.linalg.pinv(J) @ desired
-            if np.linalg.norm(J[3:, :] @ q_dot) > 0.01:
-                return None
+            if direction < 3:
+                if np.linalg.norm(J[3:, :] @ q_dot) > 0.01:
+                    return None
+            else:
+                if np.linalg.norm(J[:3, :] @ q_dot) > 0.01:
+                    return None
             return float(np.min(v_max_joint / (np.abs(q_dot) + 1e-10)))
         except (np.linalg.LinAlgError, ValueError):
             return None
 
-    def _sample_limit(n_samples: int, seed: int, v_max: NDArray) -> tuple[float, float]:
-        """Sample workspace and return (median_linear_m, mean_angular_rad)."""
+    _home_rad = np.deg2rad(_standby_deg)
+
+    def _sample_limit(
+        n_samples: int, seed: int, v_max: NDArray, spread_deg: float = 30.0
+    ) -> tuple[float, float]:
+        """Sample around home position and return (median_linear_m, median_angular_rad).
+
+        Samples joint configurations from a Gaussian centered on home with
+        std dev of ``spread_deg`` degrees, clamped to joint limits.
+        """
         rng = np.random.default_rng(seed)
-        results = []
+        spread_rad = np.deg2rad(spread_deg)
+        lin_results = []
+        ang_results = []
         for _ in range(n_samples):
-            q = np.array([rng.uniform(lo, hi) for lo, hi in _joint_limits_radian])
+            q = _home_rad + rng.normal(0, spread_rad, size=6)
+            q = np.clip(q, _joint_limits_radian[:, 0], _joint_limits_radian[:, 1])
             for d in range(3):
                 v = _compute_tcp_velocity_at_config(q, d, v_max)
                 if v is not None and v > 0.001:
-                    results.append(v)
-        linear = float(np.median(results)) if results else 0.1
-        angular = float(np.mean(v_max[3:6]))
+                    lin_results.append(v)
+            for d in range(3, 6):
+                v = _compute_tcp_velocity_at_config(q, d, v_max)
+                if v is not None and v > 0.001:
+                    ang_results.append(v)
+        linear = float(np.median(lin_results)) if lin_results else 0.1
+        angular = float(np.median(ang_results)) if ang_results else 0.1
         return linear, angular
 
     vel_lin, vel_ang = _sample_limit(500, 42, _joint_speed_rad)

parol6/ack_policy.py

@@ -39,6 +39,7 @@
     CmdType.SERVOL,
     CmdType.JOGJ,
     CmdType.JOGL,
+    CmdType.TELEPORT,
     CmdType.RESET_LOOP_STATS,
 }
 

parol6/client/async_client.py

@@ -77,6 +77,7 @@
     SetProfileCmd,
     SetToolCmd,
     SimulatorCmd,
+    TeleportCmd,
     SpeedsResultStruct,
     StatusBuffer,
     StatusResultStruct,
@@ -274,6 +275,7 @@ def __init__(
 
         # Active tool key (set by set_tool)
         self._active_tool_key: str | None = None
+        self._active_variant_key: str = ""
 
         # Bound tool specs (populated by Robot.create_async_client)
         self._bound_tools: dict[str, ToolSpec] = {}
@@ -500,23 +502,23 @@ async def _send(self, cmd: msgspec.Struct) -> int:
 
         Returns:
             int (command index ≥ 0) for ACK'd queued commands, -1 on failure,
-            bool for system/fire-and-forget/query commands.
+            1 for system/fire-and-forget/query success, 0 on failure.
         """
         await self._ensure_endpoint()
         assert self._transport is not None
 
         # Get command type from struct's tag
         cmd_type = STRUCT_TO_CMDTYPE.get(type(cmd))
         if cmd_type is None:
-            return False
+            return 0
 
         # System commands: need stable bytes across await
         if cmd_type in SYSTEM_CMD_TYPES:
             try:
                 await self._request_ok_raw(encode_command(cmd), self.timeout)
-                return True
+                return 1
             except TimeoutError:
-                return False
+                return 0
 
         # Motion and other non-query commands
         if cmd_type not in QUERY_CMD_TYPES:
@@ -530,12 +532,12 @@ async def _send(self, cmd: msgspec.Struct) -> int:
             # Fire-and-forget: reuse pre-allocated buffer (sendto copies)
             encode_command_into(cmd, self._tx_buf)
             self._transport.sendto(self._tx_buf)
-            return True
+            return 1
 
         # Queries via _send: fire-and-forget
         encode_command_into(cmd, self._tx_buf)
         self._transport.sendto(self._tx_buf)
-        return True
+        return 1
 
     async def _request(self, cmd: msgspec.Struct) -> Response | None:
         """Send a query command and wait for a typed response.
@@ -628,7 +630,7 @@ async def _request_ok_raw(self, data: bytes, timeout: float) -> OkMsg:
     # --------------- Motion / Control ---------------
 
     async def home(
-        self, wait: bool = False, timeout: float = 10.0, **wait_kwargs: Any
+        self, wait: bool = False, timeout: float = 60.0, **wait_kwargs: Any
     ) -> int:
         """Home the robot to its home position.
 
@@ -646,7 +648,9 @@ async def home(
         index = await self._send(HomeCmd())
         assert isinstance(index, int)
         if wait and index >= 0:
-            await self.wait_command_complete(index, timeout=timeout)
+            ok = await self.wait_command_complete(index, timeout=timeout)
+            if not ok:
+                raise TimeoutError(f"home() timed out after {timeout}s")
         return index
 
     async def resume(self) -> int:
@@ -658,7 +662,7 @@ async def resume(self) -> int:
             rbt.resume()
 
         Returns:
-            True if the command was acknowledged successfully.
+            1 if acknowledged, 0 on failure.
         """
         return await self._send(ResumeCmd())
 
@@ -671,7 +675,7 @@ async def halt(self) -> int:
             rbt.halt()
 
         Returns:
-            True if the command was acknowledged successfully.
+            1 if acknowledged, 0 on failure.
         """
         return await self._send(HaltCmd())
 
@@ -687,7 +691,7 @@ async def simulator_on(self) -> int:
             rbt.simulator_on()
 
         Returns:
-            True if the command was acknowledged successfully.
+            1 if acknowledged, 0 on failure.
         """
         return await self._send(SimulatorCmd(on=True))
 
@@ -700,10 +704,27 @@ async def simulator_off(self) -> int:
             rbt.simulator_off()
 
         Returns:
-            True if the command was acknowledged successfully.
+            1 if acknowledged, 0 on failure.
         """
         return await self._send(SimulatorCmd(on=False))
 
+    async def teleport(
+        self,
+        angles_deg: list[float],
+        tool_positions: list[float] | None = None,
+    ) -> int:
+        """Instantly set joint angles and optional tool positions (simulator only).
+
+        Category: Control
+
+        Example:
+            rbt.teleport([0, -90, 0, 0, 0, 0])
+            rbt.teleport([0, -90, 0, 0, 0, 0], tool_positions=[1.0])
+        """
+        return await self._send(
+            TeleportCmd(angles=angles_deg, tool_positions=tool_positions)
+        )
+
     async def set_serial_port(self, port_str: str) -> int:
         """Set the serial port for robot hardware communication.
 
@@ -716,7 +737,7 @@ async def set_serial_port(self, port_str: str) -> int:
             port_str: Serial port path (e.g., '/dev/ttyUSB0' or 'COM3').
 
         Returns:
-            True if the command was acknowledged successfully.
+            1 if acknowledged, 0 on failure.
 
         Raises:
             ValueError: If port_str is empty.
@@ -828,7 +849,7 @@ async def reset_loop_stats(self) -> int:
         """
         return await self._send(ResetLoopStatsCmd())
 
-    async def set_tool(self, tool_name: str) -> int:
+    async def set_tool(self, tool_name: str, variant_key: str = "") -> int:
         """Set the current end-effector tool configuration.
 
         Category: Configuration
@@ -838,12 +859,16 @@ async def set_tool(self, tool_name: str) -> int:
 
         Args:
             tool_name: Name of the tool ('NONE', 'PNEUMATIC', 'SSG-48', 'MSG', 'VACUUM')
+            variant_key: Optional variant within the tool type.
 
         Returns:
-            True if successful
+            Command index (>= 0) if queued, 0 on failure.
         """
         self._active_tool_key = tool_name.upper()
-        return await self._send(SetToolCmd(tool_name=self._active_tool_key))
+        self._active_variant_key = variant_key
+        return await self._send(
+            SetToolCmd(tool_name=self._active_tool_key, variant_key=variant_key)
+        )
 
     async def set_profile(self, profile: str) -> int:
         """Set the motion profile for all moves.