From 19227104bd4dbdac24a32b0c54377891d031106b Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Thu, 16 Apr 2026 16:51:39 -0400 Subject: [PATCH 01/15] fix: set command_index and clear error state on tool-action failures MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Tool actions were always throwing timeout errors because the error state from a previous command wasn't cleared before dispatching a ToolActionCmd, and tool-command failures didn't record the command_index — so clients couldn't correlate the error with the offending command. --- parol6/server/controller.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 899b75e..44d6a5d 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -10,7 +10,7 @@ import sys import threading import time -from dataclasses import dataclass +from dataclasses import dataclass, replace from typing import Any @@ -407,9 +407,10 @@ def _tick_tool_cmd(self, state: ControllerState) -> None: type(self._tool_cmd).__name__, self._tool_cmd.robot_error, ) - state.error = self._tool_cmd.robot_error or make_error( + raw_error = self._tool_cmd.robot_error or make_error( ErrorCode.MOTN_TICK_FAILED, detail=type(self._tool_cmd).__name__ ) + state.error = replace(raw_error, command_index=self._tool_cmd_index) state.action_state = ActionState.ERROR state.completed_command_index = max( state.completed_command_index, self._tool_cmd_index @@ -696,6 +697,11 @@ def _handle_motion_command( # Tool actions bypass planner — execute directly via side channel # (writes to gripper_hw, not Position_out, so concurrent with everything) if isinstance(command.p, ToolActionCmd): + # Clear error state from previous failure (same as non-streaming path) + if state.error is not None: + state.error = None + state.action_state = ActionState.IDLE + cmd_obj, _, error_msg = create_command_from_struct(command.p) if cmd_obj is None: logger.error("Failed to create tool command: %s", error_msg) From b1fe085846184ea080c6e3449f0f58e85b991043 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Thu, 16 Apr 2026 16:51:48 -0400 Subject: [PATCH 02/15] examples: fix set_tcp_offset axis for pencil offset MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit SSG-48 tool transform is along Z, but the precision demos were calling set_tcp_offset(-100, 0, 0) — shifting the TCP sideways instead of down the pencil axis. Result: TRF rotations pivoted near the gripper rather than the pencil tip. Use (0, 0, -100) to match the tool frame. --- examples/demo_showcase.py | 2 +- examples/precision.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/demo_showcase.py b/examples/demo_showcase.py index 9d6d763..547e2c4 100644 --- a/examples/demo_showcase.py +++ b/examples/demo_showcase.py @@ -121,7 +121,7 @@ def circle_pt(cx, cz, angle_deg): rbt.move_j(pose=PRECISION_POSE, speed=0.3, wait=True) # Offset TCP to pencil tip (~100mm exposed below gripper) - rbt.set_tcp_offset(-100, 0, 0) + rbt.set_tcp_offset(0, 0, -100) # Pencil tip traces straight lines (linear precision demo) # Forward/back (tool Z = world -Y at this pose) diff --git a/examples/precision.py b/examples/precision.py index 836845f..f52af15 100644 --- a/examples/precision.py +++ b/examples/precision.py @@ -40,7 +40,7 @@ rbt.move_j(pose=PRECISION_POSE, speed=0.3, wait=True) # Offset TCP to pencil tip (~100mm exposed below gripper) - rbt.set_tcp_offset(-100, 0, 0) + rbt.set_tcp_offset(0, 0, -100) # Pencil tip traces straight lines (linear precision demo) # Forward/back (tool Z = world -Y at this pose) From baeb52adb5203d6ee8c88b44e777e4a04e2eb670 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Thu, 16 Apr 2026 16:51:54 -0400 Subject: [PATCH 03/15] bump version to 0.2.3 --- pyproject.toml | 272 ++++++++++++++++++++++++------------------------- 1 file changed, 136 insertions(+), 136 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index dd20435..cbabb52 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,136 +1,136 @@ -[build-system] -requires = ["setuptools>=61.0"] -build-backend = "setuptools.build_meta" - -[project] -name = "parol6" -version = "0.2.2" -description = "Python library for controlling PAROL6 robot arms" - -requires-python = ">=3.11" -dependencies = [ - # pinokin: Pinocchio-based FK/IK bindings - # https://github.com/Jepson2k/pinokin/releases/tag/v0.1.5 - - # macOS ARM64 (Apple Silicon) - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp311-cp311-macosx_15_0_arm64.whl ; python_version == '3.11' and platform_system == 'Darwin' and platform_machine == 'arm64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp312-cp312-macosx_15_0_arm64.whl ; python_version == '3.12' and platform_system == 'Darwin' and platform_machine == 'arm64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp313-cp313-macosx_15_0_arm64.whl ; python_version == '3.13' and platform_system == 'Darwin' and platform_machine == 'arm64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp314-cp314-macosx_15_0_arm64.whl ; python_version == '3.14' and platform_system == 'Darwin' and platform_machine == 'arm64'", - - # Windows AMD64 - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp311-cp311-win_amd64.whl ; python_version == '3.11' and platform_system == 'Windows' and platform_machine == 'AMD64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp312-cp312-win_amd64.whl ; python_version == '3.12' and platform_system == 'Windows' and platform_machine == 'AMD64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp313-cp313-win_amd64.whl ; python_version == '3.13' and platform_system == 'Windows' and platform_machine == 'AMD64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp314-cp314-win_amd64.whl ; python_version == '3.14' and platform_system == 'Windows' and platform_machine == 'AMD64'", - - # Linux x86_64 - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp311-cp311-manylinux_2_39_x86_64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'x86_64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp312-cp312-manylinux_2_39_x86_64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'x86_64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp313-cp313-manylinux_2_39_x86_64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'x86_64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp314-cp314-manylinux_2_39_x86_64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'x86_64'", - - # Linux aarch64 (ARM64) - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp311-cp311-manylinux_2_39_aarch64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'aarch64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp312-cp312-manylinux_2_39_aarch64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'aarch64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp313-cp313-manylinux_2_39_aarch64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'aarch64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp314-cp314-manylinux_2_39_aarch64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'aarch64'", - - "pyserial>=3.4", - "scipy>=1.11.4", - "ruckig>=0.12.2", - "toppra>=0.6.3", - "interpolatepy>=2.0.0", - "numpy>=2.0", - "numba>=0.59", - "psutil>=5.9", - "msgspec>=0.18", - "ormsgpack>=1.4.0", - "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.2.0", -] - -[tool.setuptools.packages.find] -include = ["parol6*"] - -[project.optional-dependencies] -dev = [ - "ruff", - "ty", - "pytest", - "pytest-asyncio", - "pytest-timeout", - "pre-commit", - "trimesh", - "fast-simplification", - "rtree", - "scipy-stubs", - "types-pyserial", -] - -[project.scripts] -parol6-server = "parol6.cli.server:main_entry" - -[project.entry-points."waldoctl.robots"] -parol6 = "parol6.robot:Robot" - -[tool.pytest.ini_options] -testpaths = ["tests"] -python_files = ["test_*.py"] -python_classes = ["Test*"] -python_functions = ["test_*"] -addopts = [ - "--strict-markers", - "--strict-config", - "-ra", - "--junitxml=test-results.xml", -] -timeout = 45 -timeout_method = "thread" -markers = [ - "unit: Unit tests that test individual components in isolation", - "integration: Integration tests that test component interactions with FAKE_SERIAL", - "e2e: End-to-end tests that exercise complete workflows", - "examples: Standalone example scripts (binds port 5001, run with `pytest -m examples`)", -] -log_cli = false -log_level = "DEBUG" -filterwarnings = [ - "ignore::DeprecationWarning", - "ignore::PendingDeprecationWarning", -] - -[tool.ty] -# Dynamic Command union (built at import time from tagged structs) -# generates invalid-type-form in all files that reference it. -[[tool.ty.overrides]] -include = [ - "parol6/protocol/wire.py", - "parol6/commands/base.py", - "parol6/server/command_registry.py", - "parol6/server/command_executor.py", -] -[tool.ty.overrides.rules] -invalid-type-form = "ignore" - -[tool.setuptools] -include-package-data = true - -[tool.setuptools.package-data] -parol6 = ["urdf_model/**"] - -# Release tooling: `tbump ` edits pyproject.toml, commits, tags -# and pushes (main + tag) in one shot. Install with `pip install tbump`. -[tool.tbump] -github_url = "https://github.com/Jepson2k/PAROL6-python-API" - -[tool.tbump.version] -current = "0.2.2" -regex = '''(?P\d+)\.(?P\d+)\.(?P\d+)''' - -[tool.tbump.git] -message_template = "bump version to {new_version}" -tag_template = "v{new_version}" - -[[tool.tbump.file]] -src = "pyproject.toml" -search = 'version = "{current_version}"' +[build-system] +requires = ["setuptools>=61.0"] +build-backend = "setuptools.build_meta" + +[project] +name = "parol6" +version = "0.2.3" +description = "Python library for controlling PAROL6 robot arms" + +requires-python = ">=3.11" +dependencies = [ + # pinokin: Pinocchio-based FK/IK bindings + # https://github.com/Jepson2k/pinokin/releases/tag/v0.1.5 + + # macOS ARM64 (Apple Silicon) + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp311-cp311-macosx_15_0_arm64.whl ; python_version == '3.11' and platform_system == 'Darwin' and platform_machine == 'arm64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp312-cp312-macosx_15_0_arm64.whl ; python_version == '3.12' and platform_system == 'Darwin' and platform_machine == 'arm64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp313-cp313-macosx_15_0_arm64.whl ; python_version == '3.13' and platform_system == 'Darwin' and platform_machine == 'arm64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp314-cp314-macosx_15_0_arm64.whl ; python_version == '3.14' and platform_system == 'Darwin' and platform_machine == 'arm64'", + + # Windows AMD64 + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp311-cp311-win_amd64.whl ; python_version == '3.11' and platform_system == 'Windows' and platform_machine == 'AMD64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp312-cp312-win_amd64.whl ; python_version == '3.12' and platform_system == 'Windows' and platform_machine == 'AMD64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp313-cp313-win_amd64.whl ; python_version == '3.13' and platform_system == 'Windows' and platform_machine == 'AMD64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp314-cp314-win_amd64.whl ; python_version == '3.14' and platform_system == 'Windows' and platform_machine == 'AMD64'", + + # Linux x86_64 + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp311-cp311-manylinux_2_39_x86_64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'x86_64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp312-cp312-manylinux_2_39_x86_64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'x86_64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp313-cp313-manylinux_2_39_x86_64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'x86_64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp314-cp314-manylinux_2_39_x86_64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'x86_64'", + + # Linux aarch64 (ARM64) + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp311-cp311-manylinux_2_39_aarch64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'aarch64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp312-cp312-manylinux_2_39_aarch64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'aarch64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp313-cp313-manylinux_2_39_aarch64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'aarch64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp314-cp314-manylinux_2_39_aarch64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'aarch64'", + + "pyserial>=3.4", + "scipy>=1.11.4", + "ruckig>=0.12.2", + "toppra>=0.6.3", + "interpolatepy>=2.0.0", + "numpy>=2.0", + "numba>=0.59", + "psutil>=5.9", + "msgspec>=0.18", + "ormsgpack>=1.4.0", + "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.2.0", +] + +[tool.setuptools.packages.find] +include = ["parol6*"] + +[project.optional-dependencies] +dev = [ + "ruff", + "ty", + "pytest", + "pytest-asyncio", + "pytest-timeout", + "pre-commit", + "trimesh", + "fast-simplification", + "rtree", + "scipy-stubs", + "types-pyserial", +] + +[project.scripts] +parol6-server = "parol6.cli.server:main_entry" + +[project.entry-points."waldoctl.robots"] +parol6 = "parol6.robot:Robot" + +[tool.pytest.ini_options] +testpaths = ["tests"] +python_files = ["test_*.py"] +python_classes = ["Test*"] +python_functions = ["test_*"] +addopts = [ + "--strict-markers", + "--strict-config", + "-ra", + "--junitxml=test-results.xml", +] +timeout = 45 +timeout_method = "thread" +markers = [ + "unit: Unit tests that test individual components in isolation", + "integration: Integration tests that test component interactions with FAKE_SERIAL", + "e2e: End-to-end tests that exercise complete workflows", + "examples: Standalone example scripts (binds port 5001, run with `pytest -m examples`)", +] +log_cli = false +log_level = "DEBUG" +filterwarnings = [ + "ignore::DeprecationWarning", + "ignore::PendingDeprecationWarning", +] + +[tool.ty] +# Dynamic Command union (built at import time from tagged structs) +# generates invalid-type-form in all files that reference it. +[[tool.ty.overrides]] +include = [ + "parol6/protocol/wire.py", + "parol6/commands/base.py", + "parol6/server/command_registry.py", + "parol6/server/command_executor.py", +] +[tool.ty.overrides.rules] +invalid-type-form = "ignore" + +[tool.setuptools] +include-package-data = true + +[tool.setuptools.package-data] +parol6 = ["urdf_model/**"] + +# Release tooling: `tbump ` edits pyproject.toml, commits, tags +# and pushes (main + tag) in one shot. Install with `pip install tbump`. +[tool.tbump] +github_url = "https://github.com/Jepson2k/PAROL6-python-API" + +[tool.tbump.version] +current = "0.2.3" +regex = '''(?P\d+)\.(?P\d+)\.(?P\d+)''' + +[tool.tbump.git] +message_template = "bump version to {new_version}" +tag_template = "v{new_version}" + +[[tool.tbump.file]] +src = "pyproject.toml" +search = 'version = "{current_version}"' From 59058f19c4ddc6109a2cb136c88b5d8efa86e76c Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Thu, 16 Apr 2026 19:07:03 -0400 Subject: [PATCH 04/15] fix: complete electric gripper command on stall when grasping objects The existing object-detection path requires firmware to set the detection bit for 5 consecutive ticks. In practice the bit is too flaky for the debouncer to latch when grasping a thin object, so tool.close() would run out the 10s timeout instead of returning done as soon as the gripper stops moving. Adds a position-stall backstop: after a 100ms grace period, if feedback_position stays within 2/255 for 200ms while move_active is set, the gripper is considered blocked and the command finishes cleanly. Object-detection fast-path is unchanged. --- parol6/commands/gripper_commands.py | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/parol6/commands/gripper_commands.py b/parol6/commands/gripper_commands.py index 8c04193..dc37eba 100644 --- a/parol6/commands/gripper_commands.py +++ b/parol6/commands/gripper_commands.py @@ -87,6 +87,15 @@ class ElectricGripperCommand(MotionCommand[ElectricGripperParams]): PARAMS_TYPE = None # Not wire-registered — instantiated by ToolActionCommand + # Stall detection: when feedback_position stays within ``_STALL_DEAD_BAND`` + # of a baseline for ``_STALL_TICKS`` consecutive ticks past the grace + # period, the gripper is considered blocked (grasping an object) and the + # command completes. Backstop for cases where the firmware's + # object-detection bit is too flaky for the debouncer to latch. + _STALL_TICKS: int = 20 # 200 ms at 100 Hz + _STALL_DEAD_BAND: int = 2 # 2/255 ≈ 0.8% of full range + _GRACE_TICKS: int = 10 # 100 ms before stall checks begin + __slots__ = ( "state", "timeout_counter", @@ -94,6 +103,9 @@ class ElectricGripperCommand(MotionCommand[ElectricGripperParams]): "wait_counter", "_hw_position", "_hw_speed", + "_stall_baseline", + "_stall_remaining", + "_grace_remaining", ) def __init__(self, p: ElectricGripperParams): @@ -104,6 +116,9 @@ def __init__(self, p: ElectricGripperParams): self.wait_counter = 0 self._hw_position = 0 self._hw_speed = 1 + self._stall_baseline = 0 + self._stall_remaining = self._STALL_TICKS + self._grace_remaining = self._GRACE_TICKS @classmethod def from_tool_action( @@ -183,6 +198,19 @@ def execute_step(self, state: ControllerState) -> ExecutionStatusCode: self.finish() return ExecutionStatusCode.COMPLETED + if self._grace_remaining > 0: + self._grace_remaining -= 1 + self._stall_baseline = current_position + elif abs(current_position - self._stall_baseline) > self._STALL_DEAD_BAND: + self._stall_baseline = current_position + self._stall_remaining = self._STALL_TICKS + else: + self._stall_remaining -= 1 + if self._stall_remaining <= 0: + self.finish() + hw.set_command_bits(move_active=False, estop=estop) + return ExecutionStatusCode.COMPLETED + return ExecutionStatusCode.EXECUTING self.fail(make_error(ErrorCode.MOTN_GRIPPER_UNKNOWN)) From d094a6179beed6d619038d277870855fb4825e69 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Thu, 16 Apr 2026 19:07:40 -0400 Subject: [PATCH 05/15] bump version to 0.2.4 --- pyproject.toml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index cbabb52..2ca7f2f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "parol6" -version = "0.2.3" +version = "0.2.4" description = "Python library for controlling PAROL6 robot arms" requires-python = ">=3.11" @@ -124,7 +124,7 @@ parol6 = ["urdf_model/**"] github_url = "https://github.com/Jepson2k/PAROL6-python-API" [tool.tbump.version] -current = "0.2.3" +current = "0.2.4" regex = '''(?P\d+)\.(?P\d+)\.(?P\d+)''' [tool.tbump.git] From 503821ba22a817f2294defc2c7ea0f1709d72163 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Thu, 16 Apr 2026 19:24:37 -0400 Subject: [PATCH 06/15] fix: use no-progress stall detection to handle PID hunting on grasp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The previous baseline-position check could never fire when the gripper hunted around its stall point — any oscillation > 2 units kept resetting the baseline and the counter, so close() still timed out on real grasps. Switch to tracking the smallest distance-to-target seen so far. If distance hasn't improved by more than the dead-band for the stall window, declare stall. Oscillation around a stalled point doesn't beat the best distance, so the counter drains correctly. --- parol6/commands/gripper_commands.py | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/parol6/commands/gripper_commands.py b/parol6/commands/gripper_commands.py index dc37eba..3da58dc 100644 --- a/parol6/commands/gripper_commands.py +++ b/parol6/commands/gripper_commands.py @@ -87,13 +87,16 @@ class ElectricGripperCommand(MotionCommand[ElectricGripperParams]): PARAMS_TYPE = None # Not wire-registered — instantiated by ToolActionCommand - # Stall detection: when feedback_position stays within ``_STALL_DEAD_BAND`` - # of a baseline for ``_STALL_TICKS`` consecutive ticks past the grace - # period, the gripper is considered blocked (grasping an object) and the - # command completes. Backstop for cases where the firmware's - # object-detection bit is too flaky for the debouncer to latch. + # Stall detection: track the smallest distance-to-target seen so far and + # stall when no forward progress is made for ``_STALL_TICKS`` consecutive + # ticks past the grace period. "Forward progress" means closing the gap + # to target by at least ``_STALL_DEAD_BAND`` units below the best seen. + # This is robust to PID hunting around the stall point — oscillation + # never beats the best, so the counter keeps draining. Backstop for + # cases where the firmware's object-detection bit is too flaky for the + # debouncer to latch. _STALL_TICKS: int = 20 # 200 ms at 100 Hz - _STALL_DEAD_BAND: int = 2 # 2/255 ≈ 0.8% of full range + _STALL_DEAD_BAND: int = 2 # progress threshold, 2/255 ≈ 0.8% of full range _GRACE_TICKS: int = 10 # 100 ms before stall checks begin __slots__ = ( @@ -103,7 +106,7 @@ class ElectricGripperCommand(MotionCommand[ElectricGripperParams]): "wait_counter", "_hw_position", "_hw_speed", - "_stall_baseline", + "_stall_best_distance", "_stall_remaining", "_grace_remaining", ) @@ -116,7 +119,7 @@ def __init__(self, p: ElectricGripperParams): self.wait_counter = 0 self._hw_position = 0 self._hw_speed = 1 - self._stall_baseline = 0 + self._stall_best_distance = 256 # larger than any possible distance self._stall_remaining = self._STALL_TICKS self._grace_remaining = self._GRACE_TICKS @@ -198,11 +201,12 @@ def execute_step(self, state: ControllerState) -> ExecutionStatusCode: self.finish() return ExecutionStatusCode.COMPLETED + distance = abs(current_position - self._hw_position) if self._grace_remaining > 0: self._grace_remaining -= 1 - self._stall_baseline = current_position - elif abs(current_position - self._stall_baseline) > self._STALL_DEAD_BAND: - self._stall_baseline = current_position + self._stall_best_distance = distance + elif distance + self._STALL_DEAD_BAND <= self._stall_best_distance: + self._stall_best_distance = distance self._stall_remaining = self._STALL_TICKS else: self._stall_remaining -= 1 From 75b3df0cfd2a8bf5884e1010a80c04b5cd054c31 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Fri, 17 Apr 2026 06:05:38 -0400 Subject: [PATCH 07/15] fix: keep move_active set when gripper completes via stall detection MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit When the no-progress path fired it was clearing move_active, which tells the firmware to release motor force — so grasped items slipped out after the command reported done. Match the object-detection completion path: leave move_active on so the firmware keeps applying force on the object. Also add a regression test that runs the stall-completion path with a held feedback_position and asserts the MOVE_ACTIVE bit is still set. --- parol6/commands/gripper_commands.py | 4 ++- tests/unit/test_gripper_ramp.py | 46 +++++++++++++++++++++++++++++ 2 files changed, 49 insertions(+), 1 deletion(-) diff --git a/parol6/commands/gripper_commands.py b/parol6/commands/gripper_commands.py index 3da58dc..8115d67 100644 --- a/parol6/commands/gripper_commands.py +++ b/parol6/commands/gripper_commands.py @@ -211,8 +211,10 @@ def execute_step(self, state: ControllerState) -> ExecutionStatusCode: else: self._stall_remaining -= 1 if self._stall_remaining <= 0: + # Leave move_active set so the firmware keeps applying + # motor force on the grasped object — clearing it would + # release the grip and the item would slip out. self.finish() - hw.set_command_bits(move_active=False, estop=estop) return ExecutionStatusCode.COMPLETED return ExecutionStatusCode.EXECUTING diff --git a/tests/unit/test_gripper_ramp.py b/tests/unit/test_gripper_ramp.py index f197a0e..86444a5 100644 --- a/tests/unit/test_gripper_ramp.py +++ b/tests/unit/test_gripper_ramp.py @@ -198,3 +198,49 @@ def test_msg_100mm_tick_range_derivation(self): travel_mm = motion.travel_m * 1000.0 tick_range = (travel_mm / (math.pi * cfg.gear_pd_mm)) * cfg.encoder_cpr assert tick_range == pytest.approx(8_353, rel=0.02) + + +class TestElectricGripperStallCompletion: + """Regression: stall detection must complete the command without releasing + the grip (move_active stays set), so the firmware keeps applying motor + force on the grasped object and the item doesn't slip out. + """ + + def test_stall_completes_and_keeps_grip_pressure(self): + from parol6.commands.base import ExecutionStatusCode + from parol6.commands.gripper_commands import ( + ElectricGripperCommand, + ElectricGripperParams, + ) + from parol6.server.state import ControllerState + + state = ControllerState() + cmd = ElectricGripperCommand( + ElectricGripperParams(action="move", position=1.0, speed=0.5, current=500) + ) + cmd.do_setup(state) + + # Hold feedback_position at a value far from target_position=255 so + # neither the position-match (≤5) nor object-detection paths can + # complete first — only stall detection should fire. + state.Gripper_data_in[1] = 100 + + # Budget well above grace (10) + stall window (20). + max_ticks = 60 + code = ExecutionStatusCode.EXECUTING + for _ in range(max_ticks): + code = cmd.execute_step(state) + if code == ExecutionStatusCode.COMPLETED: + break + + assert code == ExecutionStatusCode.COMPLETED, ( + "stall detection failed to complete the command within " + f"{max_ticks} ticks while feedback_position was held at 100" + ) + + # The MOVE_ACTIVE bit (0x40) must remain set after stall completion + # so the firmware keeps motor force on the grasped object. + assert state.gripper_hw.command_bits & 0x40, ( + "move_active bit must stay set after stall completion to maintain " + f"grip pressure; got command_bits=0x{state.gripper_hw.command_bits:02x}" + ) From 06d1aec2950d7b7ce520227837d0c94893b2dd40 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Fri, 17 Apr 2026 06:05:50 -0400 Subject: [PATCH 08/15] fix: route SetTcpOffset through the planner subprocess MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit SetTcpOffsetCommand was a SystemCommand, which executes in the controller process and bypasses the planner subprocess. The planner's own robot model never learned about the offset, so subsequent trajectory IK ran against the stale TCP — TRF rotations pivoted at the flange instead of at the offset point. Switch to MotionCommand so the command flows through _planner.submit() like SelectTool, where _handle_inline already updates planner state and the planner's PAROL6_ROBOT.robot. The controller-side segment player still runs execute_step() after the planner emits the InlineSegment, so the controller's own state is updated too. Regression tests: structural check that SetTcpOffsetCommand is a MotionCommand (not SystemCommand), and behavioral check that TrajectoryPlanner.process() updates planner.state.tcp_offset_m. --- parol6/commands/system_commands.py | 12 ++++++--- tests/unit/test_tcp_offset.py | 39 +++++++++++++++++++++++++++++- 2 files changed, 47 insertions(+), 4 deletions(-) diff --git a/parol6/commands/system_commands.py b/parol6/commands/system_commands.py index 3e3c054..d2c98ef 100644 --- a/parol6/commands/system_commands.py +++ b/parol6/commands/system_commands.py @@ -11,7 +11,7 @@ import os from typing import TYPE_CHECKING -from parol6.commands.base import ExecutionStatusCode, SystemCommand +from parol6.commands.base import ExecutionStatusCode, MotionCommand, SystemCommand from parol6.config import save_com_port from parol6.protocol.wire import ( CmdType, @@ -180,8 +180,14 @@ def execute_step(self, state: ControllerState) -> ExecutionStatusCode: @register_command(CmdType.SET_TCP_OFFSET) -class SetTcpOffsetCommand(SystemCommand[SetTcpOffsetCmd]): - """Set the TCP offset in the tool's local frame.""" +class SetTcpOffsetCommand(MotionCommand[SetTcpOffsetCmd]): + """Set the TCP offset in the tool's local frame. + + Routed through the planner (like SELECT_TOOL) so the planner subprocess + updates its own robot model — otherwise subsequent trajectory IK would + compute against the old TCP and rotations would pivot around the flange + instead of the offset point. + """ PARAMS_TYPE = SetTcpOffsetCmd diff --git a/tests/unit/test_tcp_offset.py b/tests/unit/test_tcp_offset.py index c93e669..950ca1a 100644 --- a/tests/unit/test_tcp_offset.py +++ b/tests/unit/test_tcp_offset.py @@ -2,7 +2,7 @@ import numpy as np -from parol6.protocol.wire import SetTcpOffsetCmd, TcpOffsetResultStruct +from parol6.protocol.wire import SelectToolCmd, SetTcpOffsetCmd, TcpOffsetResultStruct # ── Wire round-trip ────────────────────────────────────────────────────── @@ -96,3 +96,40 @@ def test_dry_run_select_tool_resets_tcp_offset(): # Selecting a tool resets offset client.select_tool("SSG-48") assert client.tcp_offset() == [0.0, 0.0, 0.0] + + +# ── Planner routing (regression: SetTcpOffsetCmd must reach the planner) ── + + +def test_set_tcp_offset_command_is_motion_command(): + """SetTcpOffsetCommand must inherit from MotionCommand, not SystemCommand. + + SystemCommands execute in the controller process and bypass the planner + subprocess. If SetTcpOffsetCommand is a SystemCommand, the planner's + own robot model never sees the offset, so subsequent trajectory IK + runs against the stale TCP — TRF rotations end up pivoting around the + flange instead of the offset point. + """ + from parol6.commands.base import MotionCommand, SystemCommand + from parol6.commands.system_commands import SetTcpOffsetCommand + + assert issubclass(SetTcpOffsetCommand, MotionCommand) + assert not issubclass(SetTcpOffsetCommand, SystemCommand) + + +def test_planner_updates_state_on_set_tcp_offset(): + """TrajectoryPlanner.process(SetTcpOffsetCmd) must update planner state + so subsequent move_l calls use the new TCP for IK. + """ + import parol6.PAROL6_ROBOT as PAROL6_ROBOT + from parol6.server.motion_planner import TrajectoryPlanner + + planner = TrajectoryPlanner() + planner.process(SelectToolCmd(tool_name="SSG-48", variant_key="")) + assert planner.state.tcp_offset_m == (0.0, 0.0, 0.0) + + planner.process(SetTcpOffsetCmd(x=0.0, y=0.0, z=-190.0)) + assert planner.state.tcp_offset_m == (0.0, 0.0, -0.190) + + # Cleanup global robot state mutated by apply_tool + PAROL6_ROBOT.apply_tool("NONE") From eab9927fae471bf5de2bfa6697b61fe7e82ba0bd Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Fri, 17 Apr 2026 07:58:08 -0400 Subject: [PATCH 09/15] bump version to 0.2.5 --- pyproject.toml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 2ca7f2f..12ce22c 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "parol6" -version = "0.2.4" +version = "0.2.5" description = "Python library for controlling PAROL6 robot arms" requires-python = ">=3.11" @@ -124,7 +124,7 @@ parol6 = ["urdf_model/**"] github_url = "https://github.com/Jepson2k/PAROL6-python-API" [tool.tbump.version] -current = "0.2.4" +current = "0.2.5" regex = '''(?P\d+)\.(?P\d+)\.(?P\d+)''' [tool.tbump.git] From 70e7d309a144c14eafde7756a88eeb7658243257 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Sat, 25 Apr 2026 15:15:40 -0400 Subject: [PATCH 10/15] bump IK max_iter from 10 to 20 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Pinokin's wrist-flip restart strategy (introduced in pinokin fix-ik-wrist-flip-restart) only triggers after LM converges to a limits-violating q. With max_iter=10 some realistic seeds (e.g. the PRECISION_POSE call from end-of-zigzag in demo_showcase line 98) need ~11 LM iters to converge — they exited the inner loop without ever calling check_limits, so the wrist flip never applied. 20 gives enough margin for current PAROL6 IK seeds while keeping worst-case solve time well under 1 ms. Successful first-attempt convergence is unaffected (LM converges in <20 iters either way). --- parol6/utils/ik.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/parol6/utils/ik.py b/parol6/utils/ik.py index cbe5808..b856e23 100644 --- a/parol6/utils/ik.py +++ b/parol6/utils/ik.py @@ -86,7 +86,7 @@ def _ensure_cache(robot: Robot) -> None: damping=_Damping.Sugihara, tol=1e-12, lm_lambda=0.0, - max_iter=10, + max_iter=20, max_restarts=10, ) _cached_buffered_min = qlim[0, :] + IK_SAFETY_MARGINS_RAD[:, 0] From abc765f309e7d24e4a188841bc828807987db1f7 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Sat, 9 May 2026 16:40:35 -0400 Subject: [PATCH 11/15] deps: bump pinokin pin from v0.1.5 to v0.1.6 v0.1.6 ships the deterministic wrist-flip restart for limits-violating IK branches (Jepson2k/pinokin#1). Combined with this branch's max_iter bump to 20, the wrist-flip restart now actually fires for the realistic PAROL6 seeds it was designed to rescue. --- pyproject.toml | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 12ce22c..1a71886 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -10,31 +10,31 @@ description = "Python library for controlling PAROL6 robot arms" requires-python = ">=3.11" dependencies = [ # pinokin: Pinocchio-based FK/IK bindings - # https://github.com/Jepson2k/pinokin/releases/tag/v0.1.5 + # https://github.com/Jepson2k/pinokin/releases/tag/v0.1.6 # macOS ARM64 (Apple Silicon) - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp311-cp311-macosx_15_0_arm64.whl ; python_version == '3.11' and platform_system == 'Darwin' and platform_machine == 'arm64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp312-cp312-macosx_15_0_arm64.whl ; python_version == '3.12' and platform_system == 'Darwin' and platform_machine == 'arm64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp313-cp313-macosx_15_0_arm64.whl ; python_version == '3.13' and platform_system == 'Darwin' and platform_machine == 'arm64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp314-cp314-macosx_15_0_arm64.whl ; python_version == '3.14' and platform_system == 'Darwin' and platform_machine == 'arm64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp311-cp311-macosx_15_0_arm64.whl ; python_version == '3.11' and platform_system == 'Darwin' and platform_machine == 'arm64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp312-cp312-macosx_15_0_arm64.whl ; python_version == '3.12' and platform_system == 'Darwin' and platform_machine == 'arm64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp313-cp313-macosx_15_0_arm64.whl ; python_version == '3.13' and platform_system == 'Darwin' and platform_machine == 'arm64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp314-cp314-macosx_15_0_arm64.whl ; python_version == '3.14' and platform_system == 'Darwin' and platform_machine == 'arm64'", # Windows AMD64 - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp311-cp311-win_amd64.whl ; python_version == '3.11' and platform_system == 'Windows' and platform_machine == 'AMD64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp312-cp312-win_amd64.whl ; python_version == '3.12' and platform_system == 'Windows' and platform_machine == 'AMD64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp313-cp313-win_amd64.whl ; python_version == '3.13' and platform_system == 'Windows' and platform_machine == 'AMD64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp314-cp314-win_amd64.whl ; python_version == '3.14' and platform_system == 'Windows' and platform_machine == 'AMD64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp311-cp311-win_amd64.whl ; python_version == '3.11' and platform_system == 'Windows' and platform_machine == 'AMD64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp312-cp312-win_amd64.whl ; python_version == '3.12' and platform_system == 'Windows' and platform_machine == 'AMD64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp313-cp313-win_amd64.whl ; python_version == '3.13' and platform_system == 'Windows' and platform_machine == 'AMD64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp314-cp314-win_amd64.whl ; python_version == '3.14' and platform_system == 'Windows' and platform_machine == 'AMD64'", # Linux x86_64 - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp311-cp311-manylinux_2_39_x86_64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'x86_64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp312-cp312-manylinux_2_39_x86_64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'x86_64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp313-cp313-manylinux_2_39_x86_64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'x86_64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp314-cp314-manylinux_2_39_x86_64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'x86_64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp311-cp311-manylinux_2_39_x86_64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'x86_64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp312-cp312-manylinux_2_39_x86_64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'x86_64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp313-cp313-manylinux_2_39_x86_64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'x86_64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp314-cp314-manylinux_2_39_x86_64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'x86_64'", # Linux aarch64 (ARM64) - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp311-cp311-manylinux_2_39_aarch64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'aarch64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp312-cp312-manylinux_2_39_aarch64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'aarch64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp313-cp313-manylinux_2_39_aarch64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'aarch64'", - "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.5/pinokin-0.1.5-cp314-cp314-manylinux_2_39_aarch64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'aarch64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp311-cp311-manylinux_2_39_aarch64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'aarch64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp312-cp312-manylinux_2_39_aarch64.whl ; python_version == '3.12' and platform_system == 'Linux' and platform_machine == 'aarch64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp313-cp313-manylinux_2_39_aarch64.whl ; python_version == '3.13' and platform_system == 'Linux' and platform_machine == 'aarch64'", + "pinokin @ https://github.com/Jepson2k/pinokin/releases/download/v0.1.6/pinokin-0.1.6-cp314-cp314-manylinux_2_39_aarch64.whl ; python_version == '3.14' and platform_system == 'Linux' and platform_machine == 'aarch64'", "pyserial>=3.4", "scipy>=1.11.4", From a2d2073bc908ffad2d1980cd18dcd3a53cb7c0b7 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Sat, 9 May 2026 17:12:43 -0400 Subject: [PATCH 12/15] docs: drop git-commit and shell-style rules from CLAUDE.md Removes two CLAUDE.md sections: - The 'No emoji / no Generated by... / no co-author boilerplate' rule under Code Style - The 'Do NOT use compound cd commands' Shell Command Style section --- CLAUDE.md | 5 ----- 1 file changed, 5 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index 43282a3..815e6c3 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -132,7 +132,6 @@ 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**: 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 @@ -140,10 +139,6 @@ For streamable commands (`streamable = True`), `do_setup()` also runs at high fr - `np.atleast_1d()` or similar to guarantee array returns from numpy functions - Only use `# type: ignore` as a last resort for genuine type checker limitations (e.g., numpy's `ArrayLike` being too broad) -## Shell Command Style - -- **Do NOT use compound `cd` commands** like `cd foo && git status` or `cd foo && pytest`. Compound commands that cross directories require explicit user approval and slow things down. Instead, issue a standalone `cd foo` command first, then run the action as a separate command. The Bash tool's working directory persists between calls. - ## Testing Guidelines **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. From 209b6d81f6bfa6a3bfb046e51c7d4ac419171ef4 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Sun, 10 May 2026 20:47:26 -0400 Subject: [PATCH 13/15] Fix trajectory shake: smooth IK outliers, fix sim dt jitter, more IK samples MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Three independent fixes that together eliminate visible robot shake during the precision sweep section of the demo: 1. parol6/motion/trajectory.py: detect and linearly-interp IK-chain outlier runs from pinokin's LM branch hops at wrist singularities (J5 near 0). Both single-sample and multi-sample outliers occur; bookend/path-length ratio < 0.5 reliably distinguishes 'chain doubles back' (outlier) from legitimate fast joint motion (e.g. opening sweep from a singular pose). Padding spreads patched joint motion across more samples so TOPP-RA doesn't have to crank up local joint velocity. 2. parol6/server/transports/mock_serial_transport.py: pass cfg.INTERVAL_S as dt instead of wallclock dt. Wallclock dt inherits the host scheduler's jitter, causing the simulator to under-/over-advance Position_in around slow ticks — visible as ghost velocity spikes in recordings even when the commanded trajectory is bit-identical across runs. 3. parol6/config.py: bump PATH_SAMPLES default 50 -> 200. Denser cartesian sampling reduces per-sample joint-norm spread and lets the IK outlier detector be more selective. 4. examples/precision.py: synced to match demo_showcase.py's precision section (was out of date). The shake symptom was a real-position discontinuity of ~4 deg per 20ms status frame on J4/J6 during RX/RY/RZ wrist sweeps — caught by recording the multicast status while precision.py ran in WC and plotting velocity. Root cause was LM IK picking off-branch solutions at the singular pose; the smoother repairs the chain in joint space while preserving the J4+J6 invariant exactly (sub-mm FK error). --- examples/precision.py | 61 +++++++----- parol6/config.py | 7 +- parol6/motion/trajectory.py | 99 ++++++++++++++++++- .../transports/mock_serial_transport.py | 9 +- 4 files changed, 146 insertions(+), 30 deletions(-) diff --git a/examples/precision.py b/examples/precision.py index f52af15..91b9b83 100644 --- a/examples/precision.py +++ b/examples/precision.py @@ -13,14 +13,23 @@ HOST = "127.0.0.1" PORT = 5001 +HOME_ANGLES = [90.0, -90.0, 180.0, 0.0, 0.0, 180.0] +HOME_TOLERANCE_DEG = 2.0 + with Robot(host=HOST, port=PORT, normalize_logs=True) as robot: rbt = robot.create_sync_client(timeout=2.0) rbt.wait_ready(timeout=5.0) rbt.simulator(True) + # Select tool, and home only if not already near the home pose rbt.select_tool("SSG-48") rbt.tool.calibrate() - rbt.home(wait=True) + current = rbt.angles() + if ( + current is None + or max(abs(a - h) for a, h in zip(current, HOME_ANGLES)) > HOME_TOLERANCE_DEG + ): + rbt.home(wait=True) PRECISION_POSE = [0, -250, 350, -90, 0, -90] rbt.move_j(pose=PRECISION_POSE, speed=0.5, wait=True) @@ -33,44 +42,44 @@ # Approach pencil: move_j to 100mm above, descend linearly, grab, retract PENCIL_ABOVE = [-90, -81.6, 161.8, 0, -69.4, 180] - rbt.move_j(angles=PENCIL_ABOVE, speed=0.3, wait=True) - rbt.move_l([0, 0, -100, 0, 0, 0], rel=True, speed=0.2, wait=True) + rbt.move_j(angles=PENCIL_ABOVE, speed=0.8, wait=True) + rbt.move_l([0, 0, -93, 0, 0, 0], rel=True, speed=0.4, wait=True) rbt.tool.close(wait=True) - rbt.move_l([0, 0, 100, 0, 0, 0], rel=True, speed=0.2, wait=True) - rbt.move_j(pose=PRECISION_POSE, speed=0.3, wait=True) + rbt.move_l([0, 0, 93, 0, 0, 0], rel=True, speed=0.4, wait=True) + rbt.move_j(pose=PRECISION_POSE, speed=0.8, wait=True) - # Offset TCP to pencil tip (~100mm exposed below gripper) - rbt.set_tcp_offset(0, 0, -100) + # Offset TCP to pencil tip (~100mm exposed below gripper). The pencil is + # clamped perpendicular to the gripper's jaw-closing direction, hanging + # along tool -X — that's the axis the offset goes on, not Z. + rbt.set_tcp_offset(-100, 0, 0) # Pencil tip traces straight lines (linear precision demo) - # Forward/back (tool Z = world -Y at this pose) - rbt.move_l([0, 0, 100, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True) - rbt.move_l([0, 0, -200, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True) - rbt.move_l([0, 0, 100, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True) - # Side to side (tool Y = world -X at this pose) - rbt.move_l([0, 60, 0, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True) - rbt.move_l([0, -120, 0, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True) - rbt.move_l([0, 60, 0, 0, 0, 0], speed=0.3, frame="TRF", rel=True, wait=True) + rbt.move_l([0, 0, 100, 0, 0, 0], speed=0.8, frame="TRF", rel=True, wait=True) + rbt.move_l([0, 0, -200, 0, 0, 0], speed=0.8, frame="TRF", rel=True, wait=True) + rbt.move_l([0, 0, 100, 0, 0, 0], speed=0.8, frame="TRF", rel=True, wait=True) - # Precision TRF rotations — pencil tip stays stationary while wrist rotates - SWEEP = 20 + # Precision TRF rotations — pencil tip stays stationary while wrist rotates. + # 40° is the largest sweep that keeps every axis IK-reachable from this pose + # with the 100mm pencil offset. + SWEEP = 40 for axis in range(3): delta = [0, 0, 0, 0, 0, 0] delta[3 + axis] = -SWEEP - rbt.move_l(delta, speed=0.5, frame="TRF", rel=True, wait=True) + rbt.move_l(delta, speed=0.8, frame="TRF", rel=True, wait=True) delta[3 + axis] = SWEEP - rbt.move_l(delta, speed=0.5, frame="TRF", rel=True, wait=True) - rbt.move_l(delta, speed=0.5, frame="TRF", rel=True, wait=True) + rbt.move_l(delta, speed=0.8, frame="TRF", rel=True, wait=True) + rbt.move_l(delta, speed=0.8, frame="TRF", rel=True, wait=True) delta[3 + axis] = -SWEEP - rbt.move_l(delta, speed=0.5, frame="TRF", rel=True, wait=True) + rbt.move_l(delta, speed=0.8, frame="TRF", rel=True, wait=True) # Place pencil back: descend linearly, release, retract rbt.set_tcp_offset(0, 0, 0) - rbt.move_j(angles=PENCIL_ABOVE, speed=0.3, wait=True) - rbt.move_l([0, 0, -100, 0, 0, 0], rel=True, speed=0.2, wait=True) + rbt.move_j(angles=PENCIL_ABOVE, speed=0.8, wait=True) + rbt.move_l([0, 0, -93, 0, 0, 0], rel=True, speed=0.4, wait=True) rbt.tool.open(wait=True) - rbt.move_l([0, 0, 100, 0, 0, 0], rel=True, speed=0.2, wait=True) + rbt.move_l([0, 0, 93, 0, 0, 0], rel=True, speed=0.4, wait=True) - rbt.move_j(pose=PRECISION_POSE, speed=0.3, wait=True) - rbt.home(wait=True) + # Return to home position (joint move, not the full homing sequence) + rbt.move_j(pose=PRECISION_POSE, speed=0.8, wait=True) + rbt.move_j(angles=HOME_ANGLES, speed=0.8, wait=True) print("Done!") diff --git a/parol6/config.py b/parol6/config.py index 8c3ef16..476b9c6 100644 --- a/parol6/config.py +++ b/parol6/config.py @@ -48,8 +48,11 @@ def _trace(self, msg, *args, **kwargs): NEAR_MM_TOL_MM: float = 2.0 # Proximity threshold for considering positions "near" (mm) ENTRY_MM_TOL_MM: float = 5.0 # Entry trajectory threshold for smooth motion (mm) -# Trajectory path sampling (fixed samples for TOPP-RA input) -PATH_SAMPLES: int = int(os.getenv("PAROL6_PATH_SAMPLES", "50")) +# Trajectory path sampling (IK waypoints fed to TOPP-RA's piecewise-linear path). +# 200 halves per-tick acceleration jitter on the wrist relative to the old 50, +# converging by ~500. Higher values cost one extra IK solve per sample at +# trajectory build time. +PATH_SAMPLES: int = int(os.getenv("PAROL6_PATH_SAMPLES", "200")) # Centralized loop interval (seconds). INTERVAL_S: float = max(1e-6, 1.0 / max(CONTROL_RATE_HZ, 1.0)) diff --git a/parol6/motion/trajectory.py b/parol6/motion/trajectory.py index 95c0a20..62f1331 100644 --- a/parol6/motion/trajectory.py +++ b/parol6/motion/trajectory.py @@ -99,6 +99,101 @@ def from_string(cls, name: str) -> ProfileType: return cls.TOPPRA +# Threshold (radians) for detecting single-sample IK outliers in batch_ik +# output. At wrist singularities (J5 ≈ 0) the IK has a 1-parameter family of +# valid solutions; pinokin's LM picks one without continuity bias and can +# return a sample whose joints are several degrees off the natural chain +# even though FK matches. ~3° in joint norm catches the wrist-flip pattern +# (J4 ±X / J6 ∓X) without false-positiving on legitimate sharp turns. +_IK_OUTLIER_THRESHOLD_RAD: float = 0.0524 # ≈ 3° + +# Number of "good" samples on each side of an outlier run to also re-interp. +# Spreading the singular-region joint motion across more samples lowers +# the residual velocity pulse TOPPRA produces (the patched samples otherwise +# have ~15% steeper per-sample step than the surrounding chain). +_IK_OUTLIER_PADDING: int = 8 + +# Bookend / path-length ratio under which a run is treated as an LM outlier +# (chain doubles back on itself). A near-1.0 ratio means the chain is +# straight in joint space — legitimate fast motion, not an outlier. We've +# seen ratios ~0.03 for true LM outliers vs. 1.0 for the legitimate fast +# wrist motion at the start of sweeps from a singular pose. +_IK_OUTLIER_RATIO: float = 0.5 + + +def _smooth_singularity_outliers(positions: NDArray[np.float64]) -> int: + """In-place repair of IK-chain outlier runs from wrist-singularity branch hops. + + pinokin's LM IK has no continuity preference, so at wrist-singular poses + (J5 ≈ 0) it can pick an IK solution several degrees away from the natural + chain. Both single-sample and multi-sample outliers occur; both look like + a wrist flip in J4 / J6 since J4+J6 is the only invariant at the + singularity. + + Algorithm: + 1. Mark a sample "bad" if either adjacent step exceeds a joint-norm + threshold (catches the LM-picked off-branch samples). + 2. For each contiguous run of bad samples, also pull in some good + samples on either side ("padding") and linear-interp the whole + expanded window. Padding spreads the necessary joint motion across + more samples so TOPPRA's time profile doesn't have to crank up + joint velocity locally — eliminates the residual velocity pulse + that pure run-replacement leaves behind. + + The interp preserves the J4+J6 invariant exactly and gives sub-mm FK + error since both bookends lie on the cartesian path. + + Returns the number of samples patched. + """ + n = len(positions) + if n < 3: + return 0 + threshold = _IK_OUTLIER_THRESHOLD_RAD + pad = _IK_OUTLIER_PADDING + + # Per-step joint-norm magnitudes + diffs = np.linalg.norm(np.diff(positions, axis=0), axis=1) + + # Mark a sample bad if either adjacent step is over threshold. Endpoints + # always treated as good (they're the IK seed / final target). + bad = np.zeros(n, dtype=bool) + bad[1:-1] = (diffs[:-1] > threshold) | (diffs[1:] > threshold) + + n_patched = 0 + i = 1 + while i < n - 1: + if not bad[i]: + i += 1 + continue + # Find end of run + j = i + while j < n - 1 and bad[j]: + j += 1 + # Run is [i, j); bookended by good samples i-1 and j. + # Distinguish LM outlier ("out and back" jump) from legitimate fast + # joint motion: outliers have a small bookend distance vs. their + # intra-run path length, i.e. the chain doubles back on itself. + # Legitimate fast motion has bookend ≈ path length (ratio ≈ 1). + bookend_dq = float(np.linalg.norm(positions[j] - positions[i - 1])) + path_len = float(diffs[i - 1 : j].sum()) + if path_len > 0 and (bookend_dq / path_len) < _IK_OUTLIER_RATIO: + # Treat as LM outlier — replace the run + padding with linear + # interp between bookends. + lo = max(1, i - pad) + hi = min(n - 1, j + pad) + run_span = hi - (lo - 1) + delta = positions[hi] - positions[lo - 1] + for k in range(lo, hi): + alpha = (k - (lo - 1)) / run_span + positions[k] = positions[lo - 1] + alpha * delta + n_patched += 1 + i = hi + 1 + else: + # Legitimate fast joint motion — leave it alone. + i = j + 1 + return n_patched + + @dataclass class JointPath: """ @@ -190,7 +285,9 @@ def from_poses( ) if result.all_valid: - return cls(positions=result.joint_positions) + positions = np.asarray(result.joint_positions, dtype=np.float64) + _smooth_singularity_outliers(positions) + return cls(positions=positions) valid = np.array(result.valid, dtype=np.bool_) # Count consecutive valid from start diff --git a/parol6/server/transports/mock_serial_transport.py b/parol6/server/transports/mock_serial_transport.py index 4cb4c38..4434bd2 100644 --- a/parol6/server/transports/mock_serial_transport.py +++ b/parol6/server/transports/mock_serial_transport.py @@ -550,8 +550,15 @@ def tick_simulation( if not self._connected: return + # Use the controller's fixed control interval rather than wallclock + # dt so the simulator behaves like the firmware (which reads encoders + # on its own fixed-rate timer). Wallclock dt inherits the host's + # scheduling jitter, which causes the simulator to under- or + # over-advance Position_in around slow ticks — visible as ghost + # spikes in recorded velocities even though the commanded trajectory + # is bit-identical across runs. now = time.perf_counter() - dt = now - self._state.last_update + dt = cfg.INTERVAL_S self._state.last_update = now # Snap gripper position if teleport requested From ea4afc9627b0258a2b6132d307a5cf785854e3d5 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Wed, 13 May 2026 16:18:57 -0400 Subject: [PATCH 14/15] njit IK-outlier smoother, warmup hookup, regression tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The relative-threshold smoother (introduced in 209b6d8) was a pure-Python function with per-sample broadcasts that allocated on every patched index. Promote it to @njit(cache=True) with explicit scalar loops, leaving one np.empty(n-1) scratch for the step magnitudes and whatever np.median does internally — both bounded by path length, called once per plan. Add a synthetic-outlier warmup call so the JIT compile happens at server startup rather than first plan, and trim the docstring/constant comments to just the why. Cover the smoother with regression tests: smooth-path pass-through, single and multi-sample hops, start/end padding clamping, short-path/zero-motion no-ops, sub-threshold and at-threshold steps, plus a randomized fuzz that verifies endpoint invariance. Co-Authored-By: Claude Opus 4.7 (1M context) --- parol6/motion/trajectory.py | 121 +++++++++++++-------------------- parol6/utils/warmup.py | 9 +++ tests/unit/test_motion.py | 130 ++++++++++++++++++++++++++++++++++++ 3 files changed, 185 insertions(+), 75 deletions(-) diff --git a/parol6/motion/trajectory.py b/parol6/motion/trajectory.py index 62f1331..bbcddb0 100644 --- a/parol6/motion/trajectory.py +++ b/parol6/motion/trajectory.py @@ -18,6 +18,7 @@ from enum import Enum import numpy as np +from numba import njit from numpy.typing import NDArray from ruckig import InputParameter, OutputParameter, Result, Ruckig # type: ignore[unresolved-import, ty:unresolved-import] @@ -99,98 +100,68 @@ def from_string(cls, name: str) -> ProfileType: return cls.TOPPRA -# Threshold (radians) for detecting single-sample IK outliers in batch_ik -# output. At wrist singularities (J5 ≈ 0) the IK has a 1-parameter family of -# valid solutions; pinokin's LM picks one without continuity bias and can -# return a sample whose joints are several degrees off the natural chain -# even though FK matches. ~3° in joint norm catches the wrist-flip pattern -# (J4 ±X / J6 ∓X) without false-positiving on legitimate sharp turns. -_IK_OUTLIER_THRESHOLD_RAD: float = 0.0524 # ≈ 3° +# Step is anomalous if its magnitude exceeds this multiple of the chain's +# median step. Relative threshold keeps the rule invariant to sample density, +# speed, and move type. Insensitive in [10, 20+]; real LM hops exceed 20×. +_IK_OUTLIER_RATIO: float = 10.0 -# Number of "good" samples on each side of an outlier run to also re-interp. -# Spreading the singular-region joint motion across more samples lowers -# the residual velocity pulse TOPPRA produces (the patched samples otherwise -# have ~15% steeper per-sample step than the surrounding chain). -_IK_OUTLIER_PADDING: int = 8 - -# Bookend / path-length ratio under which a run is treated as an LM outlier -# (chain doubles back on itself). A near-1.0 ratio means the chain is -# straight in joint space — legitimate fast motion, not an outlier. We've -# seen ratios ~0.03 for true LM outliers vs. 1.0 for the legitimate fast -# wrist motion at the start of sweeps from a singular pose. -_IK_OUTLIER_RATIO: float = 0.5 +# Symmetric padding around each outlier run; absorbs LM seed-bleed into the +# samples just after the hop. FK deviation scales linearly with pad. +_IK_OUTLIER_PADDING: int = 4 +@njit(cache=True) def _smooth_singularity_outliers(positions: NDArray[np.float64]) -> int: - """In-place repair of IK-chain outlier runs from wrist-singularity branch hops. - - pinokin's LM IK has no continuity preference, so at wrist-singular poses - (J5 ≈ 0) it can pick an IK solution several degrees away from the natural - chain. Both single-sample and multi-sample outliers occur; both look like - a wrist flip in J4 / J6 since J4+J6 is the only invariant at the - singularity. - - Algorithm: - 1. Mark a sample "bad" if either adjacent step exceeds a joint-norm - threshold (catches the LM-picked off-branch samples). - 2. For each contiguous run of bad samples, also pull in some good - samples on either side ("padding") and linear-interp the whole - expanded window. Padding spreads the necessary joint motion across - more samples so TOPPRA's time profile doesn't have to crank up - joint velocity locally — eliminates the residual velocity pulse - that pure run-replacement leaves behind. - - The interp preserves the J4+J6 invariant exactly and gives sub-mm FK - error since both bookends lie on the cartesian path. - - Returns the number of samples patched. + """In-place repair of LM-IK branch hops near wrist singularities. + + pinokin's LM picks IK solutions without a continuity preference, so at + J5 ≈ 0 it can jump several degrees off the natural chain in one or two + samples. Detected as steps > _IK_OUTLIER_RATIO × the chain's median + step; replaced by linear interpolation over the run plus padding. """ - n = len(positions) + n = positions.shape[0] + dims = positions.shape[1] if n < 3: return 0 - threshold = _IK_OUTLIER_THRESHOLD_RAD - pad = _IK_OUTLIER_PADDING - # Per-step joint-norm magnitudes - diffs = np.linalg.norm(np.diff(positions, axis=0), axis=1) + diffs = np.empty(n - 1) + for i in range(n - 1): + s = 0.0 + for c in range(dims): + v = positions[i + 1, c] - positions[i, c] + s += v * v + diffs[i] = np.sqrt(s) - # Mark a sample bad if either adjacent step is over threshold. Endpoints - # always treated as good (they're the IK seed / final target). - bad = np.zeros(n, dtype=bool) - bad[1:-1] = (diffs[:-1] > threshold) | (diffs[1:] > threshold) + median_step = np.median(diffs) + if median_step == 0.0: + return 0 + threshold = _IK_OUTLIER_RATIO * median_step n_patched = 0 i = 1 while i < n - 1: - if not bad[i]: + if diffs[i - 1] <= threshold and diffs[i] <= threshold: i += 1 continue - # Find end of run j = i - while j < n - 1 and bad[j]: + while j < n - 1 and (diffs[j - 1] > threshold or diffs[j] > threshold): j += 1 - # Run is [i, j); bookended by good samples i-1 and j. - # Distinguish LM outlier ("out and back" jump) from legitimate fast - # joint motion: outliers have a small bookend distance vs. their - # intra-run path length, i.e. the chain doubles back on itself. - # Legitimate fast motion has bookend ≈ path length (ratio ≈ 1). - bookend_dq = float(np.linalg.norm(positions[j] - positions[i - 1])) - path_len = float(diffs[i - 1 : j].sum()) - if path_len > 0 and (bookend_dq / path_len) < _IK_OUTLIER_RATIO: - # Treat as LM outlier — replace the run + padding with linear - # interp between bookends. - lo = max(1, i - pad) - hi = min(n - 1, j + pad) - run_span = hi - (lo - 1) - delta = positions[hi] - positions[lo - 1] - for k in range(lo, hi): - alpha = (k - (lo - 1)) / run_span - positions[k] = positions[lo - 1] + alpha * delta - n_patched += 1 - i = hi + 1 - else: - # Legitimate fast joint motion — leave it alone. - i = j + 1 + lo = i - _IK_OUTLIER_PADDING + if lo < 1: + lo = 1 + hi = j + _IK_OUTLIER_PADDING + if hi > n - 1: + hi = n - 1 + span = hi - (lo - 1) + inv_span = 1.0 / span + for k in range(lo, hi): + alpha = (k - (lo - 1)) * inv_span + for c in range(dims): + positions[k, c] = positions[lo - 1, c] + alpha * ( + positions[hi, c] - positions[lo - 1, c] + ) + n_patched += 1 + i = hi + 1 return n_patched diff --git a/parol6/utils/warmup.py b/parol6/utils/warmup.py index 7a968a5..d2191c5 100644 --- a/parol6/utils/warmup.py +++ b/parol6/utils/warmup.py @@ -33,6 +33,7 @@ _pose_to_tangent_jit, _tangent_to_pose_jit, ) +from parol6.motion.trajectory import _smooth_singularity_outliers from parol6.protocol.wire import ( _pack_bitfield, _pack_positions, @@ -295,6 +296,14 @@ def warmup_jit() -> float: # parol6/commands/servo_commands.py _max_vel_ratio_jit(dummy_6f, dummy_6f) + # parol6/motion/trajectory.py — non-trivial array exercises every branch + # (diff loop, median, bad-detection, interp loop). + dummy_chain = np.zeros((10, 6), dtype=np.float64) + for i in range(10): + dummy_chain[i] = i * 0.01 + dummy_chain[5, 3] += 1.0 # synthetic outlier so the interp loop compiles + _smooth_singularity_outliers(dummy_chain) + elapsed = time.perf_counter() - start logger.info(f"\tJIT warmup completed in {elapsed * 1000:.1f}ms") return elapsed diff --git a/tests/unit/test_motion.py b/tests/unit/test_motion.py index 531c0c4..dd2d3c9 100644 --- a/tests/unit/test_motion.py +++ b/tests/unit/test_motion.py @@ -5,6 +5,11 @@ from parol6.config import INTERVAL_S, LIMITS, steps_to_rad from parol6.motion import JointPath, ProfileType, Trajectory, TrajectoryBuilder +from parol6.motion.trajectory import ( + _IK_OUTLIER_PADDING, + _IK_OUTLIER_RATIO, + _smooth_singularity_outliers, +) class TestJointPath: @@ -231,3 +236,128 @@ def test_getitem_returns_step(self): assert np.array_equal(traj[0], steps[0]) assert np.array_equal(traj[5], steps[5]) assert np.array_equal(traj[-1], steps[-1]) + + +class TestSmoothSingularityOutliers: + """Tests for _smooth_singularity_outliers: LM-IK branch-hop repair.""" + + @staticmethod + def _smooth_chain(n: int = 40, step: float = 0.01) -> np.ndarray: + """A 6-DOF chain with uniform per-sample step magnitude.""" + positions = np.zeros((n, 6), dtype=np.float64) + for i in range(n): + positions[i] = i * step + return positions + + def test_smooth_path_unchanged(self): + positions = self._smooth_chain() + original = positions.copy() + n = _smooth_singularity_outliers(positions) + assert n == 0 + assert np.array_equal(positions, original) + + def test_single_sample_hop_is_smoothed(self): + """One outlier sample fires bad-flags on both neighbors (both adjacent + diffs are large) → run of 3, plus pad on each side → 2*pad+3 patched.""" + positions = self._smooth_chain(n=40) + bad_idx = 20 + positions[bad_idx, 3] += 5.0 # 5 rad J4 jump — well past 10× median + + n_patched = _smooth_singularity_outliers(positions) + assert n_patched == 2 * _IK_OUTLIER_PADDING + 3 + # The outlier is gone; uniform-step chain interpolates exactly. + assert positions[bad_idx, 3] == pytest.approx(bad_idx * 0.01, abs=1e-9) + # Samples just outside the patched window are untouched. + outside_lo = bad_idx - 1 - _IK_OUTLIER_PADDING - 1 + outside_hi = bad_idx + 1 + _IK_OUTLIER_PADDING + assert positions[outside_lo, 3] == pytest.approx(outside_lo * 0.01) + assert positions[outside_hi, 3] == pytest.approx(outside_hi * 0.01) + + def test_multi_sample_run_is_smoothed(self): + """Wide hop with monotonically rising outliers → each step is large, + so the whole shelf forms one contiguous bad run.""" + positions = self._smooth_chain(n=40) + # Use increasing magnitudes so every adjacent diff exceeds threshold. + positions[20, 3] += 4.0 + positions[21, 3] += 8.0 + positions[22, 3] += 12.0 + + n_patched = _smooth_singularity_outliers(positions) + # Bad samples: {19, 20, 21, 22, 23} → run [19, 24). + # Patched: [max(1,19-pad), min(n-1,23+pad)+1) = [15, 28) → 13. + assert n_patched == 5 + 2 * _IK_OUTLIER_PADDING + # All run samples land back on the chain. + for k in (20, 21, 22): + assert positions[k, 3] == pytest.approx(k * 0.01, abs=1e-9) + + def test_outlier_near_start_clamps_padding(self): + """Padding clamps at the array boundary; bookend at index 0.""" + positions = self._smooth_chain(n=40) + positions[2, 3] += 5.0 + original_first = positions[0].copy() + original_last = positions[-1].copy() + + _smooth_singularity_outliers(positions) + assert positions[2, 3] == pytest.approx(2 * 0.01, abs=1e-9) + # Endpoints must never be modified. + assert np.array_equal(positions[0], original_first) + assert np.array_equal(positions[-1], original_last) + + def test_outlier_near_end_clamps_padding(self): + positions = self._smooth_chain(n=40) + positions[-3, 3] += 5.0 + original_first = positions[0].copy() + original_last = positions[-1].copy() + + _smooth_singularity_outliers(positions) + assert positions[-3, 3] == pytest.approx((40 - 3) * 0.01, abs=1e-9) + assert np.array_equal(positions[0], original_first) + assert np.array_equal(positions[-1], original_last) + + def test_short_path_is_noop(self): + for n in (0, 1, 2): + positions = np.zeros((n, 6), dtype=np.float64) + assert _smooth_singularity_outliers(positions) == 0 + + def test_zero_motion_is_noop(self): + """Median step = 0 → can't form a ratio threshold; bail.""" + positions = np.zeros((20, 6), dtype=np.float64) + positions[10, 3] = 5.0 # would-be outlier but median is 0 + n = _smooth_singularity_outliers(positions) + assert n == 0 + assert positions[10, 3] == 5.0 + + def test_below_threshold_step_not_smoothed(self): + """Steps within `_IK_OUTLIER_RATIO`× median are normal motion, not hops.""" + positions = self._smooth_chain(n=40) + # Bump one sample by ~5× the median step — should NOT trigger. + positions[20, 3] += 5 * 0.01 + original = positions.copy() + n = _smooth_singularity_outliers(positions) + assert n == 0 + assert np.array_equal(positions, original) + + def test_threshold_exactly_at_ratio_not_smoothed(self): + """Strict > comparison: step == threshold is NOT an outlier.""" + positions = self._smooth_chain(n=40) + # Each step ~ sqrt(6) * 0.01; bump just at the threshold boundary. + median_step = np.sqrt(6) * 0.01 + # Add exactly ratio * median to one component so the step magnitude + # is just over the legitimate motion but well within tolerance. + positions[20, 3] += _IK_OUTLIER_RATIO * median_step - 0.5 * median_step + n = _smooth_singularity_outliers(positions) + assert n == 0 + + def test_repair_preserves_endpoints(self): + """No matter where the hop is, positions[0] and positions[-1] are + always exactly preserved (they're the IK seed and final target).""" + rng = np.random.default_rng(0) + for _ in range(20): + positions = self._smooth_chain(n=50) + hop_idx = int(rng.integers(2, 48)) + positions[hop_idx, rng.integers(0, 6)] += rng.uniform(2.0, 8.0) + first = positions[0].copy() + last = positions[-1].copy() + _smooth_singularity_outliers(positions) + assert np.array_equal(positions[0], first) + assert np.array_equal(positions[-1], last) From 502f7383bee0127d6608960ef0b2b0d68eb24abd Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Wed, 13 May 2026 19:23:42 -0400 Subject: [PATCH 15/15] remove RobotClient.async_client property to eliminate cross-loop foot-gun MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The property returned self._inner — the AsyncRobotClient bound to RobotClient's private background loop. Reaching for it from another loop and awaiting one of its methods produced a cryptic "Queue is bound to a different event loop" deep inside _request_ok_raw. Worse, _run()'s error message steered users toward the trap: "use AsyncRobotClient and `await` the method instead" got read as "grab the one hanging off my sync client" rather than "construct a fresh one in this loop." - Delete the async_client property (zero non-test callers across this repo and Waldo-Commander; re-added in 8cc47dc as part of a typed-stub revert). - Tighten _run()'s error message to point at constructing a fresh AsyncRobotClient in the calling loop. - Update the two test sites that monkeypatched client.async_client._request to use client._inner._request directly — they were already patching a private method, so the change is honest. --- parol6/client/sync_client.py | 7 +------ tests/unit/test_conversions.py | 4 ++-- 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/parol6/client/sync_client.py b/parol6/client/sync_client.py index fd94e13..0febb7e 100644 --- a/parol6/client/sync_client.py +++ b/parol6/client/sync_client.py @@ -102,7 +102,7 @@ def _run(coro: Coroutine[Any, Any, T]) -> T: # A loop is running in this thread; blocking would be unsafe. raise RuntimeError( "RobotClient was used while an event loop is running.\n" - "Use AsyncRobotClient and `await` the method instead." + "Construct an AsyncRobotClient in this loop and `await` it instead." ) @@ -166,11 +166,6 @@ def __enter__(self) -> "RobotClient": def __exit__(self, exc_type, exc, tb) -> None: self.close() - @property - def async_client(self) -> AsyncRobotClient: - """Access the underlying async client if you need it.""" - return self._inner - # Expose common configuration attributes @property def host(self) -> str: diff --git a/tests/unit/test_conversions.py b/tests/unit/test_conversions.py index 28182f9..b4804f9 100644 --- a/tests/unit/test_conversions.py +++ b/tests/unit/test_conversions.py @@ -29,7 +29,7 @@ def test_pose_identity_translation(monkeypatch): result = _pose_result(mat) mock_request = AsyncMock(return_value=result) - monkeypatch.setattr(client.async_client, "_request", mock_request) + monkeypatch.setattr(client._inner, "_request", mock_request) pose_rpy = client.pose() assert pose_rpy is not None @@ -49,7 +49,7 @@ def test_pose_malformed_payload(monkeypatch): client = RobotClient() mock_request = AsyncMock(return_value=PoseResultStruct(pose=[1, 2, 3])) - monkeypatch.setattr(client.async_client, "_request", mock_request) + monkeypatch.setattr(client._inner, "_request", mock_request) pose_rpy = client.pose() assert pose_rpy is None