diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml new file mode 100644 index 0000000..9919f39 --- /dev/null +++ b/.github/workflows/tests.yml @@ -0,0 +1,70 @@ +name: tests + +on: + push: + paths: + - '**' + pull_request: + paths: + - '**' + +jobs: + lint: + name: Lint (ruff + mypy) + runs-on: ubuntu-latest + steps: + - name: Checkout repository + uses: actions/checkout@v4 + - name: Setup Python + uses: actions/setup-python@v5 + with: + python-version: '3.11' + cache: pip + cache-dependency-path: pyproject.toml + - name: Install dev dependencies + run: | + python -m pip install --upgrade pip + pip install -e ".[dev]" + - name: Ruff (lint) + run: ruff check parol6 + - name: Ruff (format check) + run: ruff format --check parol6 + - name: MyPy + run: mypy parol6 + test: + name: ${{ matrix.os }} / Python ${{ matrix.python-version }} + runs-on: ${{ matrix.os }} + timeout-minutes: 30 + strategy: + fail-fast: false + matrix: + os: [ubuntu-latest, windows-latest, macos-latest] + python-version: ['3.10', '3.11'] + + steps: + - name: Checkout repository (with submodules) + uses: actions/checkout@v4 + + - name: Setup Python + uses: actions/setup-python@v5 + with: + python-version: ${{ matrix.python-version }} + cache: pip + cache-dependency-path: pyproject.toml + + - name: Upgrade pip and install dependencies + run: | + python -m pip install --upgrade pip + pip install -e ".[dev]" pytest-timeout + + - name: Show environment + run: | + python -V + pip list + + - name: Run tests (skip hardware) + env: + PYTHONUNBUFFERED: '1' + PYTHONUTF8: '1' + run: | + pytest diff --git a/.gitignore b/.gitignore index db9686a..26ef302 100644 --- a/.gitignore +++ b/.gitignore @@ -124,4 +124,4 @@ cython_debug/ # VS Code .vscode/ -serial_port.txt \ No newline at end of file +serial_port.txt diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..aec62c7 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,33 @@ +default_language_version: + python: python3.11 + +repos: + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.6.0 + hooks: + - id: check-yaml + - id: end-of-file-fixer + - id: trailing-whitespace + - id: check-merge-conflict + - id: check-added-large-files + + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.6.9 + hooks: + - id: ruff + args: ["--fix"] + - id: ruff-format + + - repo: https://github.com/pre-commit/mirrors-mypy + rev: v1.11.2 + hooks: + - id: mypy + name: mypy (parol6) + files: ^parol6/ + pass_filenames: false + args: ["parol6"] + stages: [commit, push] + additional_dependencies: + - numpy==1.26.4 + - spatialmath-python==1.1.14 + - scipy==1.11.4 diff --git a/README.md b/README.md index 8cbf80f..90d17c5 100644 --- a/README.md +++ b/README.md @@ -1,933 +1,261 @@ -# PAROL6 Headless Commander Documentation +# PAROL6 Python API -## 1. Important Notes & Disclaimers -* **Software Origin**: This control system is based on the `experimental_kinematics` branch of the `PAROL_commander_software` repository. The core communication functions were derived from the `Serial_sender_good_latest.py` file; however, the approach to motion planning has been altered from the original implementation. This system was created by editing the `Commander_minimal_version.py` file, which was used as a base. -* **Automatic Homing on Startup**: By default, the controller will home on startup. To disable auto-home without editing code, set environment variable `PAROL6_NOAUTOHOME=1` (the provided ServerManager does this by default in tests). If starting programmatically, pass `no_autohome=True` when launching the server. -* **AI-Assisted Development**: This code was developed with significant AI assistance. While the core logic has been corrected and improved, it has not been exhaustively tested in all scenarios. Users should proceed with caution and verify functionality for their specific needs. +Lightweight Python client and headless controller manager for PAROL6 robot arms. -## 2. Safety Precautions & Disclaimer -This control software includes several built-in safety features designed to prevent damage to the robot and ensure predictable operation: -* **E-Stop Monitoring**: The system constantly checks the physical E-Stop button. If triggered, all motion is immediately halted, the command queue is cleared, and the robot is disabled. The system must be manually re-enabled by pressing the `'e'` key after the E-Stop is released. -* **Synchronized Speed Calculation**: For moves defined by a speed percentage (`MoveJoint`, `MovePose`), the system now calculates the maximum possible synchronized speed for all joints involved. This prevents individual joints from exceeding their limits and ensures predictable, smooth motion. -* **Inverse Kinematics (IK) Validation**: The system verifies that a valid kinematic solution exists for any pose-based command. If the target pose is unreachable, the command will fail safely before any motion occurs. +This package provides: +- AsyncRobotClient (async UDP client) +- RobotClient (sync wrapper around the async client) +- ServerManager utilities (manage_server and CLI parol6-server) -> **WARNING**: These are software-based safety measures and are not a substitute for responsible operation and a safe work environment. The user assumes all responsibility for safe operation. Always be attentive when the robot is active, ensure you have immediate access to the physical E-Stop, and operate the robot in a clear area. +It supports a headless controller process speaking a simple text-based UDP protocol. The client can run on the same machine or remotely. -## 3. Installation -### Base Software Installation -Follow the official PAROL commander software installation guide: -- Repository: [PAROL Commander Software](https://github.com/PCrnjak/PAROL-commander-software) -- Branch: Use the `experimental_kinematics` branch -- Installation Guide: [Official Instructions](https://github.com/PCrnjak/PAROL-commander-software) - -### Additional Dependencies for Headless Commander -After installing the base software, install these additional packages: +## Installation (users) +Run from this repository directory (external/PAROL6-python-API): ```bash -# Install Python 3 and pip (if not already installed) -# https://www.python.org/downloads/ - -# Install Git (if not already installed) -# https://git-scm.com/book/en/v2/Getting-Started-Installing-Git - -# Core dependencies (from official installation) -pip3 install roboticstoolbox-python==1.0.3 -pip3 install numpy==1.23.4 -pip3 install scipy==1.11.4 -pip3 install spatialmath -pip3 install pyserial -pip3 install oclock -pip3 install keyboard -``` -## 4. System Architecture - -### Client-Server Design -The system uses a UDP-based client-server architecture that separates robot control from command generation: - -* **The Robot Controller (`controller.py`)**: - - Runs on the computer physically connected to the robot via USB/Serial - - Maintains a high-frequency control loop for real-time robot control - - Handles all complex calculations (inverse kinematics, trajectory planning) - - Requires heavy dependencies (roboticstoolbox, numpy, scipy) - - Listens for UDP commands on port 5001 - -* **The Python Client (`parol6.client`)**: - - Can run on any computer (same or different from controller) - - Sends simple text commands via UDP - - Requires minimal dependencies (mostly Python standard library) - - Extremely lightweight - can run on resource-constrained devices - - Optionally receives acknowledgments on port 5002 - -* **Support Modules**: - - `parol6/smooth_motion/`: Advanced trajectory generation algorithms (modular package) - - `PAROL6_ROBOT.py`: Robot-specific parameters and kinematic model - -### Command Module Architecture -The robot controller organizes command execution through a modular architecture. All command classes are located in `./commands/`, with each module containing functionally related commands: - -* **`utils/ik.py`** - Inverse kinematics solving and helper functions (moved from commands) -* **`basic_commands.py`** - Home, Jog, and MultiJog commands -* **`cartesian_commands.py`** - Cartesian space movement commands -* **`joint_commands.py`** - Joint space movement commands -* **`gripper_commands.py`** - Pneumatic and electric gripper control -* **`utility_commands.py`** - Delay and utility functions -* **`smooth_commands.py`** - Advanced smooth trajectory generation - -This modular structure keeps the main controller lean while organizing functionality logically. - -### Why UDP? -The UDP protocol was chosen for several reasons: -- **Simplicity**: No connection management overhead -- **Low Latency**: Direct message passing without handshaking -- **Lightweight Client**: Client only needs to send text strings -- **Cross-Platform**: Works on any OS with network support -- **Flexible Deployment**: Client can run anywhere on the network - -### Command Flow -1. Client calls API function (e.g., `move_robot_joints()`) -2. API formats command as text string (e.g., `"MOVEJOINT|90|-45|90|0|45|180|5.5|None"`) -3. Command sent via UDP to controller -4. Controller queues and executes command -5. Optional: Acknowledgment sent back to client -6. Optional: Client checks status using command ID - -### Command Acknowledgment System -The system includes an optional acknowledgment tracking feature that provides feedback on command execution: -* **Tracking States**: Commands can report status as `QUEUED`, `EXECUTING`, `COMPLETED`, `FAILED`, `CANCELLED`, or `INVALID` -* **Zero Overhead**: When not used, the tracking system has zero resource overhead - no threads or sockets are created -* **Non-Blocking Mode**: Commands can be sent with `non_blocking=True` to return immediately with a command ID, allowing asynchronous operation -* **Status Checking**: Use `check_command_status(command_id)` to poll command status later - -Example of non-blocking usage: -```python -# Send command and get ID immediately -cmd_id = move_robot_joints([90, -45, 90, 0, 45, 180], - duration=5, - wait_for_ack=True, - non_blocking=True) - -# Do other work... -time.sleep(1) - -# Check status later -status = check_command_status(cmd_id) -if status and status['completed']: - print(f"Command finished with status: {status['status']}") +pip install . ``` -## 5. Command Reference & API Usage - -### Motion Commands - -#### `home_robot()` -* **Purpose**: Initiates the robot's built-in homing sequence. -* **Parameters**: - * `wait_for_ack` (bool, optional): Enable command tracking. Default: False - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 30.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* **Python API Usage**: - ```python - from robot_api import home_robot - home_robot() # Simple usage - home_robot(wait_for_ack=True, timeout=30) # With tracking - ``` - -#### `move_robot_joints()` -* **Purpose**: Moves joints to a target configuration (in degrees). -* **Parameters**: - * `joint_angles` (List[float]): List of 6 target angles in degrees for joints 1-6 - * `duration` (float, optional): Total time for the movement in seconds - * `speed_percentage` (int, optional): Speed as percentage (0-100) - * `wait_for_ack` (bool, optional): Enable command tracking. Default: False - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 2.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* > *Note: You must provide either `duration` or `speed_percentage`, but not both.* -* **Python API Usage**: - ```python - from robot_api import move_robot_joints - - # Simple move by speed - move_robot_joints([90, -45, 90, 0, 45, 180], speed_percentage=75) - - # Move with acknowledgment tracking - result = move_robot_joints([0, -90, 180, 0, 0, 180], - duration=5.5, - wait_for_ack=True) - if result['status'] == 'COMPLETED': - print("Move completed successfully") - ``` - -#### `move_robot_pose()` -* **Purpose**: Moves the end-effector to a Cartesian pose via a joint-based path. -* **Parameters**: - * `pose` (List[float]): Target pose [x, y, z, Rx, Ry, Rz] in mm and degrees - * `duration` (float, optional): Total time for the movement in seconds - * `speed_percentage` (int, optional): Speed as percentage (0-100) - * `wait_for_ack` (bool, optional): Enable command tracking. Default: False - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 2.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* > *Note: You must provide either `duration` or `speed_percentage`, but not both.* -* **Python API Usage**: - ```python - from robot_api import move_robot_pose - move_robot_pose([250, 0, 200, 180, 0, 90], speed_percentage=50) - ``` - -#### `move_robot_cartesian()` -* **Purpose**: Moves the end-effector to a target pose in a guaranteed straight-line path. -* **Parameters**: - * `pose` (List[float]): Target pose [x, y, z, Rx, Ry, Rz] in mm and degrees - * `duration` (float, optional): Time for the movement in seconds - * `speed_percentage` (float, optional): Speed as percentage (1-100) - * `wait_for_ack` (bool, optional): Enable command tracking. Default: False - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 2.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* > *Note: You must provide either `duration` or `speed_percentage`, but not both.* -* **Python API Usage**: - ```python - from robot_api import move_robot_cartesian - move_robot_cartesian([200, -50, 180, 180, 0, 135], duration=4.0) - ``` - -### Jogging Commands - -#### `jog_robot_joint()` -* **Purpose**: Jogs a single joint by time or angular distance. -* **Parameters**: - * `joint_index` (int): Joint to move (0-5 for positive direction, 6-11 for negative) - * `speed_percentage` (int): Jog speed as percentage (0-100) - * `duration` (float, optional): Time to jog in seconds - * `distance_deg` (float, optional): Distance to jog in degrees - * `wait_for_ack` (bool, optional): Enable command tracking. Default: False - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 2.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* > *Note: You must provide either `duration` or `distance_deg`, but not both.* -* **Python API Usage**: - ```python - from robot_api import jog_robot_joint - # Jog joint 1 for 2 seconds - jog_robot_joint(joint_index=0, speed_percentage=40, duration=2.0) - # Jog joint 3 backwards by 15 degrees - jog_robot_joint(joint_index=8, speed_percentage=60, distance_deg=15) - ``` - -#### `jog_multiple_joints()` -* **Purpose**: Jogs multiple joints simultaneously. -* **Parameters**: - * `joints` (List[int]): List of joint indices (0-5 positive, 6-11 negative) - * `speeds` (List[float]): List of corresponding speeds (1-100%) - * `duration` (float): Duration in seconds - * `wait_for_ack` (bool, optional): Enable command tracking. Default: False - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 2.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* **Python API Usage**: - ```python - from robot_api import jog_multiple_joints - # Jog joints 1, 4, and 6 simultaneously - jog_multiple_joints([0, 3, 5], [70, 40, 60], 1.2) - ``` - -#### `jog_cartesian()` -* **Purpose**: Jogs the end-effector continuously along an axis. -* **Parameters**: - * `frame` (str): Reference frame ('TRF' for Tool, 'WRF' for World) - * `axis` (str): Axis and direction ('X+', 'X-', 'Y+', 'Y-', 'Z+', 'Z-', 'RX+', 'RX-', 'RY+', 'RY-', 'RZ+', 'RZ-') - * `speed_percentage` (int): Jog speed as percentage (0-100) - * `duration` (float): Time to jog in seconds - * `wait_for_ack` (bool, optional): Enable command tracking. Default: False - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 2.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* **Python API Usage**: - ```python - from robot_api import jog_cartesian - jog_cartesian(frame='TRF', axis='Z+', speed_percentage=50, duration=1.5) - ``` - -### Smooth Motion Commands - -These commands create smooth, curved trajectories with continuous velocity profiles. All smooth motion commands support multiple trajectory types through the `trajectory_type` parameter: - -* **'cubic'** (default): Smooth acceleration profile with continuous velocity -* **'quintic'**: Smoother motion with continuous acceleration and zero jerk at endpoints -* **'s_curve'**: Jerk-limited motion for ultra-smooth movement (specify `jerk_limit` in mm/s³) - -> **Note on Jerk**: Jerk is the rate of change of acceleration. Lower jerk values result in smoother motion with less mechanical stress on the robot, but may increase movement time. The jerk limit parameter controls the maximum allowed jerk during S-curve trajectories. - -All smooth commands also support reference frame selection through the `frame` parameter ('WRF' for World Reference Frame or 'TRF' for Tool Reference Frame). - -#### `smooth_circle()` -* **Purpose**: Execute a smooth circular motion. -* **Parameters**: - * `center` (List[float]): Center point [x, y, z] in mm - * `radius` (float): Circle radius in mm - * `plane` (str, optional): Plane of circle ('XY', 'XZ', or 'YZ'). Default: 'XY' - * `frame` (str, optional): Reference frame ('WRF' for World, 'TRF' for Tool). Default: 'WRF' - * `center_mode` (str, optional): How the center point is interpreted. Default: 'ABSOLUTE' - - `'ABSOLUTE'`: Center is an absolute position in the selected frame - - `'TOOL'`: Center is relative to the current tool position - - `'RELATIVE'`: Center is an offset from the current position - * `entry_mode` (str, optional): How to transition into the circle. Default: 'NONE' - - `'AUTO'`: Automatically choose best entry strategy based on current position - - `'TANGENT'`: Enter the circle tangentially for smooth transition - - `'DIRECT'`: Move directly to the closest point on the circle - - `'NONE'`: Start circle from current position (assumes already on circle) - * `start_pose` (List[float], optional): Starting pose [x, y, z, rx, ry, rz], or None for current position. Default: None - * `duration` (float, optional): Time to complete circle in seconds - * `speed_percentage` (float, optional): Speed as percentage (1-100) - * `clockwise` (bool, optional): Direction of motion. Default: False - * `trajectory_type` (str, optional): Motion profile - 'cubic', 'quintic', or 's_curve'. Default: 'cubic' - * `jerk_limit` (float, optional): Maximum jerk for s_curve trajectories in mm/s³. Default: None - * `wait_for_ack` (bool, optional): Enable command tracking. Default: False - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 10.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* > *Note: You must provide either `duration` or `speed_percentage`, but not both.* -* **Python API Usage**: - ```python - from robot_api import smooth_circle - - # Draw a 50mm radius circle at absolute position - smooth_circle(center=[200, 0, 200], radius=50, plane='XY', duration=5.0) - - # Draw a circle centered 30mm ahead of current tool position - smooth_circle(center=[30, 0, 0], radius=25, center_mode='TOOL', - entry_mode='TANGENT', duration=4.0) - - # Draw a circle with automatic entry from current position - smooth_circle(center=[250, 50, 200], radius=40, entry_mode='AUTO', - speed_percentage=60) - ``` - -#### `smooth_arc_center()` -* **Purpose**: Execute a smooth arc motion defined by center point. -* **Parameters**: - * `end_pose` (List[float]): End pose [x, y, z, rx, ry, rz] in mm and degrees - * `center` (List[float]): Arc center point [x, y, z] in mm - * `frame` (str, optional): Reference frame ('WRF' for World, 'TRF' for Tool). Default: 'WRF' - * `start_pose` (List[float], optional): Starting pose, or None for current position. Default: None - * `duration` (float, optional): Time to complete arc in seconds - * `speed_percentage` (float, optional): Speed as percentage (1-100) - * `clockwise` (bool, optional): Direction of motion. Default: False - * `trajectory_type` (str, optional): Motion profile - 'cubic', 'quintic', or 's_curve'. Default: 'cubic' - * `jerk_limit` (float, optional): Maximum jerk for s_curve trajectories in mm/s³. Default: None - * `wait_for_ack` (bool, optional): Enable command tracking. Default: False - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 10.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* > *Note: You must provide either `duration` or `speed_percentage`, but not both.* -* **Python API Usage**: - ```python - from robot_api import smooth_arc_center - smooth_arc_center(end_pose=[250, 50, 200, 0, 0, 90], - center=[200, 0, 200], - duration=3.0) - ``` - -#### `smooth_arc_parametric()` -* **Purpose**: Execute a smooth arc motion defined by radius and angle. -* **Parameters**: - * `end_pose` (List[float]): End pose [x, y, z, rx, ry, rz] in mm and degrees - * `radius` (float): Arc radius in mm - * `arc_angle` (float): Arc angle in degrees - * `frame` (str, optional): Reference frame ('WRF' for World, 'TRF' for Tool). Default: 'WRF' - * `start_pose` (List[float], optional): Starting pose, or None for current position. Default: None - * `duration` (float, optional): Time to complete arc in seconds - * `speed_percentage` (float, optional): Speed as percentage (1-100) - * `clockwise` (bool, optional): Direction of motion. Default: False - * `trajectory_type` (str, optional): Motion profile - 'cubic', 'quintic', or 's_curve'. Default: 'cubic' - * `jerk_limit` (float, optional): Maximum jerk for s_curve trajectories in mm/s³. Default: None - * `wait_for_ack` (bool, optional): Enable command tracking. Default: False - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 10.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* > *Note: You must provide either `duration` or `speed_percentage`, but not both.* -* **Python API Usage**: - ```python - from robot_api import smooth_arc_parametric - smooth_arc_parametric(end_pose=[250, 50, 200, 0, 0, 90], - radius=50, arc_angle=90, duration=3.0) - ``` - -#### `smooth_spline()` -* **Purpose**: Create smooth spline through waypoints. -* **Parameters**: - * `waypoints` (List[List[float]]): List of poses [x, y, z, rx, ry, rz] to pass through - * `frame` (str, optional): Reference frame ('WRF' for World, 'TRF' for Tool). Default: 'WRF' - * `start_pose` (List[float], optional): Starting pose, or None for current position. Default: None - * `duration` (float, optional): Total time for motion in seconds - * `speed_percentage` (float, optional): Speed as percentage (1-100) - * `trajectory_type` (str, optional): Motion profile - 'cubic', 'quintic', or 's_curve'. Default: 'cubic' - * `jerk_limit` (float, optional): Maximum jerk for s_curve trajectories in mm/s³. Default: None - * `wait_for_ack` (bool, optional): Enable command tracking. Default: False - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 10.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* > *Note: You must provide either `duration` or `speed_percentage`, but not both.* -* **Python API Usage**: - ```python - from robot_api import smooth_spline - waypoints = [ - [200, 0, 100, 0, 0, 0], - [250, 50, 150, 0, 15, 45], - [200, 100, 200, 0, 30, 90] - ] - smooth_spline(waypoints, duration=8.0) - ``` - -#### `smooth_helix()` -* **Purpose**: Execute helical motion. -* **Parameters**: - * `center` (List[float]): Helix center point [x, y, z] in mm - * `radius` (float): Helix radius in mm - * `pitch` (float): Vertical distance per revolution in mm - * `height` (float): Total height of helix in mm - * `frame` (str, optional): Reference frame ('WRF' for World, 'TRF' for Tool). Default: 'WRF' - * `trajectory_type` (str, optional): Motion profile - 'cubic', 'quintic', or 's_curve'. Default: 'cubic' - * `jerk_limit` (float, optional): Maximum jerk for s_curve trajectories in mm/s³. Default: None - * `start_pose` (List[float], optional): Starting pose, or None for current position. Default: None - * `duration` (float, optional): Time to complete helix in seconds - * `speed_percentage` (float, optional): Speed as percentage (1-100) - * `clockwise` (bool, optional): Direction of motion. Default: False - * `wait_for_ack` (bool, optional): Enable command tracking. Default: False - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 10.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* > *Note: You must provide either `duration` or `speed_percentage`, but not both.* -* **Python API Usage**: - ```python - from robot_api import smooth_helix - smooth_helix(center=[200, 0, 150], radius=30, pitch=20, - height=100, duration=10.0) - ``` - -#### `smooth_blend()` -* **Purpose**: Blend multiple motion segments smoothly using AdvancedMotionBlender. -* **Parameters**: - * `segments` (List[Dict]): List of segment dictionaries defining the motion path - * `blend_time` (float, optional): Time for blending between segments in seconds. Default: 0.5 - * `frame` (str, optional): Reference frame ('WRF' for World, 'TRF' for Tool). Default: 'WRF' - * `start_pose` (List[float], optional): Starting pose, or None for current position. Default: None - * `duration` (float, optional): Total time for entire motion in seconds - * `speed_percentage` (float, optional): Speed as percentage (1-100) - * `wait_for_ack` (bool, optional): Enable command tracking. Default: False - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 15.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* **Python API Usage**: - ```python - from robot_api import smooth_blend - segments = [ - {'type': 'LINE', 'end': [250, 0, 200, 0, 0, 0], 'duration': 2.0}, - {'type': 'CIRCLE', 'center': [250, 0, 200], 'radius': 50, - 'plane': 'XY', 'duration': 4.0, 'clockwise': False}, - {'type': 'LINE', 'end': [200, 0, 200, 0, 0, 0], 'duration': 2.0} - ] - smooth_blend(segments, blend_time=0.5, duration=10.0) - ``` - -#### `smooth_waypoints()` -* **Purpose**: Execute advanced waypoint trajectory with automatic blending and constraint optimization. -* **Parameters**: - * `waypoints` (List[List[float]]): List of waypoint poses [x, y, z, rx, ry, rz] in mm and degrees - * `blend_radii` (Union[str, List[float]], optional): Blend radius for each waypoint or 'auto'. Default: 'auto' - * `blend_mode` (str, optional): Blending algorithm - 'parabolic', 'circular', or 'none'. Default: 'parabolic' - * `via_modes` (List[str], optional): Mode for each waypoint - 'via' (pass through) or 'stop'. Default: All 'via' - * `max_velocity` (float, optional): Maximum velocity constraint in mm/s. Default: None - * `max_acceleration` (float, optional): Maximum acceleration constraint in mm/s². Default: None - * `trajectory_type` (str, optional): Motion profile - 'quintic', 's_curve', or 'cubic'. Default: 'quintic' - * `frame` (str, optional): Reference frame ('WRF' for World, 'TRF' for Tool). Default: 'WRF' - * `duration` (float, optional): Total time for entire motion in seconds. Default: Auto-calculated - * `wait_for_ack` (bool, optional): Enable command tracking. Default: True - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 15.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* **Python API Usage**: - ```python - from robot_api import smooth_waypoints - waypoints = [ - [200, 0, 200, 0, 0, 0], - [250, 50, 200, 0, 0, 45], - [200, 100, 200, 0, 0, 90], - [150, 50, 200, 0, 0, 45] - ] - # Automatic blending with quintic trajectories - smooth_waypoints(waypoints, blend_radii='auto', trajectory_type='quintic') - - # Custom blend radii with S-curve profile - smooth_waypoints(waypoints, blend_radii=[10, 15, 10], - trajectory_type='s_curve', max_velocity=100.0) - ``` - -### GCODE Support - -The robot controller includes a comprehensive GCODE interpreter that allows CNC-style control using standard G-code commands. The interpreter supports linear moves, circular arcs, work coordinate systems, and various M-codes for gripper control. - -#### Key GCODE Functions - -* `execute_gcode()` - Execute a single GCODE line -* `execute_gcode_program()` - Execute a multi-line GCODE program (string or list) -* `load_gcode_file()` - Load and execute a GCODE file -* `get_gcode_status()` - Query the current state of the GCODE interpreter -* `pause_gcode_program()`, `resume_gcode_program()`, `stop_gcode_program()` - Program control - -#### Supported Commands -* **G-codes**: G0 (rapid), G1 (linear), G2/G3 (circular arcs), G4 (dwell), G17-G19 (plane selection), G20/G21 (units), G28 (home), G54-G59 (work coordinates), G90/G91 (absolute/incremental) -* **M-codes**: M0 (stop), M1 (optional stop), M3 (gripper close), M5 (gripper open), M30 (program end) - -#### Python API Usage -```python -from robot_api import execute_gcode, load_gcode_file, get_gcode_status - -# Execute single GCODE line -execute_gcode("G1 X200 Y100 Z150 F100") - -# Execute GCODE program -program = [ - "G21 ; Set units to mm", - "G90 ; Absolute positioning", - "G1 X200 Y0 Z200 F150", - "G2 X250 Y50 I25 J25 ; Arc move", - "M3 ; Close gripper" -] -execute_gcode_program(program) - -# Load from file -load_gcode_file("path/to/program.gcode") - -# Check status -status = get_gcode_status() -print(f"Current line: {status['current_line']}") -``` +Notes on robotics-toolbox-python: +- This project uses a custom robotics-toolbox-python build to fix upstream issues needed by PAROL6. Wheels are pinned in pyproject and will be selected automatically by pip based on platform and Python version. +- Supported prebuilt wheels (from pyproject): + - Linux x86_64 (cp310, cp311) + - Linux aarch64 (cp310, cp311) — Raspberry Pi 5 supported; ~1000 Hz cartesian jogging has been achieved on RPi 5 + - macOS arm64 (cp310, cp311) + - macOS x86_64 (cp310, cp311) + - Windows AMD64 (cp310, cp311) +- Primary development platform: Raspberry Pi 5. CI/CD runners are configured to help ensure a similar experience on Windows and macOS. -See `GCODE_DOCUMENTATION.md` for comprehensive GCODE reference including all supported commands, coordinate systems, and advanced features. - -### Gripper Commands - -#### `control_pneumatic_gripper()` -* **Purpose**: Controls the pneumatic gripper. -* **Parameters**: - * `action` (str): Action to perform ('open' or 'close') - * `port` (int): Digital output port (1 or 2) - * `wait_for_ack` (bool, optional): Enable command tracking. Default: False - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 2.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* **Python API Usage**: - ```python - from robot_api import control_pneumatic_gripper - control_pneumatic_gripper(action='open', port=1) - ``` - -#### `control_electric_gripper()` -* **Purpose**: Controls the electric gripper. -* **Parameters**: - * `action` (str): Action to perform ('move' or 'calibrate') - * `position` (int, optional): Target position (0-255). Default: 255 - * `speed` (int, optional): Movement speed (0-255). Default: 150 - * `current` (int, optional): Max motor current in mA (100-1000). Default: 500 - * `wait_for_ack` (bool, optional): Enable command tracking. Default: False - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 2.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* **Python API Usage**: - ```python - from robot_api import control_electric_gripper - control_electric_gripper(action='calibrate') - control_electric_gripper(action='move', position=200, speed=150) - ``` - -### Utility Commands - -#### `delay_robot()` -* **Purpose**: Pauses command queue execution. -* **Parameters**: - * `duration` (float): Pause time in seconds - * `wait_for_ack` (bool, optional): Enable command tracking. Default: False - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 2.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* **Python API Usage**: - ```python - from robot_api import delay_robot - delay_robot(2.5) - ``` - -#### `stop_robot_movement()` -* **Purpose**: Immediately stops all motion and clears command queue. -* **Parameters**: - * `wait_for_ack` (bool, optional): Enable command tracking. Default: False - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 2.0 - * `non_blocking` (bool, optional): Return immediately with command ID. Default: False -* **Python API Usage**: - ```python - from robot_api import stop_robot_movement - stop_robot_movement() - ``` - -### Query Commands - -These commands request current robot state without moving the robot: - -#### `get_robot_pose()` -* **Purpose**: Get current end-effector pose. -* **Parameters**: None -* **Returns**: List [x, y, z, Rx, Ry, Rz] in mm and degrees, or None if failed -* **Python API Usage**: - ```python - from robot_api import get_robot_pose - pose = get_robot_pose() - if pose: - print(f"Current pose: {pose}") - ``` - -#### `get_robot_joint_angles()` -* **Purpose**: Get current joint angles. -* **Parameters**: None -* **Returns**: List of 6 angles in degrees, or None if failed -* **Python API Usage**: - ```python - from robot_api import get_robot_joint_angles - angles = get_robot_joint_angles() - ``` - -#### `get_robot_joint_speeds()` -* **Purpose**: Get current joint speeds. -* **Parameters**: None -* **Returns**: List of 6 speeds in steps/sec, or None if failed - -#### `get_robot_io()` -* **Purpose**: Get digital I/O status. -* **Parameters**: - * `verbose` (bool, optional): Print formatted status to console. Default: False -* **Returns**: List [IN1, IN2, OUT1, OUT2, ESTOP] (0 or 1 values), or None if failed - -#### `get_electric_gripper_status()` -* **Purpose**: Get electric gripper status. -* **Parameters**: - * `verbose` (bool, optional): Print formatted status to console. Default: False -* **Returns**: List [ID, Position, Speed, Current, StatusByte, ObjectDetected], or None if failed - -#### `get_robot_pose_matrix()` -* **Purpose**: Get robot pose as transformation matrix. -* **Parameters**: None -* **Returns**: 4x4 numpy array, or None if failed - -#### `is_robot_stopped()` -* **Purpose**: Check if robot has stopped moving. -* **Parameters**: - * `threshold_speed` (float, optional): Speed threshold in steps/sec. Default: 2.0 -* **Returns**: Boolean (True if stopped, False otherwise) - -#### `is_estop_pressed()` -* **Purpose**: Check if E-stop is currently pressed. -* **Parameters**: None -* **Returns**: Boolean (True if pressed, False otherwise) - -#### `get_robot_status()` -* **Purpose**: Get comprehensive robot status. -* **Parameters**: None -* **Returns**: Dictionary containing pose, angles, speeds, IO, gripper status, stopped state, and E-stop state - -### Helper Functions - -#### `execute_trajectory()` -* **Purpose**: High-level trajectory execution with best method selection. -* **Parameters**: - * `trajectory` (List[List[float]]): List of poses [x, y, z, rx, ry, rz] - * `timing_mode` (str, optional): Either 'duration' or 'speed'. Default: 'duration' - * `timing_value` (float, optional): Duration in seconds or speed percentage. Default: 5.0 - * `motion_type` (str, optional): Either 'spline' or 'linear'. Default: 'spline' - * `frame` (str, optional): Reference frame ('WRF' or 'TRF') for spline motion. Default: 'WRF' - * `wait_for_ack` (bool, optional): Enable command tracking. Default: True - * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 30.0 -* > *Note: The `frame` parameter only applies when `motion_type='spline'`. Linear motions are always in WRF.* -* **Python API Usage**: - ```python - from robot_api import execute_trajectory - - # Execute trajectory in world frame - trajectory = [[200, 0, 200, 0, 0, 0], - [250, 50, 200, 0, 0, 45], - [200, 100, 200, 0, 0, 90]] - execute_trajectory(trajectory, timing_mode='duration', - timing_value=10.0, motion_type='spline') - - # Execute trajectory in tool frame (spline only) - tool_trajectory = [[20, 0, 0, 0, 0, 0], - [20, 20, 0, 0, 0, 30], - [0, 20, 10, 0, 0, 60]] - execute_trajectory(tool_trajectory, frame='TRF', - timing_mode='speed', - timing_value=40, motion_type='spline') - ``` - - -#### `wait_for_robot_stopped()` -* **Purpose**: Wait for robot to stop moving. -* **Parameters**: - * `timeout` (float, optional): Maximum time to wait in seconds. Default: 10.0 - * `poll_rate` (float, optional): How often to check in seconds. Default: 0.1 -* **Returns**: Boolean (True if robot stopped, False if timeout) - -#### `safe_move_with_retry()` -* **Purpose**: Execute move with automatic retry on failure. -* **Parameters**: - * `move_func` (callable): The movement function to call - * `*args`: Arguments for the movement function - * `max_retries` (int, optional): Maximum number of retry attempts. Default: 3 - * `retry_delay` (float, optional): Delay between retries in seconds. Default: 1.0 - * `**kwargs`: Keyword arguments for the movement function -* **Returns**: Result from the movement function or error dictionary - -#### `chain_smooth_motions()` -* **Purpose**: Chain multiple smooth motions with automatic continuity. -* **Parameters**: - * `motions` (List[Dict]): List of motion dictionaries - * `ensure_continuity` (bool, optional): Automatically set start_pose for continuity. Default: True - * `frame` (str, optional): Reference frame ('WRF' or 'TRF') for all motions. Default: 'WRF' - * `wait_for_ack` (bool, optional): Enable command tracking. Default: True - * `timeout` (float, optional): Timeout per motion in seconds. Default: 30.0 -* **Returns**: List of results for each motion -* **Python API Usage**: - ```python - from robot_api import chain_smooth_motions - - # Chain motions in world frame (default) - motions = [ - {'type': 'circle', 'center': [200, 0, 200], 'radius': 50, 'duration': 5}, - {'type': 'arc', 'end_pose': [250, 50, 200, 0, 0, 90], - 'center': [225, 25, 200], 'duration': 3} - ] - chain_smooth_motions(motions, ensure_continuity=True) - - # Chain motions in tool frame - tool_motions = [ - {'type': 'circle', 'center': [0, 30, 0], 'radius': 25, 'duration': 4}, - {'type': 'arc', 'end_pose': [30, 30, 0, 0, 0, 45], - 'center': [15, 15, 0], 'duration': 3} - ] - chain_smooth_motions(tool_motions, frame='TRF', ensure_continuity=True) - ``` - -#### `check_command_status()` -* **Purpose**: Check status of a previously sent command. -* **Parameters**: - * `command_id` (str): The command ID returned from a non-blocking command -* **Returns**: Dictionary with status information, or None if tracker not initialized -* **Dictionary Contents**: - * `status` (str): Current status ('QUEUED', 'EXECUTING', 'COMPLETED', 'FAILED', 'CANCELLED', 'INVALID') - * `details` (str): Additional status details - * `completed` (bool): Whether command has finished - * `sent_time` (datetime): When command was sent - * `ack_time` (datetime, optional): When acknowledgment was received - -#### `is_tracking_active()` -* **Purpose**: Check if command tracking system is active. -* **Parameters**: None -* **Returns**: Boolean (True if tracking is active, False otherwise) - -#### `get_tracking_stats()` -* **Purpose**: Get resource usage statistics for tracking system. -* **Parameters**: None -* **Returns**: Dictionary with tracking statistics -* **Dictionary Contents**: - * `active` (bool): Whether tracking is active - * `commands_tracked` (int): Number of commands being tracked - * `memory_bytes` (int): Approximate memory usage - * `thread_active` (bool): Whether tracking thread is running - -## 6. Setup & Operation - -### Dependencies - -The system is designed with a client-server architecture where most dependencies are only needed on the server (robot controller) side. The client API (`robot_api.py`) uses only standard Python libraries for UDP communication, making it lightweight and portable. - -#### Server Dependencies (for `controller.py`) -Install Python 3 and the following packages on the computer connected to the robot: +To launch the headless controller after installation: ```bash -# Core robotics libraries -pip3 install roboticstoolbox-python==1.0.3 -pip3 install numpy==1.23.4 -pip3 install scipy==1.11.4 -pip3 install spatialmath - -# Serial communication and timing -pip3 install pyserial -pip3 install oclock - -# User input -pip3 install keyboard +parol6-server --log-level=INFO ``` -#### Client Dependencies (for `robot_api.py`) -The client API is designed to be lightweight with minimal dependencies: -```bash -# Only needed for get_robot_pose() matrix conversion -pip3 install numpy==1.23.4 -pip3 install spatialmath +## Development setup +For contributors working on this repository: -# All other functionality uses only Python standard library: -# socket, threading, time, uuid, datetime, collections, typing +```bash +# From external/PAROL6-python-API/ +pip install -e .[dev] +pre-commit install ``` -**Note**: If you only need to send commands and don't use `get_robot_pose()`, the client requires NO external dependencies - only Python's built-in libraries. - -### File Structure - -#### Server Side (Robot Controller Computer) -Required files in the same folder: -* `controller.py` - Main server/controller -* `PAROL6_ROBOT.py` - Robot configuration and kinematic model -* `parol6/smooth_motion/` - Advanced trajectory generation package (split modules) -* `commands/` - Modular command classes directory - - `utils/ik.py` - IK solving and helper functions - - `basic_commands.py` - Home, Jog, MultiJog commands - - `cartesian_commands.py` - Cartesian movement commands - - `joint_commands.py` - Joint space movements - - `gripper_commands.py` - Gripper control - - `utility_commands.py` - Delay and utility functions - - `smooth_commands.py` - Advanced trajectory commands -* `gcode/` - GCODE interpreter and parser - - `interpreter.py` - Main GCODE interpreter - - `parser.py` - GCODE parsing and validation - - `commands.py` - GCODE to robot command mapping - - `state.py` - Modal state tracking - - `coordinates.py` - Work coordinate systems - - `utils.py` - Utility functions -* `GCODE_DOCUMENTATION.md` - Comprehensive GCODE reference - -Optional: -* `com_port.txt` - Contains the USB COM port (e.g., COM5) -* `GUI/files/` folder structure - For GUI mode compatibility - -#### Client Side (Any Computer) -Only required file: -* `robot_api.py` - Client API for sending commands - -The client can run on any computer on the same network as the server, or on the same computer in a different process. - -### How to Operate - -#### Starting the Server (Robot Controller) - -1. **Connect Robot**: Ensure the robot is connected via USB to the controller computer. - -2. **Start Controller**: On the robot controller computer, navigate to the folder containing the server files and run: - ```bash - python controller.py - ``` - The controller will: - - Connect to the robot via serial port (prompts if `com_port.txt` not found) - - Start listening for UDP commands on port 5001 - - Optionally home the robot on startup (unless disabled) - -#### Sending Commands (Client) - -Commands can be sent from: -- **Same Computer**: Run Python scripts or interactive sessions in another terminal -- **Different Computer**: Ensure network connectivity and update `SERVER_IP` in `robot_api.py` - -3. **Send Commands**: Use the API functions from `robot_api.py`: - ```python - from robot_api import * - - # Example sequence - home_robot() - move_robot_joints([90, -90, 160, 12, 12, 180], duration=5.5) - delay_robot(0.5) - - # Smooth motion example - smooth_circle([200, 0, 200], radius=50, duration=5.0) - - # Non-blocking example with status checking - cmd_id = move_robot_pose([250, 0, 200, 180, 0, 90], - speed_percentage=50, - wait_for_ack=True, - non_blocking=True) - - # Check status after some time - import time - time.sleep(2) - status = check_command_status(cmd_id) - if status: - print(f"Command status: {status['status']}") - ``` - -#### Network Configuration - -If running client and server on different computers: - -1. **Update Server IP**: In `robot_api.py`, modify the `SERVER_IP` variable: - ```python - SERVER_IP = "192.168.1.100" # Replace with robot controller's IP - SERVER_PORT = 5001 # Default port (usually no change needed) - ``` - -2. **Firewall Settings**: Ensure UDP port 5001 is open on the robot controller computer. - -3. **Network Requirements**: - - Both computers must be on the same network - - Low latency recommended for real-time control - - Command acknowledgments use port 5002 (optional feature) - -### Advanced Usage with Acknowledgments - -The acknowledgment system allows for sophisticated command management: +- Run all pre-commit hooks locally: `pre-commit run -a` +- Run tests with pytest: + - `pytest -m "unit or integration"` + - Simulator is used by default (PAROL6_FAKE_SERIAL=1). Hardware tests are marked and require explicit opt-in. + +Adding a command (overview): +- Create a class under `parol6/commands/` and decorate it with `@register_command("NAME")` (see `parol6/server/command_registry.py`). +- Implement `match(parts)`, `setup(state)`, and `tick(state)`; set `streamable=True` on motion commands that support streaming. +- Use helpers from `parol6/commands/base.py` (parsers, motion profiles). The wire name should match your decorator name. + + +## Architecture overview +PAROL6 is split into a headless controller and a lightweight client. + +- Headless Controller (`parol6.server.controller`) + - Binds a UDP server for commands (default 0.0.0.0:5001) + - Runs a fixed-rate control loop translating high-level commands into low-level joint commands + - Publishes push-based STATUS frames at a configurable rate using multicast (with unicast fallback) + - Talks to robot hardware via a SerialTransport, or to a software simulator via MockSerialTransport + +- Client (`parol6.client`) + - AsyncRobotClient: async UDP client implementing commands, queries, waits and status streaming helpers + - RobotClient: synchronous facade over AsyncRobotClient for imperative scripts + - ServerManager: spawns the controller in a subprocess and manages its lifecycle + +- Simulator + - Simulator mode uses `MockSerialTransport` to emulate the serial protocol and robot dynamics (no hardware required) + - Toggle via the `SIMULATOR` system command (client methods `simulator_on()` / `simulator_off()`). The controller updates `PAROL6_FAKE_SERIAL` and seamlessly reinitializes the transport; when enabling, it syncs the simulator to the current controller state + - Important: the simulator helps validate sequences and visualize motion, but it cannot guarantee success on hardware. For example, multi‑joint jogging while fully extended with a heavy payload (e.g., the electric gripper) may succeed in simulation but fail on the real robot due to motor/current limits. + +Why multicast status? +- The controller pushes status via UDP multicast to avoid client-side polling and reduce command-channel contention +- Multicast works well for loopback and multiple observers; when multicast isn’t available, it falls back to unicast automatically + + +## Security and multiple senders +Important: The controller has no authentication or authorization mechanism. It will accept any correctly parsed command on its UDP port. Deploy only on a trusted local network and avoid exposing the controller to untrusted networks. + +Multiple senders: The controller intentionally allows commands from multiple senders concurrently. This enables a GUI or higher-level orchestrator to run sub‑programs while another sender can issue commands like STOP. While useful, this design increases the attack surface. Combine it with network isolation (trusted LAN), firewall rules, or host‑level ACLs. + + +## Control rate and performance +- Default control loop: `PAROL6_CONTROL_RATE_HZ=250` (chosen for consistent cross-platform behavior) +- Higher control rates require stronger hardware and generally produce smoother motion, but there are diminishing returns—the motion will not become infinitely smooth just by increasing the rate +- If the controller is falling behind you will see warnings like: + + `Control loop avg period degraded by +XX% (avg=Ys target=Zs); latest overrun=Ws` + + If you see this, or motion feels inconsistent, reduce `PAROL6_CONTROL_RATE_HZ` +- TRACE logging impacts performance; enable only when necessary (set `PAROL_TRACE=1` or use `--log-level=TRACE`) + + +## Streaming mode +- Streaming (`STREAM|ON` / `STREAM|OFF`; `client.stream_on()` / `client.stream_off()`) is intended for high-rate jogging and continuous updates +- In stream mode the server de-duplicates stale inputs, reduces ACK chatter, and can reuse the active streamable command fast-path to minimize overhead +- Use streaming for UI-driven jog or live teleoperation; use non-streaming for discrete motions and queued programs + +## Kinematics, IK, and singularities +Numerical IK vs. analytical: +- This project uses numerical IK (via robotics-toolbox-python) for flexibility: it adapts to tool changes and hardware modifications without deriving new closed forms +- Trade-offs: numerical IK can be slower and less robust near singularities compared to an ideal analytical solver + +Known behaviors and limitations: +- A slight stutter may occur when jogging near singularities; this is a known limitation of the current kinematics implementation +- The current robotics-toolbox-python backend used here does not expose null-space manipulation in C. As a result, some cartesian targets can fail to solve—joint 4 (J4) is particularly sensitive. Future work in the ported backend may add null-space features + +Adapting to modified hardware: +- Update `parol6/PAROL6_ROBOT.py` (gear ratios, joint limits, speed/acc/jerk limits) +- Update tool transforms in `parol6/tools.py` (4×4 SE3 matrices) +- Optionally update the URDF in `parol6/urdf_model/` for visualization or geometry changes +- Advanced: you can also add or replace DH joints programmatically without modifying the URDF. For example: + + ```python + # Example: replace the 3rd joint with a custom DH link without editing the URDF + from roboticstoolbox import Link, ET + from parol6.PAROL6_ROBOT import _cached_urdf + import roboticstoolbox as rtb + + base_links = list(_cached_urdf.elinks) + + # Create a new DH link (Revolute) – customize a, d, alpha, and any fixed offset + j3_custom = Link(ET.DH(a=0.045, d=0.0, alpha=0.0), name="J3_custom", parent=base_links[1]) + + # Rebuild link chain: keep upstream joints as-is, insert replacement, then reuse downstream links + all_links = [ + base_links[0], # J1 + base_links[1], # J2 + j3_custom, # new J3 + *base_links[3:], # J4..end reuse + ] + + robot = rtb.Robot(all_links, name=_cached_urdf.name) + # Apply a tool afterward if needed (see parol6.tools) + ``` + +- Note: The bundled URDF and STL assets have been adjusted from the originals to make robotics‑toolbox‑python Cartesian transforms behave correctly. Programmatic DH edits let you experiment without modifying those files. + + +## Tools +Currently supported tools (see `parol6/tools.py`): +- `NONE` (bare flange) +- `PNEUMATIC` (pneumatic gripper) + +Set tool at runtime from the client: ```python -from robot_api import * -import time - -# Send multiple commands non-blocking -cmd1 = move_robot_joints([90, -45, 90, 0, 45, 180], - duration=3, - wait_for_ack=True, - non_blocking=True) - -cmd2 = smooth_circle([200, 0, 200], radius=30, - duration=5, - wait_for_ack=True, - non_blocking=True) - -# Monitor both commands -while True: - status1 = check_command_status(cmd1) - status2 = check_command_status(cmd2) - - if status1 and status1['completed'] and status2 and status2['completed']: - print("Both commands completed!") - break - - time.sleep(0.1) +from parol6 import RobotClient +with RobotClient() as c: + c.set_tool("PNEUMATIC") ``` -## 7. Troubleshooting +Add a new tool by extending `TOOL_CONFIGS` with a name, description, and `transform` (SE3 → 4×4 matrix). -* **Serial Connection Issues**: Check COM port in Device Manager (Windows) and update `com_port.txt` -* **Command Not Executing**: Verify robot is homed and E-stop is not pressed -* **Tracking Not Working**: Ensure `wait_for_ack=True` is set for commands -* **IK Failures**: Target pose may be unreachable; check robot workspace limits -* **Smooth Motion Errors**: Verify waypoints are reachable and properly formatted -For additional support, refer to the [PAROL commander software repository](https://github.com/PCrnjak/PAROL-commander-software). +## Quickstart -Or you can head over to the [PAROL6 Discord channel](https://discord.com/invite/prjUvjmGpZ) for extra support +### Async client (recommended API) +```python +import asyncio +from parol6 import AsyncRobotClient + +async def main(): + async with AsyncRobotClient(host="127.0.0.1", port=5001) as client: + ready = await client.wait_for_server_ready(timeout=3) + print("server ready:", ready) + print("ping:", await client.ping()) + status = await client.get_status() + print("status keys:", list(status.keys()) if status else None) + +asyncio.run(main()) +``` -## Refactored Python API quick start (parol6) +### Sync client (convenience wrapper) +```python +from parol6 import RobotClient -The legacy examples use `robot_api.py`. In the refactored API, use the packaged client: +with RobotClient(host="127.0.0.1", port=5001) as client: + print("ping:", client.ping()) + print("pose:", client.get_pose()) +``` +### Starting/stopping the controller from Python ```python -from parol6 import RobotClient -# or: from parol6.client import AsyncRobotClient +from parol6 import manage_server, RobotClient + +mgr = manage_server(host="127.0.0.1", port=5001) # blocks until PING works +try: + with RobotClient() as client: + print("ready:", client.wait_for_server_ready(timeout=3)) + print("ping:", client.ping()) +finally: + mgr.stop_controller() +``` -client = RobotClient(host="127.0.0.1", port=5001, ack_port=5002) -# Ping server -assert client.ping() is not None +## Examples +Three runnable examples are provided under examples/ demonstrating typical usage. They intentionally mix simulator usage: +- `examples/async_client_quickstart.py` — managed server + `simulator_on()`; demonstrates `status_stream` and a small relative TRF move +- `examples/sync_client_quickstart.py` — assumes a running controller; no simulator calls; simple ping and status query +- `examples/manage_server_demo.py` — demonstrates `manage_server()` lifecycle; toggles simulator on/off -# Tracked home -result = client.home(wait_for_ack=True, timeout=15.0) -print(result) # {'status': 'QUEUED'/'COMPLETED'/..., 'command_id': '...', 'completed': bool, 'details': '...', 'ack_time': datetime or None} +Run an example from the repository root: -# Basic getters -angles = client.get_angles() -io = client.get_io() +```bash +python external/PAROL6-python-API/examples/manage_server_demo.py ``` -Notes: -- To disable auto-homing on server startup, set `PAROL6_NOAUTOHOME=1`. -- Smooth motion internals are now a modular package at `parol6/smooth_motion/` with stable re-exports in `parol6.smooth_motion`. + +## Environment variables +- `PAROL6_CONTROL_RATE_HZ` — control loop frequency in Hz (default 250) +- `PAROL6_STATUS_RATE_HZ` — STATUS broadcast rate in Hz (default 50) +- `PAROL6_STATUS_STALE_S` — skip broadcast if cache is older than this (default 0.2) +- `PAROL6_MCAST_GROUP` — multicast group for status (default 239.255.0.101) +- `PAROL6_MCAST_PORT` — multicast port for status (default 50510) +- `PAROL6_MCAST_TTL` — multicast TTL (default 1) +- `PAROL6_MCAST_IF` — interface/IP for multicast (default 127.0.0.1) +- `PAROL6_STATUS_TRANSPORT` — MULTICAST (default) or UNICAST +- `PAROL6_STATUS_UNICAST_HOST` — unicast target host (default 127.0.0.1) +- `PAROL6_CONTROLLER_IP` / `PAROL6_CONTROLLER_PORT` — bind host/port for controller +- `PAROL6_FORCE_ACK` — force ACK for motion commands regardless of policy +- `PAROL6_FAKE_SERIAL` — enable simulator ("1"/"true"/"on"); used internally by simulator_on/off +- `PAROL6_TX_KEEPALIVE_S` — TX keepalive period seconds for serial writes (default 0.2) +- `PAROL6_HOME_ANGLES_DEG` — CSV of 6 joint angles for home pose in simulation/tests +- `PAROL6_COM_FILE` — path to persistent COM port file (default `~/.parol6/com_port.txt`) +- `PAROL6_COM_PORT` / `PAROL6_SERIAL` — explicit serial port override (e.g., `/dev/ttyUSB0` or `COM3`) +- `PAROL_TRACE` — `1` enables TRACE logging level unless overridden by CLI + + +## Controller CLI +From `parol6-server` (or `python -m parol6.server.controller`): + +- `--host` — UDP host address (default 0.0.0.0) +- `--port` — UDP port (default 5001) +- `--serial` — Serial port (e.g., `/dev/ttyUSB0` or `COM3`) +- `--baudrate` — Serial baudrate (default 3000000) +- `--auto-home` — Queue HOME on startup (default off) +- `-v` / `-vv` / `-vvv` — Increase verbosity (INFO/DEBUG/TRACE) +- `-q` / `--quiet` — Set WARNING level +- `--log-level` — TRACE/DEBUG/INFO/WARNING/ERROR/CRITICAL (overrides -v/-q) + + +## FAQ / Troubleshooting +- I see `Control loop avg period degraded by …` warnings + - The loop is falling behind. Reduce `PAROL6_CONTROL_RATE_HZ` and ensure TRACE logging is disabled. +- Motion feels inconsistent or jittery on my machine + - Lower the control rate; avoid heavy background tasks; disable TRACE logging. +- There is a slight stutter when jogging near singularities + - Known limitation of the current numerical kinematics; try different approach angles or reduce speed. +- Some cartesian targets fail to solve (especially around J4) + - Without null-space control in the backend, some poses are hard to reach. Re-plan the path, adjust the target, or change the starting posture. Future backend updates may add null-space manipulation. + + +## Safety notes +- Keep physical E‑Stop accessible at all times when connected to hardware +- The headless controller can halt motion via `disable()/stop()` and reacts to E‑Stop inputs when on real hardware +- Prefer `simulator_on()` for development without hardware and validate motions before switching to real serial diff --git a/examples/async_client_quickstart.py b/examples/async_client_quickstart.py new file mode 100644 index 0000000..61f5ea1 --- /dev/null +++ b/examples/async_client_quickstart.py @@ -0,0 +1,56 @@ +""" +Async client quickstart for PAROL6. +- Starts a local headless controller at 127.0.0.1:5001 +- Enables simulator mode for safety +- Shows basic queries and status streaming + +Run from the repository root: + python external/PAROL6-python-API/examples/async_client_quickstart.py +""" + +import asyncio +from parol6 import AsyncRobotClient +from parol6.client.manager import managed_server + +HOST = "127.0.0.1" +PORT = 5001 + + +async def run_client() -> int: + async with AsyncRobotClient(host=HOST, port=PORT, timeout=2.0) as client: + ready = await client.wait_for_server_ready(timeout=5.0) + print(f"server ready: {ready}") + if not ready: + return 1 + + # Safety: enable simulator for this demo + ok = await client.simulator_on() + print(f"simulator_on: {ok}") + + print("ping:", await client.ping()) + pose_xyz = await client.get_pose_xyz() + print("pose xyz:", pose_xyz) + + # Consume one status broadcast + print("one status frame speeds:") + async for status in client.status_stream(): + print(status.get("speeds")) + break + + # Small relative TRF move (safe in simulator) + # Move +5mm in Z over 1.0s + moved = await client.move_cartesian_rel_trf([0, 0, 5, 0, 0, 0], duration=1.0) + print("move_cartesian_rel_trf ->", moved) + + return 0 + + +def main() -> None: + # Auto-start and stop controller for this example + with managed_server(host=HOST, port=PORT, normalize_logs=True): + code = asyncio.run(run_client()) + raise SystemExit(code) + + +if __name__ == "__main__": + main() diff --git a/examples/manage_server_demo.py b/examples/manage_server_demo.py new file mode 100644 index 0000000..882b6c2 --- /dev/null +++ b/examples/manage_server_demo.py @@ -0,0 +1,48 @@ +""" +Manage server lifecycle demonstration for PAROL6. +- Starts a local headless controller at 127.0.0.1:5001 +- Waits until ready, then connects with RobotClient +- Toggles simulator ON for a safe demo motion, then OFF +- Stops the controller on exit + +Run from the repository root: + python external/PAROL6-python-API/examples/manage_server_demo.py +""" + +from parol6 import manage_server, RobotClient + +HOST = "127.0.0.1" +PORT = 5001 + + +def main() -> None: + mgr = manage_server(host=HOST, port=PORT, normalize_logs=True) + try: + with RobotClient(host=HOST, port=PORT, timeout=2.0) as client: + ready = client.wait_for_server_ready(timeout=5.0) + print(f"server ready: {ready}") + if not ready: + raise SystemExit(1) + + print("ping:", client.ping()) + + # Enable simulator for a safe motion + sim_on = client.simulator_on() + print("simulator_on:", sim_on) + + if sim_on: + # Small relative TRF move: +3mm in Z over 0.8s + moved = client.move_cartesian_rel_trf([0, 0, 3, 0, 0, 0], duration=0.8) + print("move_cartesian_rel_trf ->", moved) + + # Demonstrate toggling simulator off again (no motion follows) + sim_off = client.simulator_off() + print("simulator_off:", sim_off) + + raise SystemExit(0) + finally: + mgr.stop_controller() + + +if __name__ == "__main__": + main() diff --git a/examples/sync_client_quickstart.py b/examples/sync_client_quickstart.py new file mode 100644 index 0000000..686e6f5 --- /dev/null +++ b/examples/sync_client_quickstart.py @@ -0,0 +1,29 @@ +""" +Sync client quickstart for PAROL6. +- Assumes a controller is already running at 127.0.0.1:5001 +- Does not enable simulator in this example +- Performs ping and basic queries + +Run from the repository root: + python external/PAROL6-python-API/examples/sync_client_quickstart.py +""" + +from parol6 import RobotClient + +HOST = "127.0.0.1" +PORT = 5001 + + +def main() -> None: + with RobotClient(host=HOST, port=PORT, timeout=2.0) as client: + ready = client.wait_for_server_ready(timeout=3.0) + print(f"server ready: {ready}") + print("ping:", client.ping()) + print("pose xyz:", client.get_pose_xyz()) + print("angles:", client.get_angles()) + code = 0 if ready else 1 + raise SystemExit(code) + + +if __name__ == "__main__": + main() diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py index 8b60739..baa490e 100644 --- a/parol6/PAROL6_ROBOT.py +++ b/parol6/PAROL6_ROBOT.py @@ -1,518 +1,605 @@ -# Clean, hierarchical, vectorized, and typed robot configuration and helpers -from dataclasses import dataclass -from math import pi -from typing import Final, Callable, Sequence, Union, Any -import logging -from numpy.typing import ArrayLike -import numpy as np -from numpy.typing import NDArray -from roboticstoolbox import DHRobot, RevoluteDH - -logger = logging.getLogger(__name__) - -# ----------------------------- -# Typing aliases -# ----------------------------- -IndexArg = Union[int, NDArray[np.int_], None] - -Vec6f = NDArray[np.float64] -Vec6i = NDArray[np.int32] -Limits2f = NDArray[np.float64] # shape (6,2) - -# ----------------------------- -# Kinematics and conversion constants -# ----------------------------- -Joint_num = 6 -Microstep = 32 -steps_per_revolution = 200 - -# Conversion constants -degree_per_step_constant: float = 360.0 / (Microstep * steps_per_revolution) -radian_per_step_constant: float = (2.0 * np.pi) / (Microstep * steps_per_revolution) -radian_per_sec_2_deg_per_sec_const: float = 360.0 / (2.0 * np.pi) -deg_per_sec_2_radian_per_sec_const: float = (2.0 * np.pi) / 360.0 - -# ----------------------------- -# Robot geometry (meters) -# ----------------------------- -a1 = 110.50 / 1000.0 -a2 = 23.42 / 1000.0 -a3 = 180.0 / 1000.0 -a4 = 43.5 / 1000.0 -a5 = 176.35 / 1000.0 -a6 = 62.8 / 1000.0 -a7 = 45.25 / 1000.0 - -# For electric gripper, these may change: -# a6 = 117 / 1000.0 -# a7 = 0 / 1000.0 - -alpha_DH = np.array([-pi / 2, pi, pi / 2, -pi / 2, pi / 2, pi], dtype=np.float64) - -# DH Robot model -robot = DHRobot( - [ - RevoluteDH(d=a1, a=a2, alpha=float(alpha_DH[0])), - RevoluteDH(a=a3, d=0.0, alpha=float(alpha_DH[1])), - RevoluteDH(alpha=float(alpha_DH[2]), a=-a4), - RevoluteDH(d=-a5, a=0.0, alpha=float(alpha_DH[3])), - RevoluteDH(a=0.0, d=0.0, alpha=float(alpha_DH[4])), - RevoluteDH(alpha=float(alpha_DH[5]), a=-a7, d=-a6), - ], - name="PAROL6", -) - -# ----------------------------- -# Raw parameter arrays -# ----------------------------- -# Limits (deg) you get after homing and moving to extremes -_joint_limits_degree: Limits2f = np.array( - [ - [-123.046875, 123.046875], - [-145.0088, -3.375], - [107.866, 287.8675], - [-105.46975, 105.46975], - [-90.0, 90.0], - [0.0, 360.0], - ], - dtype=np.float64, -) - -_joint_limits_radian: Limits2f = np.deg2rad(_joint_limits_degree).astype(np.float64) - -# Reduction ratio per joint -_joint_ratio: NDArray[np.float64] = np.array( - [6.4, 20.0, 20.0 * (38.0 / 42.0), 4.0, 4.0, 10.0], dtype=np.float64 -) - -# Joint speeds (steps/s) -_joint_max_speed: Vec6i = np.array([6500, 18000, 20000, 20000, 22000, 22000], dtype=np.int32) -_joint_min_speed: Vec6i = np.array([100, 100, 100, 100, 100, 100], dtype=np.int32) - -# Jog speeds (steps/s) -_joint_max_jog_speed: Vec6i = np.array([1500, 3000, 3600, 7000, 7000, 18000], dtype=np.int32) -_joint_min_jog_speed: Vec6i = np.array([100, 100, 100, 100, 100, 100], dtype=np.int32) - -# Joint accelerations (rad/s^2) - scalar limits applied per joint -_joint_max_acc_rad: float = float(32000) -_joint_min_acc_rad: float = float(100) - -# Maximum jerk limits (steps/s^3) per joint -_joint_max_jerk: Vec6i = np.array([1600, 1000, 1100, 3000, 3000, 2000], dtype=np.int32) - -# Cartesian limits -_cart_linear_velocity_min_JOG: float = float(0.002) -_cart_linear_velocity_max_JOG: float = float(0.06) - -_cart_linear_velocity_min: float = float(0.002) -_cart_linear_velocity_max: float = float(0.06) - -_cart_linear_acc_min: float = float(0.002) -_cart_linear_acc_max: float = float(0.06) - -_cart_angular_velocity_min: float = float(0.7) # deg/s -_cart_angular_velocity_max: float = float(25.0) # deg/s - -# Standby positions -_standby_deg: Vec6f = np.array([90.0, -90.0, 180.0, 0.0, 0.0, 180.0], dtype=np.float64) -_standby_rad: Vec6f = np.deg2rad(_standby_deg).astype(np.float64) - -# ----------------------------- -# Vectorized helpers (ops) -# ----------------------------- -def _apply_ratio(values: NDArray, idx: IndexArg) -> NDArray: - """ - Apply per-joint gear ratio. - If idx is None, broadcast ratio across last dimension (length 6). - If idx is an int or ndarray of ints, select ratios accordingly. - """ - if idx is None: - return values * _joint_ratio - idx_arr = np.asarray(idx) - return values * _joint_ratio[idx_arr] - - -def deg_to_steps(deg: ArrayLike, idx: IndexArg = None) -> np.int32 | NDArray[np.int32]: - """Degrees to steps (gear ratio aware). Fast scalar path when idx is int.""" - if isinstance(idx, (int, np.integer)) and np.isscalar(deg): - return np.int32((deg / degree_per_step_constant) * _joint_ratio[idx]) # type: ignore - deg_arr = np.asarray(deg, dtype=np.float64) - steps_f = _apply_ratio(deg_arr / degree_per_step_constant, idx) - return steps_f.astype(np.int32, copy=False) - - -def steps_to_deg(steps: ArrayLike, idx: IndexArg = None) -> np.float64 | NDArray[np.float64]: - """Steps to degrees (gear ratio aware). Fast scalar path when idx is int.""" - if isinstance(idx, (int, np.integer)) and np.isscalar(steps): - return np.float64((steps * degree_per_step_constant) / _joint_ratio[idx]) # type: ignore - steps_arr = np.asarray(steps, dtype=np.float64) - ratio = _joint_ratio if idx is None else _joint_ratio[np.asarray(idx)] - return (steps_arr * degree_per_step_constant) / ratio - - -def rad_to_steps(rad: ArrayLike, idx: IndexArg = None) -> np.int32 | NDArray[np.int32]: - """Radians to steps. Fast scalar path when idx is int.""" - if isinstance(idx, (int, np.integer)) and np.isscalar(rad): - return np.int32((rad / radian_per_step_constant) * _joint_ratio[idx]) # type: ignore - rad_arr = np.asarray(rad, dtype=np.float64) - deg_arr = np.rad2deg(rad_arr) - return deg_to_steps(deg_arr, idx) - - -def steps_to_rad(steps: ArrayLike, idx: IndexArg = None) -> np.float64 | NDArray[np.float64]: - """Steps to radians. Fast scalar path when idx is int.""" - if isinstance(idx, (int, np.integer)) and np.isscalar(steps): - return np.float64((steps * radian_per_step_constant) / _joint_ratio[idx]) # type: ignore - deg_arr = steps_to_deg(steps, idx) - return np.deg2rad(deg_arr) - - -def speed_steps_to_deg(sps: ArrayLike, idx: IndexArg = None) -> np.float64 | NDArray[np.float64]: - """Speed: steps/s to deg/s. Fast scalar path when idx is int.""" - if isinstance(idx, (int, np.integer)) and np.isscalar(sps): - return np.float64((sps * degree_per_step_constant) / _joint_ratio[idx]) # type: ignore - sps_arr = np.asarray(sps, dtype=np.float64) - ratio = _joint_ratio if idx is None else _joint_ratio[np.asarray(idx)] - return (sps_arr * degree_per_step_constant) / ratio - - -def speed_deg_to_steps(dps: ArrayLike, idx: IndexArg = None) -> np.int32 | NDArray[np.int32]: - """Speed: deg/s to steps/s. Fast scalar path when idx is int.""" - if isinstance(idx, (int, np.integer)) and np.isscalar(dps): - return np.int32((dps / degree_per_step_constant) * _joint_ratio[idx]) # type: ignore - dps_arr = np.asarray(dps, dtype=np.float64) - stepsps = _apply_ratio(dps_arr / degree_per_step_constant, idx) - return stepsps.astype(np.int32, copy=False) - - -def speed_steps_to_rad(sps: ArrayLike, idx: IndexArg = None) -> np.float64 | NDArray[np.float64]: - """Speed: steps/s to rad/s. Fast scalar path when idx is int.""" - if isinstance(idx, (int, np.integer)) and np.isscalar(sps): - return np.float64((sps * radian_per_step_constant) / _joint_ratio[idx]) # type: ignore - sps_arr = np.asarray(sps, dtype=np.float64) - ratio = _joint_ratio if idx is None else _joint_ratio[np.asarray(idx)] - return (sps_arr * radian_per_step_constant) / ratio - - -def speed_rad_to_steps(rps: ArrayLike, idx: IndexArg = None) -> np.int32 | NDArray[np.int32]: - """Speed: rad/s to steps/s. Fast scalar path when idx is int.""" - if isinstance(idx, (int, np.integer)) and np.isscalar(rps): - return np.int32((rps / radian_per_step_constant) * _joint_ratio[idx]) # type: ignore - rps_arr = np.asarray(rps, dtype=np.float64) - stepsps = _apply_ratio(rps_arr / radian_per_step_constant, idx) - return stepsps.astype(np.int32, copy=False) - - -def clip_speed_to_joint_limits(sps: ArrayLike) -> NDArray[np.int32]: - """Clip steps/s vector to per-joint limits (int32).""" - sps_arr = np.asarray(sps, dtype=np.int32) - return np.clip(sps_arr, -_joint_max_speed, _joint_max_speed).astype(np.int32, copy=False) - - -def clamp_steps_delta( - prev_steps: ArrayLike, target_steps: ArrayLike, dt: float, safety: float = 1.2 -) -> NDArray[np.int32]: - """ - Clamp per-tick step change to max allowed based on joint.max_speed and dt. - Returns int32 array. - """ - prev_arr = np.asarray(prev_steps, dtype=np.int32) - tgt_arr = np.asarray(target_steps, dtype=np.int32) - step_diff = tgt_arr - prev_arr - max_step_diff = (_joint_max_speed * dt * safety).astype(np.int32) - sign = np.sign(step_diff).astype(np.int32) - over = np.abs(step_diff) > max_step_diff - clamped = tgt_arr.copy() - clamped[over] = prev_arr[over] + sign[over] * max_step_diff[over] - return clamped.astype(np.int32, copy=False) - -# ----------------------------- -# Limits (steps) derived from deg -# ----------------------------- -_joint_limits_steps_list: list[list[int]] = [] -for j in range(6): - mn_deg, mx_deg = float(_joint_limits_degree[j, 0]), float(_joint_limits_degree[j, 1]) - mn_steps = int(deg_to_steps(mn_deg, idx=j)) - mx_steps = int(deg_to_steps(mx_deg, idx=j)) - _joint_limits_steps_list.append([mn_steps, mx_steps]) -_joint_limits_steps: NDArray[np.int32] = np.array(_joint_limits_steps_list, dtype=np.int32) # (6,2) - -# ----------------------------- -# Typed hierarchical API -# ----------------------------- -@dataclass(frozen=True) -class JointLimits: - deg: Limits2f - rad: Limits2f - steps: NDArray[np.int32] - - -@dataclass(frozen=True) -class JointJogSpeed: - max: Vec6i - min: Vec6i - - -@dataclass(frozen=True) -class JointSpeed: - max: Vec6i - min: Vec6i - jog: JointJogSpeed - - -@dataclass(frozen=True) -class JointAcc: - max_rad: float - min_rad: float - - -@dataclass(frozen=True) -class JointJerk: - max: Vec6i - - -@dataclass(frozen=True) -class Standby: - deg: Vec6f - rad: Vec6f - - -@dataclass(frozen=True) -class Joint: - limits: JointLimits - speed: JointSpeed - acc: JointAcc - jerk: JointJerk - ratio: NDArray[np.float64] - standby: Standby - - -@dataclass(frozen=True) -class RangeF: - min: float - max: float - - -@dataclass(frozen=True) -class CartVel: - linear: RangeF - jog: RangeF - angular: RangeF - - -@dataclass(frozen=True) -class CartAcc: - linear: RangeF - - -@dataclass(frozen=True) -class Cart: - vel: CartVel - acc: CartAcc - - -@dataclass(frozen=True) -class Conv: - degree_per_step: float - radian_per_step: float - rad_sec_to_deg_sec: float - deg_sec_to_rad_sec: float - - -@dataclass(frozen=True) -class Ops: - # Use Callable[..., T] to allow optional idx parameter without arity errors in type checkers - deg_to_steps: Callable[..., np.int32 | NDArray[np.int32]] - steps_to_deg: Callable[..., np.float64 | NDArray[np.float64]] - rad_to_steps: Callable[..., np.int32 | NDArray[np.int32]] - steps_to_rad: Callable[..., np.float64 | NDArray[np.float64]] - speed_deg_to_steps: Callable[..., np.int32 | NDArray[np.int32]] - speed_steps_to_deg: Callable[..., np.float64 | NDArray[np.float64]] - speed_rad_to_steps: Callable[..., np.int32 | NDArray[np.int32]] - speed_steps_to_rad: Callable[..., np.float64 | NDArray[np.float64]] - clip_speed_to_joint_limits: Callable[[ArrayLike], NDArray[np.int32]] - clamp_steps_delta: Callable[[ArrayLike, ArrayLike, float, float], NDArray[np.int32]] - - -joint: Final[Joint] = Joint( - limits=JointLimits( - deg=_joint_limits_degree, - rad=_joint_limits_radian, - steps=_joint_limits_steps, - ), - speed=JointSpeed( - max=_joint_max_speed, - min=_joint_min_speed, - jog=JointJogSpeed( - max=_joint_max_jog_speed, - min=_joint_min_jog_speed, - ), - ), - acc=JointAcc( - max_rad=_joint_max_acc_rad, - min_rad=_joint_min_acc_rad, - ), - jerk=JointJerk( - max=_joint_max_jerk, - ), - ratio=_joint_ratio, - standby=Standby( - deg=_standby_deg, - rad=_standby_rad, - ), -) - -cart: Final[Cart] = Cart( - vel=CartVel( - linear=RangeF(min=_cart_linear_velocity_min, max=_cart_linear_velocity_max), - jog=RangeF(min=_cart_linear_velocity_min_JOG, max=_cart_linear_velocity_max_JOG), - angular=RangeF(min=_cart_angular_velocity_min, max=_cart_angular_velocity_max), - ), - acc=CartAcc( - linear=RangeF(min=_cart_linear_acc_min, max=_cart_linear_acc_max), - ), -) - -conv: Final[Conv] = Conv( - degree_per_step=degree_per_step_constant, - radian_per_step=radian_per_step_constant, - rad_sec_to_deg_sec=radian_per_sec_2_deg_per_sec_const, - deg_sec_to_rad_sec=deg_per_sec_2_radian_per_sec_const, -) - -ops: Final[Ops] = Ops( - deg_to_steps=deg_to_steps, - steps_to_deg=steps_to_deg, - rad_to_steps=rad_to_steps, - steps_to_rad=steps_to_rad, - speed_deg_to_steps=speed_deg_to_steps, - speed_steps_to_deg=speed_steps_to_deg, - speed_rad_to_steps=speed_rad_to_steps, - speed_steps_to_rad=speed_steps_to_rad, - clip_speed_to_joint_limits=clip_speed_to_joint_limits, - clamp_steps_delta=clamp_steps_delta, -) - -# ----------------------------- -# Fast, vectorized limit checking with edge-triggered logging -# ----------------------------- -_last_violation_mask = np.zeros(6, dtype=bool) -_last_any_violation = False -# TODO: confirm whether this is actually faster than the previous loop based approach -def check_limits(q: ArrayLike, target_q: ArrayLike | None = None, allow_recovery: bool = True, *, log: bool = True) -> bool: - """ - Vectorized limits check in radians. - - q: current joint angles in radians (array-like) - - target_q: optional target joint angles in radians (array-like) - - allow_recovery: allow movement that heads back toward valid range if currently violating - - log: emit edge-triggered warning/info logs on violation state changes - - Returns True if move is allowed (within limits or valid recovery), False otherwise. - """ - global _last_violation_mask, _last_any_violation - - q_arr = np.asarray(q, dtype=np.float64).reshape(-1) - mn = joint.limits.rad[:, 0] - mx = joint.limits.rad[:, 1] - - below = q_arr < mn - above = q_arr > mx - cur_viol = below | above - - if target_q is None: - ok_mask = ~cur_viol - t_below = t_above = None - else: - t = np.asarray(target_q, dtype=np.float64).reshape(-1) - t_below = t < mn - t_above = t > mx - t_viol = t_below | t_above - if allow_recovery: - rec_ok = (above & (t <= q_arr)) | (below & (t >= q_arr)) - else: - rec_ok = np.zeros(6, dtype=bool) - ok_mask = (~cur_viol & ~t_viol) | (cur_viol & rec_ok) - - all_ok = bool(np.all(ok_mask)) - - if log: - viol = ~ok_mask - any_viol = bool(np.any(viol)) - - # Edge-triggered violation logs - if any_viol and (np.any(viol != _last_violation_mask) or not _last_any_violation): - idxs = np.where(viol)[0] - tokens = [] - for i in idxs: - if cur_viol[i]: - tokens.append(f"J{i+1}:" + ("curmax")) - else: - # target violates - if t_below is not None and t_below[i]: - tokens.append(f"J{i+1}:targetmax") - else: - tokens.append(f"J{i+1}:violation") - logger.warning("LIMIT VIOLATION: %s", " ".join(tokens)) - elif (not any_viol) and _last_any_violation: - logger.info("Limits back in range") - - _last_violation_mask[:] = viol - _last_any_violation = any_viol - - return all_ok - - -def check_limits_mask(q: ArrayLike, target_q: ArrayLike | None = None, allow_recovery: bool = True) -> NDArray[np.bool_]: - """Return per-joint boolean mask (True if OK for that joint).""" - q_arr = np.asarray(q, dtype=np.float64).reshape(-1) - mn = joint.limits.rad[:, 0] - mx = joint.limits.rad[:, 1] - below = q_arr < mn - above = q_arr > mx - cur_viol = below | above - - if target_q is None: - return ~cur_viol - t = np.asarray(target_q, dtype=np.float64).reshape(-1) - t_below = t < mn - t_above = t > mx - t_viol = t_below | t_above - if allow_recovery: - rec_ok = (above & (t <= q_arr)) | (below & (t >= q_arr)) - else: - rec_ok = np.zeros(6, dtype=bool) - ok_mask = (~cur_viol & ~t_viol) | (cur_viol & rec_ok) - return ok_mask - -# ----------------------------- -# CAN helpers and bitfield utils (used by transports/gripper) -# ----------------------------- -def extract_from_can_id(can_id: int): - id2 = (can_id >> 7) & 0xF - can_command = (can_id >> 1) & 0x3F - error_bit = can_id & 0x1 - return id2, can_command, error_bit - -def combine_2_can_id(id2: int, can_command: int, error_bit: int) -> int: - can_id = 0 - can_id |= (id2 & 0xF) << 7 - can_id |= (can_command & 0x3F) << 1 - can_id |= (error_bit & 0x1) - return can_id - -def fuse_bitfield_2_bytearray(var_in): - number = 0 - for b in var_in: - number = (2 * number) + int(b) - return bytes([number]) - -def split_2_bitfield(var_in: int): - return [(var_in >> i) & 1 for i in range(7, -1, -1)] - -if __name__ == "__main__": - # Simple sanity prints - j_step_rad = steps_to_rad(np.array([1, 1, 1, 1, 1, 1], dtype=np.int32)) - print("Smallest step (deg):", np.rad2deg(j_step_rad)) - print("Standby rad:", joint.standby.rad) +# Clean, hierarchical, vectorized, and typed robot configuration and helpers +import logging +from collections.abc import Callable +from dataclasses import dataclass +from pathlib import Path +from typing import Final, Union + +import numpy as np +import roboticstoolbox as rtb +from numpy.typing import ArrayLike, NDArray +from roboticstoolbox import ET, Link +from roboticstoolbox.tools.urdf import URDF +from spatialmath import SE3 + +from parol6.tools import get_tool_transform + +logger = logging.getLogger(__name__) + +# ----------------------------- +# Typing aliases +# ----------------------------- +IndexArg = Union[int, NDArray[np.int_], None] + +Vec6f = NDArray[np.float64] +Vec6i = NDArray[np.int32] +Limits2f = NDArray[np.float64] # shape (6,2) + +# ----------------------------- +# Kinematics and conversion constants +# ----------------------------- +Joint_num = 6 +Microstep = 32 +steps_per_revolution = 200 + +# Conversion constants +degree_per_step_constant: float = 360.0 / (Microstep * steps_per_revolution) +radian_per_step_constant: float = (2.0 * np.pi) / (Microstep * steps_per_revolution) +radian_per_sec_2_deg_per_sec_const: float = 360.0 / (2.0 * np.pi) +deg_per_sec_2_radian_per_sec_const: float = (2.0 * np.pi) / 360.0 + +# ----------------------------- +# Joint limits +# ----------------------------- +# Limits (deg) you get after homing and moving to extremes +_joint_limits_degree: Limits2f = np.array( + [ + [-123.046875, 123.046875], + [-145.0088, -3.375], + [107.866, 287.8675], + [-105.46975, 105.46975], + [-90.0, 90.0], + [0.0, 360.0], + ], + dtype=np.float64, +) + +_joint_limits_radian: Limits2f = np.deg2rad(_joint_limits_degree).astype(np.float64) + + +# URDF-based robot model (frames/limits aligned with controller) +def _load_urdf() -> URDF: + """Load and cache the URDF object for robot reconstruction.""" + base_path = Path(__file__).resolve().parent / "urdf_model" + urdf_path = base_path / "urdf" / "PAROL6.urdf" + urdf_string = urdf_path.read_text(encoding="utf-8") + return URDF.loadstr(urdf_string, str(urdf_path), base_path=base_path) + + +# Cache the URDF object (parsed once, reused for robot reconstruction) +_cached_urdf = _load_urdf() + +# Current robot instance (rebuilt when tool changes) +robot = None + + +def apply_tool(tool_name: str) -> None: + """ + Rebuild the robot with the specified tool as an additional link. + This ensures the tool transform is properly integrated into the kinematic chain + and affects forward kinematics calculations. + + Parameters + ---------- + tool_name : str + Name of the tool from tools.TOOL_CONFIGS + """ + global robot + + # Get tool transform + T_tool = get_tool_transform(tool_name) + + # Get the base elinks from cached URDF + base_links = list(_cached_urdf.elinks) + + # Create a tool link if there's a non-identity transform + if tool_name != "NONE" and not np.allclose(T_tool, np.eye(4)): + # Create an ELink for the tool + # The tool is a fixed transform from the last joint + tool_link = Link( + ET.SE3(SE3(T_tool)), + name=f"tool_{tool_name}", + parent=base_links[-1], # Attach to the last link + ) + + # Add tool link to the chain + all_links = base_links + [tool_link] + logger.info(f"Applied tool '{tool_name}' to robot model as link") + else: + all_links = base_links + logger.info(f"Applied tool '{tool_name}' (no additional link needed)") + + # Create robot with the complete link chain + robot = rtb.Robot( + all_links, + name=_cached_urdf.name, + ) + + +# Initialize with no tool +apply_tool("NONE") + +# ----------------------------- +# Additional raw parameter arrays +# ----------------------------- +# Reduction ratio per joint +_joint_ratio: NDArray[np.float64] = np.array( + [6.4, 20.0, 20.0 * (38.0 / 42.0), 4.0, 4.0, 10.0], dtype=np.float64 +) + +# Joint speeds (steps/s) +_joint_max_speed: Vec6i = np.array( + [6500, 18000, 20000, 20000, 22000, 22000], dtype=np.int32 +) +_joint_min_speed: Vec6i = np.array([100, 100, 100, 100, 100, 100], dtype=np.int32) + +# Jog speeds (steps/s) +_joint_max_jog_speed: Vec6i = np.array( + [1500, 3000, 3600, 7000, 7000, 18000], dtype=np.int32 +) +_joint_min_jog_speed: Vec6i = np.array([100, 100, 100, 100, 100, 100], dtype=np.int32) + +# Joint accelerations (rad/s^2) - scalar limits applied per joint +_joint_max_acc_rad: float = float(32000) +_joint_min_acc_rad: float = float(100) + +# Maximum jerk limits (steps/s^3) per joint +_joint_max_jerk: Vec6i = np.array([1600, 1000, 1100, 3000, 3000, 2000], dtype=np.int32) + +# Cartesian limits +_cart_linear_velocity_min_JOG: float = 0.002 +_cart_linear_velocity_max_JOG: float = 0.06 + +_cart_linear_velocity_min: float = 0.002 +_cart_linear_velocity_max: float = 0.06 + +_cart_linear_acc_min: float = 0.002 +_cart_linear_acc_max: float = 0.06 + +_cart_angular_velocity_min: float = 0.7 # deg/s +_cart_angular_velocity_max: float = 25.0 # deg/s + +# Standby positions +_standby_deg: Vec6f = np.array([90.0, -90.0, 180.0, 0.0, 0.0, 180.0], dtype=np.float64) +_standby_rad: Vec6f = np.deg2rad(_standby_deg).astype(np.float64) + + +# ----------------------------- +# Vectorized helpers (ops) +# ----------------------------- +def _apply_ratio(values: NDArray, idx: IndexArg) -> NDArray: + """ + Apply per-joint gear ratio. + If idx is None, broadcast ratio across last dimension (length 6). + If idx is an int or ndarray of ints, select ratios accordingly. + """ + if idx is None: + return values * _joint_ratio + idx_arr = np.asarray(idx) + return values * _joint_ratio[idx_arr] + + +def deg_to_steps(deg: ArrayLike, idx: IndexArg = None) -> np.int32 | NDArray[np.int32]: + """Degrees to steps (gear ratio aware). Fast scalar path when idx is int.""" + if isinstance(idx, (int, np.integer)) and np.isscalar(deg): + return np.int32((deg / degree_per_step_constant) * _joint_ratio[idx]) # type: ignore + deg_arr = np.asarray(deg, dtype=np.float64) + steps_f = _apply_ratio(deg_arr / degree_per_step_constant, idx) + return steps_f.astype(np.int32, copy=False) + + +def steps_to_deg( + steps: ArrayLike, idx: IndexArg = None +) -> np.float64 | NDArray[np.float64]: + """Steps to degrees (gear ratio aware). Fast scalar path when idx is int.""" + if isinstance(idx, (int, np.integer)) and np.isscalar(steps): + return np.float64((steps * degree_per_step_constant) / _joint_ratio[idx]) # type: ignore + steps_arr = np.asarray(steps, dtype=np.float64) + ratio = _joint_ratio if idx is None else _joint_ratio[np.asarray(idx)] + return (steps_arr * degree_per_step_constant) / ratio + + +def rad_to_steps(rad: ArrayLike, idx: IndexArg = None) -> np.int32 | NDArray[np.int32]: + """Radians to steps. Fast scalar path when idx is int.""" + if isinstance(idx, (int, np.integer)) and np.isscalar(rad): + return np.int32((rad / radian_per_step_constant) * _joint_ratio[idx]) # type: ignore + rad_arr = np.asarray(rad, dtype=np.float64) + deg_arr = np.rad2deg(rad_arr) + return deg_to_steps(deg_arr, idx) + + +def steps_to_rad( + steps: ArrayLike, idx: IndexArg = None +) -> np.float64 | NDArray[np.float64]: + """Steps to radians. Fast scalar path when idx is int.""" + if isinstance(idx, (int, np.integer)) and np.isscalar(steps): + return np.float64((steps * radian_per_step_constant) / _joint_ratio[idx]) # type: ignore + deg_arr = steps_to_deg(steps, idx) + return np.deg2rad(deg_arr) + + +def speed_steps_to_deg( + sps: ArrayLike, idx: IndexArg = None +) -> np.float64 | NDArray[np.float64]: + """Speed: steps/s to deg/s. Fast scalar path when idx is int.""" + if isinstance(idx, (int, np.integer)) and np.isscalar(sps): + return np.float64((sps * degree_per_step_constant) / _joint_ratio[idx]) # type: ignore + sps_arr = np.asarray(sps, dtype=np.float64) + ratio = _joint_ratio if idx is None else _joint_ratio[np.asarray(idx)] + return (sps_arr * degree_per_step_constant) / ratio + + +def speed_deg_to_steps( + dps: ArrayLike, idx: IndexArg = None +) -> np.int32 | NDArray[np.int32]: + """Speed: deg/s to steps/s. Fast scalar path when idx is int.""" + if isinstance(idx, (int, np.integer)) and np.isscalar(dps): + return np.int32((dps / degree_per_step_constant) * _joint_ratio[idx]) # type: ignore + dps_arr = np.asarray(dps, dtype=np.float64) + stepsps = _apply_ratio(dps_arr / degree_per_step_constant, idx) + return stepsps.astype(np.int32, copy=False) + + +def speed_steps_to_rad( + sps: ArrayLike, idx: IndexArg = None +) -> np.float64 | NDArray[np.float64]: + """Speed: steps/s to rad/s. Fast scalar path when idx is int.""" + if isinstance(idx, (int, np.integer)) and np.isscalar(sps): + return np.float64((sps * radian_per_step_constant) / _joint_ratio[idx]) # type: ignore + sps_arr = np.asarray(sps, dtype=np.float64) + ratio = _joint_ratio if idx is None else _joint_ratio[np.asarray(idx)] + return (sps_arr * radian_per_step_constant) / ratio + + +def speed_rad_to_steps( + rps: ArrayLike, idx: IndexArg = None +) -> np.int32 | NDArray[np.int32]: + """Speed: rad/s to steps/s. Fast scalar path when idx is int.""" + if isinstance(idx, (int, np.integer)) and np.isscalar(rps): + return np.int32((rps / radian_per_step_constant) * _joint_ratio[idx]) # type: ignore + rps_arr = np.asarray(rps, dtype=np.float64) + stepsps = _apply_ratio(rps_arr / radian_per_step_constant, idx) + return stepsps.astype(np.int32, copy=False) + + +def clip_speed_to_joint_limits(sps: ArrayLike) -> NDArray[np.int32]: + """Clip steps/s vector to per-joint limits (int32).""" + sps_arr = np.asarray(sps, dtype=np.int32) + return np.clip(sps_arr, -_joint_max_speed, _joint_max_speed).astype( + np.int32, copy=False + ) + + +def clamp_steps_delta( + prev_steps: ArrayLike, target_steps: ArrayLike, dt: float, safety: float = 1.2 +) -> NDArray[np.int32]: + """ + Clamp per-tick step change to max allowed based on joint.max_speed and dt. + Returns int32 array. + """ + prev_arr = np.asarray(prev_steps, dtype=np.int32) + tgt_arr = np.asarray(target_steps, dtype=np.int32) + step_diff = tgt_arr - prev_arr + max_step_diff = (_joint_max_speed * dt * safety).astype(np.int32) + sign = np.sign(step_diff).astype(np.int32) + over = np.abs(step_diff) > max_step_diff + clamped = tgt_arr.copy() + clamped[over] = prev_arr[over] + sign[over] * max_step_diff[over] + return clamped.astype(np.int32, copy=False) + + +# ----------------------------- +# Limits (steps) derived from deg +# ----------------------------- +_joint_limits_steps_list: list[list[int]] = [] +for j in range(6): + mn_deg, mx_deg = ( + float(_joint_limits_degree[j, 0]), + float(_joint_limits_degree[j, 1]), + ) + mn_steps = int(deg_to_steps(mn_deg, idx=j)) + mx_steps = int(deg_to_steps(mx_deg, idx=j)) + _joint_limits_steps_list.append([mn_steps, mx_steps]) +_joint_limits_steps: NDArray[np.int32] = np.array( + _joint_limits_steps_list, dtype=np.int32 +) # (6,2) + + +# ----------------------------- +# Typed hierarchical API +# ----------------------------- +@dataclass(frozen=True) +class JointLimits: + deg: Limits2f + rad: Limits2f + steps: NDArray[np.int32] + + +@dataclass(frozen=True) +class JointJogSpeed: + max: Vec6i + min: Vec6i + + +@dataclass(frozen=True) +class JointSpeed: + max: Vec6i + min: Vec6i + jog: JointJogSpeed + + +@dataclass(frozen=True) +class JointAcc: + max_rad: float + min_rad: float + + +@dataclass(frozen=True) +class JointJerk: + max: Vec6i + + +@dataclass(frozen=True) +class Standby: + deg: Vec6f + rad: Vec6f + + +@dataclass(frozen=True) +class Joint: + limits: JointLimits + speed: JointSpeed + acc: JointAcc + jerk: JointJerk + ratio: NDArray[np.float64] + standby: Standby + + +@dataclass(frozen=True) +class RangeF: + min: float + max: float + + +@dataclass(frozen=True) +class CartVel: + linear: RangeF + jog: RangeF + angular: RangeF + + +@dataclass(frozen=True) +class CartAcc: + linear: RangeF + + +@dataclass(frozen=True) +class Cart: + vel: CartVel + acc: CartAcc + + +@dataclass(frozen=True) +class Conv: + degree_per_step: float + radian_per_step: float + rad_sec_to_deg_sec: float + deg_sec_to_rad_sec: float + + +@dataclass(frozen=True) +class Ops: + # Use Callable[..., T] to allow optional idx parameter without arity errors in type checkers + deg_to_steps: Callable[..., np.int32 | NDArray[np.int32]] + steps_to_deg: Callable[..., np.float64 | NDArray[np.float64]] + rad_to_steps: Callable[..., np.int32 | NDArray[np.int32]] + steps_to_rad: Callable[..., np.float64 | NDArray[np.float64]] + speed_deg_to_steps: Callable[..., np.int32 | NDArray[np.int32]] + speed_steps_to_deg: Callable[..., np.float64 | NDArray[np.float64]] + speed_rad_to_steps: Callable[..., np.int32 | NDArray[np.int32]] + speed_steps_to_rad: Callable[..., np.float64 | NDArray[np.float64]] + clip_speed_to_joint_limits: Callable[[ArrayLike], NDArray[np.int32]] + clamp_steps_delta: Callable[[ArrayLike, ArrayLike, float, float], NDArray[np.int32]] + + +joint: Final[Joint] = Joint( + limits=JointLimits( + deg=_joint_limits_degree, + rad=_joint_limits_radian, + steps=_joint_limits_steps, + ), + speed=JointSpeed( + max=_joint_max_speed, + min=_joint_min_speed, + jog=JointJogSpeed( + max=_joint_max_jog_speed, + min=_joint_min_jog_speed, + ), + ), + acc=JointAcc( + max_rad=_joint_max_acc_rad, + min_rad=_joint_min_acc_rad, + ), + jerk=JointJerk( + max=_joint_max_jerk, + ), + ratio=_joint_ratio, + standby=Standby( + deg=_standby_deg, + rad=_standby_rad, + ), +) + +cart: Final[Cart] = Cart( + vel=CartVel( + linear=RangeF(min=_cart_linear_velocity_min, max=_cart_linear_velocity_max), + jog=RangeF( + min=_cart_linear_velocity_min_JOG, max=_cart_linear_velocity_max_JOG + ), + angular=RangeF(min=_cart_angular_velocity_min, max=_cart_angular_velocity_max), + ), + acc=CartAcc( + linear=RangeF(min=_cart_linear_acc_min, max=_cart_linear_acc_max), + ), +) + +conv: Final[Conv] = Conv( + degree_per_step=degree_per_step_constant, + radian_per_step=radian_per_step_constant, + rad_sec_to_deg_sec=radian_per_sec_2_deg_per_sec_const, + deg_sec_to_rad_sec=deg_per_sec_2_radian_per_sec_const, +) + +ops: Final[Ops] = Ops( + deg_to_steps=deg_to_steps, + steps_to_deg=steps_to_deg, + rad_to_steps=rad_to_steps, + steps_to_rad=steps_to_rad, + speed_deg_to_steps=speed_deg_to_steps, + speed_steps_to_deg=speed_steps_to_deg, + speed_rad_to_steps=speed_rad_to_steps, + speed_steps_to_rad=speed_steps_to_rad, + clip_speed_to_joint_limits=clip_speed_to_joint_limits, + clamp_steps_delta=clamp_steps_delta, +) + +# ----------------------------- +# Fast, vectorized limit checking with edge-triggered logging +# ----------------------------- +_last_violation_mask = np.zeros(6, dtype=bool) +_last_any_violation = False + + +# TODO: confirm whether this is actually faster than the previous loop based approach +def check_limits( + q: ArrayLike, + target_q: ArrayLike | None = None, + allow_recovery: bool = True, + *, + log: bool = True, +) -> bool: + """ + Vectorized limits check in radians. + - q: current joint angles in radians (array-like) + - target_q: optional target joint angles in radians (array-like) + - allow_recovery: allow movement that heads back toward valid range if currently violating + - log: emit edge-triggered warning/info logs on violation state changes + + Returns True if move is allowed (within limits or valid recovery), False otherwise. + """ + global _last_violation_mask, _last_any_violation + + q_arr = np.asarray(q, dtype=np.float64).reshape(-1) + mn = joint.limits.rad[:, 0] + mx = joint.limits.rad[:, 1] + + below = q_arr < mn + above = q_arr > mx + cur_viol = below | above + + if target_q is None: + ok_mask = ~cur_viol + t_below = t_above = None + else: + t = np.asarray(target_q, dtype=np.float64).reshape(-1) + t_below = t < mn + t_above = t > mx + t_viol = t_below | t_above + if allow_recovery: + rec_ok = (above & (t <= q_arr)) | (below & (t >= q_arr)) + else: + rec_ok = np.zeros(6, dtype=bool) + ok_mask = (~cur_viol & ~t_viol) | (cur_viol & rec_ok) + + all_ok = bool(np.all(ok_mask)) + + if log: + viol = ~ok_mask + any_viol = bool(np.any(viol)) + + # Edge-triggered violation logs + if any_viol and ( + np.any(viol != _last_violation_mask) or not _last_any_violation + ): + idxs = np.where(viol)[0] + tokens = [] + for i in idxs: + if cur_viol[i]: + tokens.append(f"J{i + 1}:" + ("curmax")) + else: + # target violates + if t_below is not None and t_below[i]: + tokens.append(f"J{i + 1}:targetmax") + else: + tokens.append(f"J{i + 1}:violation") + logger.warning("LIMIT VIOLATION: %s", " ".join(tokens)) + elif (not any_viol) and _last_any_violation: + logger.info("Limits back in range") + + _last_violation_mask[:] = viol + _last_any_violation = any_viol + + return all_ok + + +def check_limits_mask( + q: ArrayLike, target_q: ArrayLike | None = None, allow_recovery: bool = True +) -> NDArray[np.bool_]: + """Return per-joint boolean mask (True if OK for that joint).""" + q_arr = np.asarray(q, dtype=np.float64).reshape(-1) + mn = joint.limits.rad[:, 0] + mx = joint.limits.rad[:, 1] + below = q_arr < mn + above = q_arr > mx + cur_viol = below | above + + if target_q is None: + return ~cur_viol + t = np.asarray(target_q, dtype=np.float64).reshape(-1) + t_below = t < mn + t_above = t > mx + t_viol = t_below | t_above + if allow_recovery: + rec_ok = (above & (t <= q_arr)) | (below & (t >= q_arr)) + else: + rec_ok = np.zeros(6, dtype=bool) + ok_mask = (~cur_viol & ~t_viol) | (cur_viol & rec_ok) + return ok_mask + + +# ----------------------------- +# CAN helpers and bitfield utils (used by transports/gripper) +# ----------------------------- +def extract_from_can_id(can_id: int) -> tuple[int, int, int]: + id2 = (can_id >> 7) & 0xF + can_command = (can_id >> 1) & 0x3F + error_bit = can_id & 0x1 + return id2, can_command, error_bit + + +def combine_2_can_id(id2: int, can_command: int, error_bit: int) -> int: + can_id = 0 + can_id |= (id2 & 0xF) << 7 + can_id |= (can_command & 0x3F) << 1 + can_id |= error_bit & 0x1 + return can_id + + +def fuse_bitfield_2_bytearray(var_in: list[int] | tuple[int, ...]) -> bytes: + number = 0 + for b in var_in: + number = (2 * number) + int(b) + return bytes([number]) + + +def split_2_bitfield(var_in: int) -> list[int]: + return [(var_in >> i) & 1 for i in range(7, -1, -1)] + + +if __name__ == "__main__": + # Simple sanity prints + j_step_rad = steps_to_rad(np.array([1, 1, 1, 1, 1, 1], dtype=np.int32)) + print("Smallest step (deg):", np.rad2deg(j_step_rad)) + print("Standby rad:", joint.standby.rad) diff --git a/parol6/__init__.py b/parol6/__init__.py index bb84032..6997a08 100644 --- a/parol6/__init__.py +++ b/parol6/__init__.py @@ -8,20 +8,22 @@ - AsyncRobotClient: Async UDP client for robot operations - RobotClient: Sync wrapper with automatic event loop handling - ServerManager: Manages headless controller process lifecycle -- ensure_server: Convenience function to auto-start controller when needed +- manage_server: Convenience function to start a controller process +- is_server_running: Helper to probe for an existing controller """ +from . import PAROL6_ROBOT from ._version import __version__ from .client.async_client import AsyncRobotClient +from .client.manager import ServerManager, is_server_running, manage_server from .client.sync_client import RobotClient -from .client.manager import ServerManager, ensure_server -from . import PAROL6_ROBOT __all__ = [ "__version__", - "AsyncRobotClient", + "AsyncRobotClient", "RobotClient", "ServerManager", - "ensure_server", - "PAROL6_ROBOT" + "manage_server", + "is_server_running", + "PAROL6_ROBOT", ] diff --git a/parol6/ack_policy.py b/parol6/ack_policy.py index 0fdbbc7..16b1754 100644 --- a/parol6/ack_policy.py +++ b/parol6/ack_policy.py @@ -1,12 +1,10 @@ import os -from typing import Callable, Optional - +from collections.abc import Callable SYSTEM_COMMANDS: set[str] = { "STOP", "ENABLE", "DISABLE", - "CLEAR_ERROR", "SET_PORT", "STREAM", "SIMULATOR", @@ -21,6 +19,8 @@ "GET_STATUS", "GET_GCODE_STATUS", "GET_LOOP_STATS", + "GET_CURRENT_ACTION", + "GET_QUEUE", "PING", } @@ -45,7 +45,7 @@ class AckPolicy: def __init__( self, get_stream_mode: Callable[[], bool], - force_ack: Optional[bool] = None, + force_ack: bool | None = None, ) -> None: self._get_stream_mode = get_stream_mode self._force_ack = force_ack diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index 9b7665a..c431b61 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -1,22 +1,30 @@ """ Async UDP client for PAROL6 robot control. """ + import asyncio +import contextlib import json -import math +import logging import random import time -from typing import List, Dict, Optional, Literal, cast, AsyncIterator -from collections.abc import Iterable +from collections.abc import AsyncIterator, Callable, Iterable +from typing import Literal, cast + +import numpy as np +from spatialmath import SO3 -from ..protocol.types import Frame, Axis, StatusAggregate -from ..protocol import wire -from ..ack_policy import AckPolicy, SYSTEM_COMMANDS, QUERY_COMMANDS -from ..client.status_subscriber import subscribe_status from .. import config as cfg -import logging +from ..ack_policy import QUERY_COMMANDS, SYSTEM_COMMANDS, AckPolicy +from ..client.status_subscriber import subscribe_status +from ..protocol import wire +from ..protocol.types import Axis, Frame, StatusAggregate + logger = logging.getLogger(__name__) +# Sentinel used to signal status_stream termination +_STATUS_SENTINEL = cast(StatusAggregate, object()) + class _UDPClientProtocol(asyncio.DatagramProtocol): def __init__(self, rx_queue: asyncio.Queue[tuple[bytes, tuple[str, int]]]): @@ -24,9 +32,9 @@ def __init__(self, rx_queue: asyncio.Queue[tuple[bytes, tuple[str, int]]]): self.transport: asyncio.DatagramTransport | None = None def connection_made(self, transport: asyncio.BaseTransport) -> None: - self.transport = transport # type: ignore[assignment] + self.transport = cast(asyncio.DatagramTransport, transport) - def datagram_received(self, data: bytes, addr) -> None: + def datagram_received(self, data: bytes, addr: tuple[str, int]) -> None: try: self.rx_queue.put_nowait((data, addr)) except Exception: @@ -51,18 +59,21 @@ def __init__( self, host: str = "127.0.0.1", port: int = 5001, - timeout: float = 2.0, + timeout: float = 1.0, retries: int = 1, ) -> None: - self.host = host - self.port = port + # Endpoint configuration (host/port immutable after endpoint creation) + self._host = host + self._port = port self.timeout = timeout self.retries = retries # Persistent asyncio datagram endpoint self._transport: asyncio.DatagramTransport | None = None self._protocol: _UDPClientProtocol | None = None - self._rx_queue: asyncio.Queue[tuple[bytes, tuple[str, int]]] = asyncio.Queue(maxsize=256) + self._rx_queue: asyncio.Queue[tuple[bytes, tuple[str, int]]] = asyncio.Queue( + maxsize=256 + ) self._ep_lock = asyncio.Lock() # Serialize request/response @@ -72,18 +83,51 @@ def __init__( self._stream_mode = False # ACK policy self._ack_policy = AckPolicy.from_env(lambda: self._stream_mode) - + # Multicast listener using subscribe_status self._multicast_task: asyncio.Task | None = None self._status_queue: asyncio.Queue[StatusAggregate] = asyncio.Queue(maxsize=100) + # Lifecycle flag + self._closed: bool = False + + # --------------- Endpoint configuration properties --------------- + + @property + def host(self) -> str: + return self._host + + @host.setter + def host(self, value: str) -> None: + if self._transport is not None: + raise RuntimeError( + "AsyncRobotClient.host is read-only after endpoint creation" + ) + self._host = value + + @property + def port(self) -> int: + return self._port + + @port.setter + def port(self, value: int) -> None: + if self._transport is not None: + raise RuntimeError( + "AsyncRobotClient.port is read-only after endpoint creation" + ) + self._port = value + # --------------- Internal helpers --------------- async def _ensure_endpoint(self) -> None: """Lazily create a persistent asyncio UDP datagram endpoint.""" + if self._closed: + raise RuntimeError("AsyncRobotClient is closed") if self._transport is not None: return async with self._ep_lock: + if self._closed: + raise RuntimeError("AsyncRobotClient is closed") if self._transport is not None: return loop = asyncio.get_running_loop() @@ -92,10 +136,12 @@ async def _ensure_endpoint(self) -> None: lambda: _UDPClientProtocol(self._rx_queue), remote_addr=(self.host, self.port), ) - self._transport = transport # type: ignore[assignment] - self._protocol = protocol # type: ignore[assignment] - logger.info(f"AsyncRobotClient UDP endpoint: remote={self.host}:{self.port}, timeout={self.timeout}, retries={self.retries}") - + self._transport = transport + self._protocol = protocol + logger.info( + f"AsyncRobotClient UDP endpoint: remote={self.host}:{self.port}, timeout={self.timeout}, retries={self.retries}" + ) + # Start multicast listener await self._start_multicast_listener() @@ -103,17 +149,26 @@ async def _start_multicast_listener(self) -> None: """Start listening for multicast status broadcasts using subscribe_status.""" if self._multicast_task is not None and not self._multicast_task.done(): return - - logger.info(f"Status subscriber config: group={cfg.MCAST_GROUP} port={cfg.MCAST_PORT} iface={cfg.MCAST_IF}") + + logger.info( + f"Status subscriber config: group={cfg.MCAST_GROUP} port={cfg.MCAST_PORT} iface={cfg.MCAST_IF}" + ) # Quick readiness check (no blind wait): bounded by client's own timeout try: - await self.wait_for_server_ready(timeout=min(1.0, float(self.timeout or 0.3)), interval=0.5) + await self.wait_for_server_ready( + timeout=min(1.0, float(self.timeout or 0.3)), interval=0.5 + ) except Exception: pass + async def _listener(): """Consume status broadcasts and queue them.""" try: async for status in subscribe_status(): + # Exit promptly if the client is closing + if self._closed: + break + # Put in queue, drop old if full if self._status_queue.full(): try: @@ -124,25 +179,83 @@ async def _listener(): self._status_queue.put_nowait(status) except asyncio.QueueFull: pass + except asyncio.CancelledError: + # Normal shutdown path; propagate cancellation so the task + # is marked as cancelled and can be awaited cleanly. + raise except Exception: - # Subscriber ended, could retry but for now just exit + # Subscriber ended unexpectedly, could retry but for now just exit pass - + # Start listener task self._multicast_task = asyncio.create_task(_listener()) - - async def status_stream(self) -> AsyncIterator[StatusAggregate]: + + # --------------- Lifecycle / context management --------------- + + async def close(self) -> None: + """Release UDP transport and background tasks. + + Safe to call multiple times. """ - Async generator that yields status updates from multicast broadcasts. - + if self._closed: + return + logging.debug("Closing Client...") + self._closed = True + + # status_stream consumers will be signaled via sentinel after stopping the multicast listener. + + # Stop multicast listener + if self._multicast_task is not None: + self._multicast_task.cancel() + with contextlib.suppress(asyncio.CancelledError, Exception): + await self._multicast_task + self._multicast_task = None + + # Wake status_stream consumer(s) promptly: clear queue then enqueue sentinel so it's next + with contextlib.suppress(Exception): + while not self._status_queue.empty(): + self._status_queue.get_nowait() + with contextlib.suppress(asyncio.QueueFull): + self._status_queue.put_nowait(_STATUS_SENTINEL) + + # Close UDP transport + if self._transport is not None: + with contextlib.suppress(Exception): + self._transport.close() + self._transport = None + self._protocol = None + + # Best-effort drain for RX queue to free memory. + # Do not drain status_queue here to ensure sentinel reaches consumers. + with contextlib.suppress(Exception): + while not self._rx_queue.empty(): + self._rx_queue.get_nowait() + + async def __aenter__(self) -> "AsyncRobotClient": + if self._closed: + raise RuntimeError("AsyncRobotClient is closed") + return self + + async def __aexit__(self, exc_type, exc, tb) -> None: + await self.close() + + async def status_stream(self) -> AsyncIterator[StatusAggregate]: + """Async generator that yields status updates from multicast broadcasts. + Usage: async for status in client.status_stream(): print(f"Speeds: {status.get('speeds')}") + + This generator terminates automatically when :meth:`close` is + called on the client, so callers do not need to manually cancel + their consumer tasks. """ await self._ensure_endpoint() while True: - status = await self._status_queue.get() - yield status + item = await self._status_queue.get() + if item is _STATUS_SENTINEL: + break + yield item async def _send(self, message: str) -> bool: """ @@ -180,11 +293,13 @@ async def _request(self, message: str, bufsize: int = 2048) -> str | None: try: async with self._req_lock: self._transport.sendto(message.encode("ascii")) - data, _addr = await asyncio.wait_for(self._rx_queue.get(), timeout=self.timeout) + data, _addr = await asyncio.wait_for( + self._rx_queue.get(), timeout=self.timeout + ) return data.decode("ascii", errors="ignore") - except asyncio.TimeoutError: + except (asyncio.TimeoutError, TimeoutError): if attempt < self.retries: - backoff = min(0.5, 0.05 * (2 ** attempt)) + random.uniform(0, 0.05) + backoff = min(0.5, 0.05 * (2**attempt)) + random.uniform(0, 0.05) await asyncio.sleep(backoff) continue except Exception: @@ -213,7 +328,7 @@ async def _request_ok(self, message: str, timeout: float) -> bool: if text.startswith("ERROR|"): raise RuntimeError(text) # Ignore unrelated datagrams - except asyncio.TimeoutError: + except (asyncio.TimeoutError, TimeoutError): break except Exception: break @@ -224,17 +339,19 @@ async def _request_ok(self, message: str, timeout: float) -> bool: async def home(self) -> bool: return await self._send("HOME") - async def stop(self) -> bool: - return await self._send("STOP") - async def enable(self) -> bool: return await self._send("ENABLE") async def disable(self) -> bool: return await self._send("DISABLE") - async def clear_error(self) -> bool: - return await self._send("CLEAR_ERROR") + async def stop(self) -> bool: + """Alias for disable() - stops motion and disables controller.""" + return await self.disable() + + async def start(self) -> bool: + """Alias for enable() - enables controller.""" + return await self.enable() async def stream_on(self) -> bool: self._stream_mode = True @@ -243,7 +360,7 @@ async def stream_on(self) -> bool: async def stream_off(self) -> bool: self._stream_mode = False return await self._send("STREAM|OFF") - + async def simulator_on(self) -> bool: return await self._send("SIMULATOR|ON") @@ -265,39 +382,47 @@ async def get_angles(self) -> list[float] | None: if not resp: return None vals = wire.decode_simple(resp, "ANGLES") - return cast(List[float] | None, vals) + return cast(list[float] | None, vals) async def get_io(self) -> list[int] | None: resp = await self._request("GET_IO", bufsize=1024) if not resp: return None vals = wire.decode_simple(resp, "IO") - return cast(List[int] | None, vals) + return cast(list[int] | None, vals) async def get_gripper_status(self) -> list[int] | None: resp = await self._request("GET_GRIPPER", bufsize=1024) if not resp: return None vals = wire.decode_simple(resp, "GRIPPER") - return cast(List[int] | None, vals) + return cast(list[int] | None, vals) async def get_speeds(self) -> list[float] | None: resp = await self._request("GET_SPEEDS", bufsize=1024) if not resp: return None vals = wire.decode_simple(resp, "SPEEDS") - return cast(List[float] | None, vals) + return cast(list[float] | None, vals) - async def get_pose(self) -> list[float] | None: + async def get_pose( + self, frame: Literal["WRF", "TRF"] = "WRF" + ) -> list[float] | None: """ - Returns 16-element transformation matrix (flattened) or None on failure. + Returns 16-element transformation matrix (flattened) with translation in mm. + + Args: + frame: Reference frame - "WRF" for World Reference Frame (default), + "TRF" for Tool Reference Frame + Expected wire format: "POSE|p0,p1,p2,...,p15" """ - resp = await self._request("GET_POSE", bufsize=2048) + command = f"GET_POSE {frame}" if frame != "WRF" else "GET_POSE" + resp = await self._request(command, bufsize=2048) if not resp: return None vals = wire.decode_simple(resp, "POSE") - return cast(List[float] | None, vals) + return cast(list[float] | None, vals) async def get_gripper(self) -> list[int] | None: """Alias for get_gripper_status for compatibility.""" @@ -308,7 +433,7 @@ async def get_status(self) -> dict | None: Aggregate status. Expected format: STATUS|POSE=p0,p1,...,p15|ANGLES=a0,...,a5|IO=in1,in2,out1,out2,estop|GRIPPER=id,pos,spd,cur,status,obj - Returns dict with keys: pose (list[float] len=16), angles (list[float] len=6), + Returns dict with keys: pose (list[float] len=16 with translation in mm), angles (list[float] len=6), io (list[int] len=5), gripper (list[int] len>=6) """ resp = await self._request("GET_STATUS", bufsize=4096) @@ -324,49 +449,84 @@ async def get_loop_stats(self) -> dict | None: resp = await self._request("GET_LOOP_STATS", bufsize=1024) if not resp or not resp.startswith("LOOP_STATS|"): return None - return cast(Dict, json.loads(resp.split("|", 1)[1])) + return cast(dict, json.loads(resp.split("|", 1)[1])) + + async def set_tool(self, tool_name: str) -> bool: + """ + Set the current end-effector tool configuration. + + Args: + tool_name: Name of the tool ('NONE', 'PNEUMATIC', 'ELECTRIC') + + Returns: + True if successful + """ + return await self._send(f"SET_TOOL|{tool_name.upper()}") + + async def get_tool(self) -> dict | None: + """ + Get the current tool configuration and available tools. + + Returns: + Dict with keys: 'tool' (current tool name), 'available' (list of available tools) + Expected wire format: "TOOL|{json}" + """ + resp = await self._request("GET_TOOL", bufsize=1024) + if not resp or not resp.startswith("TOOL|"): + return None + return cast(dict, json.loads(resp.split("|", 1)[1])) + + async def get_current_action(self) -> dict | None: + """ + Get the current executing action/command and its state. + + Returns: + Dict with keys: 'current' (current action name), 'state' (action state), + 'next' (next action if any) + Expected wire format: "ACTION|{json}" + """ + resp = await self._request("GET_CURRENT_ACTION", bufsize=1024) + if not resp or not resp.startswith("ACTION|"): + return None + return cast(dict, json.loads(resp.split("|", 1)[1])) + + async def get_queue(self) -> dict | None: + """ + Get the list of queued non-streamable commands. + + Returns: + Dict with keys: 'non_streamable' (list of queued commands), 'size' (queue size) + Expected wire format: "QUEUE|{json}" + """ + resp = await self._request("GET_QUEUE", bufsize=2048) + if not resp or not resp.startswith("QUEUE|"): + return None + return cast(dict, json.loads(resp.split("|", 1)[1])) # --------------- Helper methods --------------- async def get_pose_rpy(self) -> list[float] | None: """ - Get robot pose as [x, y, z, rx, ry, rz] in mm and degrees. - Converts 4x4 matrix to xyz + RPY Euler angles. + Get robot pose as [x_mm, y_mm, z_mm, rx_deg, ry_deg, rz_deg] using RPY order='xyz'. """ pose_matrix = await self.get_pose() if not pose_matrix or len(pose_matrix) != 16: return None - + try: - # Extract translation x, y, z = pose_matrix[3], pose_matrix[7], pose_matrix[11] - - # Extract rotation matrix elements - r11, _, _ = pose_matrix[0], pose_matrix[1], pose_matrix[2] - r21, r22, r23 = pose_matrix[4], pose_matrix[5], pose_matrix[6] - r31, r32, r33 = pose_matrix[8], pose_matrix[9], pose_matrix[10] - - # Convert to RPY (XYZ convention) in degrees - # Handle gimbal lock cases - sy = math.sqrt(r11*r11 + r21*r21) - - if sy > 1e-6: # Not at gimbal lock - rx = math.atan2(r32, r33) - ry = math.atan2(-r31, sy) - rz = math.atan2(r21, r11) - else: # Gimbal lock case - rx = math.atan2(-r23, r22) - ry = math.atan2(-r31, sy) - rz = 0.0 - - # Convert to degrees - rx_deg = math.degrees(rx) - ry_deg = math.degrees(ry) - rz_deg = math.degrees(rz) - - return [x, y, z, rx_deg, ry_deg, rz_deg] - - except (ValueError, IndexError): + # Rotation matrix rows (row-major layout) + R = np.array( + [ + [pose_matrix[0], pose_matrix[1], pose_matrix[2]], + [pose_matrix[4], pose_matrix[5], pose_matrix[6]], + [pose_matrix[8], pose_matrix[9], pose_matrix[10]], + ] + ) + # Use xyz convention (rx, ry, rz) - Roll-Pitch-Yaw + rpy_deg = SO3(R).rpy(order="xyz", unit="deg") + return [x, y, z, rpy_deg[0], rpy_deg[1], rpy_deg[2]] + except (ValueError, IndexError, ImportError): return None async def get_pose_xyz(self) -> list[float] | None: @@ -384,10 +544,10 @@ async def is_estop_pressed(self) -> bool: async def is_robot_stopped(self, threshold_speed: float = 2.0) -> bool: """ Check if robot has stopped moving. - + Args: threshold_speed: Speed threshold in steps/sec - + Returns: True if all joints below threshold """ @@ -401,22 +561,22 @@ async def wait_until_stopped( timeout: float = 90.0, settle_window: float = 1.0, speed_threshold: float = 2.0, - angle_threshold: float = 0.5 + angle_threshold: float = 0.5, ) -> bool: """ Wait for robot to stop moving using multicast status broadcasts. - + Args: timeout: Maximum time to wait in seconds settle_window: How long robot must be stable to be considered stopped speed_threshold: Max joint speed to be considered stopped (steps/sec) angle_threshold: Max angle change to be considered stopped (degrees) - + Returns: True if robot stopped, False if timeout """ await self._ensure_endpoint() - + last_angles = None settle_start = None timeout_task = asyncio.create_task(asyncio.sleep(timeout)) @@ -425,9 +585,9 @@ async def wait_until_stopped( async for status in self.status_stream(): if timeout_task.done(): return False - + # Check speeds from status - speeds = status.get('speeds') + speeds = status.get("speeds") if speeds and isinstance(speeds, Iterable): if max(abs(s) for s in speeds) < speed_threshold: if settle_start is None: @@ -436,12 +596,15 @@ async def wait_until_stopped( return True else: settle_start = None - + # Also check angles as fallback - angles = status.get('angles') + angles = status.get("angles") if angles and not speeds: if last_angles is not None: - max_change = max(abs(a - b) for a, b in zip(angles, last_angles)) + max_change = max( + abs(a - b) + for a, b in zip(angles, last_angles, strict=False) + ) if max_change < angle_threshold: if settle_start is None: settle_start = time.time() @@ -456,12 +619,14 @@ async def wait_until_stopped( await timeout_task except asyncio.CancelledError: pass - + return False # --------------- Additional waits and utilities --------------- - async def wait_for_server_ready(self, timeout: float = 5.0, interval: float = 0.05) -> bool: + async def wait_for_server_ready( + self, timeout: float = 5.0, interval: float = 0.05 + ) -> bool: """Poll ping() until server responds or timeout.""" end_time = time.time() + timeout while time.time() < end_time: @@ -471,15 +636,19 @@ async def wait_for_server_ready(self, timeout: float = 5.0, interval: float = 0. await asyncio.sleep(interval) return False - async def wait_for_status(self, predicate, timeout: float = 5.0) -> bool: + async def wait_for_status( + self, predicate: Callable[[StatusAggregate], bool], timeout: float = 5.0 + ) -> bool: """Wait until a multicast status satisfies predicate(status) within timeout.""" await self._ensure_endpoint() end_time = time.time() + timeout while time.time() < end_time: remaining = max(0.0, end_time - time.time()) try: - status = await asyncio.wait_for(self._status_queue.get(), timeout=remaining) - except asyncio.TimeoutError: + status = await asyncio.wait_for( + self._status_queue.get(), timeout=remaining + ) + except (asyncio.TimeoutError, TimeoutError): break try: if predicate(status): @@ -489,7 +658,9 @@ async def wait_for_status(self, predicate, timeout: float = 5.0) -> bool: pass return False - async def send_raw(self, message: str, await_reply: bool = False, timeout: float = 2.0) -> bool | str | None: + async def send_raw( + self, message: str, await_reply: bool = False, timeout: float = 2.0 + ) -> bool | str | None: """Send a raw UDP message; optionally await a single reply.""" await self._ensure_endpoint() assert self._transport is not None @@ -499,9 +670,11 @@ async def send_raw(self, message: str, await_reply: bool = False, timeout: float return True async with self._req_lock: self._transport.sendto(message.encode("ascii")) - data, _addr = await asyncio.wait_for(self._rx_queue.get(), timeout=timeout) + data, _addr = await asyncio.wait_for( + self._rx_queue.get(), timeout=timeout + ) return data.decode("ascii", errors="ignore") - except asyncio.TimeoutError: + except (asyncio.TimeoutError, TimeoutError): return None except Exception: return None @@ -518,7 +691,9 @@ async def move_joints( tracking: str | None = None, # accepted but not sent ) -> bool: if duration is None and speed_percentage is None: - raise RuntimeError("You must provide either a duration or a speed_percentage.") + raise RuntimeError( + "You must provide either a duration or a speed_percentage." + ) message = wire.encode_move_joint(joint_angles, duration, speed_percentage) return await self._send(message) @@ -532,7 +707,9 @@ async def move_pose( tracking: str | None = None, ) -> bool: if duration is None and speed_percentage is None: - raise RuntimeError("You must provide either a duration or a speed_percentage.") + raise RuntimeError( + "You must provide either a duration or a speed_percentage." + ) message = wire.encode_move_pose(pose, duration, speed_percentage) return await self._send(message) @@ -546,7 +723,9 @@ async def move_cartesian( tracking: str | None = None, ) -> bool: if duration is None and speed_percentage is None: - raise RuntimeError("Error: You must provide either a duration or a speed_percentage.") + raise RuntimeError( + "Error: You must provide either a duration or a speed_percentage." + ) message = wire.encode_move_cartesian(pose, duration, speed_percentage) return await self._send(message) @@ -564,7 +743,9 @@ async def move_cartesian_rel_trf( Provide either duration or speed_percentage (1..100). """ if duration is None and speed_percentage is None: - raise RuntimeError("Error: You must provide either a duration or a speed_percentage.") + raise RuntimeError( + "Error: You must provide either a duration or a speed_percentage." + ) message = wire.encode_move_cartesian_rel_trf( deltas, duration, speed_percentage, accel_percentage, profile, tracking ) @@ -582,8 +763,12 @@ async def jog_joint( duration and distance_deg are optional; at least one should be provided for one-shot jog. """ if duration is None and distance_deg is None: - raise RuntimeError("Error: You must provide either a duration or a distance_deg.") - message = wire.encode_jog_joint(joint_index, speed_percentage, duration, distance_deg) + raise RuntimeError( + "Error: You must provide either a duration or a distance_deg." + ) + message = wire.encode_jog_joint( + joint_index, speed_percentage, duration, distance_deg + ) return await self._send(message) async def jog_cartesian( @@ -609,7 +794,9 @@ async def jog_multiple( Send a MULTIJOG command to jog multiple joints simultaneously for 'duration' seconds. """ if len(joints) != len(speeds): - raise ValueError("Error: The number of joints must match the number of speeds.") + raise ValueError( + "Error: The number of joints must match the number of speeds." + ) joints_str = ",".join(str(j) for j in joints) speeds_str = ",".join(str(s) for s in speeds) message = f"MULTIJOG|{joints_str}|{speeds_str}|{duration}" @@ -688,7 +875,7 @@ async def execute_gcode_program(self, program_lines: list[str]) -> bool: """ for i, line in enumerate(program_lines): if "|" in line: - raise SyntaxError(f"Line {i+1} contains invalid '|'") + raise SyntaxError(f"Line {i + 1} contains invalid '|'") message = wire.encode_gcode_program_inline(program_lines) return await self._send(message) @@ -706,8 +893,8 @@ async def get_gcode_status(self) -> dict | None: resp = await self._request("GET_GCODE_STATUS", bufsize=2048) if not resp or not resp.startswith("GCODE_STATUS|"): return None - - status_json = resp.split('|', 1)[1] + + status_json = resp.split("|", 1)[1] return json.loads(status_json) async def pause_gcode_program(self) -> bool: @@ -726,22 +913,22 @@ async def stop_gcode_program(self) -> bool: async def smooth_circle( self, - center: List[float], + center: list[float], radius: float, plane: Literal["XY", "XZ", "YZ"] = "XY", frame: Literal["WRF", "TRF"] = "WRF", center_mode: Literal["ABSOLUTE", "TOOL", "RELATIVE"] = "ABSOLUTE", entry_mode: Literal["AUTO", "TANGENT", "DIRECT", "NONE"] = "NONE", - start_pose: Optional[List[float]] = None, - duration: Optional[float] = None, - speed_percentage: Optional[float] = None, + start_pose: list[float] | None = None, + duration: float | None = None, + speed_percentage: float | None = None, clockwise: bool = False, trajectory_type: Literal["cubic", "quintic", "s_curve"] = "cubic", - jerk_limit: Optional[float] = None, + jerk_limit: float | None = None, ) -> bool: """ Execute a smooth circular motion. - + Args: center: [x, y, z] center point in mm radius: Circle radius in mm @@ -757,11 +944,17 @@ async def smooth_circle( jerk_limit: Optional jerk limit for s_curve trajectory """ if duration is None and speed_percentage is None: - raise RuntimeError("Error: You must provide either duration or speed_percentage.") + raise RuntimeError( + "Error: You must provide either duration or speed_percentage." + ) center_str = ",".join(map(str, center)) start_str = ",".join(map(str, start_pose)) if start_pose else "CURRENT" clockwise_str = "1" if clockwise else "0" - timing_str = f"DURATION|{duration}" if duration is not None else f"SPEED|{speed_percentage}" + timing_str = ( + f"DURATION|{duration}" + if duration is not None + else f"SPEED|{speed_percentage}" + ) traj_params = f"|{trajectory_type}" if trajectory_type == "s_curve" and jerk_limit is not None: traj_params += f"|{jerk_limit}" @@ -776,19 +969,19 @@ async def smooth_circle( async def smooth_arc_center( self, - end_pose: List[float], - center: List[float], + end_pose: list[float], + center: list[float], frame: Literal["WRF", "TRF"] = "WRF", - start_pose: Optional[List[float]] = None, - duration: Optional[float] = None, - speed_percentage: Optional[float] = None, + start_pose: list[float] | None = None, + duration: float | None = None, + speed_percentage: float | None = None, clockwise: bool = False, trajectory_type: Literal["cubic", "quintic", "s_curve"] = "cubic", - jerk_limit: Optional[float] = None, + jerk_limit: float | None = None, ) -> bool: """ Execute a smooth arc motion defined by center point. - + Args: end_pose: [x, y, z, rx, ry, rz] end pose (mm and degrees) center: [x, y, z] arc center point in mm @@ -801,12 +994,18 @@ async def smooth_arc_center( jerk_limit: Optional jerk limit for s_curve trajectory """ if duration is None and speed_percentage is None: - raise RuntimeError("Error: You must provide either a duration or a speed_percentage.") + raise RuntimeError( + "Error: You must provide either a duration or a speed_percentage." + ) end_str = ",".join(map(str, end_pose)) center_str = ",".join(map(str, center)) start_str = ",".join(map(str, start_pose)) if start_pose else "CURRENT" clockwise_str = "1" if clockwise else "0" - timing_str = f"DURATION|{duration}" if duration is not None else f"SPEED|{speed_percentage}" + timing_str = ( + f"DURATION|{duration}" + if duration is not None + else f"SPEED|{speed_percentage}" + ) traj_params = f"|{trajectory_type}" if trajectory_type == "s_curve" and jerk_limit is not None: traj_params += f"|{jerk_limit}" @@ -820,25 +1019,31 @@ async def smooth_arc_center( async def smooth_arc_param( self, - end_pose: List[float], + end_pose: list[float], radius: float, arc_angle: float, frame: Literal["WRF", "TRF"] = "WRF", - start_pose: Optional[List[float]] = None, - duration: Optional[float] = None, - speed_percentage: Optional[float] = None, + start_pose: list[float] | None = None, + duration: float | None = None, + speed_percentage: float | None = None, trajectory_type: Literal["cubic", "quintic", "s_curve"] = "cubic", - jerk_limit: Optional[float] = None, + jerk_limit: float | None = None, clockwise: bool = False, ) -> bool: """ Execute a smooth arc motion defined parametrically (radius and angle). """ if duration is None and speed_percentage is None: - raise RuntimeError("You must provide either a duration or a speed_percentage.") + raise RuntimeError( + "You must provide either a duration or a speed_percentage." + ) end_str = ",".join(map(str, end_pose)) start_str = ",".join(map(str, start_pose)) if start_pose else "CURRENT" - timing_str = f"DURATION|{duration}" if duration is not None else f"SPEED|{speed_percentage}" + timing_str = ( + f"DURATION|{duration}" + if duration is not None + else f"SPEED|{speed_percentage}" + ) parts = [ "SMOOTH_ARC_PARAM", end_str, @@ -857,17 +1062,17 @@ async def smooth_arc_param( async def smooth_spline( self, - waypoints: List[List[float]], + waypoints: list[list[float]], frame: Literal["WRF", "TRF"] = "WRF", - start_pose: Optional[List[float]] = None, - duration: Optional[float] = None, - speed_percentage: Optional[float] = None, + start_pose: list[float] | None = None, + duration: float | None = None, + speed_percentage: float | None = None, trajectory_type: Literal["cubic", "quintic", "s_curve"] = "cubic", - jerk_limit: Optional[float] = None, + jerk_limit: float | None = None, ) -> bool: """ Execute a smooth spline motion through waypoints. - + Args: waypoints: List of [x, y, z, rx, ry, rz] poses (mm and degrees) frame: Reference frame ('WRF' for World, 'TRF' for Tool) @@ -881,14 +1086,27 @@ async def smooth_spline( non_blocking: Return immediately with command ID """ if duration is None and speed_percentage is None: - raise RuntimeError("Error: You must provide either duration or speed_percentage.") + raise RuntimeError( + "Error: You must provide either duration or speed_percentage." + ) num_waypoints = len(waypoints) start_str = ",".join(map(str, start_pose)) if start_pose else "CURRENT" - timing_str = f"DURATION|{duration}" if duration is not None else f"SPEED|{speed_percentage}" + timing_str = ( + f"DURATION|{duration}" + if duration is not None + else f"SPEED|{speed_percentage}" + ) waypoint_strs: list[str] = [] for wp in waypoints: waypoint_strs.extend(map(str, wp)) - parts = ["SMOOTH_SPLINE", str(num_waypoints), frame, start_str, timing_str, trajectory_type] + parts = [ + "SMOOTH_SPLINE", + str(num_waypoints), + frame, + start_str, + timing_str, + trajectory_type, + ] if trajectory_type == "s_curve" and jerk_limit is not None: parts.append(str(jerk_limit)) elif trajectory_type == "s_curve": @@ -898,21 +1116,21 @@ async def smooth_spline( async def smooth_helix( self, - center: List[float], + center: list[float], radius: float, pitch: float, height: float, frame: Literal["WRF", "TRF"] = "WRF", trajectory_type: Literal["cubic", "quintic", "s_curve"] = "cubic", - jerk_limit: Optional[float] = None, - start_pose: Optional[List[float]] = None, - duration: Optional[float] = None, - speed_percentage: Optional[float] = None, + jerk_limit: float | None = None, + start_pose: list[float] | None = None, + duration: float | None = None, + speed_percentage: float | None = None, clockwise: bool = False, ) -> bool: """ Execute a smooth helical motion. - + Args: center: [x, y, z] helix center point in mm radius: Helix radius in mm @@ -927,11 +1145,17 @@ async def smooth_helix( clockwise: Direction of motion """ if duration is None and speed_percentage is None: - raise RuntimeError("Error: You must provide either duration or speed_percentage.") + raise RuntimeError( + "Error: You must provide either duration or speed_percentage." + ) center_str = ",".join(map(str, center)) start_str = ",".join(map(str, start_pose)) if start_pose else "CURRENT" clockwise_str = "1" if clockwise else "0" - timing_str = f"DURATION|{duration}" if duration is not None else f"SPEED|{speed_percentage}" + timing_str = ( + f"DURATION|{duration}" + if duration is not None + else f"SPEED|{speed_percentage}" + ) traj_params = f"|{trajectory_type}" if trajectory_type == "s_curve" and jerk_limit is not None: traj_params += f"|{jerk_limit}" @@ -945,16 +1169,16 @@ async def smooth_helix( async def smooth_blend( self, - segments: List[Dict], + segments: list[dict], blend_time: float = 0.5, frame: Literal["WRF", "TRF"] = "WRF", - start_pose: Optional[List[float]] = None, - duration: Optional[float] = None, - speed_percentage: Optional[float] = None, + start_pose: list[float] | None = None, + duration: float | None = None, + speed_percentage: float | None = None, ) -> bool: """ Execute a blended motion through multiple segments. - + Args: segments: List of segment dictionaries blend_time: Time to blend between segments in seconds @@ -991,7 +1215,9 @@ async def smooth_blend( clockwise_str = "1" if seg.get("clockwise", False) else "0" seg_str = f"ARC|{end_str}|{center_str}|{seg.get('duration', 2.0)}|{clockwise_str}" elif seg_type == "SPLINE": - waypoints_str = ";".join([",".join(map(str, wp)) for wp in seg["waypoints"]]) + waypoints_str = ";".join( + [",".join(map(str, wp)) for wp in seg["waypoints"]] + ) seg_str = f"SPLINE|{len(seg['waypoints'])}|{waypoints_str}|{seg.get('duration', 3.0)}" else: continue @@ -1005,15 +1231,15 @@ async def smooth_blend( async def smooth_waypoints( self, - waypoints: List[List[float]], - blend_radii: Literal["AUTO"] | List[float] = "AUTO", + waypoints: list[list[float]], + blend_radii: Literal["AUTO"] | list[float] = "AUTO", blend_mode: Literal["parabolic", "circular", "none"] = "parabolic", - via_modes: Optional[List[str]] = None, + via_modes: list[str] | None = None, max_velocity: float = 100.0, max_acceleration: float = 500.0, frame: Literal["WRF", "TRF"] = "WRF", trajectory_type: Literal["cubic", "quintic", "s_curve"] = "quintic", - duration: Optional[float] = None, + duration: float | None = None, ) -> bool: """ Execute a waypoint trajectory with blending. @@ -1025,10 +1251,12 @@ async def smooth_waypoints( radii_str = "AUTO" else: if len(blend_radii) != max(0, len(waypoints) - 2): - raise ValueError(f"Blend radii count must be {max(0, len(waypoints) - 2)}") + raise ValueError( + f"Blend radii count must be {max(0, len(waypoints) - 2)}" + ) radii_str = ",".join(map(str, blend_radii)) if via_modes is None: - via_modes_list: List[str] = ["via"] * len(waypoints) + via_modes_list: list[str] = ["via"] * len(waypoints) else: via_modes_list = list(via_modes) if len(via_modes_list) != len(waypoints): @@ -1062,26 +1290,28 @@ async def set_work_coordinate_offset( ) -> bool: """ Set work coordinate system offsets (G54-G59). - + Args: coordinate_system: Work coordinate system to set ('G54' through 'G59') x: X axis offset in mm (None to keep current) y: Y axis offset in mm (None to keep current) z: Z axis offset in mm (None to keep current) - + Returns: Success message, command ID, or dict with status details - + Example: # Set G54 origin to current position await client.set_work_coordinate_offset('G54', x=0, y=0, z=0) - + # Offset G55 by 100mm in X await client.set_work_coordinate_offset('G55', x=100) """ valid_systems = ["G54", "G55", "G56", "G57", "G58", "G59"] if coordinate_system not in valid_systems: - raise RuntimeError(f"Invalid coordinate system: {coordinate_system}. Must be one of {valid_systems}") + raise RuntimeError( + f"Invalid coordinate system: {coordinate_system}. Must be one of {valid_systems}" + ) coord_num = int(coordinate_system[1:]) - 53 # G54=1, G55=2, etc. offset_params = [] @@ -1107,19 +1337,16 @@ async def zero_work_coordinates( ) -> bool: """ Set the current position as zero in the specified work coordinate system. - + Args: coordinate_system: Work coordinate system to zero ('G54' through 'G59') - + Returns: Success message, command ID, or dict with status details - + Example: # Set current position as origin in G54 await client.zero_work_coordinates('G54') """ # This sets the current position as 0,0,0 in the work coordinate system - return await self.set_work_coordinate_offset( - coordinate_system, - x=0, y=0, z=0 - ) + return await self.set_work_coordinate_offset(coordinate_system, x=0, y=0, z=0) diff --git a/parol6/client/manager.py b/parol6/client/manager.py index 3a755b2..d7ac6e7 100644 --- a/parol6/client/manager.py +++ b/parol6/client/manager.py @@ -7,22 +7,23 @@ import asyncio import contextlib import logging -import re import os -import signal +import re +import socket import subprocess import sys import threading import time +from typing import Tuple from dataclasses import dataclass from pathlib import Path -from typing import Optional # Precompiled regex patterns for server log normalization _SIMPLE_FORMAT_RE = re.compile( - r'^\s*(\d{2}:\d{2}:\d{2})\s+(DEBUG|INFO|WARNING|ERROR|CRITICAL|TRACE)\s+([A-Za-z0-9_.-]+):\s+(.*)$' + r"^\s*(\d{2}:\d{2}:\d{2})\s+(DEBUG|INFO|WARNING|ERROR|CRITICAL|TRACE)\s+([A-Za-z0-9_.-]+):\s+(.*)$" ) + @dataclass class ServerOptions: """Options for launching the controller.""" @@ -42,10 +43,12 @@ class ServerManager: - Can be used with a custom controller script path or defaults to the package's bundled controller. """ - def __init__(self, controller_path: str | None = None, normalize_logs: bool = False) -> None: + def __init__( + self, controller_path: str | None = None, normalize_logs: bool = False + ) -> None: """ Initialize the ServerManager. - + Args: controller_path: Optional path to controller script. If None, uses bundled controller. normalize_logs: If True, parse and normalize controller log output to avoid duplicate @@ -60,7 +63,7 @@ def __init__(self, controller_path: str | None = None, normalize_logs: bool = Fa else: # Use the package's bundled commander self.controller_path = Path(__file__).parent / "controller.py" - + self._proc: subprocess.Popen | None = None self._reader_thread: threading.Thread | None = None self._stop_reader = threading.Event() @@ -73,7 +76,7 @@ def pid(self) -> int | None: def is_running(self) -> bool: return self._proc is not None and self._proc.poll() is None - async def start_controller( + def start_controller( self, com_port: str | None = None, no_autohome: bool = True, @@ -86,7 +89,9 @@ async def start_controller( return # Working directory should be the PAROL6-python-API repo root to find dependencies # Use a more direct approach to find the package root - cwd = Path(__file__).resolve().parents[2] # parol6/server/manager.py -> repo root + cwd = ( + Path(__file__).resolve().parents[2] + ) # parol6/server/manager.py -> repo root env = os.environ.copy() # Disable autohome unless explicitly overridden @@ -101,22 +106,41 @@ async def start_controller( env["PAROL6_CONTROLLER_PORT"] = str(server_port) # Ensure the subprocess can import the local 'parol6' package existing_py_path = env.get("PYTHONPATH", "") - env["PYTHONPATH"] = f"{cwd}{os.pathsep}{existing_py_path}" if existing_py_path else str(cwd) + env["PYTHONPATH"] = ( + f"{cwd}{os.pathsep}{existing_py_path}" if existing_py_path else str(cwd) + ) # Launch the controller as a module to ensure package imports resolve args = [sys.executable, "-u", "-m", "parol6.server.controller"] - - level_name = logging.getLevelName(logging.getLogger().level) + + # Decide controller log level: + # - If PAROL_TRACE is set in the environment, prefer TRACE for the child + # - Otherwise, inherit the current root logger level name + root_logger = logging.getLogger() + root_level = root_logger.level + + parol_trace_flag = str(env.get("PAROL_TRACE", "0")).strip().lower() + if parol_trace_flag in ("1", "true", "yes", "on"): + level_name = "TRACE" + else: + level_name = logging.getLevelName(root_level) + # Normalize custom/unnamed levels (e.g. "Level 5") + if isinstance(level_name, str) and level_name.upper().startswith("LEVEL"): + if root_level == 5: + level_name = "TRACE" + else: + level_name = "INFO" + args.append(f"--log-level={level_name}") if com_port: args.append(f"--serial={com_port}") - + try: self._proc = subprocess.Popen( args, cwd=str(cwd), env=env, - stdout=subprocess.PIPE, + stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True, bufsize=1, # line-buffered @@ -160,7 +184,9 @@ def _stream_output(self, proc: subprocess.Popen) -> None: _, level_name, logger_name, actual_message = s.groups() logger_name = (logger_name or "").strip() msg = actual_message - level = getattr(logging, (level_name or "INFO").upper(), logging.INFO) + level = getattr( + logging, (level_name or "INFO").upper(), logging.INFO + ) elif line.startswith("Traceback"): # Traceback and continuation lines -> keep last context, escalate on Traceback level = logging.ERROR @@ -179,44 +205,39 @@ def _stream_output(self, proc: subprocess.Popen) -> None: except Exception as e: logging.warning("ServerManager: output reader stopped: %s", e) - async def stop_controller(self, timeout: float = 5.0) -> None: - """Stop the controller process if running.""" - if not self.is_running(): - self._proc = None - return - - proc = self._proc - assert proc is not None + def stop_controller(self, timeout: float = 2.0) -> None: + """Stop the controller process if running. - try: - if os.name == "nt": - proc.terminate() - else: - proc.send_signal(signal.SIGTERM) - except Exception: - # Fall back to kill below - pass - - # Wait for graceful exit - t0 = time.time() - while proc.poll() is None and (time.time() - t0) < timeout: - await asyncio.sleep(0.1) - - if proc.poll() is None: - with contextlib.suppress(Exception): - proc.kill() - - # Stop and join background reader thread - with contextlib.suppress(Exception): - self._stop_reader.set() - if proc.stdout: - proc.stdout.close() + This method attempts a graceful shutdown first using SIGTERM (or terminate() on Windows) + and then escalates to a forced kill if the process does not exit within ``timeout``. + After sending SIGKILL it will wait up to ``kill_timeout`` seconds for the process to + actually exit before giving up. + """ + self._stop_reader.set() if self._reader_thread and self._reader_thread.is_alive(): - with contextlib.suppress(Exception): - self._reader_thread.join(timeout=1.0) + self._reader_thread.join(timeout=timeout) self._reader_thread = None - - self._proc = None + if self._proc and self._proc.poll() is None: + logging.debug("Stopping Controller...") + try: + self._proc.terminate() + self._proc.wait(timeout=timeout) + except Exception as e: + logging.warning("stop_controller: terminate/wait failed: %s", e) + + # Step 2: escalate to forced kill if still running + if self._proc and self._proc.poll() is None: + logging.warning( + "Controller did not exit after SIGTERM within %.1fs, sending SIGKILL", + timeout, + ) + try: + self._proc.kill() + self._proc.wait(timeout=timeout) + except Exception as e: + logging.warning("stop_controller: kill/wait failed: %s", e) + # Clear reference + self._proc = None async def await_ready( self, @@ -226,87 +247,92 @@ async def await_ready( poll_interval: float = 0.2, ) -> bool: """ - Wait until the controller responds to PING. + Wait until the controller responds to PING over UDP, asynchronously. Returns: True if the server becomes ready within timeout, else False. """ - import socket as _socket - import time as _time + loop = asyncio.get_running_loop() + deadline = loop.time() + max(0.0, float(timeout)) + addr: Tuple[str, int] = (host, port) - deadline = _time.time() + max(0.0, float(timeout)) - while _time.time() < deadline: - # Try a simple PING - try: - with _socket.socket(_socket.AF_INET, _socket.SOCK_DGRAM) as sock: - sock.settimeout(min(0.5, poll_interval)) - sock.sendto(b"PING", (host, port)) - data, _ = sock.recvfrom(256) - if data.decode("ascii").startswith("PONG"): - return True - except Exception: - pass - - await asyncio.sleep(poll_interval) + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.setblocking(False) + try: + while True: + now = loop.time() + if now >= deadline: + return False + + # Try a PING and wait up to poll_interval (or remaining time) + recv_timeout = min(poll_interval, deadline - now) + + try: + # send PING + await loop.sock_sendto(sock, b"PING", addr) + + # wait for PONG with a timeout + data, _ = await asyncio.wait_for( + loop.sock_recvfrom(sock, 256), + timeout=recv_timeout, + ) + if data.decode("ascii", errors="ignore").startswith("PONG"): + return True + except (asyncio.TimeoutError, OSError): + # Timeout or send/recv error -> just try again until deadline + pass + + # Optional extra delay to avoid tight loop; keep within deadline + remain = deadline - loop.time() + if remain <= 0: + return False + await asyncio.sleep(min(poll_interval, remain)) + finally: + sock.close() + + +def is_server_running( + host: str = "127.0.0.1", + port: int = 5001, + timeout: float = 1.0, +) -> bool: + """Return True if a PAROL6 controller responds to UDP PING at host:port.""" + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.settimeout(timeout) + sock.sendto(b"PING", (host, port)) + data, _ = sock.recvfrom(256) + return data.decode("ascii", errors="ignore").startswith("PONG") + except Exception: return False -async def ensure_server( - host: str = "127.0.0.1", - port: int = 5001, - manage: bool = False, - com_port: str | None = None, +def manage_server( + host: str = "127.0.0.1", + port: int = 5001, + com_port: str | None = None, extra_env: dict | None = None, - normalize_logs: bool = False -) -> Optional[ServerManager]: - """ - Ensure a PAROL6 server is running and accessible. - - Args: - host: Server host to check/connect to - port: Server port to check/connect to - manage: If True, automatically spawn controller if ping fails - com_port: COM port for spawned controller - extra_env: Additional environment variables for spawned controller - normalize_logs: If True, parse and normalize controller log output to avoid duplicate - timestamp/level/module info. Set to True when used from web GUI. + normalize_logs: bool = False, +) -> ServerManager: + """Synchronously start a PAROL6 controller at host:port and block until ready. - Returns: - ServerManager instance if manage=True and server was spawned, None otherwise - - Usage: - # Just check if server is running - await ensure_server() - - # Auto-spawn if not running - mgr = await ensure_server(manage=True, com_port="/dev/ttyACM0") + Fast-fails if a server is already running there. + + Returns a ServerManager that owns the spawned controller. """ - # Test if server is already running - try: - import socket - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: - sock.settimeout(1.0) - sock.sendto(b"PING", (host, port)) - data, _ = sock.recvfrom(256) - if data.decode('ascii').startswith("PONG"): - logging.info(f"Server already running at {host}:{port}") - return None - except Exception: - pass - - if not manage: - logging.warning(f"Server not responding at {host}:{port} and manage=False") - return None - - # Spawn controller + if is_server_running(host=host, port=port): + raise RuntimeError(f"Server already running at {host}:{port}") + logging.info(f"Server not responding at {host}:{port}, starting controller...") + # Prepare environment for child controller bind tuple env_to_pass = dict(extra_env) if extra_env else {} env_to_pass["PAROL6_CONTROLLER_IP"] = host env_to_pass["PAROL6_CONTROLLER_PORT"] = str(port) + manager = ServerManager(normalize_logs=normalize_logs) - await manager.start_controller( + manager.start_controller( com_port=com_port, no_autohome=True, extra_env=env_to_pass, @@ -314,12 +340,44 @@ async def ensure_server( server_port=port, ) - # Wait for readiness within a short window - ok = await manager.await_ready(host=host, port=port, timeout=5.0) - if ok: - logging.info(f"Successfully started server at {host}:{port}") - return manager + # Block until PING responds or timeout + deadline = time.time() + 5.0 + while time.time() < deadline: + try: + if is_server_running(host=host, port=port, timeout=0.2): + logging.info(f"Successfully started server at {host}:{port}") + return manager + except Exception: + pass + time.sleep(0.05) logging.error("Server spawn failed or not responding after startup") - await manager.stop_controller() - return None + manager.stop_controller() + raise RuntimeError("Failed to start PAROL6 controller") + + +@contextlib.contextmanager +def managed_server( + host: str = "127.0.0.1", + port: int = 5001, + com_port: str | None = None, + extra_env: dict | None = None, + normalize_logs: bool = False, +): + """Synchronous context manager that ensures the controller is stopped on exit. + + Usage: + with managed_server(host, port) as mgr: + ... + """ + mgr = manage_server( + host=host, + port=port, + com_port=com_port, + extra_env=extra_env, + normalize_logs=normalize_logs, + ) + try: + yield mgr + finally: + mgr.stop_controller() diff --git a/parol6/client/status_subscriber.py b/parol6/client/status_subscriber.py index 30c2abb..b6b3bef 100644 --- a/parol6/client/status_subscriber.py +++ b/parol6/client/status_subscriber.py @@ -1,38 +1,38 @@ import asyncio +import contextlib +import logging import socket import struct import time -import logging -import contextlib -from typing import AsyncIterator +from collections.abc import AsyncIterator from parol6 import config as cfg -from parol6.protocol.wire import decode_status from parol6.protocol.types import StatusAggregate +from parol6.protocol.wire import decode_status logger = logging.getLogger(__name__) -class MulticastProtocol(asyncio.DatagramProtocol): - """Protocol handler for multicast UDP datagrams that works with uvloop.""" - +class UDPProtocol(asyncio.DatagramProtocol): + """Protocol handler for UDP datagrams (multicast or unicast).""" + def __init__(self, queue: asyncio.Queue): self.queue = queue self.transport = None self.receive_count = 0 self.last_log_time = time.time() - - # EMA rate tracking for multicast RX + + # EMA rate tracking for RX self._rx_count = 0 self._rx_last_time = time.monotonic() - self._rx_ema_period = 0.05 # Initialize with 20 Hz expected + self._rx_ema_period = 0.05 # Initialize with ~20 Hz expected self._rx_last_log_time = time.monotonic() - + def connection_made(self, transport): self.transport = transport - + def datagram_received(self, data, addr): - # Track multicast RX rate with EMA + # Track RX rate with EMA now = time.monotonic() if self._rx_count > 0: # Skip first sample for period calculation period = now - self._rx_last_time @@ -41,13 +41,13 @@ def datagram_received(self, data, addr): self._rx_ema_period = 0.1 * period + 0.9 * self._rx_ema_period self._rx_last_time = now self._rx_count += 1 - + # Log rate every 3 seconds if now - self._rx_last_log_time >= 3.0 and self._rx_ema_period > 0: rx_hz = 1.0 / self._rx_ema_period - logger.debug(f"Multicast RX: {rx_hz:.1f} Hz (count={self._rx_count})") + logger.debug(f"Status RX: {rx_hz:.1f} Hz (count={self._rx_count})") self._rx_last_log_time = now - + try: self.queue.put_nowait((data, addr)) except asyncio.QueueFull: @@ -55,12 +55,12 @@ def datagram_received(self, data, addr): try: self.queue.get_nowait() self.queue.put_nowait((data, addr)) - except: + except Exception: pass - + def error_received(self, exc): logger.error(f"Error received: {exc}") - + def connection_lost(self, exc): logger.info(f"Connection lost: {exc}") @@ -103,7 +103,9 @@ def _detect_primary_ip() -> str: # Retry using primary NIC IP try: primary_ip = _detect_primary_ip() - mreq = struct.pack("=4s4s", socket.inet_aton(group), socket.inet_aton(primary_ip)) + mreq = struct.pack( + "=4s4s", socket.inet_aton(group), socket.inet_aton(primary_ip) + ) sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq) except Exception: # Final fallback: INADDR_ANY variant @@ -115,20 +117,34 @@ def _detect_primary_ip() -> str: return sock +def _create_unicast_socket(port: int, host: str) -> socket.socket: + """Create and configure a plain UDP socket for unicast reception. + + Binds to the provided host (default 127.0.0.1) and port with large RCVBUF. + """ + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + try: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) + except Exception: + pass + sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 1 << 20) + try: + sock.bind((host, port)) + except OSError: + # Fallback to wildcard + sock.bind(("", port)) + sock.setblocking(False) + return sock + + async def subscribe_status( - group: str | None = None, - port: int | None = None, - iface_ip: str | None = None + group: str | None = None, port: int | None = None, iface_ip: str | None = None ) -> AsyncIterator[StatusAggregate]: """ - Async generator that yields decoded STATUS dicts from the UDP multicast broadcaster. - - Uses create_datagram_endpoint for uvloop compatibility. + Async generator that yields decoded STATUS dicts from the UDP broadcaster. - Usage: - async for status in subscribe_status(): - # status is a dict with keys pose, angles, io, gripper (or None on parse failure) - ... + Uses create_datagram_endpoint for uvloop compatibility. Notes: - Uses loopback multicast by default (cfg.MCAST_* values). @@ -137,37 +153,48 @@ async def subscribe_status( group = group or cfg.MCAST_GROUP port = port or cfg.MCAST_PORT iface_ip = iface_ip or cfg.MCAST_IF - - logger.info(f"subscribe_status starting: group={group}, port={port}, iface_ip={iface_ip}") + + logger.info( + f"subscribe_status starting: transport={cfg.STATUS_TRANSPORT} group={group}, port={port}, iface_ip={iface_ip}" + ) loop = asyncio.get_running_loop() - queue = asyncio.Queue(maxsize=100) # type: ignore - - # Create the socket with multicast configuration - sock = _create_multicast_socket(group, port, iface_ip) - + queue = asyncio.Queue(maxsize=100) # type: ignore + + # Create the socket based on configured transport + if cfg.STATUS_TRANSPORT == "UNICAST": + sock = _create_unicast_socket(port, cfg.STATUS_UNICAST_HOST) + else: + # Multicast socket bound to ("", port) will also receive unicast datagrams to that port + sock = _create_multicast_socket(group, port, iface_ip) + # Create the datagram endpoint with our protocol transport = None try: transport, _ = await loop.create_datagram_endpoint( - lambda: MulticastProtocol(queue), - sock=sock + lambda: UDPProtocol(queue), sock=sock ) - + while True: try: # Wait for data with timeout data, addr = await asyncio.wait_for(queue.get(), timeout=2.0) text = data.decode("ascii", errors="ignore") - + parsed = decode_status(text) if parsed is not None: yield parsed - - except asyncio.TimeoutError: - logger.warning(f"No multicast received for 2s on {group}:{port} (iface={iface_ip})") + + except (asyncio.TimeoutError, TimeoutError): + logger.warning( + f"No status received for 2s on {('unicast' if cfg.STATUS_TRANSPORT == 'UNICAST' else 'multicast')} {group}:{port} (iface={iface_ip})" + ) continue - + + except asyncio.CancelledError: + # Normal shutdown path when consumer task is cancelled + logger.info("subscribe_status cancelled") + raise except Exception as e: logger.error(f"Error in subscribe_status: {e}", exc_info=True) raise @@ -177,5 +204,5 @@ async def subscribe_status( transport.close() try: sock.close() - except: + except Exception: pass diff --git a/parol6/client/sync_client.py b/parol6/client/sync_client.py index 78901d5..384a93f 100644 --- a/parol6/client/sync_client.py +++ b/parol6/client/sync_client.py @@ -6,12 +6,13 @@ """ import asyncio -import threading import atexit -from typing import TypeVar, Union, Optional, List, Literal, Dict, Coroutine, Any +import threading +from collections.abc import Callable, Coroutine +from typing import Any, Literal, TypeVar +from ..protocol.types import Axis, Frame, StatusAggregate from .async_client import AsyncRobotClient -from ..protocol.types import Frame, Axis T = TypeVar("T") @@ -45,7 +46,10 @@ def _ensure_sync_loop() -> None: if _SYNC_LOOP is None: _SYNC_LOOP = asyncio.new_event_loop() _SYNC_THREAD = threading.Thread( - target=_loop_worker, args=(_SYNC_LOOP,), name="parol6-sync-loop", daemon=True + target=_loop_worker, + args=(_SYNC_LOOP,), + name="parol6-sync-loop", + daemon=True, ) _SYNC_THREAD.start() _SYNC_LOOP_READY.wait(timeout=1.0) @@ -76,6 +80,12 @@ class RobotClient: """ Synchronous wrapper around AsyncRobotClient. All methods return concrete results (never coroutines). + + Can be used as a context manager to ensure proper cleanup: + + with RobotClient() as client: + client.enable() + ... """ # ---------- lifecycle ---------- @@ -91,6 +101,16 @@ def __init__( host=host, port=port, timeout=timeout, retries=retries ) + def close(self) -> None: + """Close underlying AsyncRobotClient and release resources.""" + _run(self._inner.close()) + + def __enter__(self) -> "RobotClient": + return self + + 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.""" @@ -110,24 +130,26 @@ def port(self) -> int: def home(self) -> bool: return _run(self._inner.home()) - def stop(self) -> bool: - return _run(self._inner.stop()) - def enable(self) -> bool: return _run(self._inner.enable()) def disable(self) -> bool: return _run(self._inner.disable()) - def clear_error(self) -> bool: - return _run(self._inner.clear_error()) + def stop(self) -> bool: + """Alias for disable() - stops motion and disables controller.""" + return self.disable() + + def start(self) -> bool: + """Alias for enable() - enables controller.""" + return self.enable() def stream_on(self) -> bool: return _run(self._inner.stream_on()) def stream_off(self) -> bool: return _run(self._inner.stream_off()) - + def simulator_on(self) -> bool: return _run(self._inner.simulator_on()) @@ -141,22 +163,22 @@ def set_serial_port(self, port_str: str) -> bool: def ping(self) -> str | None: return _run(self._inner.ping()) - def get_angles(self) -> List[float] | None: + def get_angles(self) -> list[float] | None: return _run(self._inner.get_angles()) - def get_io(self) -> List[int] | None: + def get_io(self) -> list[int] | None: return _run(self._inner.get_io()) - def get_gripper_status(self) -> List[int] | None: + def get_gripper_status(self) -> list[int] | None: return _run(self._inner.get_gripper_status()) - def get_speeds(self) -> List[float] | None: + def get_speeds(self) -> list[float] | None: return _run(self._inner.get_speeds()) - def get_pose(self) -> List[float] | None: + def get_pose(self) -> list[float] | None: return _run(self._inner.get_pose()) - def get_gripper(self) -> List[int] | None: + def get_gripper(self) -> list[int] | None: return _run(self._inner.get_gripper()) def get_status(self) -> dict | None: @@ -165,12 +187,52 @@ def get_status(self) -> dict | None: def get_loop_stats(self) -> dict | None: return _run(self._inner.get_loop_stats()) + def get_tool(self) -> dict | None: + """ + Get the current tool configuration and available tools. + + Returns: + Dict with keys: 'tool' (current tool name), 'available' (list of available tools) + """ + return _run(self._inner.get_tool()) + + def set_tool(self, tool_name: str) -> bool: + """ + Set the current end-effector tool configuration. + + Args: + tool_name: Name of the tool ('NONE', 'PNEUMATIC', 'ELECTRIC') + + Returns: + True if successful + """ + return _run(self._inner.set_tool(tool_name)) + + def get_current_action(self) -> dict | None: + """ + Get the current executing action/command and its state. + + Returns: + Dict with keys: 'current' (current action name), 'state' (action state), + 'next' (next action if any) + """ + return _run(self._inner.get_current_action()) + + def get_queue(self) -> dict | None: + """ + Get the list of queued non-streamable commands. + + Returns: + Dict with keys: 'non_streamable' (list of queued commands), 'size' (queue size) + """ + return _run(self._inner.get_queue()) + # ---------- helper methods ---------- - def get_pose_rpy(self) -> List[float] | None: + def get_pose_rpy(self) -> list[float] | None: return _run(self._inner.get_pose_rpy()) - def get_pose_xyz(self) -> List[float] | None: + def get_pose_xyz(self) -> list[float] | None: return _run(self._inner.get_pose_xyz()) def is_estop_pressed(self) -> bool: @@ -184,7 +246,7 @@ def wait_until_stopped( timeout: float = 90.0, settle_window: float = 1.0, speed_threshold: float = 2.0, - angle_threshold: float = 0.5 + angle_threshold: float = 0.5, ) -> bool: return _run( self._inner.wait_until_stopped( @@ -197,29 +259,39 @@ def wait_until_stopped( # ---------- responsive waits / raw send ---------- - def wait_for_server_ready(self, timeout: float = 5.0, interval: float = 0.05) -> bool: + def wait_for_server_ready( + self, timeout: float = 5.0, interval: float = 0.05 + ) -> bool: """Poll ping() until server responds or timeout.""" - return _run(self._inner.wait_for_server_ready(timeout=timeout, interval=interval)) + return _run( + self._inner.wait_for_server_ready(timeout=timeout, interval=interval) + ) - def wait_for_status(self, predicate, timeout: float = 5.0) -> bool: + def wait_for_status( + self, predicate: Callable[[StatusAggregate], bool], timeout: float = 5.0 + ) -> bool: """ Wait until a multicast status satisfies predicate(status) within timeout. Note: predicate is executed in the client's event loop thread. """ return _run(self._inner.wait_for_status(predicate, timeout=timeout)) - def send_raw(self, message: str, await_reply: bool = False, timeout: float = 2.0) -> bool | str | None: + def send_raw( + self, message: str, await_reply: bool = False, timeout: float = 2.0 + ) -> bool | str | None: """ Send a raw UDP message; optionally await a single reply and return its text. Returns True on fire-and-forget send, str on reply, or None on timeout/error when awaiting. """ - return _run(self._inner.send_raw(message, await_reply=await_reply, timeout=timeout)) + return _run( + self._inner.send_raw(message, await_reply=await_reply, timeout=timeout) + ) # ---------- extended controls / motion ---------- def move_joints( self, - joint_angles: List[float], + joint_angles: list[float], duration: float | None = None, speed_percentage: int | None = None, accel_percentage: int | None = None, @@ -239,7 +311,7 @@ def move_joints( def move_pose( self, - pose: List[float], + pose: list[float], duration: float | None = None, speed_percentage: int | None = None, accel_percentage: int | None = None, @@ -259,7 +331,7 @@ def move_pose( def move_cartesian( self, - pose: List[float], + pose: list[float], duration: float | None = None, speed_percentage: float | None = None, accel_percentage: int | None = None, @@ -279,7 +351,7 @@ def move_cartesian( def move_cartesian_rel_trf( self, - deltas: List[float], + deltas: list[float], duration: float | None = None, speed_percentage: float | None = None, accel_percentage: int | None = None, @@ -320,16 +392,12 @@ def jog_cartesian( speed_percentage: int, duration: float, ) -> bool: - return _run( - self._inner.jog_cartesian( - frame, axis, speed_percentage, duration - ) - ) + return _run(self._inner.jog_cartesian(frame, axis, speed_percentage, duration)) def jog_multiple( self, - joints: List[int], - speeds: List[float], + joints: list[int], + speeds: list[float], duration: float, ) -> bool: return _run(self._inner.jog_multiple(joints, speeds, duration)) @@ -359,9 +427,7 @@ def control_electric_gripper( current: int | None = 500, ) -> bool: return _run( - self._inner.control_electric_gripper( - action, position, speed, current - ) + self._inner.control_electric_gripper(action, position, speed, current) ) # ---------- GCODE ---------- @@ -374,7 +440,7 @@ def execute_gcode( def execute_gcode_program( self, - program_lines: List[str], + program_lines: list[str], ) -> bool: return _run(self._inner.execute_gcode_program(program_lines)) @@ -400,18 +466,18 @@ def stop_gcode_program(self) -> bool: def smooth_circle( self, - center: List[float], + center: list[float], radius: float, plane: Literal["XY", "XZ", "YZ"] = "XY", frame: Literal["WRF", "TRF"] = "WRF", center_mode: Literal["ABSOLUTE", "TOOL", "RELATIVE"] = "ABSOLUTE", entry_mode: Literal["AUTO", "TANGENT", "DIRECT", "NONE"] = "NONE", - start_pose: Optional[List[float]] = None, - duration: Optional[float] = None, - speed_percentage: Optional[float] = None, + start_pose: list[float] | None = None, + duration: float | None = None, + speed_percentage: float | None = None, clockwise: bool = False, trajectory_type: Literal["cubic", "quintic", "s_curve"] = "cubic", - jerk_limit: Optional[float] = None, + jerk_limit: float | None = None, ) -> bool: return _run( self._inner.smooth_circle( @@ -432,15 +498,15 @@ def smooth_circle( def smooth_arc_center( self, - end_pose: List[float], - center: List[float], + end_pose: list[float], + center: list[float], frame: Literal["WRF", "TRF"] = "WRF", - start_pose: Optional[List[float]] = None, - duration: Optional[float] = None, - speed_percentage: Optional[float] = None, + start_pose: list[float] | None = None, + duration: float | None = None, + speed_percentage: float | None = None, clockwise: bool = False, trajectory_type: Literal["cubic", "quintic", "s_curve"] = "cubic", - jerk_limit: Optional[float] = None, + jerk_limit: float | None = None, ) -> bool: return _run( self._inner.smooth_arc_center( @@ -458,15 +524,15 @@ def smooth_arc_center( def smooth_arc_param( self, - end_pose: List[float], + end_pose: list[float], radius: float, arc_angle: float, frame: Literal["WRF", "TRF"] = "WRF", - start_pose: Optional[List[float]] = None, - duration: Optional[float] = None, - speed_percentage: Optional[float] = None, + start_pose: list[float] | None = None, + duration: float | None = None, + speed_percentage: float | None = None, trajectory_type: Literal["cubic", "quintic", "s_curve"] = "cubic", - jerk_limit: Optional[float] = None, + jerk_limit: float | None = None, clockwise: bool = False, ) -> bool: return _run( @@ -486,13 +552,13 @@ def smooth_arc_param( def smooth_spline( self, - waypoints: List[List[float]], + waypoints: list[list[float]], frame: Literal["WRF", "TRF"] = "WRF", - start_pose: Optional[List[float]] = None, - duration: Optional[float] = None, - speed_percentage: Optional[float] = None, + start_pose: list[float] | None = None, + duration: float | None = None, + speed_percentage: float | None = None, trajectory_type: Literal["cubic", "quintic", "s_curve"] = "cubic", - jerk_limit: Optional[float] = None, + jerk_limit: float | None = None, ) -> bool: return _run( self._inner.smooth_spline( @@ -508,16 +574,16 @@ def smooth_spline( def smooth_helix( self, - center: List[float], + center: list[float], radius: float, pitch: float, height: float, frame: Literal["WRF", "TRF"] = "WRF", trajectory_type: Literal["cubic", "quintic", "s_curve"] = "cubic", - jerk_limit: Optional[float] = None, - start_pose: Optional[List[float]] = None, - duration: Optional[float] = None, - speed_percentage: Optional[float] = None, + jerk_limit: float | None = None, + start_pose: list[float] | None = None, + duration: float | None = None, + speed_percentage: float | None = None, clockwise: bool = False, ) -> bool: return _run( @@ -538,12 +604,12 @@ def smooth_helix( def smooth_blend( self, - segments: List[Dict], + segments: list[dict], blend_time: float = 0.5, frame: Literal["WRF", "TRF"] = "WRF", - start_pose: Optional[List[float]] = None, - duration: Optional[float] = None, - speed_percentage: Optional[float] = None, + start_pose: list[float] | None = None, + duration: float | None = None, + speed_percentage: float | None = None, ) -> bool: return _run( self._inner.smooth_blend( @@ -558,15 +624,15 @@ def smooth_blend( def smooth_waypoints( self, - waypoints: List[List[float]], - blend_radii: Literal["AUTO"] | List[float] = "AUTO", + waypoints: list[list[float]], + blend_radii: Literal["AUTO"] | list[float] = "AUTO", blend_mode: Literal["parabolic", "circular", "none"] = "parabolic", - via_modes: Optional[List[str]] = None, + via_modes: list[str] | None = None, max_velocity: float = 100.0, max_acceleration: float = 500.0, frame: Literal["WRF", "TRF"] = "WRF", trajectory_type: Literal["cubic", "quintic", "s_curve"] = "quintic", - duration: Optional[float] = None, + duration: float | None = None, ) -> bool: return _run( self._inner.smooth_waypoints( diff --git a/parol6/commands/__init__.py b/parol6/commands/__init__.py index df86d23..0910028 100644 --- a/parol6/commands/__init__.py +++ b/parol6/commands/__init__.py @@ -1,29 +1,18 @@ -""" -Commands package for PAROL6. - -Important: -- Do NOT import individual command modules here to avoid circular imports. -- The controller uses dynamic discovery via CommandRegistry.discover_commands() - which imports command modules at runtime to trigger @register_command decorators. -""" - -# Re-export non-problematic IK helpers for convenience -from parol6.utils.ik import ( - CommandValue, - normalize_angle, - unwrap_angles, - calculate_adaptive_tolerance, - solve_ik_with_adaptive_tol_subdivision, - quintic_scaling, - AXIS_MAP, -) - -__all__ = [ - "CommandValue", - "normalize_angle", - "unwrap_angles", - "calculate_adaptive_tolerance", - "solve_ik_with_adaptive_tol_subdivision", - "quintic_scaling", - "AXIS_MAP", -] +""" +Commands package for PAROL6. +""" + +# Re-export IK helpers for convenience +from parol6.utils.ik import ( + AXIS_MAP, + quintic_scaling, + solve_ik, + unwrap_angles, +) + +__all__ = [ + "unwrap_angles", + "solve_ik", + "quintic_scaling", + "AXIS_MAP", +] diff --git a/parol6/commands/base.py b/parol6/commands/base.py index 6b30369..2ba229c 100644 --- a/parol6/commands/base.py +++ b/parol6/commands/base.py @@ -1,29 +1,30 @@ """ Base abstractions and helpers for command implementations. """ -from dataclasses import dataclass -from typing import Tuple, Optional, Dict, Any, TYPE_CHECKING, List, ClassVar, cast -from abc import ABC, abstractmethod -from enum import Enum -import logging + import json +import logging import time +from abc import ABC, abstractmethod +from dataclasses import dataclass +from enum import Enum +from typing import Any, ClassVar, cast -import roboticstoolbox as rp import numpy as np +import roboticstoolbox as rp import parol6.PAROL6_ROBOT as PAROL6_ROBOT -from parol6.protocol.wire import CommandCode from parol6.config import INTERVAL_S, TRACE -from parol6.utils.ik import AXIS_MAP +from parol6.protocol.wire import CommandCode from parol6.server.state import ControllerState - +from parol6.utils.ik import AXIS_MAP logger = logging.getLogger(__name__) class ExecutionStatusCode(Enum): """Enumeration for command execution status codes.""" + QUEUED = "QUEUED" EXECUTING = "EXECUTING" COMPLETED = "COMPLETED" @@ -36,60 +37,79 @@ class ExecutionStatus: """ Status returned from command execution steps. """ + code: ExecutionStatusCode message: str - error: Optional[Exception] = None - details: Optional[Dict[str, Any]] = None - error_type: Optional[str] = None + error: Exception | None = None + details: dict[str, Any] | None = None + error_type: str | None = None @classmethod - def executing(cls, message: str = "Executing", details: Optional[Dict[str, Any]] = None) -> "ExecutionStatus": + def executing( + cls, message: str = "Executing", details: dict[str, Any] | None = None + ) -> "ExecutionStatus": return cls(ExecutionStatusCode.EXECUTING, message, error=None, details=details) @classmethod - def completed(cls, message: str = "Completed", details: Optional[Dict[str, Any]] = None) -> "ExecutionStatus": + def completed( + cls, message: str = "Completed", details: dict[str, Any] | None = None + ) -> "ExecutionStatus": return cls(ExecutionStatusCode.COMPLETED, message, error=None, details=details) @classmethod - def failed(cls, message: str, error: Optional[Exception] = None, details: Optional[Dict[str, Any]] = None) -> "ExecutionStatus": + def failed( + cls, + message: str, + error: Exception | None = None, + details: dict[str, Any] | None = None, + ) -> "ExecutionStatus": et = type(error).__name__ if error is not None else None - return cls(ExecutionStatusCode.FAILED, message, error=error, details=details, error_type=et) + return cls( + ExecutionStatusCode.FAILED, + message, + error=error, + details=details, + error_type=et, + ) # ----- Shared context and small utilities ----- + @dataclass class CommandContext: """Shared dynamic execution context for commands.""" + udp_transport: Any = None - addr: Optional[tuple] = None + addr: tuple | None = None gcode_interpreter: Any = None dt: float = INTERVAL_S + # Parsing utilities (lightweight, shared) -def _noneify(token: Any) -> Optional[str]: +def _noneify(token: Any) -> str | None: if token is None: return None t = str(token).strip() return None if t == "" or t.upper() in ("NONE", "NULL") else t -def parse_int(token: Any) -> Optional[int]: +def parse_int(token: Any) -> int | None: t = _noneify(token) return None if t is None else int(t) -def parse_float(token: Any) -> Optional[float]: +def parse_float(token: Any) -> float | None: t = _noneify(token) return None if t is None else float(t) -def csv_ints(token: Any) -> List[int]: +def csv_ints(token: Any) -> list[int]: t = _noneify(token) return [] if t is None else [int(x) for x in t.split(",") if x != ""] -def csv_floats(token: Any) -> List[float]: +def csv_floats(token: Any) -> list[float]: t = _noneify(token) return [] if t is None else [float(x) for x in t.split(",") if x != ""] @@ -99,7 +119,7 @@ def parse_bool(token: Any) -> bool: return t in ("1", "true", "yes", "on") -def typed(token: Any, type_=float): +def typed(token: Any, type_: type[Any] = float) -> Any | None: """Parse token with type, supporting None/Null/empty as None.""" t = _noneify(token) if t is None: @@ -109,16 +129,18 @@ def typed(token: Any, type_=float): return type_(t) -def expect_len(parts: List[str], n: int, cmd: str) -> None: +def expect_len(parts: list[str], n: int, cmd: str) -> None: """Ensure parts list has exactly n elements.""" if len(parts) != n: - raise ValueError(f"{cmd} requires {n-1} parameters, got {len(parts)-1}") + raise ValueError(f"{cmd} requires {n - 1} parameters, got {len(parts) - 1}") -def at_least_len(parts: List[str], n: int, cmd: str) -> None: +def at_least_len(parts: list[str], n: int, cmd: str) -> None: """Ensure parts list has at least n elements.""" if len(parts) < n: - raise ValueError(f"{cmd} requires at least {n-1} parameters, got {len(parts)-1}") + raise ValueError( + f"{cmd} requires at least {n - 1} parameters, got {len(parts) - 1}" + ) def parse_frame(token: Any) -> str: @@ -144,9 +166,10 @@ def parse_axis(token: Any) -> str: class Countdown: """Simple count-down timer.""" + def __init__(self, count: int): self.count = max(0, int(count)) - + def tick(self) -> bool: """Decrement and return True when reaches zero.""" if self.count > 0: @@ -156,6 +179,7 @@ def tick(self) -> bool: class Debouncer: """Simple count-based debouncer.""" + def __init__(self, count: int = 5) -> None: self.count_init = max(0, int(count)) self.count = self.count_init @@ -181,11 +205,21 @@ class CommandBase(ABC): """ Reusable base for commands with shared lifecycle and safety helpers. """ + # Set by @register_command decorator; used by controller stream fast-path _registered_name: ClassVar[str] = "" - __slots__ = ("is_valid", "is_finished", "error_state", "error_message", - "udp_transport", "addr", "gcode_interpreter", "_t0", "_t_end") + __slots__ = ( + "is_valid", + "is_finished", + "error_state", + "error_message", + "udp_transport", + "addr", + "gcode_interpreter", + "_t0", + "_t_end", + ) def __init__(self) -> None: self.is_valid: bool = True @@ -195,8 +229,8 @@ def __init__(self) -> None: self.udp_transport: Any = None self.addr: Any = None self.gcode_interpreter: Any = None - self._t0: Optional[float] = None - self._t_end: Optional[float] = None + self._t0: float | None = None + self._t_end: float | None = None # Ensure command objects are usable as dict keys (e.g., in server command_id_map) def __hash__(self) -> int: @@ -228,7 +262,7 @@ def stop_and_idle(state: ControllerState) -> None: state.Speed_out.fill(0) state.Command_out = CommandCode.IDLE - def bind(self, context: CommandContext): + def bind(self, context: CommandContext) -> None: """ Bind dynamic execution context. Controller should call this prior to setup(). """ @@ -237,7 +271,7 @@ def bind(self, context: CommandContext): self.gcode_interpreter = context.gcode_interpreter @abstractmethod - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """ Check if this command can handle the given message parts. @@ -251,7 +285,7 @@ def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: """ raise NotImplementedError - def match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + def match(self, parts: list[str]) -> tuple[bool, str | None]: """ Wrapper that guards subclass do_match() to avoid propagating exceptions. Centralizes try/except so subclasses don't repeat it. @@ -326,6 +360,7 @@ def progress01(self, duration_s: float) -> float: p = (time.perf_counter() - self._t0) / duration_s return 0.0 if p < 0.0 else (1.0 if p > 1.0 else p) + class QueryCommand(CommandBase): """ Base class for query commands that execute immediately and bypass the queue. @@ -342,7 +377,7 @@ def reply_text(self, message: str) -> None: except Exception as e: self.log_warning("Failed to send UDP reply: %s", e) - def reply_ascii(self, prefix_or_message: str, payload: Optional[str] = None) -> None: + def reply_ascii(self, prefix_or_message: str, payload: str | None = None) -> None: """ Reply as 'PREFIX|payload' if payload provided; otherwise send prefix_or_message verbatim. """ @@ -365,7 +400,11 @@ def tick(self, state: ControllerState) -> ExecutionStatus: Controllers should prefer tick() over calling execute_step() directly. """ if self.is_finished or not self.is_valid: - return ExecutionStatus.completed("Already finished") if self.is_finished else ExecutionStatus.failed("Invalid command") + return ( + ExecutionStatus.completed("Already finished") + if self.is_finished + else ExecutionStatus.failed("Invalid command") + ) if not self.udp_transport or not self.addr: self.fail("Missing UDP transport or address") return ExecutionStatus.failed("Missing UDP transport or address") @@ -373,7 +412,7 @@ def tick(self, state: ControllerState) -> ExecutionStatus: status = self.execute_step(state) except Exception as e: # Hard failure safeguards - self.fail(f"Unhandled exception: {e}") + self.fail(str(e)) return ExecutionStatus.failed("Execution error", error=e) return status @@ -385,6 +424,7 @@ class MotionCommand(CommandBase): Motion commands involve robot movement and require the controller to be in an enabled state. Some motion commands (like jog commands) can be replaced in stream mode. """ + streamable: bool = False # Can be replaced in stream mode (only for jog commands) # Limits and kinematic constants @@ -414,28 +454,39 @@ def linmap_pct(pct: float, lo: float, hi: float) -> float: # ---- per-joint max speed/acc ---- def joint_vmax(self, velocity_percent: float) -> np.ndarray: - return self.J_MIN + (self.J_MAX - self.J_MIN) * (max(0.0, min(100.0, velocity_percent)) / 100.0) + return self.J_MIN + (self.J_MAX - self.J_MIN) * ( + max(0.0, min(100.0, velocity_percent)) / 100.0 + ) def joint_amax_steps(self, accel_percent: float) -> np.ndarray: a_rad = self.linmap_pct(accel_percent, self.ACC_MIN_RAD, self.ACC_MAX_RAD) - return np.asarray(PAROL6_ROBOT.ops.speed_rad_to_steps(np.full(6, a_rad)), dtype=float) + return np.asarray( + PAROL6_ROBOT.ops.speed_rad_to_steps(np.full(6, a_rad)), dtype=float + ) # ---- speed scaling & limits ---- def scale_speeds_to_joint_max(self, speeds: np.ndarray) -> np.ndarray: denom = np.where(self.J_MAX != 0.0, self.J_MAX, 1.0) scale = float(np.max(np.abs(speeds) / denom)) - return np.rint(speeds / scale) if scale > 1.0 else speeds + if scale > 1.0: + return np.rint(speeds / scale).astype(np.int32) + else: + return np.asarray(speeds, dtype=np.int32) def limit_hit_mask(self, pos_steps: np.ndarray, speeds: np.ndarray) -> np.ndarray: - return ((speeds > 0) & (pos_steps >= self.LIMS_STEPS[:, 1])) | ((speeds < 0) & (pos_steps <= self.LIMS_STEPS[:, 0])) + return ((speeds > 0) & (pos_steps >= self.LIMS_STEPS[:, 1])) | ( + (speeds < 0) & (pos_steps <= self.LIMS_STEPS[:, 0]) + ) # ---- trapezoid batch planner for step-space ---- @staticmethod - def plan_trapezoids(start_steps: np.ndarray, target_steps: np.ndarray, tgrid: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + def plan_trapezoids( + start_steps: np.ndarray, target_steps: np.ndarray, tgrid: np.ndarray + ) -> tuple[np.ndarray, np.ndarray]: n = int(tgrid.size) q = np.empty((n, 6), dtype=float) qd = np.empty((n, 6), dtype=float) - stationary = (target_steps == start_steps) + stationary = target_steps == start_steps if np.any(stationary): q[:, stationary] = start_steps[stationary] qd[:, stationary] = 0.0 @@ -452,7 +503,7 @@ def fail_and_idle(self, state: ControllerState, message: str) -> None: # ---- Higher-level IO helpers for common patterns ---- def set_move_position(self, state: ControllerState, steps: np.ndarray) -> None: """Set position for MOVE command (zero speeds, Command=MOVE).""" - np.copyto(state.Position_out, steps, casting='no') + np.copyto(state.Position_out, steps, casting="no") state.Speed_out.fill(0) state.Command_out = CommandCode.MOVE @@ -462,16 +513,21 @@ def tick(self, state: ControllerState) -> ExecutionStatus: Controllers should prefer tick() over calling execute_step() directly. """ if self.is_finished or not self.is_valid: - return ExecutionStatus.completed("Already finished") if self.is_finished else ExecutionStatus.failed("Invalid command") + return ( + ExecutionStatus.completed("Already finished") + if self.is_finished + else ExecutionStatus.failed("Invalid command") + ) try: status = self.execute_step(state) except Exception as e: # Hard failure safeguards - self.fail_and_idle(state, f"Unhandled exception: {e}") - self.log_error(f"Unhandled exception: {e}") + self.fail_and_idle(state, str(e)) + self.log_error(str(e)) return ExecutionStatus.failed("Execution error", error=e) return status + # TODO: need to get and support the other motion profiles from the original program class MotionProfile: """ @@ -496,7 +552,9 @@ def from_duration_steps( n = max(2, int(np.ceil(dur / max(1e-9, dt)))) tgrid = np.linspace(0.0, dur, n, dtype=float) q, _qd = MotionCommand.plan_trapezoids( - np.asarray(start_steps, dtype=float), np.asarray(target_steps, dtype=float), tgrid + np.asarray(start_steps, dtype=float), + np.asarray(target_steps, dtype=float), + tgrid, ) return cast(np.ndarray, q.astype(np.int32, copy=False)) @@ -517,11 +575,17 @@ def from_velocity_percent( # Per-joint vmax and amax (steps/s and steps/s^2) jmin = MotionCommand.J_MIN jmax = MotionCommand.J_MAX - v_max_joint = jmin + (jmax - jmin) * (max(0.0, min(100.0, velocity_percent)) / 100.0) + v_max_joint = jmin + (jmax - jmin) * ( + max(0.0, min(100.0, velocity_percent)) / 100.0 + ) # Compute accel steps without instantiating MotionCommand - a_rad = MotionCommand.linmap_pct(accel_percent, MotionCommand.ACC_MIN_RAD, MotionCommand.ACC_MAX_RAD) - a_steps_vec = np.asarray(PAROL6_ROBOT.ops.speed_rad_to_steps(np.full(6, a_rad)), dtype=float) + a_rad = MotionCommand.linmap_pct( + accel_percent, MotionCommand.ACC_MIN_RAD, MotionCommand.ACC_MAX_RAD + ) + a_steps_vec = np.asarray( + PAROL6_ROBOT.ops.speed_rad_to_steps(np.full(6, a_rad)), dtype=float + ) if np.any(v_max_joint <= 0) or np.any(a_steps_vec <= 0): raise ValueError("Invalid speed/acceleration (must be positive).") @@ -533,17 +597,19 @@ def from_velocity_percent( # Safe accel time for short paths t_accel_adj = t_accel.copy() mask = short_path - # Guard divide-by-zero - safe = a_steps_vec[mask] > 0 t_accel_adj[mask] = 0.0 - if np.any(mask): - t_accel_adj[mask][safe] = np.sqrt(path[mask][safe] / a_steps_vec[mask][safe]) # type: ignore[index] + mask_safe = mask & (a_steps_vec > 0) + t_accel_adj[mask_safe] = np.sqrt(path[mask_safe] / a_steps_vec[mask_safe]) # Per-joint total time, then horizon - joint_time = np.where(short_path, 2.0 * t_accel_adj, path / v_max_joint + t_accel) + joint_time = np.where( + short_path, 2.0 * t_accel_adj, path / v_max_joint + t_accel + ) total_time = float(np.max(joint_time)) if total_time <= 0.0: - return cast(np.ndarray, np.asarray(start_steps, dtype=np.int32).reshape(1, -1)) + return cast( + np.ndarray, np.asarray(start_steps, dtype=np.int32).reshape(1, -1) + ) if total_time < (2 * dt): total_time = 2 * dt @@ -556,7 +622,7 @@ def from_velocity_percent( class SystemCommand(CommandBase): """ Base class for system control commands that can execute regardless of enable state. - + System commands control the overall state of the robot controller (enable/disable, stop, etc.) and can execute even when the controller is disabled. """ @@ -566,11 +632,15 @@ def tick(self, state: "ControllerState") -> ExecutionStatus: Centralized lifecycle/error handling for system commands. """ if self.is_finished or not self.is_valid: - return ExecutionStatus.completed("Already finished") if self.is_finished else ExecutionStatus.failed("Invalid command") + return ( + ExecutionStatus.completed("Already finished") + if self.is_finished + else ExecutionStatus.failed("Invalid command") + ) try: status = self.execute_step(state) except Exception as e: - self.fail(f"Unhandled exception: {e}") - self.log_error("Unhandled exception: %s", e) + self.fail(str(e)) + self.log_error(str(e)) return ExecutionStatus.failed("Execution error", error=e) return status diff --git a/parol6/commands/basic_commands.py b/parol6/commands/basic_commands.py index 300f6f4..c2e7d0e 100644 --- a/parol6/commands/basic_commands.py +++ b/parol6/commands/basic_commands.py @@ -1,398 +1,509 @@ -""" -Basic Robot Commands -Contains fundamental movement commands: Home, Jog, and MultiJog -""" - -import logging -import numpy as np -from typing import List, Tuple, Optional -import parol6.PAROL6_ROBOT as PAROL6_ROBOT -from .base import MotionCommand, ExecutionStatus, parse_int, parse_float, csv_ints, csv_floats -from parol6.config import INTERVAL_S -from parol6.protocol.wire import CommandCode -from parol6.server.command_registry import register_command -from math import ceil - -logger = logging.getLogger(__name__) - -@register_command("HOME") -class HomeCommand(MotionCommand): - """ - 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. - """ - - __slots__ = ( - "state", - "start_cmd_counter", - "timeout_counter", - ) - - def __init__(self): - super().__init__() - # State machine: START -> WAIT_FOR_UNHOMED -> WAIT_FOR_HOMED -> FINISHED - self.state = "START" - # Counter to send the home command for multiple cycles - self.start_cmd_counter = 10 # Send command 100 for 10 cycles (0.1s) - # Safety timeout (20 seconds at 0.01s interval) - self.timeout_counter = 2000 - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """ - Parse HOME command (no parameters). - - Format: HOME - """ - if len(parts) != 1: - return (False, "HOME command takes no parameters") - self.log_trace("Parsed HOME command") - return (True, None) - - def execute_step(self, state) -> ExecutionStatus: - """ - Manages the homing command and monitors for completion using a state machine. - """ - # --- State: START --- - # On the first few executions, continuously send the 'home' (100) command. - if self.state == "START": - logger.debug(f" -> Sending home signal (100)... Countdown: {self.start_cmd_counter}") - state.Command_out = CommandCode.HOME - self.start_cmd_counter -= 1 - if self.start_cmd_counter <= 0: - # Once sent for enough cycles, move to the next state - self.state = "WAITING_FOR_UNHOMED" - return ExecutionStatus.executing("Homing: start") - - # --- State: WAITING_FOR_UNHOMED --- - # The robot's firmware should reset the homed status. We wait to see that happen. - # During this time, we send 'idle' (255) to let the robot's controller take over. - if self.state == "WAITING_FOR_UNHOMED": - state.Command_out = CommandCode.IDLE - # Homing sequence initiated detection - if np.any(state.Homed_in[:6] == 0): - logger.info(" -> Homing sequence initiated by robot.") - self.state = "WAITING_FOR_HOMED" - # Homing timeout protection - self.timeout_counter -= 1 - if self.timeout_counter <= 0: - raise TimeoutError("Timeout waiting for robot to start homing sequence.") - return ExecutionStatus.executing("Homing: waiting for unhomed") - - # --- State: WAITING_FOR_HOMED --- - # Now we wait for all joints to report that they are homed (all flags are 1). - if self.state == "WAITING_FOR_HOMED": - state.Command_out = CommandCode.IDLE - # Homing completion verification - if np.all(state.Homed_in[:6] == 1): - self.log_info("Homing sequence complete. All joints reported home.") - self.is_finished = True - self.stop_and_idle(state) - return ExecutionStatus.completed("Homing complete") - - return ExecutionStatus.executing("Homing: waiting for homed") - - return ExecutionStatus.executing("Homing") - - -@register_command("JOG") -class JogCommand(MotionCommand): - """ - A non-blocking command to jog a joint for a specific duration or distance. - It performs all safety and validity checks upon initialization. - """ - streamable = True # Can be replaced in stream mode - - __slots__ = ( - "mode", - "command_step", - "joint", - "speed_percentage", - "duration", - "distance_deg", - "direction", - "joint_index", - "speed_out", - "command_len", - "target_position" - ) - - def __init__(self): - """ - Initializes the jog command. - Parameters are parsed in do_match() method. - """ - super().__init__() - self.mode = None - self.command_step = 0 - - # Parameters (set in match()) - self.joint = None - self.speed_percentage = None - self.duration = None - self.distance_deg = None - - # Calculated values - self.direction = 1 - self.joint_index = 0 - self.speed_out = 0 - self.target_position = 0 - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """ - Parse JOG command parameters. - - Format: JOG|joint|speed_pct|duration|distance - Example: JOG|0|50|2.0|None - - Args: - parts: Pre-split message parts - - Returns: - Tuple of (can_handle, error_message) - """ - if len(parts) != 5: - return (False, "JOG requires 4 parameters: joint, speed, duration, distance") - - # Parse parameters using utilities - self.joint = parse_int(parts[1]) - self.speed_percentage = parse_float(parts[2]) - self.duration = parse_float(parts[3]) - self.distance_deg = parse_float(parts[4]) - - if self.joint is None or self.speed_percentage is None: - return (False, "Joint and speed percentage are required") - - # Determine mode - if self.duration and self.distance_deg: - self.mode = 'distance' - self.log_trace("Parsed Jog: Joint %s, Distance %s deg, Duration %ss.", self.joint, self.distance_deg, self.duration) - elif self.duration: - self.mode = 'time' - self.log_trace("Parsed Jog: Joint %s, Speed %s%%, Duration %ss.", self.joint, self.speed_percentage, self.duration) - elif self.distance_deg: - self.mode = 'distance' - self.log_trace("Parsed Jog: Joint %s, Speed %s%%, Distance %s deg.", self.joint, self.speed_percentage, self.distance_deg) - else: - return (False, "JOG requires either duration or distance") - - self.is_valid = True - return (True, None) - - def setup(self, state): - """Pre-computes speeds and target positions using live data.""" - # Validate joint is set - if self.joint is None: - raise RuntimeError("Joint index not set") - - # Joint direction and index mapping - self.direction = 1 if 0 <= self.joint <= 5 else -1 - self.joint_index = self.joint if self.direction == 1 else self.joint - 6 - - lims = self.LIMS_STEPS[self.joint_index] - min_limit, max_limit = lims[0], lims[1] - - distance_steps = 0 - if self.distance_deg is not None: - distance_steps = PAROL6_ROBOT.ops.deg_to_steps(abs(self.distance_deg), self.joint_index) - self.target_position = state.Position_in[self.joint_index] + (distance_steps * self.direction) - - if not (min_limit <= self.target_position <= max_limit): - # Convert to degrees for clearer error message - target_deg = PAROL6_ROBOT.ops.steps_to_deg(self.target_position, self.joint_index) - min_deg = PAROL6_ROBOT.ops.steps_to_deg(min_limit, self.joint_index) - max_deg = PAROL6_ROBOT.ops.steps_to_deg(max_limit, self.joint_index) - raise ValueError(f"Target position {target_deg:.2f}° is out of joint limits ({min_deg:.2f}°, {max_deg:.2f}°).") - - # Motion timing calculations - jog_min = self.JOG_MIN[self.joint_index] - jog_max = self.JOG_MAX[self.joint_index] - - if self.mode == 'distance' and self.duration: - speed_steps_per_sec = int(distance_steps / self.duration) if self.duration > 0 else 0 - if speed_steps_per_sec > jog_max: - raise ValueError(f"Required speed ({speed_steps_per_sec} steps/s) exceeds joint's max jog speed ({jog_max} steps/s).") - # Ensure speed is at least the minimum jog speed if not zero - if speed_steps_per_sec > 0: - speed_steps_per_sec = max(speed_steps_per_sec, jog_min) - else: - if self.speed_percentage is None: - raise ValueError("'speed_percentage' must be provided if not calculating automatically.") - speed_steps_per_sec = int(self.linmap_pct(abs(self.speed_percentage), jog_min, jog_max)) - - self.speed_out = speed_steps_per_sec * self.direction - - # Start timer for time-based mode - if self.mode == 'time' and self.duration and self.duration > 0: - self.start_timer(self.duration) - - def execute_step(self, state) -> ExecutionStatus: - """This is the EXECUTION phase. It runs on every loop cycle.""" - - # Type guard to ensure joint_index is valid - if self.joint_index is None or not isinstance(self.joint_index, int): - raise RuntimeError("Invalid joint index in execute_step") - - stop_reason = None - cur = state.Position_in[self.joint_index] - - if self.mode == 'time' and self.timer_expired(): - stop_reason = "Timed jog finished." - elif self.mode == 'distance' and \ - ((self.direction == 1 and cur >= self.target_position) or \ - (self.direction == -1 and cur <= self.target_position)): - stop_reason = "Distance jog finished." - - if not stop_reason: - # Use base class limit_hit_mask helper - speeds_array = np.zeros(6) - speeds_array[self.joint_index] = self.speed_out - limit_mask = self.limit_hit_mask(state.Position_in, speeds_array) - if limit_mask[self.joint_index]: - stop_reason = f"Limit reached on joint {self.joint_index + 1}." - - if stop_reason: - if stop_reason.startswith("Limit"): - logger.warning(stop_reason) - else: - self.log_trace(stop_reason) - - self.is_finished = True - self.stop_and_idle(state) - return ExecutionStatus.completed(stop_reason) - - state.Speed_out.fill(0) - state.Speed_out[self.joint_index] = self.speed_out - state.Command_out = CommandCode.JOG - self.command_step += 1 - return ExecutionStatus.executing("Jogging") - - -@register_command("MULTIJOG") -class MultiJogCommand(MotionCommand): - """ - A non-blocking command to jog multiple joints simultaneously for a specific duration. - It performs all safety and validity checks upon initialization. - """ - streamable = True # Can be replaced in stream mode - - __slots__ = ( - "command_step", - "joints", - "speed_percentages", - "duration", - "command_len", - "speeds_out", - "_lims_steps", - ) - - def __init__(self): - """ - Initializes the multi-jog command. - Parameters are parsed in do_match() method. - """ - super().__init__() - self.command_step = 0 - - # Parameters (set in do_match()) - self.joints = None - self.speed_percentages = None - self.duration = None - self.command_len = 0 - - # Calculated values - self.speeds_out = np.zeros(6, dtype=np.int32) - self._lims_steps = PAROL6_ROBOT.joint.limits.steps - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """ - Parse MULTIJOG command parameters. - - Format: MULTIJOG|joints_csv|speeds_csv|duration - Example: MULTIJOG|0,1,2|50,75,100|3.0 - - Args: - parts: Pre-split message parts - - Returns: - Tuple of (can_handle, error_message) - """ - if len(parts) != 4: - return (False, "MULTIJOG requires 3 parameters: joints, speeds, duration") - - # Parse parameters using utilities - self.joints = csv_ints(parts[1]) - self.speed_percentages = csv_floats(parts[2]) - self.duration = parse_float(parts[3]) or 0.0 - - # Validate - if len(self.joints) != len(self.speed_percentages): - return (False, "Number of joints must match number of speeds") - - if self.duration <= 0: - return (False, "Duration must be positive") - - # Conflict detection on base joints - base = set() - for j in self.joints: - b = j % 6 - if b in base: - return (False, f"Conflicting commands for Joint {b + 1}") - base.add(b) - - self.log_trace("Parsed MultiJog for joints %s with speeds %s%% for %ss.", self.joints, self.speed_percentages, self.duration) - - self.command_len = ceil(self.duration / INTERVAL_S) - self.is_valid = True - return (True, None) - - def setup(self, state): - """Pre-computes the speeds for each joint.""" - # Validate joints and speed_percentages are set - if self.joints is None or self.speed_percentages is None: - raise ValueError("Joints or speed percentages not set") - - # Vectorized computation for all joints - joints_arr = np.asarray(self.joints, dtype=int) - speeds_pct = np.asarray(self.speed_percentages, dtype=float) - - # Map to base joint index (0-5) and direction (+/-) - direction = np.where((joints_arr >= 0) & (joints_arr <= 5), 1, -1) - joint_index = np.where(direction == 1, joints_arr, joints_arr - 6) - - # Validate indices - invalid_mask = (joint_index < 0) | (joint_index >= 6) - if np.any(invalid_mask): - bad = joint_index[invalid_mask] - raise ValueError(f"Invalid joint indices {bad.tolist()}") - - pct = np.clip(np.abs(speeds_pct) / 100.0, 0.0, 1.0) - for i, idx in enumerate(joint_index): - self.speeds_out[idx] = int(self.linmap_pct( - pct[i] * 100.0, self.JOG_MIN[idx], self.JOG_MAX[idx])) * direction[i] - - # Start timer if duration is specified - if self.duration and self.duration > 0: - self.start_timer(self.duration) - - def execute_step(self, state) -> ExecutionStatus: - """This is the EXECUTION phase. It runs on every loop cycle.""" - # Stop if the duration has elapsed (check both timer and step count) - if self.timer_expired() or self.command_step >= self.command_len: - self.is_finished = True - self.stop_and_idle(state) - return ExecutionStatus.completed("MultiJog") - - # Use base class helper for limit checks - limit_mask = self.limit_hit_mask(state.Position_in, self.speeds_out) - if np.any(limit_mask): - i = np.argmax(limit_mask) # first violating joint - logger.warning(f"Limit reached on joint {i + 1}. Stopping jog.") - self.is_finished = True - self.stop_and_idle(state) - return ExecutionStatus.completed(f"Limit reached on J{i+1}") - - # Apply self.speeds_out - np.copyto(state.Speed_out, self.speeds_out, casting='no') - state.Command_out = CommandCode.JOG - self.command_step += 1 - return ExecutionStatus.executing("MultiJog") +""" +Basic Robot Commands +Contains fundamental movement commands: Home, Jog, and MultiJog +""" + +import logging +from math import ceil + +import numpy as np + +import parol6.PAROL6_ROBOT as PAROL6_ROBOT +from parol6.config import INTERVAL_S +from parol6.protocol.wire import CommandCode +from parol6.server.command_registry import register_command +from parol6.server.state import ControllerState +from parol6.tools import TOOL_CONFIGS, list_tools + +from .base import ( + ExecutionStatus, + MotionCommand, + csv_floats, + csv_ints, + parse_float, + parse_int, +) + +logger = logging.getLogger(__name__) + + +@register_command("HOME") +class HomeCommand(MotionCommand): + """ + 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. + """ + + __slots__ = ( + "state", + "start_cmd_counter", + "timeout_counter", + ) + + def __init__(self): + super().__init__() + # State machine: START -> WAIT_FOR_UNHOMED -> WAIT_FOR_HOMED -> FINISHED + self.state = "START" + # Counter to send the home command for multiple cycles + self.start_cmd_counter = 10 # Send command 100 for 10 cycles (0.1s) + # Safety timeout (20 seconds at 0.01s interval) + self.timeout_counter = 2000 + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse HOME command (no parameters). + + Format: HOME + """ + if len(parts) != 1: + return (False, "HOME command takes no parameters") + self.log_trace("Parsed HOME command") + return (True, None) + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + """ + Manages the homing command and monitors for completion using a state machine. + """ + # --- State: START --- + # On the first few executions, continuously send the 'home' (100) command. + if self.state == "START": + logger.debug( + f" -> Sending home signal (100)... Countdown: {self.start_cmd_counter}" + ) + state.Command_out = CommandCode.HOME + self.start_cmd_counter -= 1 + if self.start_cmd_counter <= 0: + # Once sent for enough cycles, move to the next state + self.state = "WAITING_FOR_UNHOMED" + return ExecutionStatus.executing("Homing: start") + + # --- State: WAITING_FOR_UNHOMED --- + # The robot's firmware should reset the homed status. We wait to see that happen. + # During this time, we send 'idle' (255) to let the robot's controller take over. + if self.state == "WAITING_FOR_UNHOMED": + state.Command_out = CommandCode.IDLE + # Homing sequence initiated detection + if np.any(state.Homed_in[:6] == 0): + logger.info(" -> Homing sequence initiated by robot.") + self.state = "WAITING_FOR_HOMED" + # Homing timeout protection + self.timeout_counter -= 1 + if self.timeout_counter <= 0: + raise TimeoutError( + "Timeout waiting for robot to start homing sequence." + ) + return ExecutionStatus.executing("Homing: waiting for unhomed") + + # --- State: WAITING_FOR_HOMED --- + # Now we wait for all joints to report that they are homed (all flags are 1). + if self.state == "WAITING_FOR_HOMED": + state.Command_out = CommandCode.IDLE + # Homing completion verification + if np.all(state.Homed_in[:6] == 1): + self.log_info("Homing sequence complete. All joints reported home.") + self.is_finished = True + self.stop_and_idle(state) + return ExecutionStatus.completed("Homing complete") + + return ExecutionStatus.executing("Homing: waiting for homed") + + return ExecutionStatus.executing("Homing") + + +@register_command("JOG") +class JogCommand(MotionCommand): + """ + A non-blocking command to jog a joint for a specific duration or distance. + It performs all safety and validity checks upon initialization. + """ + + streamable = True # Can be replaced in stream mode + + __slots__ = ( + "mode", + "command_step", + "joint", + "speed_percentage", + "duration", + "distance_deg", + "direction", + "joint_index", + "speed_out", + "command_len", + "target_position", + ) + + def __init__(self): + """ + Initializes the jog command. + Parameters are parsed in do_match() method. + """ + super().__init__() + self.mode = None + self.command_step = 0 + + # Parameters (set in match()) + self.joint = None + self.speed_percentage = None + self.duration = None + self.distance_deg = None + + # Calculated values + self.direction = 1 + self.joint_index = 0 + self.speed_out = 0 + self.target_position = 0 + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse JOG command parameters. + + Format: JOG|joint|speed_pct|duration|distance + Example: JOG|0|50|2.0|None + + Args: + parts: Pre-split message parts + + Returns: + Tuple of (can_handle, error_message) + """ + if len(parts) != 5: + return ( + False, + "JOG requires 4 parameters: joint, speed, duration, distance", + ) + + # Parse parameters using utilities + self.joint = parse_int(parts[1]) + self.speed_percentage = parse_float(parts[2]) + self.duration = parse_float(parts[3]) + self.distance_deg = parse_float(parts[4]) + + if self.joint is None or self.speed_percentage is None: + return (False, "Joint and speed percentage are required") + + # Determine mode + if self.duration and self.distance_deg: + self.mode = "distance" + self.log_trace( + "Parsed Jog: Joint %s, Distance %s deg, Duration %ss.", + self.joint, + self.distance_deg, + self.duration, + ) + elif self.duration: + self.mode = "time" + self.log_trace( + "Parsed Jog: Joint %s, Speed %s%%, Duration %ss.", + self.joint, + self.speed_percentage, + self.duration, + ) + elif self.distance_deg: + self.mode = "distance" + self.log_trace( + "Parsed Jog: Joint %s, Speed %s%%, Distance %s deg.", + self.joint, + self.speed_percentage, + self.distance_deg, + ) + else: + return (False, "JOG requires either duration or distance") + + self.is_valid = True + return (True, None) + + def setup(self, state): + """Pre-computes speeds and target positions using live data.""" + # Validate joint is set + if self.joint is None: + raise RuntimeError("Joint index not set") + + # Joint direction and index mapping + self.direction = 1 if 0 <= self.joint <= 5 else -1 + self.joint_index = self.joint if self.direction == 1 else self.joint - 6 + + lims = self.LIMS_STEPS[self.joint_index] + min_limit, max_limit = lims[0], lims[1] + + distance_steps = 0 + if self.distance_deg is not None: + distance_steps = int( + PAROL6_ROBOT.ops.deg_to_steps(abs(self.distance_deg), self.joint_index) + ) + self.target_position = state.Position_in[self.joint_index] + ( + distance_steps * self.direction + ) + + if not (min_limit <= self.target_position <= max_limit): + # Convert to degrees for clearer error message + target_deg = PAROL6_ROBOT.ops.steps_to_deg( + self.target_position, self.joint_index + ) + min_deg = PAROL6_ROBOT.ops.steps_to_deg(min_limit, self.joint_index) + max_deg = PAROL6_ROBOT.ops.steps_to_deg(max_limit, self.joint_index) + raise ValueError( + f"Target position {target_deg:.2f}° is out of joint limits ({min_deg:.2f}°, {max_deg:.2f}°)." + ) + + # Motion timing calculations + jog_min = self.JOG_MIN[self.joint_index] + jog_max = self.JOG_MAX[self.joint_index] + + if self.mode == "distance" and self.duration: + speed_steps_per_sec = ( + int(distance_steps / self.duration) if self.duration > 0 else 0 + ) + if speed_steps_per_sec > jog_max: + raise ValueError( + f"Required speed ({speed_steps_per_sec} steps/s) exceeds joint's max jog speed ({jog_max} steps/s)." + ) + # Ensure speed is at least the minimum jog speed if not zero + if speed_steps_per_sec > 0: + speed_steps_per_sec = max(speed_steps_per_sec, jog_min) + else: + if self.speed_percentage is None: + raise ValueError( + "'speed_percentage' must be provided if not calculating automatically." + ) + speed_steps_per_sec = int( + self.linmap_pct(abs(self.speed_percentage), jog_min, jog_max) + ) + + self.speed_out = speed_steps_per_sec * self.direction + + # Start timer for time-based mode + if self.mode == "time" and self.duration and self.duration > 0: + self.start_timer(self.duration) + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + """This is the EXECUTION phase. It runs on every loop cycle.""" + + # Type guard to ensure joint_index is valid + if self.joint_index is None or not isinstance(self.joint_index, int): + raise RuntimeError("Invalid joint index in execute_step") + + stop_reason = None + cur = state.Position_in[self.joint_index] + + if self.mode == "time" and self.timer_expired(): + stop_reason = "Timed jog finished." + elif self.mode == "distance" and ( + (self.direction == 1 and cur >= self.target_position) + or (self.direction == -1 and cur <= self.target_position) + ): + stop_reason = "Distance jog finished." + + if not stop_reason: + # Use base class limit_hit_mask helper + speeds_array = np.zeros(6) + speeds_array[self.joint_index] = self.speed_out + limit_mask = self.limit_hit_mask(state.Position_in, speeds_array) + if limit_mask[self.joint_index]: + stop_reason = f"Limit reached on joint {self.joint_index + 1}." + + if stop_reason: + if stop_reason.startswith("Limit"): + logger.warning(stop_reason) + else: + self.log_trace(stop_reason) + + self.is_finished = True + self.stop_and_idle(state) + return ExecutionStatus.completed(stop_reason) + + state.Speed_out.fill(0) + state.Speed_out[self.joint_index] = self.speed_out + state.Command_out = CommandCode.JOG + self.command_step += 1 + return ExecutionStatus.executing("Jogging") + + +@register_command("MULTIJOG") +class MultiJogCommand(MotionCommand): + """ + A non-blocking command to jog multiple joints simultaneously for a specific duration. + It performs all safety and validity checks upon initialization. + """ + + streamable = True # Can be replaced in stream mode + + __slots__ = ( + "command_step", + "joints", + "speed_percentages", + "duration", + "command_len", + "speeds_out", + "_lims_steps", + ) + + def __init__(self): + """ + Initializes the multi-jog command. + Parameters are parsed in do_match() method. + """ + super().__init__() + self.command_step = 0 + + # Parameters (set in do_match()) + self.joints = None + self.speed_percentages = None + self.duration = None + self.command_len = 0 + + # Calculated values + self.speeds_out = np.zeros(6, dtype=np.int32) + self._lims_steps = PAROL6_ROBOT.joint.limits.steps + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse MULTIJOG command parameters. + + Format: MULTIJOG|joints_csv|speeds_csv|duration + Example: MULTIJOG|0,1,2|50,75,100|3.0 + + Args: + parts: Pre-split message parts + + Returns: + Tuple of (can_handle, error_message) + """ + if len(parts) != 4: + return (False, "MULTIJOG requires 3 parameters: joints, speeds, duration") + + # Parse parameters using utilities + self.joints = csv_ints(parts[1]) + self.speed_percentages = csv_floats(parts[2]) + self.duration = parse_float(parts[3]) or 0.0 + + # Validate + if len(self.joints) != len(self.speed_percentages): + return (False, "Number of joints must match number of speeds") + + if self.duration <= 0: + return (False, "Duration must be positive") + + # Conflict detection on base joints + base = set() + for j in self.joints: + b = j % 6 + if b in base: + return (False, f"Conflicting commands for Joint {b + 1}") + base.add(b) + + self.log_trace( + "Parsed MultiJog for joints %s with speeds %s%% for %ss.", + self.joints, + self.speed_percentages, + self.duration, + ) + + self.command_len = ceil(self.duration / INTERVAL_S) + self.is_valid = True + return (True, None) + + def setup(self, state): + """Pre-computes the speeds for each joint.""" + # Validate joints and speed_percentages are set + if self.joints is None or self.speed_percentages is None: + raise ValueError("Joints or speed percentages not set") + + # Vectorized computation for all joints + joints_arr = np.asarray(self.joints, dtype=int) + speeds_pct = np.asarray(self.speed_percentages, dtype=float) + + # Map to base joint index (0-5) and direction (+/-) + direction = np.where((joints_arr >= 0) & (joints_arr <= 5), 1, -1) + joint_index = np.where(direction == 1, joints_arr, joints_arr - 6) + + # Validate indices + invalid_mask = (joint_index < 0) | (joint_index >= 6) + if np.any(invalid_mask): + bad = joint_index[invalid_mask] + raise ValueError(f"Invalid joint indices {bad.tolist()}") + + pct = np.clip(np.abs(speeds_pct) / 100.0, 0.0, 1.0) + for i, idx in enumerate(joint_index): + self.speeds_out[idx] = ( + int( + self.linmap_pct( + pct[i] * 100.0, self.JOG_MIN[idx], self.JOG_MAX[idx] + ) + ) + * direction[i] + ) + + # Start timer if duration is specified + if self.duration and self.duration > 0: + self.start_timer(self.duration) + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + """This is the EXECUTION phase. It runs on every loop cycle.""" + # Stop if the duration has elapsed (check both timer and step count) + if self.timer_expired() or self.command_step >= self.command_len: + self.is_finished = True + self.stop_and_idle(state) + return ExecutionStatus.completed("MultiJog") + + # Use base class helper for limit checks + limit_mask = self.limit_hit_mask(state.Position_in, self.speeds_out) + if np.any(limit_mask): + i = np.argmax(limit_mask) # first violating joint + logger.warning(f"Limit reached on joint {i + 1}. Stopping jog.") + self.is_finished = True + self.stop_and_idle(state) + return ExecutionStatus.completed(f"Limit reached on J{i + 1}") + + # Apply self.speeds_out + np.copyto(state.Speed_out, self.speeds_out, casting="no") + state.Command_out = CommandCode.JOG + self.command_step += 1 + return ExecutionStatus.executing("MultiJog") + + +@register_command("SET_TOOL") +class SetToolCommand(MotionCommand): + """ + Set the current end-effector tool configuration. + Changes the tool transform used for forward/inverse kinematics. + """ + + __slots__ = ("tool_name",) + + def __init__(self): + super().__init__() + self.tool_name = None + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse SET_TOOL command parameters. + + Format: SET_TOOL|tool_name + Example: SET_TOOL|PNEUMATIC + """ + if len(parts) != 2: + return (False, "SET_TOOL requires 1 parameter: tool_name") + + self.tool_name = parts[1].strip().upper() + + # Validate tool name during parsing + if self.tool_name not in TOOL_CONFIGS: + available = list_tools() + return (False, f"Unknown tool '{self.tool_name}'. Available: {available}") + + self.log_trace(f"Parsed SET_TOOL command: {self.tool_name}") + return (True, None) + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + """Set the tool in state and update robot kinematics.""" + # Type guard + if self.tool_name is None: + raise RuntimeError("Tool name not set") + + # Update server state - property setter handles tool application and cache invalidation + state.current_tool = self.tool_name + + self.log_info(f"Tool set to: {self.tool_name}") + self.is_finished = True + return ExecutionStatus.completed(f"Tool set: {self.tool_name}") diff --git a/parol6/commands/cartesian_commands.py b/parol6/commands/cartesian_commands.py index 4586d12..2dfb8fe 100644 --- a/parol6/commands/cartesian_commands.py +++ b/parol6/commands/cartesian_commands.py @@ -1,443 +1,506 @@ -""" -Cartesian Movement Commands -Contains commands for Cartesian space movements: CartesianJog, MovePose, and MoveCart -""" - -import logging -import time -import numpy as np -from numpy.typing import NDArray -from typing import List, Tuple, Optional, cast -import parol6.PAROL6_ROBOT as PAROL6_ROBOT -from spatialmath import SE3 -from parol6.utils.ik import solve_ik_with_adaptive_tol_subdivision, quintic_scaling, AXIS_MAP -from .base import ExecutionStatus, MotionCommand, MotionProfile -from parol6.utils.errors import IKError -from parol6.protocol.wire import CommandCode -from parol6.config import JOG_IK_ILIMIT, INTERVAL_S, TRACE, DEFAULT_ACCEL_PERCENT -from parol6.server.command_registry import register_command - -logger = logging.getLogger(__name__) -# TODO: we really need to normalize and be consistent with the logging such that it lines of with the lifecycle and includes the commands name, etc. -@register_command("CARTJOG") -class CartesianJogCommand(MotionCommand): - """ - A non-blocking command to jog the robot's end-effector in Cartesian space. - """ - streamable = True - - __slots__ = ( - "frame", - "axis", - "speed_percentage", - "duration", - "axis_vectors", - "is_rotation", - ) - - def __init__(self): - """ - Initializes the Cartesian jog command. - Parameters are parsed in do_match() method. - """ - super().__init__() - - # Parameters (set in do_match()) - self.frame = None - self.axis = None - self.speed_percentage = 50 - self.duration = 1.5 - - # Runtime state - self.axis_vectors = None - self.is_rotation = False - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """ - Parse CARTJOG command parameters. - - Format: CARTJOG|frame|axis|speed_pct|duration - Example: CARTJOG|WRF|+X|50|2.0 - - Args: - parts: Pre-split message parts - - Returns: - Tuple of (can_handle, error_message) - """ - if len(parts) != 5: - return (False, "CARTJOG requires 4 parameters: frame, axis, speed, duration") - - # Parse parameters - self.frame = parts[1].upper() - self.axis = parts[2] - self.speed_percentage = float(parts[3]) - self.duration = float(parts[4]) - - # Validate frame - if self.frame not in ['WRF', 'TRF']: - return (False, f"Invalid frame: {self.frame}. Must be WRF or TRF") - - # Validate axis - if self.axis not in AXIS_MAP: - return (False, f"Invalid axis: {self.axis}") - - # Store axis vectors for execution - self.axis_vectors = AXIS_MAP[self.axis] - self.is_rotation = any(self.axis_vectors[1]) - - self.is_valid = True - return (True, None) - - def do_setup(self, state): - """Set the end time when the command actually starts.""" - self.start_timer(float(self.duration)) - - def execute_step(self, state) -> ExecutionStatus: - # --- A. Check for completion --- - if self._t_end is None: - # Initialize timer if missing (stream update or late init) - self.start_timer(max(0.1, self.duration if self.duration is not None else 0.1)) - if self.timer_expired(): - self.is_finished = True - self.stop_and_idle(state) - return ExecutionStatus.completed("CARTJOG complete") - - # --- B. Calculate Target Pose using clean vector math --- - state.Command_out = CommandCode.JOG - - q_current = PAROL6_ROBOT.ops.steps_to_rad(state.Position_in) - T_current = PAROL6_ROBOT.robot.fkine(q_current) - - if not isinstance(T_current, SE3): - return ExecutionStatus.executing("Waiting for valid pose") - if self.axis_vectors is None: - return ExecutionStatus.executing("Waiting for axis vectors") - - linear_speed_ms = self.linmap_pct(self.speed_percentage, self.CART_LIN_JOG_MIN, self.CART_LIN_JOG_MAX) - angular_speed_degs = self.linmap_pct(self.speed_percentage, self.CART_ANG_JOG_MIN, self.CART_ANG_JOG_MAX) - - delta_linear = linear_speed_ms * INTERVAL_S - delta_angular_rad = np.deg2rad(angular_speed_degs * INTERVAL_S) - - # Create the small incremental transformation (delta_pose) - # Use explicit per-axis rotations to match original GUI behavior - trans_vec = np.array(self.axis_vectors[0]) * delta_linear - rot_vec = np.array(self.axis_vectors[1]) * delta_angular_rad - - # Build delta transformation using explicit rotation matrices - if np.any(rot_vec != 0): - # Find which axis has rotation (should be only one for single-axis jog) - if rot_vec[0] != 0: # RX rotation - delta_pose = SE3.Rx(rot_vec[0]) * SE3(trans_vec) - elif rot_vec[1] != 0: # RY rotation - delta_pose = SE3.Ry(rot_vec[1]) * SE3(trans_vec) - elif rot_vec[2] != 0: # RZ rotation - delta_pose = SE3.Rz(rot_vec[2]) * SE3(trans_vec) - else: - delta_pose = SE3(trans_vec) - else: - delta_pose = SE3(trans_vec) - - # Apply the transformation in the correct reference frame - if self.frame == 'WRF': - # Pre-multiply to apply the change in the World Reference Frame - target_pose = delta_pose * T_current - else: # TRF - # Post-multiply to apply the change in the Tool Reference Frame - target_pose = T_current * delta_pose - - # --- C. Solve IK and Calculate Velocities --- - var = solve_ik_with_adaptive_tol_subdivision(PAROL6_ROBOT.robot, target_pose, q_current, ilimit=JOG_IK_ILIMIT, jogging=True) - - if var.success: - q_velocities = (var.q - q_current) / INTERVAL_S - sps = PAROL6_ROBOT.ops.speed_rad_to_steps(q_velocities) - np.copyto(state.Speed_out, np.asarray(sps), casting='no') - else: - raise IKError("IK Warning: Could not find solution for jog step. Stopping.") - - # --- D. Speed Scaling using base class helper --- - scaled_speeds = self.scale_speeds_to_joint_max(state.Speed_out) - np.copyto(state.Speed_out, scaled_speeds, casting='no') - - return ExecutionStatus.executing("CARTJOG") - - -@register_command("MOVEPOSE") -class MovePoseCommand(MotionCommand): - """ - A non-blocking command to move the robot to a specific Cartesian pose. - The movement itself is a joint-space interpolation. - """ - __slots__ = ( - "command_step", - "trajectory_steps", - "pose", - "duration", - "velocity_percent", - "accel_percent", - "trajectory_type", - ) - def __init__(self, pose=None, duration=None): - super().__init__() - self.command_step = 0 - self.trajectory_steps = [] - - # Parameters (set in do_match()) - self.pose = pose - self.duration = duration - self.velocity_percent = None - self.accel_percent = DEFAULT_ACCEL_PERCENT - self.trajectory_type = 'trapezoid' - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """ - Parse MOVEPOSE command parameters. - - Format: MOVEPOSE|x|y|z|rx|ry|rz|duration|speed - Example: MOVEPOSE|100|200|300|0|0|0|None|50 - - Args: - parts: Pre-split message parts - - Returns: - Tuple of (can_handle, error_message) - """ - if len(parts) != 9: - return (False, "MOVEPOSE requires 8 parameters: x, y, z, rx, ry, rz, duration, speed") - - # Parse pose (6 values) - self.pose = [float(parts[i]) for i in range(1, 7)] - - # Parse duration and speed - self.duration = None if parts[7].upper() == 'NONE' else float(parts[7]) - self.velocity_percent = None if parts[8].upper() == 'NONE' else float(parts[8]) - - self.log_debug("Parsed MovePose: %s", self.pose) - self.is_valid = True - return (True, None) - - def do_setup(self, state): - """Calculates the full trajectory just-in-time before execution.""" - self.log_trace(" -> Preparing trajectory for MovePose to %s...", self.pose) - - initial_pos_rad = PAROL6_ROBOT.ops.steps_to_rad(state.Position_in) - pose = cast(List[float], self.pose) - target_pose = SE3(pose[0] / 1000.0, pose[1] / 1000.0, pose[2] / 1000.0) * SE3.RPY(pose[3:6], unit='deg', order='xyz') - - ik_solution = solve_ik_with_adaptive_tol_subdivision( - PAROL6_ROBOT.robot, target_pose, initial_pos_rad, ilimit=100) - - if not ik_solution.success: - error_str = "An intermediate point on the path is unreachable." - if ik_solution.violations: - error_str += f" Reason: Path violates joint limits: {ik_solution.violations}" - raise IKError(error_str) - - target_pos_rad = ik_solution.q - - if self.duration and self.duration > 0: - if self.velocity_percent is not None: - self.log_trace(" -> INFO: Both duration and velocity were provided. Using duration.") - initial_pos_steps = state.Position_in - target_pos_steps = np.asarray(PAROL6_ROBOT.ops.rad_to_steps(target_pos_rad), dtype=np.int32) - dur = float(self.duration) - self.trajectory_steps = MotionProfile.from_duration_steps( - initial_pos_steps, target_pos_steps, dur, dt=INTERVAL_S - ) - - elif self.velocity_percent is not None: - initial_pos_steps = state.Position_in - target_pos_steps = np.asarray(PAROL6_ROBOT.ops.rad_to_steps(target_pos_rad), dtype=np.int32) - accel_percent = float(self.accel_percent) if self.accel_percent is not None else float(DEFAULT_ACCEL_PERCENT) - self.trajectory_steps = MotionProfile.from_velocity_percent( - initial_pos_steps, - target_pos_steps, - float(self.velocity_percent), - accel_percent, - dt=INTERVAL_S, - ) - self.log_trace(" -> Command is valid (velocity profile).") - else: - self.log_trace(" -> Using conservative values for MovePose.") - command_len = 200 - initial_pos_steps = state.Position_in - target_pos_steps = np.asarray(PAROL6_ROBOT.ops.rad_to_steps(target_pos_rad), dtype=np.int32) - total_dur = float(command_len) * INTERVAL_S - self.trajectory_steps = MotionProfile.from_duration_steps( - initial_pos_steps, target_pos_steps, total_dur, dt=INTERVAL_S - ) - - if len(self.trajectory_steps) == 0: - raise IKError("Trajectory calculation resulted in no steps. Command is invalid.") - logger.log(TRACE, " -> Trajectory prepared with %s steps.", len(self.trajectory_steps)) - - def execute_step(self, state) -> ExecutionStatus: - if self.command_step >= len(self.trajectory_steps): - logger.info(f"{type(self).__name__} finished.") - self.is_finished = True - self.stop_and_idle(state) - return ExecutionStatus.completed("MOVEPOSE complete") - else: - self.set_move_position(state, self.trajectory_steps[self.command_step]) - self.command_step += 1 - return ExecutionStatus.executing("MovePose") - - -@register_command("MOVECART") -class MoveCartCommand(MotionCommand): - """ - A non-blocking command to move the robot's end-effector in a straight line - in Cartesian space, completing the move in an exact duration. - - It works by: - 1. Pre-validating the final target pose. - 2. Interpolating the pose in Cartesian space in real-time. - 3. Solving Inverse Kinematics for each intermediate step to ensure path validity. - """ - __slots__ = ( - "pose", - "duration", - "velocity_percent", - "start_time", - "initial_pose", - "target_pose", - ) - def __init__(self): - super().__init__() - - # Parameters (set in do_match()) - self.pose = None - self.duration = None - self.velocity_percent = None - - # Runtime state - self.start_time = None - self.initial_pose = None - self.target_pose = None - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """ - Parse MOVECART command parameters. - - Format: MOVECART|x|y|z|rx|ry|rz|duration|speed - Example: MOVECART|100|200|300|0|0|0|2.0|None - - Args: - parts: Pre-split message parts - - Returns: - Tuple of (can_handle, error_message) - """ - if len(parts) != 9: - return (False, "MOVECART requires 8 parameters: x, y, z, rx, ry, rz, duration, speed") - - # Parse pose (6 values) - self.pose = [float(parts[i]) for i in range(1, 7)] - - # Parse duration and speed - self.duration = None if parts[7].upper() == 'NONE' else float(parts[7]) - self.velocity_percent = None if parts[8].upper() == 'NONE' else float(parts[8]) - - # Validate that at least one timing parameter is given - if self.duration is None and self.velocity_percent is None: - return (False, "MOVECART requires either duration or velocity_percent") - - if self.duration is not None and self.velocity_percent is not None: - logger.info(" -> INFO: Both duration and velocity_percent provided. Using duration.") - self.velocity_percent = None # Prioritize duration - - self.log_debug("Parsed MoveCart: %s", self.pose) - self.is_valid = True - return (True, None) - - def do_setup(self, state): - """Captures the initial state and validates the path just before execution.""" - # Capture initial state from live data - initial_q_rad = PAROL6_ROBOT.ops.steps_to_rad(state.Position_in) - self.initial_pose = PAROL6_ROBOT.robot.fkine(initial_q_rad) - pose = cast(List[float], self.pose) - self.target_pose = SE3(pose[0]/1000.0, pose[1]/1000.0, pose[2]/1000.0) * SE3.RPY(pose[3:6], unit='deg', order='xyz') - - ik_check = solve_ik_with_adaptive_tol_subdivision( - PAROL6_ROBOT.robot, cast(SE3, self.target_pose), initial_q_rad - ) - - if not ik_check.success: - error_str = "An intermediate point on the path is unreachable." - if ik_check.violations: - error_str += f" Reason: Path violates joint limits: {ik_check.violations}" - raise IKError(error_str) - - if self.velocity_percent is not None: - # Calculate the total distance for translation and rotation - tp = cast(SE3, self.target_pose) - ip = cast(SE3, self.initial_pose) - linear_distance = np.linalg.norm(tp.t - ip.t) - angular_distance_rad = ip.angdist(tp) - - target_linear_speed = self.linmap_pct(self.velocity_percent, self.CART_LIN_JOG_MIN, self.CART_LIN_JOG_MAX) - target_angular_speed_rad = np.deg2rad(self.linmap_pct(self.velocity_percent, self.CART_ANG_JOG_MIN, self.CART_ANG_JOG_MAX)) - - # Calculate time required for each component of the movement - time_linear = linear_distance / target_linear_speed if target_linear_speed > 0 else 0 - time_angular = angular_distance_rad / target_angular_speed_rad if target_angular_speed_rad > 0 else 0 - - # The total duration is the longer of the two times to ensure synchronization - calculated_duration = max(time_linear, time_angular) - - if calculated_duration <= 0: - logger.info(" -> INFO: MoveCart has zero duration. Marking as finished.") - self.is_finished = True - self.is_valid = True # It's valid, just already done. - return - - self.duration = calculated_duration - self.log_debug(" -> Calculated MoveCart duration: %.2fs", self.duration) - - self.log_debug(" -> Command is valid and ready for execution.") - if self.duration and float(self.duration) > 0.0: - self.start_timer(float(self.duration)) - - def execute_step(self, state) -> ExecutionStatus: - dur = float(self.duration or 0.0) - if dur <= 0.0: - self.is_finished = True - self.stop_and_idle(state) - return ExecutionStatus.completed("MOVECART complete") - s = self.progress01(dur) - s_scaled = quintic_scaling(float(s)) - - assert self.initial_pose is not None and self.target_pose is not None - _ctp = cast(SE3, self.initial_pose).interp(cast(SE3, self.target_pose), s_scaled) - if not isinstance(_ctp, SE3): - return ExecutionStatus.executing("Waiting for pose interpolation") - current_target_pose = cast(SE3, _ctp) - - current_q_rad = PAROL6_ROBOT.ops.steps_to_rad(state.Position_in) - # TODO: is it doing the expensive IK solving twice per command??? once in setup and once in execution?? - ik_solution = solve_ik_with_adaptive_tol_subdivision( - PAROL6_ROBOT.robot, current_target_pose, current_q_rad - ) - - if not ik_solution.success: - error_str = "An intermediate point on the path is unreachable." - if ik_solution.violations: - error_str += f" Reason: Path violates joint limits: {ik_solution.violations}" - raise IKError(error_str) - - current_pos_rad = ik_solution.q - - # Send only the target position and let the firmware's P-controller handle speed. - # Set feed-forward velocity to zero for smooth P-control. - steps = PAROL6_ROBOT.ops.rad_to_steps(current_pos_rad) - self.set_move_position(state, np.asarray(steps)) - - if s >= 1.0: - actual_elapsed = (time.perf_counter() - self._t0) if self._t0 is not None else dur - self.log_info("MoveCart finished in ~%.2fs.", actual_elapsed) - self.is_finished = True - return ExecutionStatus.completed("MOVECART complete") - - return ExecutionStatus.executing("MoveCart") +""" +Cartesian Movement Commands +Contains commands for Cartesian space movements: CartesianJog, MovePose, and MoveCart +""" + +import logging +import time +from typing import cast + +import numpy as np +from spatialmath import SE3 + +import parol6.PAROL6_ROBOT as PAROL6_ROBOT +from parol6.config import DEFAULT_ACCEL_PERCENT, INTERVAL_S, TRACE +from parol6.protocol.wire import CommandCode +from parol6.server.command_registry import register_command +from parol6.server.state import ControllerState, get_fkine_se3 +from parol6.utils.errors import IKError +from parol6.utils.ik import AXIS_MAP, quintic_scaling, solve_ik + +from .base import ExecutionStatus, MotionCommand, MotionProfile + +logger = logging.getLogger(__name__) + + +@register_command("CARTJOG") +class CartesianJogCommand(MotionCommand): + """ + A non-blocking command to jog the robot's end-effector in Cartesian space. + """ + + streamable = True + + __slots__ = ( + "frame", + "axis", + "speed_percentage", + "duration", + "axis_vectors", + "is_rotation", + ) + + def __init__(self): + """ + Initializes the Cartesian jog command. + Parameters are parsed in do_match() method. + """ + super().__init__() + + # Parameters (set in do_match()) + self.frame = None + self.axis = None + self.speed_percentage: float = 50.0 + self.duration: float = 1.5 + + # Runtime state + self.axis_vectors = None + self.is_rotation = False + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse CARTJOG command parameters. + + Format: CARTJOG|frame|axis|speed_pct|duration + Example: CARTJOG|WRF|+X|50|2.0 + + Args: + parts: Pre-split message parts + + Returns: + Tuple of (can_handle, error_message) + """ + if len(parts) != 5: + return ( + False, + "CARTJOG requires 4 parameters: frame, axis, speed, duration", + ) + + # Parse parameters + self.frame = parts[1].upper() + self.axis = parts[2] + self.speed_percentage = float(parts[3]) + self.duration = float(parts[4]) + + # Validate frame + if self.frame not in ["WRF", "TRF"]: + return (False, f"Invalid frame: {self.frame}. Must be WRF or TRF") + + # Validate axis + if self.axis not in AXIS_MAP: + return (False, f"Invalid axis: {self.axis}") + + # Store axis vectors for execution + self.axis_vectors = AXIS_MAP[self.axis] + self.is_rotation = any(self.axis_vectors[1]) + + self.is_valid = True + return (True, None) + + def do_setup(self, state: "ControllerState") -> None: + """Set the end time when the command actually starts.""" + self.start_timer(float(self.duration)) + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + # --- A. Check for completion --- + if self._t_end is None: + # Initialize timer if missing (stream update or late init) + self.start_timer( + max(0.1, self.duration if self.duration is not None else 0.1) + ) + if self.timer_expired(): + self.is_finished = True + self.stop_and_idle(state) + return ExecutionStatus.completed("CARTJOG complete") + + # --- B. Calculate Target Pose using clean vector math --- + state.Command_out = CommandCode.JOG + + q_current = np.asarray( + PAROL6_ROBOT.ops.steps_to_rad(state.Position_in), dtype=float + ) + T_current = get_fkine_se3() + + if not isinstance(T_current, SE3): + return ExecutionStatus.executing("Waiting for valid pose") + if self.axis_vectors is None: + return ExecutionStatus.executing("Waiting for axis vectors") + + linear_speed_ms = self.linmap_pct( + self.speed_percentage, self.CART_LIN_JOG_MIN, self.CART_LIN_JOG_MAX + ) + angular_speed_degs = self.linmap_pct( + self.speed_percentage, self.CART_ANG_JOG_MIN, self.CART_ANG_JOG_MAX + ) + + delta_linear = linear_speed_ms * INTERVAL_S + delta_angular_rad = np.deg2rad(angular_speed_degs * INTERVAL_S) + + # Create the small incremental transformation (delta_pose) + trans_vec = np.array(self.axis_vectors[0]) * delta_linear + rot_vec = np.array(self.axis_vectors[1]) * delta_angular_rad + + # Build delta transformation + if not self.is_rotation: + target_pose = SE3.Rt(T_current.R, T_current.t) + + if self.frame == "WRF": + target_pose.t = T_current.t + trans_vec + else: # TRF + target_pose.t = T_current.t + (T_current.R @ trans_vec) + else: + if rot_vec[0] != 0: # RX rotation + delta_pose = SE3.Rx(rot_vec[0]) * SE3(trans_vec) + elif rot_vec[1] != 0: # RY rotation + delta_pose = SE3.Ry(rot_vec[1]) * SE3(trans_vec) + elif rot_vec[2] != 0: # RZ rotation + delta_pose = SE3.Rz(rot_vec[2]) * SE3(trans_vec) + else: + delta_pose = SE3(trans_vec) + # Apply the transformation in the correct reference frame + if self.frame == "WRF": + # Pre-multiply to apply the change in the World Reference Frame + target_pose = delta_pose * T_current + else: # TRF + # Post-multiply to apply the change in the Tool Reference Frame + target_pose = T_current * delta_pose + + # --- C. Solve IK and Calculate Velocities --- + var = solve_ik(PAROL6_ROBOT.robot, target_pose, q_current, jogging=True) + + if var.success and var.q is not None: + q = np.asarray(var.q, dtype=float) + q_velocities = (q - q_current) / INTERVAL_S + sps = PAROL6_ROBOT.ops.speed_rad_to_steps(q_velocities) + np.copyto(state.Speed_out, np.asarray(sps), casting="no") + else: + raise IKError("IK Warning: Could not find solution for jog step. Stopping.") + + # --- D. Speed Scaling using base class helper --- + scaled_speeds = self.scale_speeds_to_joint_max(state.Speed_out) + np.copyto(state.Speed_out, scaled_speeds) + + return ExecutionStatus.executing("CARTJOG") + + +@register_command("MOVEPOSE") +class MovePoseCommand(MotionCommand): + """ + A non-blocking command to move the robot to a specific Cartesian pose. + The movement itself is a joint-space interpolation. + """ + + __slots__ = ( + "command_step", + "trajectory_steps", + "pose", + "duration", + "velocity_percent", + "accel_percent", + "trajectory_type", + ) + + def __init__(self, pose=None, duration=None): + super().__init__() + self.command_step = 0 + self.trajectory_steps: np.ndarray = np.empty((0, 6), dtype=np.int32) + + # Parameters (set in do_match()) + self.pose = pose + self.duration = duration + self.velocity_percent = None + self.accel_percent = DEFAULT_ACCEL_PERCENT + self.trajectory_type = "trapezoid" + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse MOVEPOSE command parameters. + + Format: MOVEPOSE|x|y|z|rx|ry|rz|duration|speed + Example: MOVEPOSE|100|200|300|0|0|0|None|50 + + Args: + parts: Pre-split message parts + + Returns: + Tuple of (can_handle, error_message) + """ + if len(parts) != 9: + return ( + False, + "MOVEPOSE requires 8 parameters: x, y, z, rx, ry, rz, duration, speed", + ) + + # Parse pose (6 values) + self.pose = [float(parts[i]) for i in range(1, 7)] + + # Parse duration and speed + self.duration = None if parts[7].upper() == "NONE" else float(parts[7]) + self.velocity_percent = None if parts[8].upper() == "NONE" else float(parts[8]) + + self.log_debug("Parsed MovePose: %s", self.pose) + self.is_valid = True + return (True, None) + + def do_setup(self, state: "ControllerState") -> None: + """Calculates the full trajectory just-in-time before execution.""" + self.log_trace(" -> Preparing trajectory for MovePose to %s...", self.pose) + + initial_pos_rad = np.asarray( + PAROL6_ROBOT.ops.steps_to_rad(state.Position_in), dtype=float + ) + pose = cast(list[float], self.pose) + target_pose = SE3.RPY(pose[3:6], unit="deg", order="xyz") + target_pose.t = np.array(pose[:3]) / 1000.0 + + ik_solution = solve_ik(PAROL6_ROBOT.robot, target_pose, initial_pos_rad) + + if not ik_solution.success: + error_str = "An intermediate point on the path is unreachable." + if ik_solution.violations: + error_str += ( + f" Reason: Path violates joint limits: {ik_solution.violations}" + ) + raise IKError(error_str) + + target_pos_rad = ik_solution.q + + if self.duration and self.duration > 0: + if self.velocity_percent is not None: + self.log_trace( + " -> INFO: Both duration and velocity were provided. Using duration." + ) + initial_pos_steps = state.Position_in + target_pos_steps = np.asarray( + PAROL6_ROBOT.ops.rad_to_steps(target_pos_rad), dtype=np.int32 + ) + dur = float(self.duration) + self.trajectory_steps = MotionProfile.from_duration_steps( + initial_pos_steps, target_pos_steps, dur, dt=INTERVAL_S + ) + + elif self.velocity_percent is not None: + initial_pos_steps = state.Position_in + target_pos_steps = np.asarray( + PAROL6_ROBOT.ops.rad_to_steps(target_pos_rad), dtype=np.int32 + ) + accel_percent = ( + float(self.accel_percent) + if self.accel_percent is not None + else float(DEFAULT_ACCEL_PERCENT) + ) + self.trajectory_steps = MotionProfile.from_velocity_percent( + initial_pos_steps, + target_pos_steps, + float(self.velocity_percent), + accel_percent, + dt=INTERVAL_S, + ) + self.log_trace(" -> Command is valid (velocity profile).") + else: + self.log_trace(" -> Using conservative values for MovePose.") + command_len = 200 + initial_pos_steps = state.Position_in + target_pos_steps = np.asarray( + PAROL6_ROBOT.ops.rad_to_steps(target_pos_rad), dtype=np.int32 + ) + total_dur = float(command_len) * INTERVAL_S + self.trajectory_steps = MotionProfile.from_duration_steps( + initial_pos_steps, target_pos_steps, total_dur, dt=INTERVAL_S + ) + + if len(self.trajectory_steps) == 0: + raise IKError( + "Trajectory calculation resulted in no steps. Command is invalid." + ) + logger.log( + TRACE, " -> Trajectory prepared with %s steps.", len(self.trajectory_steps) + ) + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + if self.command_step >= len(self.trajectory_steps): + logger.info(f"{type(self).__name__} finished.") + self.is_finished = True + self.stop_and_idle(state) + return ExecutionStatus.completed("MOVEPOSE complete") + else: + self.set_move_position(state, self.trajectory_steps[self.command_step]) + self.command_step += 1 + return ExecutionStatus.executing("MovePose") + + +@register_command("MOVECART") +class MoveCartCommand(MotionCommand): + """ + A non-blocking command to move the robot's end-effector in a straight line + in Cartesian space, completing the move in an exact duration. + + It works by: + 1. Pre-validating the final target pose. + 2. Interpolating the pose in Cartesian space in real-time. + 3. Solving Inverse Kinematics for each intermediate step to ensure path validity. + """ + + __slots__ = ( + "pose", + "duration", + "velocity_percent", + "start_time", + "initial_pose", + "target_pose", + ) + + def __init__(self): + super().__init__() + + # Parameters (set in do_match()) + self.pose = None + self.duration = None + self.velocity_percent = None + + # Runtime state + self.start_time = None + self.initial_pose = None + self.target_pose = None + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse MOVECART command parameters. + + Format: MOVECART|x|y|z|rx|ry|rz|duration|speed + Example: MOVECART|100|200|300|0|0|0|2.0|None + + Args: + parts: Pre-split message parts + + Returns: + Tuple of (can_handle, error_message) + """ + if len(parts) != 9: + return ( + False, + "MOVECART requires 8 parameters: x, y, z, rx, ry, rz, duration, speed", + ) + + # Parse pose (6 values) + self.pose = [float(parts[i]) for i in range(1, 7)] + + # Parse duration and speed + self.duration = None if parts[7].upper() == "NONE" else float(parts[7]) + self.velocity_percent = None if parts[8].upper() == "NONE" else float(parts[8]) + + # Validate that at least one timing parameter is given + if self.duration is None and self.velocity_percent is None: + return (False, "MOVECART requires either duration or velocity_percent") + + if self.duration is not None and self.velocity_percent is not None: + logger.info( + " -> INFO: Both duration and velocity_percent provided. Using duration." + ) + self.velocity_percent = None # Prioritize duration + + self.log_debug("Parsed MoveCart: %s", self.pose) + self.is_valid = True + return (True, None) + + def do_setup(self, state: "ControllerState") -> None: + """Captures the initial state and validates the path just before execution.""" + self.initial_pose = get_fkine_se3() + pose = cast(list[float], self.pose) + + # Construct pose: rotation first, then set translation (xyz convention) + self.target_pose = SE3.RPY(pose[3:6], unit="deg", order="xyz") + self.target_pose.t = ( + np.array(pose[:3]) / 1000.0 + ) # Vectorized translation assignment + + if self.velocity_percent is not None: + # Calculate the total distance for translation and rotation + tp = cast(SE3, self.target_pose) + ip = cast(SE3, self.initial_pose) + linear_distance = np.linalg.norm(tp.t - ip.t) + angular_distance_rad = ip.angdist(tp) + + target_linear_speed = self.linmap_pct( + self.velocity_percent, self.CART_LIN_JOG_MIN, self.CART_LIN_JOG_MAX + ) + target_angular_speed_rad = np.deg2rad( + self.linmap_pct( + self.velocity_percent, self.CART_ANG_JOG_MIN, self.CART_ANG_JOG_MAX + ) + ) + + # Calculate time required for each component of the movement + time_linear = ( + linear_distance / target_linear_speed if target_linear_speed > 0 else 0 + ) + time_angular = ( + angular_distance_rad / target_angular_speed_rad + if target_angular_speed_rad > 0 + else 0 + ) + + # The total duration is the longer of the two times to ensure synchronization + calculated_duration = max(time_linear, time_angular) + + if calculated_duration <= 0: + logger.info( + " -> INFO: MoveCart has zero duration. Marking as finished." + ) + self.is_finished = True + self.is_valid = True # It's valid, just already done. + return + + self.duration = calculated_duration + self.log_debug(" -> Calculated MoveCart duration: %.2fs", self.duration) + + self.log_debug(" -> Command is valid and ready for execution.") + if self.duration and float(self.duration) > 0.0: + self.start_timer(float(self.duration)) + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + dur = float(self.duration or 0.0) + if dur <= 0.0: + self.is_finished = True + self.stop_and_idle(state) + return ExecutionStatus.completed("MOVECART complete") + s = self.progress01(dur) + s_scaled = quintic_scaling(float(s)) + + assert self.initial_pose is not None and self.target_pose is not None + _ctp = cast(SE3, self.initial_pose).interp( + cast(SE3, self.target_pose), s_scaled + ) + if not isinstance(_ctp, SE3): + return ExecutionStatus.executing("Waiting for pose interpolation") + current_target_pose = cast(SE3, _ctp) + + current_q_rad = np.asarray( + PAROL6_ROBOT.ops.steps_to_rad(state.Position_in), dtype=float + ) + ik_solution = solve_ik(PAROL6_ROBOT.robot, current_target_pose, current_q_rad) + + if not ik_solution.success: + error_str = "An intermediate point on the path is unreachable." + if ik_solution.violations: + error_str += ( + f" Reason: Path violates joint limits: {ik_solution.violations}" + ) + raise IKError(error_str) + + current_pos_rad = ik_solution.q + + # Send only the target position and let the firmware's P-controller handle speed. + # Set feed-forward velocity to zero for smooth P-control. + steps = PAROL6_ROBOT.ops.rad_to_steps(current_pos_rad) + self.set_move_position(state, np.asarray(steps)) + + if s >= 1.0: + actual_elapsed = ( + (time.perf_counter() - self._t0) if self._t0 is not None else dur + ) + self.log_info("MoveCart finished in ~%.2fs.", actual_elapsed) + self.is_finished = True + return ExecutionStatus.completed("MOVECART complete") + + return ExecutionStatus.executing("MoveCart") diff --git a/parol6/commands/gcode_commands.py b/parol6/commands/gcode_commands.py index e74c429..cb37638 100644 --- a/parol6/commands/gcode_commands.py +++ b/parol6/commands/gcode_commands.py @@ -3,13 +3,13 @@ These commands integrate the GCODE interpreter with the robot command system. """ -from typing import Tuple, Optional, List, TYPE_CHECKING + +from typing import TYPE_CHECKING from parol6.commands.base import CommandBase, ExecutionStatus -from parol6.server.state import ControllerState -from parol6.server.command_registry import register_command from parol6.gcode import GcodeInterpreter -import parol6.PAROL6_ROBOT as PAROL6_ROBOT +from parol6.server.command_registry import register_command +from parol6.server.state import ControllerState, get_fkine_matrix if TYPE_CHECKING: from parol6.server.state import ControllerState @@ -18,41 +18,52 @@ @register_command("GCODE") class GcodeCommand(CommandBase): """Execute a single GCODE line.""" - - __slots__ = ("gcode_line", "interpreter", "generated_commands", "current_command_index") - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + + __slots__ = ( + "gcode_line", + "interpreter", + "generated_commands", + "current_command_index", + ) + gcode_line: str + interpreter: GcodeInterpreter | None + generated_commands: list[str] + current_command_index: int + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """Check if this is a GCODE command.""" if parts[0].upper() == "GCODE" and len(parts) >= 2: # Rejoin the GCODE line (it might contain | characters) - self.gcode_line = '|'.join(parts[1:]) + self.gcode_line = "|".join(parts[1:]) return True, None return False, None - - def do_setup(self, state: 'ControllerState') -> None: + + def do_setup(self, state: "ControllerState") -> None: """Set up GCODE interpreter and parse the line.""" # Use injected interpreter or create one - self.interpreter = self.gcode_interpreter or self.interpreter or GcodeInterpreter() + self.interpreter = ( + self.gcode_interpreter or self.interpreter or GcodeInterpreter() + ) assert self.interpreter is not None # Update interpreter position with current robot position - # Vectorized: convert all joints at once - current_angles_rad = PAROL6_ROBOT.ops.steps_to_rad(state.Position_in) - current_pose_matrix = PAROL6_ROBOT.robot.fkine(current_angles_rad).A + current_pose_matrix = get_fkine_matrix() current_xyz = current_pose_matrix[:3, 3] - self.interpreter.state.update_position({ - 'X': current_xyz[0] * 1000, - 'Y': current_xyz[1] * 1000, - 'Z': current_xyz[2] * 1000 - }) + self.interpreter.state.update_position( + { + "X": current_xyz[0] * 1000, + "Y": current_xyz[1] * 1000, + "Z": current_xyz[2] * 1000, + } + ) # Parse and store generated robot commands (strings) self.generated_commands = self.interpreter.parse_line(self.gcode_line) or [] - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: """Return generated commands for the controller to enqueue.""" # Report back the list so controller can enqueue them details = {} if self.generated_commands: - details['enqueue'] = self.generated_commands + details["enqueue"] = self.generated_commands self.finish() return ExecutionStatus.completed("GCODE parsed", details=details) @@ -60,43 +71,48 @@ def execute_step(self, state: 'ControllerState') -> ExecutionStatus: @register_command("GCODE_PROGRAM") class GcodeProgramCommand(CommandBase): """Load and execute a GCODE program.""" - + __slots__ = ("program_type", "program_data", "interpreter") - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + program_type: str + program_data: str + interpreter: GcodeInterpreter | None + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """Check if this is a GCODE_PROGRAM command.""" if parts[0].upper() == "GCODE_PROGRAM" and len(parts) >= 3: self.program_type = parts[1].upper() - + if self.program_type == "FILE": self.program_data = parts[2] elif self.program_type == "INLINE": # Join remaining parts and split by semicolon for inline programs - self.program_data = '|'.join(parts[2:]) + self.program_data = "|".join(parts[2:]) else: return False, "Invalid GCODE_PROGRAM type (expected FILE or INLINE)" - + return True, None return False, None - + def do_setup(self, state: ControllerState) -> None: """Load the GCODE program using the interpreter.""" # Use injected interpreter or create one - self.interpreter = self.gcode_interpreter or self.interpreter or GcodeInterpreter() + self.interpreter = ( + self.gcode_interpreter or self.interpreter or GcodeInterpreter() + ) assert self.interpreter is not None if self.program_type == "FILE": if not self.interpreter.load_file(self.program_data): raise RuntimeError(f"Failed to load GCODE file: {self.program_data}") elif self.program_type == "INLINE": - program_lines = self.program_data.split(';') + program_lines = self.program_data.split(";") if not self.interpreter.load_program(program_lines): raise RuntimeError("Failed to load inline GCODE program") else: raise ValueError("Invalid GCODE_PROGRAM type (expected FILE or INLINE)") # Start program execution self.interpreter.start_program() - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: """Signal that the program was loaded; controller will fetch commands.""" self.finish() return ExecutionStatus.completed("GCODE program loaded") @@ -105,17 +121,17 @@ def execute_step(self, state: 'ControllerState') -> ExecutionStatus: @register_command("GCODE_STOP") class GcodeStopCommand(CommandBase): """Stop GCODE program execution.""" - + __slots__ = () is_immediate: bool = True - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """Check if this is a GCODE_STOP command.""" if parts[0].upper() == "GCODE_STOP": return True, None return False, None - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: """Stop the GCODE program.""" if self.gcode_interpreter: self.gcode_interpreter.stop_program() @@ -126,17 +142,17 @@ def execute_step(self, state: 'ControllerState') -> ExecutionStatus: @register_command("GCODE_PAUSE") class GcodePauseCommand(CommandBase): """Pause GCODE program execution.""" - + __slots__ = () is_immediate: bool = True - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """Check if this is a GCODE_PAUSE command.""" if parts[0].upper() == "GCODE_PAUSE": return True, None return False, None - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: """Pause the GCODE program.""" if self.gcode_interpreter: self.gcode_interpreter.pause_program() @@ -147,17 +163,17 @@ def execute_step(self, state: 'ControllerState') -> ExecutionStatus: @register_command("GCODE_RESUME") class GcodeResumeCommand(CommandBase): """Resume GCODE program execution.""" - + __slots__ = () is_immediate: bool = True - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """Check if this is a GCODE_RESUME command.""" if parts[0].upper() == "GCODE_RESUME": return True, None return False, None - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: """Resume the GCODE program.""" if self.gcode_interpreter: self.gcode_interpreter.start_program() # resumes if already loaded diff --git a/parol6/commands/gripper_commands.py b/parol6/commands/gripper_commands.py index 25593dc..b8a2cf3 100644 --- a/parol6/commands/gripper_commands.py +++ b/parol6/commands/gripper_commands.py @@ -1,226 +1,259 @@ -""" -Gripper Control Commands -Contains commands for electric and pneumatic gripper control -""" - -import logging -from enum import Enum -from typing import List, Tuple, Optional -import parol6.PAROL6_ROBOT as PAROL6_ROBOT -from parol6.commands.base import MotionCommand, ExecutionStatus, Debouncer -from parol6.server.command_registry import register_command - -logger = logging.getLogger(__name__) - -# Lifecycle TRACE is centralized in higher layers; keep semantic logs here only. - -class GripperState(Enum): - START = "START" - SEND_CALIBRATE = "SEND_CALIBRATE" - WAITING_CALIBRATION = "WAITING_CALIBRATION" - WAIT_FOR_POSITION = "WAIT_FOR_POSITION" - -@register_command("PNEUMATICGRIPPER") -@register_command("ELECTRICGRIPPER") -class GripperCommand(MotionCommand): - """ - A single, unified, non-blocking command to control all gripper functions. - It internally selects the correct logic (position-based waiting, timed delay, - or instantaneous) based on the specified action. - """ - __slots__ = ( - "state", - "timeout_counter", - "object_debouncer", - "wait_counter", - "gripper_type", - "action", - "target_position", - "speed", - "current", - "state_to_set", - "port_index", - ) - def __init__(self): - """ - Initializes the Gripper command. - Parameters are parsed in do_match() method. - """ - super().__init__() - self.state = GripperState.START - self.timeout_counter = 1000 # 10-second safety timeout for all waiting states - self.object_debouncer = Debouncer(5) # 0.05s debounce for object detection - self.wait_counter = 0 - - # Parameters (set in do_match()) - self.gripper_type = None - self.action = None - self.target_position = None - self.speed = None - self.current = None - self.state_to_set = None - self.port_index = None - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """ - Parse gripper command parameters. - - Formats: - - PNEUMATICGRIPPER|action|port - - ELECTRICGRIPPER|action|pos|spd|curr - - Args: - parts: Pre-split message parts - - Returns: - Tuple of (can_handle, error_message) - """ - command_name = parts[0].upper() - - if command_name == "PNEUMATICGRIPPER": - if len(parts) != 3: - return (False, "PNEUMATICGRIPPER requires 2 parameters: action, port") - - self.gripper_type = 'pneumatic' - self.action = parts[1].lower() - output_port = int(parts[2]) - - # Validate action - if self.action not in ['open', 'close']: - return (False, f"Invalid pneumatic gripper action: {self.action}") - - # Configure pneumatic settings - self.state_to_set = 1 if self.action == 'open' else 0 - self.port_index = 2 if output_port == 1 else 3 - - self.log_debug("Parsed PNEUMATICGRIPPER: action=%s, port=%s", self.action, output_port) - self.is_valid = True - return (True, None) - - elif command_name == "ELECTRICGRIPPER": - if len(parts) != 5: - return (False, "ELECTRICGRIPPER requires 4 parameters: action, position, speed, current") - - self.gripper_type = 'electric' - - # Parse action - action_token = parts[1].upper() - self.action = 'move' if action_token in ('NONE', 'MOVE') else parts[1].lower() - - # Parse numeric parameters - position = int(parts[2]) - speed = int(parts[3]) - current = int(parts[4]) - - # Configure based on action - if self.action == 'move': - self.target_position = position - self.speed = speed - self.current = current - - # Validate ranges - if not (0 <= position <= 255): - return (False, f"Position must be 0-255, got {position}") - if not (0 <= speed <= 255): - return (False, f"Speed must be 0-255, got {speed}") - if not (100 <= current <= 1000): - return (False, f"Current must be 100-1000, got {current}") - - elif self.action == 'calibrate': - self.wait_counter = 200 # 2-second fixed delay for calibration - else: - return (False, f"Invalid electric gripper action: {self.action}") - - self.log_debug("Parsed ELECTRICGRIPPER: action=%s, pos=%s, speed=%s, current=%s", self.action, position, speed, current) - self.is_valid = True - return (True, None) - - return (False, f"Unknown gripper command: {command_name}") - - def execute_step(self, state) -> ExecutionStatus: - """State-based execution for pneumatic and electric grippers.""" - self.timeout_counter -= 1 - if self.timeout_counter <= 0: - raise TimeoutError(f"Gripper command timed out in state {self.state}.") - - # --- Pneumatic Logic (Instantaneous) --- - if self.gripper_type == 'pneumatic': - state.InOut_out[self.port_index] = self.state_to_set - logger.info(" -> Pneumatic gripper command sent.") - self.is_finished = True - return ExecutionStatus.completed("Pneumatic gripper toggled") - - # --- Electric Gripper Logic --- - if self.gripper_type == 'electric': - # On the first run, transition to the correct state for the action - if self.state == GripperState.START: - if self.action == 'calibrate': - self.state = GripperState.SEND_CALIBRATE - else: # 'move' - self.state = GripperState.WAIT_FOR_POSITION - # --- Calibrate Logic (Timed Delay) --- - if self.state == GripperState.SEND_CALIBRATE: - logger.debug(" -> Sending one-shot calibrate command...") - state.Gripper_data_out[4] = 1 # Set mode to calibrate - self.state = GripperState.WAITING_CALIBRATION - return ExecutionStatus.executing("Calibrating") - - if self.state == GripperState.WAITING_CALIBRATION: - self.wait_counter -= 1 - if self.wait_counter <= 0: - logger.info(" -> Calibration delay finished.") - state.Gripper_data_out[4] = 0 # Reset to operation mode - self.is_finished = True - return ExecutionStatus.completed("Calibration complete") - return ExecutionStatus.executing("Calibrating") - - # --- Move Logic (Position-Based) --- - if self.state == GripperState.WAIT_FOR_POSITION: - # Persistently send the move command - state.Gripper_data_out[0] = self.target_position - state.Gripper_data_out[1] = self.speed - state.Gripper_data_out[2] = self.current - state.Gripper_data_out[4] = 0 # Operation mode - - # Pack bitfield with direct bitwise operations (avoid bytearray/hex conversions) - bits = [1, 1, int(not state.InOut_in[4]), 1, 0, 0, 0, 0] - val = 0 - for b in bits: - val = (val << 1) | int(b) - state.Gripper_data_out[3] = val - - object_detection = state.Gripper_data_in[5] if len(state.Gripper_data_in) > 5 else 0 - logger.debug(f" -> Gripper moving to {self.target_position} (current: {state.Gripper_data_in[1]}), object detected: {object_detection}") - - # Use Debouncer from base class for object detection - object_detected = self.object_debouncer.tick(object_detection != 0) - - # Check for completion - current_position = state.Gripper_data_in[1] - if abs(current_position - self.target_position) <= 5: - logger.info(" -> Gripper move complete.") - self.is_finished = True - # Set command back to idle - bits = [1, 0, int(not state.InOut_in[4]), 1, 0, 0, 0, 0] - val = 0 - for b in bits: - val = (val << 1) | int(b) - state.Gripper_data_out[3] = val - return ExecutionStatus.completed("Gripper move complete") - - # Check for object detection after debouncing - if object_detected: - if (object_detection == 1) and (self.target_position > current_position): - logger.info(" -> Gripper move holding position due to object detection when closing.") - self.is_finished = True - return ExecutionStatus.completed("Object detected while closing - hold") - - if (object_detection == 2) and (self.target_position < current_position): - logger.info(" -> Gripper move holding position due to object detection when opening.") - self.is_finished = True - return ExecutionStatus.completed("Object detected while opening - hold") - - return ExecutionStatus.executing("Moving gripper") - - # Should not reach here for known gripper types - return ExecutionStatus.failed("Unknown gripper type") +""" +Gripper Control Commands +Contains commands for electric and pneumatic gripper control +""" + +import logging +from enum import Enum + +from parol6.commands.base import Debouncer, ExecutionStatus, MotionCommand +from parol6.server.command_registry import register_command +from parol6.server.state import ControllerState + +logger = logging.getLogger(__name__) + +# Lifecycle TRACE is centralized in higher layers; keep semantic logs here only. + + +class GripperState(Enum): + START = "START" + SEND_CALIBRATE = "SEND_CALIBRATE" + WAITING_CALIBRATION = "WAITING_CALIBRATION" + WAIT_FOR_POSITION = "WAIT_FOR_POSITION" + + +@register_command("PNEUMATICGRIPPER") +@register_command("ELECTRICGRIPPER") +class GripperCommand(MotionCommand): + """ + A single, unified, non-blocking command to control all gripper functions. + It internally selects the correct logic (position-based waiting, timed delay, + or instantaneous) based on the specified action. + """ + + __slots__ = ( + "state", + "timeout_counter", + "object_debouncer", + "wait_counter", + "gripper_type", + "action", + "target_position", + "speed", + "current", + "state_to_set", + "port_index", + ) + + def __init__(self): + """ + Initializes the Gripper command. + Parameters are parsed in do_match() method. + """ + super().__init__() + self.state = GripperState.START + self.timeout_counter = 1000 # 10-second safety timeout for all waiting states + self.object_debouncer = Debouncer(5) # 0.05s debounce for object detection + self.wait_counter = 0 + + # Parameters (set in do_match()) + self.gripper_type = None + self.action = None + self.target_position = None + self.speed = None + self.current = None + self.state_to_set = None + self.port_index = None + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse gripper command parameters. + + Formats: + - PNEUMATICGRIPPER|action|port + - ELECTRICGRIPPER|action|pos|spd|curr + + Args: + parts: Pre-split message parts + + Returns: + Tuple of (can_handle, error_message) + """ + command_name = parts[0].upper() + + if command_name == "PNEUMATICGRIPPER": + if len(parts) != 3: + return (False, "PNEUMATICGRIPPER requires 2 parameters: action, port") + + self.gripper_type = "pneumatic" + self.action = parts[1].lower() + output_port = int(parts[2]) + + # Validate action + if self.action not in ["open", "close"]: + return (False, f"Invalid pneumatic gripper action: {self.action}") + + # Configure pneumatic settings + self.state_to_set = 1 if self.action == "open" else 0 + self.port_index = 2 if output_port == 1 else 3 + + self.log_debug( + "Parsed PNEUMATICGRIPPER: action=%s, port=%s", self.action, output_port + ) + self.is_valid = True + return (True, None) + + elif command_name == "ELECTRICGRIPPER": + if len(parts) != 5: + return ( + False, + "ELECTRICGRIPPER requires 4 parameters: action, position, speed, current", + ) + + self.gripper_type = "electric" + + # Parse action + action_token = parts[1].upper() + self.action = ( + "move" if action_token in ("NONE", "MOVE") else parts[1].lower() + ) + + # Parse numeric parameters + position = int(parts[2]) + speed = int(parts[3]) + current = int(parts[4]) + + # Configure based on action + if self.action == "move": + self.target_position = position + self.speed = speed + self.current = current + + # Validate ranges + if not (0 <= position <= 255): + return (False, f"Position must be 0-255, got {position}") + if not (0 <= speed <= 255): + return (False, f"Speed must be 0-255, got {speed}") + if not (100 <= current <= 1000): + return (False, f"Current must be 100-1000, got {current}") + + elif self.action == "calibrate": + self.wait_counter = 200 # 2-second fixed delay for calibration + else: + return (False, f"Invalid electric gripper action: {self.action}") + + self.log_debug( + "Parsed ELECTRICGRIPPER: action=%s, pos=%s, speed=%s, current=%s", + self.action, + position, + speed, + current, + ) + self.is_valid = True + return (True, None) + + return (False, f"Unknown gripper command: {command_name}") + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + """State-based execution for pneumatic and electric grippers.""" + self.timeout_counter -= 1 + if self.timeout_counter <= 0: + raise TimeoutError(f"Gripper command timed out in state {self.state}.") + + # --- Pneumatic Logic (Instantaneous) --- + if self.gripper_type == "pneumatic": + state.InOut_out[self.port_index] = self.state_to_set + logger.info(" -> Pneumatic gripper command sent.") + self.is_finished = True + return ExecutionStatus.completed("Pneumatic gripper toggled") + + # --- Electric Gripper Logic --- + if self.gripper_type == "electric": + # On the first run, transition to the correct state for the action + if self.state == GripperState.START: + if self.action == "calibrate": + self.state = GripperState.SEND_CALIBRATE + else: # 'move' + self.state = GripperState.WAIT_FOR_POSITION + # --- Calibrate Logic (Timed Delay) --- + if self.state == GripperState.SEND_CALIBRATE: + logger.debug(" -> Sending one-shot calibrate command...") + state.Gripper_data_out[4] = 1 # Set mode to calibrate + self.state = GripperState.WAITING_CALIBRATION + return ExecutionStatus.executing("Calibrating") + + if self.state == GripperState.WAITING_CALIBRATION: + self.wait_counter -= 1 + if self.wait_counter <= 0: + logger.info(" -> Calibration delay finished.") + state.Gripper_data_out[4] = 0 # Reset to operation mode + self.is_finished = True + return ExecutionStatus.completed("Calibration complete") + return ExecutionStatus.executing("Calibrating") + + # --- Move Logic (Position-Based) --- + if self.state == GripperState.WAIT_FOR_POSITION: + # Persistently send the move command + state.Gripper_data_out[0] = self.target_position + state.Gripper_data_out[1] = self.speed + state.Gripper_data_out[2] = self.current + state.Gripper_data_out[4] = 0 # Operation mode + + # Pack bitfield with direct bitwise operations (avoid bytearray/hex conversions) + bits = [1, 1, int(not state.InOut_in[4]), 1, 0, 0, 0, 0] + val = 0 + for b in bits: + val = (val << 1) | int(b) + state.Gripper_data_out[3] = val + + object_detection = ( + state.Gripper_data_in[5] if len(state.Gripper_data_in) > 5 else 0 + ) + logger.debug( + f" -> Gripper moving to {self.target_position} (current: {state.Gripper_data_in[1]}), object detected: {object_detection}" + ) + + # Use Debouncer from base class for object detection + object_detected = self.object_debouncer.tick(object_detection != 0) + + # Check for completion + current_position = state.Gripper_data_in[1] + if abs(current_position - self.target_position) <= 5: + logger.info(" -> Gripper move complete.") + self.is_finished = True + # Set command back to idle + bits = [1, 0, int(not state.InOut_in[4]), 1, 0, 0, 0, 0] + val = 0 + for b in bits: + val = (val << 1) | int(b) + state.Gripper_data_out[3] = val + return ExecutionStatus.completed("Gripper move complete") + + # Check for object detection after debouncing + if object_detected: + if (object_detection == 1) and ( + self.target_position > current_position + ): + logger.info( + " -> Gripper move holding position due to object detection when closing." + ) + self.is_finished = True + return ExecutionStatus.completed( + "Object detected while closing - hold" + ) + + if (object_detection == 2) and ( + self.target_position < current_position + ): + logger.info( + " -> Gripper move holding position due to object detection when opening." + ) + self.is_finished = True + return ExecutionStatus.completed( + "Object detected while opening - hold" + ) + + return ExecutionStatus.executing("Moving gripper") + + # Should not reach here for known gripper types + return ExecutionStatus.failed("Unknown gripper type") diff --git a/parol6/commands/joint_commands.py b/parol6/commands/joint_commands.py index 1b25959..792f985 100644 --- a/parol6/commands/joint_commands.py +++ b/parol6/commands/joint_commands.py @@ -1,133 +1,167 @@ -""" -Joint Movement Commands -Contains commands for direct joint angle movements -""" - -import logging -import numpy as np -from typing import List, Tuple, Optional -import parol6.PAROL6_ROBOT as PAROL6_ROBOT -from parol6.commands.base import MotionCommand, ExecutionStatus, MotionProfile -from parol6.config import INTERVAL_S, TRACE, DEFAULT_ACCEL_PERCENT -from parol6.server.command_registry import register_command - -logger = logging.getLogger(__name__) - - -@register_command("MOVEJOINT") -class MoveJointCommand(MotionCommand): - """ - A non-blocking command to move the robot's joints to a specific configuration. - It pre-calculates the entire trajectory upon initialization. - """ - __slots__ = ( - "command_step", - "trajectory_steps", - "target_angles", - "target_radians", - "duration", - "velocity_percent", - "accel_percent", - "trajectory_type", - ) - def __init__(self): - super().__init__() - self.command_step = 0 - self.trajectory_steps = np.ndarray([]) - - # Parameters (set in do_match()) - self.target_angles = None - self.target_radians = None - self.duration = None - self.velocity_percent = None - self.accel_percent = DEFAULT_ACCEL_PERCENT - self.trajectory_type = 'trapezoid' - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """ - Parse MOVEJOINT command parameters. - - Format: MOVEJOINT|j1|j2|j3|j4|j5|j6|duration|speed - Example: MOVEJOINT|0|45|90|-45|30|0|None|50 - - Args: - parts: Pre-split message parts - - Returns: - Tuple of (can_handle, error_message) - """ - if len(parts) != 9: - return (False, "MOVEJOINT requires 8 parameters: 6 joint angles, duration, speed") - - # Parse joint angles - self.target_angles = np.asarray([float(parts[i]) for i in range(1, 7)], dtype=float) - - # Parse duration and speed - self.duration = None if parts[7].upper() == 'NONE' else float(parts[7]) - self.velocity_percent = None if parts[8].upper() == 'NONE' else float(parts[8]) - - # Validate joint limits - self.target_radians = np.deg2rad(self.target_angles) - for i in range(6): - min_rad, max_rad = PAROL6_ROBOT.joint.limits.rad[i] - if not (min_rad <= self.target_radians[i] <= max_rad): - return (False, f"Joint {i+1} target ({self.target_angles[i]} deg) is out of range") - - self.log_debug("Parsed MoveJoint: %s", self.target_angles) - self.is_valid = True - return (True, None) - - def do_setup(self, state): - """Calculates the trajectory just before execution begins.""" - self.log_trace("Preparing trajectory for MoveJoint to %s...", self.target_angles) - - if self.duration and self.duration > 0: - if self.velocity_percent is not None: - self.log_trace(" -> INFO: Both duration and velocity were provided. Using duration.") - initial_pos_steps = state.Position_in - target_pos_steps = np.asarray(PAROL6_ROBOT.ops.rad_to_steps(self.target_radians), dtype=np.int32) - dur = float(self.duration) - self.trajectory_steps = MotionProfile.from_duration_steps( - initial_pos_steps, target_pos_steps, dur, dt=INTERVAL_S - ) - - elif self.velocity_percent is not None: - initial_pos_steps = state.Position_in - target_pos_steps = np.asarray(PAROL6_ROBOT.ops.rad_to_steps(self.target_radians), dtype=np.int32) - accel_percent = float(self.accel_percent) if self.accel_percent is not None else float(DEFAULT_ACCEL_PERCENT) - self.trajectory_steps = MotionProfile.from_velocity_percent( - initial_pos_steps, - target_pos_steps, - float(self.velocity_percent), - accel_percent, - dt=INTERVAL_S, - ) - self.log_trace(" -> Command is valid (duration calculated from speed).") - - else: - logger.log(TRACE, " -> Using conservative values for MoveJoint.") - command_len = 200 - initial_pos_steps = state.Position_in - target_pos_steps = np.asarray(PAROL6_ROBOT.ops.rad_to_steps(self.target_radians), dtype=np.int32) - total_dur = float(command_len) * INTERVAL_S - self.trajectory_steps = MotionProfile.from_duration_steps( - initial_pos_steps, target_pos_steps, total_dur, dt=INTERVAL_S - ) - - if len(self.trajectory_steps) == 0: - raise ValueError("Trajectory calculation resulted in no steps. Command is invalid.") - self.log_trace(" -> Trajectory prepared with %s steps.", len(self.trajectory_steps)) - - def execute_step(self, state) -> ExecutionStatus: - if self.is_finished or not self.is_valid: - return ExecutionStatus.completed("Already finished") if self.is_finished else ExecutionStatus.failed("Invalid command") - - if self.command_step >= len(self.trajectory_steps): - logger.log(TRACE, f"{type(self).__name__} finished.") - self.is_finished = True - self.stop_and_idle(state) - return ExecutionStatus.completed("MOVEJOINT") - else: - self.set_move_position(state, self.trajectory_steps[self.command_step]) - self.command_step += 1 - return ExecutionStatus.executing("MOVEJOINT") +""" +Joint Movement Commands +Contains commands for direct joint angle movements +""" + +import logging + +import numpy as np + +import parol6.PAROL6_ROBOT as PAROL6_ROBOT +from parol6.commands.base import ExecutionStatus, MotionCommand, MotionProfile +from parol6.config import DEFAULT_ACCEL_PERCENT, INTERVAL_S, TRACE +from parol6.server.command_registry import register_command +from parol6.server.state import ControllerState + +logger = logging.getLogger(__name__) + + +@register_command("MOVEJOINT") +class MoveJointCommand(MotionCommand): + """ + A non-blocking command to move the robot's joints to a specific configuration. + It pre-calculates the entire trajectory upon initialization. + """ + + __slots__ = ( + "command_step", + "trajectory_steps", + "target_angles", + "target_radians", + "duration", + "velocity_percent", + "accel_percent", + "trajectory_type", + ) + + def __init__(self): + super().__init__() + self.command_step = 0 + self.trajectory_steps: np.ndarray = np.empty((0, 6), dtype=np.int32) + + # Parameters (set in do_match()) + self.target_angles = None + self.target_radians = None + self.duration = None + self.velocity_percent = None + self.accel_percent = DEFAULT_ACCEL_PERCENT + self.trajectory_type = "trapezoid" + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse MOVEJOINT command parameters. + + Format: MOVEJOINT|j1|j2|j3|j4|j5|j6|duration|speed + Example: MOVEJOINT|0|45|90|-45|30|0|None|50 + + Args: + parts: Pre-split message parts + + Returns: + Tuple of (can_handle, error_message) + """ + if len(parts) != 9: + return ( + False, + "MOVEJOINT requires 8 parameters: 6 joint angles, duration, speed", + ) + + # Parse joint angles + self.target_angles = np.asarray( + [float(parts[i]) for i in range(1, 7)], dtype=float + ) + + # Parse duration and speed + self.duration = None if parts[7].upper() == "NONE" else float(parts[7]) + self.velocity_percent = None if parts[8].upper() == "NONE" else float(parts[8]) + + # Validate joint limits + self.target_radians = np.deg2rad(self.target_angles) + for i in range(6): + min_rad, max_rad = PAROL6_ROBOT.joint.limits.rad[i] + if not (min_rad <= self.target_radians[i] <= max_rad): + return ( + False, + f"Joint {i + 1} target ({self.target_angles[i]} deg) is out of range", + ) + + self.log_debug("Parsed MoveJoint: %s", self.target_angles) + self.is_valid = True + return (True, None) + + def do_setup(self, state: "ControllerState") -> None: + """Calculates the trajectory just before execution begins.""" + self.log_trace( + "Preparing trajectory for MoveJoint to %s...", self.target_angles + ) + + if self.duration and self.duration > 0: + if self.velocity_percent is not None: + self.log_trace( + " -> INFO: Both duration and velocity were provided. Using duration." + ) + initial_pos_steps = state.Position_in + target_pos_steps = np.asarray( + PAROL6_ROBOT.ops.rad_to_steps(self.target_radians), dtype=np.int32 + ) + dur = float(self.duration) + self.trajectory_steps = MotionProfile.from_duration_steps( + initial_pos_steps, target_pos_steps, dur, dt=INTERVAL_S + ) + + elif self.velocity_percent is not None: + initial_pos_steps = state.Position_in + target_pos_steps = np.asarray( + PAROL6_ROBOT.ops.rad_to_steps(self.target_radians), dtype=np.int32 + ) + accel_percent = ( + float(self.accel_percent) + if self.accel_percent is not None + else float(DEFAULT_ACCEL_PERCENT) + ) + self.trajectory_steps = MotionProfile.from_velocity_percent( + initial_pos_steps, + target_pos_steps, + float(self.velocity_percent), + accel_percent, + dt=INTERVAL_S, + ) + self.log_trace(" -> Command is valid (duration calculated from speed).") + + else: + logger.log(TRACE, " -> Using conservative values for MoveJoint.") + command_len = 200 + initial_pos_steps = state.Position_in + target_pos_steps = np.asarray( + PAROL6_ROBOT.ops.rad_to_steps(self.target_radians), dtype=np.int32 + ) + total_dur = float(command_len) * INTERVAL_S + self.trajectory_steps = MotionProfile.from_duration_steps( + initial_pos_steps, target_pos_steps, total_dur, dt=INTERVAL_S + ) + + if len(self.trajectory_steps) == 0: + raise ValueError( + "Trajectory calculation resulted in no steps. Command is invalid." + ) + self.log_trace( + " -> Trajectory prepared with %s steps.", len(self.trajectory_steps) + ) + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + if self.is_finished or not self.is_valid: + return ( + ExecutionStatus.completed("Already finished") + if self.is_finished + else ExecutionStatus.failed("Invalid command") + ) + + if self.command_step >= len(self.trajectory_steps): + logger.log(TRACE, f"{type(self).__name__} finished.") + self.is_finished = True + self.stop_and_idle(state) + return ExecutionStatus.completed("MOVEJOINT") + else: + self.set_move_position(state, self.trajectory_steps[self.command_step]) + self.command_step += 1 + return ExecutionStatus.executing("MOVEJOINT") diff --git a/parol6/commands/query_commands.py b/parol6/commands/query_commands.py index a43e1a0..597f7cb 100644 --- a/parol6/commands/query_commands.py +++ b/parol6/commands/query_commands.py @@ -2,14 +2,18 @@ Query commands that return immediate status information. """ +from typing import TYPE_CHECKING + import numpy as np -from typing import Tuple, Optional, List, TYPE_CHECKING -from parol6.commands.base import QueryCommand, ExecutionStatus -from parol6.server.command_registry import register_command import parol6.PAROL6_ROBOT as PAROL6_ROBOT -from parol6.server.status_cache import get_cache from parol6 import config as cfg +from parol6.commands.base import ExecutionStatus, QueryCommand +from parol6.server.command_registry import register_command +from parol6.server.state import get_fkine_flat_mm, get_fkine_matrix +from parol6.server.status_cache import get_cache +from parol6.server.transports import is_simulation_mode +from parol6.tools import list_tools if TYPE_CHECKING: from parol6.server.state import ControllerState @@ -17,23 +21,44 @@ @register_command("GET_POSE") class GetPoseCommand(QueryCommand): - """Get current robot pose matrix.""" - __slots__ = () - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """Check if this is a GET_POSE command.""" + """Get current robot pose matrix in specified frame (WRF or TRF).""" + + __slots__ = ("frame",) + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """Check if this is a GET_POSE command and parse optional frame parameter.""" if parts[0].upper() == "GET_POSE": + # Parse optional frame parameter (default WRF for backward compatibility) + if len(parts) > 1: + self.frame = parts[1].upper() + if self.frame not in ("WRF", "TRF"): + return False, f"Invalid frame: {self.frame}. Must be WRF or TRF" + else: + self.frame = "WRF" return True, None return False, None - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: - """Execute immediately and return pose data.""" - q_current = PAROL6_ROBOT.ops.steps_to_rad(state.Position_in) - current_pose_matrix = PAROL6_ROBOT.robot.fkine(q_current).A - pose_flat = current_pose_matrix.flatten() + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + """Execute immediately and return pose data with translation in mm.""" + if self.frame == "TRF": + # Get current pose as 4x4 matrix (translation in meters) + T = get_fkine_matrix(state) + + # Compute inverse to express world in tool frame: T^(-1) = [R^T | -R^T * t] + T_inv = np.linalg.inv(T) + + # Convert translation to mm + T_inv[0:3, 3] *= 1000.0 + + # Flatten row-major (same format as WRF) + pose_flat = T_inv.reshape(-1) + else: + # WRF: use existing implementation + pose_flat = get_fkine_flat_mm(state) + pose_str = ",".join(map(str, pose_flat)) self.reply_ascii("POSE", pose_str) - + self.finish() return ExecutionStatus.completed("Pose sent") @@ -41,21 +66,22 @@ def execute_step(self, state: 'ControllerState') -> ExecutionStatus: @register_command("GET_ANGLES") class GetAnglesCommand(QueryCommand): """Get current joint angles in degrees.""" + __slots__ = () - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """Check if this is a GET_ANGLES command.""" if parts[0].upper() == "GET_ANGLES": return True, None return False, None - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: """Execute immediately and return angle data.""" angles_rad = PAROL6_ROBOT.ops.steps_to_rad(state.Position_in) angles_deg = np.rad2deg(angles_rad) angles_str = ",".join(map(str, angles_deg)) self.reply_ascii("ANGLES", angles_str) - + self.finish() return ExecutionStatus.completed("Angles sent") @@ -63,19 +89,20 @@ def execute_step(self, state: 'ControllerState') -> ExecutionStatus: @register_command("GET_IO") class GetIOCommand(QueryCommand): """Get current I/O status.""" + __slots__ = () - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """Check if this is a GET_IO command.""" if parts[0].upper() == "GET_IO": return True, None return False, None - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: """Execute immediately and return I/O data.""" io_status_str = ",".join(map(str, state.InOut_in[:5])) self.reply_ascii("IO", io_status_str) - + self.finish() return ExecutionStatus.completed("I/O sent") @@ -83,19 +110,20 @@ def execute_step(self, state: 'ControllerState') -> ExecutionStatus: @register_command("GET_GRIPPER") class GetGripperCommand(QueryCommand): """Get current gripper status.""" + __slots__ = () - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """Check if this is a GET_GRIPPER command.""" if parts[0].upper() == "GET_GRIPPER": return True, None return False, None - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: """Execute immediately and return gripper data.""" gripper_status_str = ",".join(map(str, state.Gripper_data_in)) self.reply_ascii("GRIPPER", gripper_status_str) - + self.finish() return ExecutionStatus.completed("Gripper sent") @@ -103,19 +131,20 @@ def execute_step(self, state: 'ControllerState') -> ExecutionStatus: @register_command("GET_SPEEDS") class GetSpeedsCommand(QueryCommand): """Get current joint speeds.""" + __slots__ = () - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """Check if this is a GET_SPEEDS command.""" if parts[0].upper() == "GET_SPEEDS": return True, None return False, None - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: """Execute immediately and return speed data.""" speeds_str = ",".join(map(str, state.Speed_in)) self.reply_ascii("SPEEDS", speeds_str) - + self.finish() return ExecutionStatus.completed("Speeds sent") @@ -123,22 +152,23 @@ def execute_step(self, state: 'ControllerState') -> ExecutionStatus: @register_command("GET_STATUS") class GetStatusCommand(QueryCommand): """Get aggregated robot status (pose, angles, I/O, gripper) from cache.""" + __slots__ = () - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """Check if this is a GET_STATUS command.""" if parts[0].upper() == "GET_STATUS": return True, None return False, None - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: """Execute immediately and return cached aggregated status (ASCII).""" # Always refresh cache from current state before replying cache = get_cache() cache.update_from_state(state) payload = cache.to_ascii() self.reply_text(payload) # Already has full format - + self.finish() return ExecutionStatus.completed("Status sent") @@ -146,46 +176,46 @@ def execute_step(self, state: 'ControllerState') -> ExecutionStatus: @register_command("GET_GCODE_STATUS") class GetGcodeStatusCommand(QueryCommand): """Get GCODE interpreter status.""" + __slots__ = () - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """Check if this is a GET_GCODE_STATUS command.""" if parts[0].upper() == "GET_GCODE_STATUS": return True, None return False, None - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: """Execute immediately and return GCODE status.""" if self.gcode_interpreter: gcode_status = self.gcode_interpreter.get_status() else: gcode_status = { - 'is_running': False, - 'is_paused': False, - 'current_line': None, - 'total_lines': 0, - 'state': {} + "is_running": False, + "is_paused": False, + "current_line": None, + "total_lines": 0, + "state": {}, } - + self.reply_json("GCODE_STATUS", gcode_status) - + self.finish() return ExecutionStatus.completed("GCODE status sent") - - @register_command("GET_LOOP_STATS") class GetLoopStatsCommand(QueryCommand): """Return control-loop metrics (no ACK dependency).""" + __slots__ = () - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: if parts[0].upper() == "GET_LOOP_STATS": return True, None return False, None - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + def execute_step(self, state: "ControllerState") -> ExecutionStatus: target_hz = 1.0 / max(cfg.INTERVAL_S, 1e-9) ema_hz = (1.0 / state.ema_period_s) if state.ema_period_s > 0.0 else 0.0 payload = { @@ -204,19 +234,99 @@ def execute_step(self, state: 'ControllerState') -> ExecutionStatus: @register_command("PING") class PingCommand(QueryCommand): """Respond to ping requests.""" + __slots__ = () - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """Check if this is a PING command.""" if parts[0].upper() == "PING": return True, None return False, None - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: - """Execute immediately and return PONG with serial connectivity bit.""" - # Consider serial "connected" if we've observed a fresh serial frame recently - serial_connected = 1 if get_cache().age_s() <= cfg.STATUS_STALE_S else 0 + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + """Execute immediately and return PONG with serial connectivity bit (0 in simulator mode).""" + # Check if we're in simulator mode + sim = is_simulation_mode() + + # In simulator mode, report SERIAL=0 (not real hardware) + # Otherwise, check if we've observed fresh serial frames recently + if sim: + serial_connected = 0 + else: + serial_connected = 1 if get_cache().age_s() <= cfg.STATUS_STALE_S else 0 + self.reply_ascii("PONG", f"SERIAL={serial_connected}") - + self.finish() return ExecutionStatus.completed("PONG") + + +@register_command("GET_TOOL") +class GetToolCommand(QueryCommand): + """Get current tool configuration and available tools.""" + + __slots__ = () + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """Check if this is a GET_TOOL command.""" + if parts[0].upper() == "GET_TOOL": + return True, None + return False, None + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + """Execute immediately and return current tool info.""" + + payload = {"tool": state.current_tool, "available": list_tools()} + self.reply_json("TOOL", payload) + + self.finish() + return ExecutionStatus.completed("Tool info sent") + + +@register_command("GET_CURRENT_ACTION") +class GetCurrentActionCommand(QueryCommand): + """Get the current executing action/command and its state.""" + + __slots__ = () + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """Check if this is a GET_CURRENT_ACTION command.""" + if parts[0].upper() == "GET_CURRENT_ACTION": + return True, None + return False, None + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + """Execute immediately and return current action info.""" + payload = { + "current": state.action_current, + "state": state.action_state, + "next": state.action_next, + } + self.reply_json("ACTION", payload) + + self.finish() + return ExecutionStatus.completed("Current action info sent") + + +@register_command("GET_QUEUE") +class GetQueueCommand(QueryCommand): + """Get the list of queued non-streamable commands.""" + + __slots__ = () + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """Check if this is a GET_QUEUE command.""" + if parts[0].upper() == "GET_QUEUE": + return True, None + return False, None + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + """Execute immediately and return queue info.""" + payload = { + "non_streamable": state.queue_nonstreamable, + "size": len(state.queue_nonstreamable), + } + self.reply_json("QUEUE", payload) + + self.finish() + return ExecutionStatus.completed("Queue info sent") diff --git a/parol6/commands/smooth_commands.py b/parol6/commands/smooth_commands.py index fe3cc3d..394a754 100644 --- a/parol6/commands/smooth_commands.py +++ b/parol6/commands/smooth_commands.py @@ -1,1842 +1,2155 @@ -""" -Smooth Motion Commands -Contains all smooth trajectory generation commands for advanced robot movements -""" -import logging -import numpy as np -from numpy.typing import NDArray -from typing import Tuple, Optional, List, Any, TYPE_CHECKING, Sequence, Union - -import parol6.PAROL6_ROBOT as PAROL6_ROBOT -import json -from spatialmath import SE3 -from parol6.smooth_motion import ( - CircularMotion, SplineMotion, HelixMotion, WaypointTrajectoryPlanner -) -from parol6.utils.ik import solve_ik_with_adaptive_tol_subdivision -from parol6.commands.base import CommandBase, ExecutionStatus, ExecutionStatusCode, MotionCommand -from parol6.utils.errors import IKError -from parol6.utils.frames import ( - point_trf_to_wrf_mm, - pose6_trf_to_wrf, - se3_to_pose6_mm_deg, - transform_center_trf_to_wrf, - transform_start_pose_if_needed, - transform_command_params_to_wrf, -) -from parol6.config import INTERVAL_S, NEAR_MM_TOL_MM, ENTRY_MM_TOL_MM -from parol6.commands.cartesian_commands import MovePoseCommand -from parol6.server.command_registry import register_command -from parol6.protocol.wire import CommandCode -from parol6.smooth_motion.advanced import AdvancedMotionBlender - -if TYPE_CHECKING: - from parol6.server.state import ControllerState - -logger = logging.getLogger(__name__) - - -class BaseSmoothMotionCommand(MotionCommand): - """ - Base class for all smooth motion commands with proper error tracking. - """ - __slots__ = ( - "description", - "trajectory", - "trajectory_command", - "transition_command", - "specified_start_pose", - "transition_complete", - "trajectory_prepared", - "trajectory_generated", - ) - - def __init__(self, description="smooth motion"): - super().__init__() - self.description = description - self.trajectory: Optional[np.ndarray] = None - self.trajectory_command: Optional["SmoothTrajectoryCommand"] = None - self.transition_command: Optional["MovePoseCommand"] = None - self.specified_start_pose: Optional[NDArray[np.floating]] = None - self.transition_complete = False - self.trajectory_prepared = False - self.trajectory_generated = False - - # Parsing utility methods - @staticmethod - def parse_start_pose(start_str: str) -> Optional[NDArray[np.floating]]: - """ - Parse start pose from string. - - Args: - start_str: Either 'CURRENT', 'NONE', or comma-separated pose values - - Returns: - None for CURRENT/NONE, or numpy array of floats for specified pose - """ - if start_str.upper() in ('CURRENT', 'NONE'): - return None - else: - try: - return np.asarray(list(map(float, start_str.split(','))), dtype=np.float64) - except Exception: - raise ValueError(f"Invalid start pose format: {start_str}") - - @staticmethod - def parse_timing(timing_type: str, timing_value: float, path_length: float) -> float: - """ - Convert timing specification to duration. - - Args: - timing_type: 'DURATION' or 'SPEED' - timing_value: Duration in seconds or speed in mm/s - path_length: Estimated path length in mm - - Returns: - Duration in seconds - """ - if timing_type.upper() == 'DURATION': - return timing_value - elif timing_type.upper() == 'SPEED': - if timing_value <= 0: - raise ValueError(f"Speed must be positive, got {timing_value}") - return path_length / timing_value - else: - raise ValueError(f"Unknown timing type: {timing_type}") - - @staticmethod - def calculate_path_length(command_type: str, params: dict) -> float: - """ - Estimate trajectory path length for timing calculations. - - Args: - command_type: Type of smooth motion command - params: Command parameters - - Returns: - Estimated path length in mm - """ - if command_type == 'SMOOTH_CIRCLE': - # Full circle circumference - return 2 * np.pi * params.get('radius', 100) - elif command_type in ['SMOOTH_ARC_CENTER', 'SMOOTH_ARC_PARAM']: - # Estimate arc length (approximate) - radius = params.get('radius', 100) - angle = params.get('arc_angle', 90) # degrees - return radius * np.radians(angle) - elif command_type == 'SMOOTH_HELIX': - # Helix path length - radius = params.get('radius', 100) - height = params.get('height', 100) - turns = height / params.get('pitch', 10) if params.get('pitch', 10) > 0 else 1 - return np.sqrt((2 * np.pi * radius * turns)**2 + height**2) - else: - # Default estimate - return 300 # mm - - @staticmethod - def parse_trajectory_type(parts: List[str], index: int) -> Tuple[str, Optional[float], int]: - """ - Parse trajectory type and optional jerk limit. - - Args: - parts: Command parts - index: Current index in parts - - Returns: - Tuple of (trajectory_type, jerk_limit, next_index) - """ - if index >= len(parts): - return 'cubic', None, index - - traj_type = parts[index].lower() - index += 1 - - if traj_type not in ['cubic', 'quintic', 's_curve']: - # Not a valid trajectory type, use default - return 'cubic', None, index - 1 - - # Check for jerk limit if s_curve - jerk_limit = None - if traj_type == 's_curve' and index < len(parts): - try: - jerk_limit = float(parts[index]) - index += 1 - except (ValueError, IndexError): - jerk_limit = 1000 # Default jerk limit - - return traj_type, jerk_limit, index - - def create_transition_command(self, current_pose: np.ndarray, target_pose: NDArray[np.floating]) -> Optional["MovePoseCommand"]: - """Create a MovePose command for smooth transition to start position.""" - pos_error = np.linalg.norm(target_pose[:3] - current_pose[:3]) - - if pos_error < NEAR_MM_TOL_MM: # proximity threshold - self.log_info(" -> Already near start position (error: %.1fmm)", pos_error) - return None - - self.log_info(" -> Creating smooth transition to start (%.1fmm away)", pos_error) - - # Calculate transition speed based on distance - if pos_error < 10: - transition_speed = 20.0 # mm/s for short distances - elif pos_error < 30: - transition_speed = 30.0 # mm/s for medium distances - else: - transition_speed = 40.0 # mm/s for long distances - - transition_duration = max(pos_error / transition_speed, 0.5) # Minimum 0.5s - - # MovePoseCommand expects a list, so convert array to list here - transition_cmd: MovePoseCommand = MovePoseCommand(target_pose.tolist(), transition_duration) - - return transition_cmd - - def get_current_pose_from_position(self, position_in): - """Convert current position to pose [x,y,z,rx,ry,rz]""" - current_q = PAROL6_ROBOT.ops.steps_to_rad(position_in) - current_pose_se3 = PAROL6_ROBOT.robot.fkine(current_q) - - current_xyz = current_pose_se3.t * 1000 # Convert to mm - current_rpy = current_pose_se3.rpy(unit='deg', order='xyz') - return np.concatenate([current_xyz, current_rpy]) - - def do_setup(self, state: 'ControllerState') -> None: - """Minimal preparation - just check if we need a transition.""" - self.log_debug(" -> Preparing %s...", self.description) - - # If there's a specified start pose, prepare transition - if self.specified_start_pose is not None: - actual_current_pose = self.get_current_pose_from_position(state.Position_in) - self.transition_command = self.create_transition_command( - actual_current_pose, self.specified_start_pose - ) - - if self.transition_command: - self.transition_command.setup(state) - if not self.transition_command.is_valid: - self.log_error(" -> ERROR: Cannot reach specified start position") - self.fail("Cannot reach specified start position") - return - else: - self.transition_command = None - - # DON'T generate trajectory yet - wait until execution - self.trajectory_generated = False - self.trajectory_prepared = False - self.log_debug(" -> %s preparation complete (trajectory will be generated at execution)", self.description) - - def generate_main_trajectory(self, effective_start_pose): - """Override this in subclasses to generate the specific motion trajectory.""" - raise NotImplementedError("Subclasses must implement generate_main_trajectory") - - def execute_step(self, state: "ControllerState") -> ExecutionStatus: - """Execute transition first if needed, then generate and execute trajectory.""" - if self.is_finished or not self.is_valid: - return ExecutionStatus.completed("Already finished") if self.is_finished else ExecutionStatus.failed("Invalid smooth command") - - # Execute transition first if needed - if self.transition_command and not self.transition_complete: - status = self.transition_command.execute_step(state) - if status.code == ExecutionStatusCode.COMPLETED: - self.log_info(" -> Transition complete") - self.transition_complete = True - # Continue to main trajectory generation next tick - return ExecutionStatus.executing("Transition completed") - elif status.code == ExecutionStatusCode.FAILED: - self.fail(getattr(self.transition_command, 'error_message', 'Transition failed')) - self.finish() - MotionCommand.stop_and_idle(state) - return ExecutionStatus.failed("Transition failed") - else: - return ExecutionStatus.executing("Transitioning") - - # Generate trajectory on first execution step (not during preparation!) - if not self.trajectory_generated: - # Get ACTUAL current position NOW - actual_current_pose = self.get_current_pose_from_position(state.Position_in) - self.log_info(" -> Generating %s from ACTUAL position: %s", self.description, [round(p, 1) for p in actual_current_pose[:3]]) - - # Generate trajectory from where we ACTUALLY are - self.trajectory = self.generate_main_trajectory(actual_current_pose) - self.trajectory_command = SmoothTrajectoryCommand(self.trajectory, self.description) - - # Quick validation of first point only - current_q = PAROL6_ROBOT.ops.steps_to_rad(state.Position_in) - first_pose = self.trajectory[0] - target_se3 = SE3(first_pose[0] / 1000, first_pose[1] / 1000, first_pose[2] / 1000) * SE3.RPY(first_pose[3:], unit='deg', order='xyz') - - ik_result = solve_ik_with_adaptive_tol_subdivision( - PAROL6_ROBOT.robot, target_se3, current_q, ilimit=50, jogging=False - ) - - if not ik_result.success: - self.log_error(" -> ERROR: Cannot reach first trajectory point") - self.finish() - self.fail("Cannot reach trajectory start") - MotionCommand.stop_and_idle(state) - return ExecutionStatus.failed("Cannot reach trajectory start", error=IKError("Cannot reach trajectory start")) - - self.trajectory_generated = True - self.trajectory_prepared = True - - # Verify first point is close to current - distance = np.linalg.norm(first_pose[:3] - np.array(actual_current_pose[:3])) - if distance > 5.0: - self.log_warning(" -> WARNING: First trajectory point %.1fmm from current!", distance) - - # Execute main trajectory - if self.trajectory_command and self.trajectory_prepared: - status = self.trajectory_command.execute_step(state) - - # Check for errors in trajectory execution - if hasattr(self.trajectory_command, 'error_state') and self.trajectory_command.error_state: - self.fail(self.trajectory_command.error_message) - - if status.code == ExecutionStatusCode.COMPLETED: - self.finish() - return ExecutionStatus.completed(f"Smooth {self.description} complete") - elif status.code == ExecutionStatusCode.FAILED: - self.finish() - return status - else: - return ExecutionStatus.executing(f"Smooth {self.description}") - - self.finish() - return ExecutionStatus.completed(f"Smooth {self.description} complete") - - def _generate_radial_entry(self, start_pose, center, normal, radius, duration, sample_rate: float = 100.0): - """ - Generate a simple radial entry trajectory from the current pose to the circle/helix perimeter. - Produces a straight-line interpolation in Cartesian space. - """ - start_pose = np.array(start_pose, dtype=float) - center = np.array(center, dtype=float) - normal = np.array(normal, dtype=float) - if np.linalg.norm(normal) > 0: - normal = normal / np.linalg.norm(normal) - - start_pos = start_pose[:3] - to_start = start_pos - center - # Project onto plane - to_plane = to_start - np.dot(to_start, normal) * normal - dist = float(np.linalg.norm(to_plane)) - - if dist < 1e-6: - # Choose arbitrary direction perpendicular to normal - axis = np.array([1.0, 0.0, 0.0]) - if abs(np.dot(axis, normal)) > 0.9: - axis = np.array([0.0, 1.0, 0.0]) - direction = np.cross(normal, axis) - direction = direction / np.linalg.norm(direction) - else: - direction = to_plane / dist - - target_pos = center + direction * float(radius) - # Keep orientation constant - target_orient = start_pose[3:] - - # Number of samples - n = max(2, int(max(0.05, float(duration)) * float(sample_rate))) - ts = np.linspace(0.0, 1.0, n) - traj = [] - for s in ts: - pos = (1.0 - s) * start_pos + s * target_pos - traj.append(np.concatenate([pos, target_orient])) - return np.array(traj) - -class SmoothTrajectoryCommand: - """Command class for executing pre-generated smooth trajectories.""" - - def __init__(self, trajectory, description="smooth motion"): - self.trajectory = np.array(trajectory) - self.description = description - self.trajectory_index = 0 - self.is_valid = True - self.is_finished = False - self.first_step = True - self.error_state = False - self.error_message = "" - - logger.debug(f"Initializing smooth {description} with {len(trajectory)} points") - - def setup(self, state: "ControllerState"): - """Skip validation - trajectory is already generated from correct position""" - self.is_valid = True - return - - def execute_step(self, state: "ControllerState") -> ExecutionStatus: - """Execute one step of the smooth trajectory""" - if self.is_finished or not self.is_valid: - return ExecutionStatus.completed("Already finished") if self.is_finished else ExecutionStatus.failed("Invalid smooth trajectory") - - if self.trajectory_index >= len(self.trajectory): - logger.info(f"Smooth {self.description} finished.") - self.is_finished = True - MotionCommand.stop_and_idle(state) - return ExecutionStatus.completed(f"Smooth {self.description} complete") - - # Get target pose for this step - target_pose = self.trajectory[self.trajectory_index] - - # Convert to SE3 - target_se3 = SE3(target_pose[0]/1000, target_pose[1]/1000, target_pose[2]/1000) * SE3.RPY(target_pose[3:], unit='deg', order='xyz') - - # Get current joint configuration - current_q = PAROL6_ROBOT.ops.steps_to_rad(state.Position_in) - - # Solve IK - ik_result = solve_ik_with_adaptive_tol_subdivision( - PAROL6_ROBOT.robot, target_se3, current_q, ilimit=50, jogging=False - ) - - if not ik_result.success: - logger.error(f" -> IK failed at trajectory point {self.trajectory_index}") - self.is_finished = True - self.error_state = True - self.error_message = f"IK failed at point {self.trajectory_index}/{len(self.trajectory)}" - MotionCommand.stop_and_idle(state) - return ExecutionStatus.failed(self.error_message, error=IKError(self.error_message)) - - # Convert to steps - target_steps = PAROL6_ROBOT.ops.rad_to_steps(ik_result.q) - - # ADD VELOCITY LIMITING - This prevents violent movements - if self.trajectory_index > 0: - # Vectorized per-tick clamping with 1.2x safety margin - target_steps = PAROL6_ROBOT.ops.clamp_steps_delta( - state.Position_in, - np.asarray(target_steps, dtype=np.int32), - dt=INTERVAL_S, - safety=1.2 - ) - - # Send position command (inline to avoid instance-method binding) - np.copyto(state.Position_out, np.asarray(target_steps, dtype=np.int32), casting='no') - state.Speed_out.fill(0) - state.Command_out = CommandCode.MOVE - - # Advance to next point - self.trajectory_index += 1 - - return ExecutionStatus.executing(f"Smooth {self.description}") - - -@register_command("SMOOTH_CIRCLE") -class SmoothCircleCommand(BaseSmoothMotionCommand): - """Execute smooth circular motion.""" - - __slots__ = ( - "center", - "radius", - "plane", - "duration", - "clockwise", - "frame", - "trajectory_type", - "jerk_limit", - "center_mode", - "entry_mode", - "normal_vector", - "current_position_in", - ) - def __init__(self) -> None: - super().__init__(description="smooth circle") - self.center: Optional[NDArray[np.floating]] = None - self.radius: float = 100.0 - self.plane: str = 'XY' - self.duration: float = 5.0 - self.clockwise: bool = False - self.frame: str = 'WRF' - self.trajectory_type: str = 'cubic' - self.jerk_limit: Optional[float] = None - self.center_mode: str = 'ABSOLUTE' - self.entry_mode: str = 'NONE' - self.normal_vector: Optional[NDArray] = None - self.current_position_in: Optional[NDArray[np.int32]] = None - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """ - Parse SMOOTH_CIRCLE command. - Format: SMOOTH_CIRCLE|center_x,y,z|radius|plane|frame|start_pose|timing_type|timing_value|[trajectory_type]|[jerk_limit]|[clockwise] - """ - if parts[0].upper() != "SMOOTH_CIRCLE": - return False, None - - if len(parts) < 8: - return False, "SMOOTH_CIRCLE requires at least 8 parameters" - - try: - # Parse center - center_list = list(map(float, parts[1].split(','))) - if len(center_list) != 3: - return False, "Center must have 3 coordinates" - self.center = np.asarray(center_list, dtype=np.float64) - - # Parse radius - self.radius = float(parts[2]) - - # Parse plane - self.plane = parts[3].upper() - if self.plane not in ['XY', 'XZ', 'YZ']: - return False, f"Invalid plane: {self.plane}" - - # Parse frame - self.frame = parts[4].upper() - if self.frame not in ['WRF', 'TRF']: - return False, f"Invalid frame: {self.frame}" - - # Parse start pose - self.specified_start_pose = self.parse_start_pose(parts[5]) - - # Parse timing - timing_type = parts[6].upper() - timing_value = float(parts[7]) - path_length = 2 * np.pi * self.radius - self.duration = self.parse_timing(timing_type, timing_value, path_length) - - # Parse optional trajectory type and jerk limit - idx = 8 - if idx < len(parts): - self.trajectory_type, self.jerk_limit, idx = self.parse_trajectory_type(parts, idx) - - # Parse optional clockwise flag - if idx < len(parts) and parts[idx].upper() in ['CW', 'CLOCKWISE', 'TRUE']: - self.clockwise = True - idx += 1 - - # Parse optional center mode - if idx < len(parts): - self.center_mode = parts[idx].upper() - if self.center_mode not in ['ABSOLUTE', 'TOOL', 'RELATIVE']: - self.center_mode = 'ABSOLUTE' - idx += 1 - - # Parse optional entry mode - if idx < len(parts): - self.entry_mode = parts[idx].upper() - if self.entry_mode not in ['AUTO', 'TANGENT', 'DIRECT', 'NONE']: - self.entry_mode = 'NONE' - - # Initialize description - self.description = f"circle (r={self.radius}mm, {self.frame}, {self.trajectory_type})" - - return True, None - - except (ValueError, IndexError) as e: - return False, f"Invalid SMOOTH_CIRCLE parameters: {e}" - - def do_setup(self, state: 'ControllerState') -> None: - """Transform parameters if in TRF, then prepare normally.""" - - # Store current position for potential use in generate_main_trajectory - self.current_position_in = np.asarray(state.Position_in, dtype=np.int32) - - if self.frame == 'TRF': - # Transform parameters to WRF - params = { - 'center': self.center, - 'plane': self.plane - } - transformed = transform_command_params_to_wrf( - 'SMOOTH_CIRCLE', params, 'TRF', state.Position_in - ) - - # Update with transformed values - self.center = transformed['center'] - self.normal_vector = transformed.get('normal_vector') - - logger.info(f" -> TRF Circle: center {self.center[:3].tolist()} (WRF), normal {self.normal_vector}") - - # Transform start_pose if specified - convert array to list for the API - if self.specified_start_pose is not None: - result = transform_start_pose_if_needed( - self.specified_start_pose.tolist(), self.frame, state.Position_in - ) - if result is not None: - self.specified_start_pose = np.asarray(result, dtype=np.float64) - - # Now do normal preparation with transformed parameters - return super().do_setup(state) - - def generate_main_trajectory(self, effective_start_pose): - """Generate circle starting from the actual start position.""" - motion_gen = CircularMotion() - - # Use transformed normal for TRF, or standard for WRF - if self.normal_vector is not None: - # TRF - use the transformed normal vector - normal = np.array(self.normal_vector) - logger.info(f" Using transformed normal: {normal.round(3)}") - else: - # WRF - use standard plane definition - plane_normals = {'XY': [0, 0, 1], 'XZ': [0, 1, 0], 'YZ': [1, 0, 0]} - normal = np.array(plane_normals.get(self.plane, [0, 0, 1])) - logger.info(f" Using WRF plane {self.plane} with normal: {normal}") - - logger.info(f" Generating circle from position: {[round(p, 1) for p in effective_start_pose[:3]]}") - if self.center is not None: - logger.info(f" Circle center: {[round(c, 1) for c in self.center]}") - - # Handle center_mode - if self.center is not None: - actual_center = self.center.copy() - else: - actual_center = np.array([0.0, 0.0, 0.0]) - if self.center_mode == 'TOOL': - # Center at current tool position - actual_center = np.array(effective_start_pose[:3]) - logger.info(f" Center mode: TOOL - centering at current position {actual_center}") - elif self.center_mode == 'RELATIVE': - # Center relative to current position - actual_center = np.array([effective_start_pose[i] + self.center[i] for i in range(3)]) - logger.info(f" Center mode: RELATIVE - center offset from current position to {actual_center}") - else: - # ABSOLUTE mode uses provided center as-is - actual_center = np.array(actual_center) - - # Check if entry trajectory might be needed - distance_to_center = np.linalg.norm(np.array(effective_start_pose[:3]) - np.array(actual_center)) - distance_from_perimeter = float(abs(distance_to_center - self.radius)) - - # Automatically generate entry trajectory if needed - entry_trajectory = None - if distance_from_perimeter > ENTRY_MM_TOL_MM: # perimeter tolerance - effective_entry_mode = self.entry_mode - - # Auto-detect need for entry if not specified - if self.entry_mode == 'NONE' and distance_from_perimeter > 5.0: # Auto-enable for > 5mm - logger.warning(f" Robot is {distance_from_perimeter:.1f}mm from circle perimeter - auto-enabling entry trajectory") - effective_entry_mode = 'AUTO' - - if effective_entry_mode != 'NONE': - logger.info(f" Generating {effective_entry_mode} entry trajectory (distance: {distance_from_perimeter:.1f}mm)") - - # Calculate entry duration based on distance (0.5s min, 2.0s max) - entry_duration = min(2.0, max(0.5, distance_from_perimeter / 50.0)) - - # Generate entry trajectory (radial approach) - entry_trajectory = self._generate_radial_entry( - effective_start_pose, actual_center, normal, self.radius, entry_duration - ) - - if entry_trajectory is not None and len(entry_trajectory) > 0: - logger.info(f" Entry trajectory generated: {len(entry_trajectory)} points over {entry_duration:.1f}s") - - # Generate circle with specified trajectory profile - trajectory = motion_gen.generate_circle_with_profile( - center=actual_center, # Use adjusted center - radius=self.radius, - normal=normal, # This normal now correctly represents the plane - start_point=effective_start_pose, # Pass full pose including orientation - duration=self.duration, - trajectory_type=self.trajectory_type, - jerk_limit=self.jerk_limit - ) - - if self.clockwise: - trajectory = trajectory[::-1] - - # Update orientations to match start pose - for i in range(len(trajectory)): - trajectory[i][3:] = effective_start_pose[3:] - - # Concatenate entry trajectory if it exists - if entry_trajectory is not None and len(entry_trajectory) > 0: - full_trajectory = np.concatenate([entry_trajectory, trajectory]) - return full_trajectory - else: - return trajectory - - -@register_command("SMOOTH_ARC_CENTER") -class SmoothArcCenterCommand(BaseSmoothMotionCommand): - """Execute smooth arc motion defined by center point.""" - - __slots__ = ( - "end_pose", - "center", - "duration", - "clockwise", - "frame", - "trajectory_type", - "jerk_limit", - "normal_vector", - ) - def __init__(self) -> None: - super().__init__(description="smooth arc (center)") - self.end_pose: Optional[List[float]] = None - self.center: Optional[List[float]] = None - self.duration: float = 5.0 - self.clockwise: bool = False - self.frame: str = 'WRF' - self.trajectory_type: str = 'cubic' - self.jerk_limit: Optional[float] = None - self.normal_vector: Optional[NDArray] = None - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """ - Parse SMOOTH_ARC_CENTER command. - Format: SMOOTH_ARC_CENTER|end_x,y,z,rx,ry,rz|center_x,y,z|frame|start_pose|timing_type|timing_value|[trajectory_type]|[jerk_limit]|[clockwise] - """ - if parts[0].upper() != "SMOOTH_ARC_CENTER": - return False, None - - if len(parts) < 7: - return False, "SMOOTH_ARC_CENTER requires at least 7 parameters" - - try: - # Parse end pose - self.end_pose = list(map(float, parts[1].split(','))) - if len(self.end_pose) != 6: - return False, "End pose must have 6 values (x,y,z,rx,ry,rz)" - - # Parse center - self.center = list(map(float, parts[2].split(','))) - if len(self.center) != 3: - return False, "Center must have 3 coordinates" - - # Parse frame - self.frame = parts[3].upper() - if self.frame not in ['WRF', 'TRF']: - return False, f"Invalid frame: {self.frame}" - - # Parse start pose - self.specified_start_pose = self.parse_start_pose(parts[4]) - - # Parse timing - timing_type = parts[5].upper() - timing_value = float(parts[6]) - # Estimate arc length - path_length = 300 # Default estimate, could be improved - self.duration = self.parse_timing(timing_type, timing_value, path_length) - - # Parse optional trajectory type and jerk limit - idx = 7 - if idx < len(parts): - self.trajectory_type, self.jerk_limit, idx = self.parse_trajectory_type(parts, idx) - - # Parse optional clockwise flag - if idx < len(parts) and parts[idx].upper() in ['CW', 'CLOCKWISE', 'TRUE']: - self.clockwise = True - - # Initialize description - self.description = f"arc (center-based, {self.frame}, {self.trajectory_type})" - - return True, None - - except (ValueError, IndexError) as e: - return False, f"Invalid SMOOTH_ARC_CENTER parameters: {e}" - - def do_setup(self, state: 'ControllerState') -> None: - """Transform parameters if in TRF.""" - - if self.frame == 'TRF': - params = { - 'end_pose': self.end_pose, - 'center': self.center - } - transformed = transform_command_params_to_wrf( - 'SMOOTH_ARC_CENTER', params, 'TRF', state.Position_in - ) - self.end_pose = transformed['end_pose'] - self.center = transformed['center'] - self.normal_vector = transformed.get('normal_vector') - - # Transform start_pose if specified - self.specified_start_pose = transform_start_pose_if_needed( - self.specified_start_pose, self.frame, state.Position_in - ) - - return super().do_setup(state) - - def generate_main_trajectory(self, effective_start_pose): - """Generate arc from actual start to end with direct velocity profile.""" - motion_gen = CircularMotion() - - # Use new direct profile generation method - trajectory = motion_gen.generate_arc_with_profile( - effective_start_pose, self.end_pose, self.center, - normal=self.normal_vector, # Use transformed normal if TRF - clockwise=self.clockwise, - duration=self.duration, - trajectory_type=self.trajectory_type, - jerk_limit=self.jerk_limit - ) - - return trajectory - - -@register_command("SMOOTH_ARC_PARAM") -class SmoothArcParamCommand(BaseSmoothMotionCommand): - """Execute smooth arc motion defined by radius and angle.""" - - __slots__ = ( - "end_pose", - "radius", - "arc_angle", - "duration", - "clockwise", - "frame", - "trajectory_type", - "jerk_limit", - "normal_vector", - "current_position_in", - ) - def __init__(self) -> None: - super().__init__(description="smooth arc (param)") - self.end_pose: Optional[List[float]] = None - self.radius: float = 100.0 - self.arc_angle: float = 90.0 - self.duration: float = 5.0 - self.clockwise: bool = False - self.frame: str = 'WRF' - self.trajectory_type: str = 'cubic' - self.jerk_limit: Optional[float] = None - self.normal_vector: Optional[NDArray] = None - self.current_position_in: Optional[Sequence[int]] = None - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """ - Parse SMOOTH_ARC_PARAM command. - Format: SMOOTH_ARC_PARAM|end_x,y,z,rx,ry,rz|radius|arc_angle|frame|start_pose|timing_type|timing_value|[trajectory_type]|[jerk_limit]|[clockwise] - """ - if parts[0].upper() != "SMOOTH_ARC_PARAM": - return False, None - - if len(parts) < 8: - return False, "SMOOTH_ARC_PARAM requires at least 8 parameters" - - try: - # Parse end pose - self.end_pose = list(map(float, parts[1].split(','))) - if len(self.end_pose) != 6: - return False, "End pose must have 6 values (x,y,z,rx,ry,rz)" - - # Parse radius and arc angle - self.radius = float(parts[2]) - self.arc_angle = float(parts[3]) - - # Parse frame - self.frame = parts[4].upper() - if self.frame not in ['WRF', 'TRF']: - return False, f"Invalid frame: {self.frame}" - - # Parse start pose - self.specified_start_pose = self.parse_start_pose(parts[5]) - - # Parse timing - timing_type = parts[6].upper() - timing_value = float(parts[7]) - path_length = self.radius * np.radians(self.arc_angle) - self.duration = self.parse_timing(timing_type, timing_value, path_length) - - # Parse optional trajectory type and jerk limit - idx = 8 - if idx < len(parts): - self.trajectory_type, self.jerk_limit, idx = self.parse_trajectory_type(parts, idx) - - # Parse optional clockwise flag - if idx < len(parts) and parts[idx].upper() in ['CW', 'CLOCKWISE', 'TRUE']: - self.clockwise = True - - # Initialize description - self.description = f"parametric arc (r={self.radius}mm, θ={self.arc_angle}°, {self.frame}, {self.trajectory_type})" - - return True, None - - except (ValueError, IndexError) as e: - return False, f"Invalid SMOOTH_ARC_PARAM parameters: {e}" - - def do_setup(self, state: 'ControllerState') -> None: - """Transform parameters if in TRF, then prepare normally.""" - - self.current_position_in = state.Position_in - - if self.frame == 'TRF': - # Transform parameters to WRF - params = { - 'end_pose': self.end_pose, - 'plane': 'XY' # Default plane for parametric arc - } - transformed = transform_command_params_to_wrf( - 'SMOOTH_ARC_PARAM', params, 'TRF', state.Position_in - ) - - # Update with transformed values - self.end_pose = transformed['end_pose'] - self.normal_vector = transformed.get('normal_vector') - - logger.info(f" -> TRF Parametric Arc: end {self.end_pose[:3]} (WRF)") - - # Transform start_pose if specified - self.specified_start_pose = transform_start_pose_if_needed( - self.specified_start_pose, self.frame, state.Position_in - ) - - return super().do_setup(state) - - def generate_main_trajectory(self, effective_start_pose): - """Generate arc based on radius and angle from actual start.""" - # Get start and end positions - start_xyz = np.array(effective_start_pose[:3]) - end_xyz = np.array(self.end_pose[:3]) - - # If we have a transformed normal (TRF), use it to define the arc plane - if self.normal_vector is not None: - normal = np.array(self.normal_vector) - - # Find center of arc using radius and angle - chord_vec = end_xyz - start_xyz - chord_length = np.linalg.norm(chord_vec) - - if chord_length > 2 * self.radius: - logger.warning(f" -> Warning: Points too far apart ({chord_length:.1f}mm) for radius {self.radius}mm") - self.radius = chord_length / 2 + 1 - - # Calculate center position using the normal vector - chord_mid = (start_xyz + end_xyz) / 2 - - # Height from chord midpoint to arc center - if chord_length > 0: - h = np.sqrt(max(0, self.radius**2 - (chord_length/2)**2)) - - # Find perpendicular in the plane defined by normal - chord_dir = chord_vec / chord_length - perp_in_plane = np.cross(normal, chord_dir) - if np.linalg.norm(perp_in_plane) > 0.001: - perp_in_plane = perp_in_plane / np.linalg.norm(perp_in_plane) - else: - # Chord is parallel to normal (shouldn't happen) - perp_in_plane = np.array([1, 0, 0]) - - # Arc center - if self.clockwise: - center_3d = chord_mid - h * perp_in_plane - else: - center_3d = chord_mid + h * perp_in_plane - else: - center_3d = start_xyz - - else: - # WRF - use XY plane (standard behavior) - normal = np.array([0, 0, 1]) - - # Calculate arc center in XY plane - start_xy = start_xyz[:2] - end_xy = end_xyz[:2] - - # Midpoint - mid = (start_xy + end_xy) / 2 - - # Distance between points - d = np.linalg.norm(end_xy - start_xy) - - if d > 2 * self.radius: - logger.warning(f" -> Warning: Points too far apart ({d:.1f}mm) for radius {self.radius}mm") - self.radius = d / 2 + 1 - - # Height of arc center from midpoint - h = np.sqrt(max(0, self.radius**2 - (d/2)**2)) - - # Perpendicular direction - if d > 0: - perp = np.array([-(end_xy[1] - start_xy[1]), end_xy[0] - start_xy[0]]) - perp = perp / np.linalg.norm(perp) - else: - perp = np.array([1, 0]) - - # Arc center (choose based on clockwise) - if self.clockwise: - center_2d = mid - h * perp - else: - center_2d = mid + h * perp - - # Use average Z for center - center_3d = [center_2d[0], center_2d[1], (start_xyz[2] + end_xyz[2]) / 2] - - # Generate arc trajectory with direct velocity profile - motion_gen = CircularMotion() - - # Use new direct profile generation method - trajectory = motion_gen.generate_arc_with_profile( - effective_start_pose, self.end_pose, center_3d, - normal=normal if self.normal_vector is not None else None, - clockwise=self.clockwise, - duration=self.duration, - trajectory_type=self.trajectory_type, - jerk_limit=self.jerk_limit - ) - - return trajectory - - -@register_command("SMOOTH_HELIX") -class SmoothHelixCommand(BaseSmoothMotionCommand): - """Execute smooth helical motion.""" - - __slots__ = ( - "center", - "radius", - "pitch", - "height", - "duration", - "clockwise", - "frame", - "trajectory_type", - "jerk_limit", - "helix_axis", - "up_vector", - ) - def __init__(self) -> None: - super().__init__(description="smooth helix") - self.center: Optional[List[float]] = None - self.radius: float = 100.0 - self.pitch: float = 10.0 - self.height: float = 100.0 - self.duration: float = 5.0 - self.clockwise: bool = False - self.frame: str = 'WRF' - self.trajectory_type: str = 'cubic' - self.jerk_limit: Optional[float] = None - self.helix_axis: Optional[NDArray] = None - self.up_vector: Optional[NDArray] = None - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """ - Parse SMOOTH_HELIX command. - Format: SMOOTH_HELIX|center_x,y,z|radius|pitch|height|frame|start_pose|timing_type|timing_value|[trajectory_type]|[jerk_limit]|[clockwise] - """ - if parts[0].upper() != "SMOOTH_HELIX": - return False, None - - if len(parts) < 9: - return False, "SMOOTH_HELIX requires at least 9 parameters" - - try: - # Parse center - self.center = list(map(float, parts[1].split(','))) - if len(self.center) != 3: - return False, "Center must have 3 coordinates" - - # Parse radius, pitch, height - self.radius = float(parts[2]) - self.pitch = float(parts[3]) - self.height = float(parts[4]) - - # Parse frame - self.frame = parts[5].upper() - if self.frame not in ['WRF', 'TRF']: - return False, f"Invalid frame: {self.frame}" - - # Parse start pose - self.specified_start_pose = self.parse_start_pose(parts[6]) - - # Parse timing - timing_type = parts[7].upper() - timing_value = float(parts[8]) - turns = self.height / self.pitch if self.pitch > 0 else 1 - path_length = np.sqrt((2 * np.pi * self.radius * turns)**2 + self.height**2) - self.duration = self.parse_timing(timing_type, timing_value, path_length) - - # Parse optional trajectory type and jerk limit - idx = 9 - if idx < len(parts): - self.trajectory_type, self.jerk_limit, idx = self.parse_trajectory_type(parts, idx) - - # Parse optional clockwise flag - if idx < len(parts) and parts[idx].upper() in ['CW', 'CLOCKWISE', 'TRUE']: - self.clockwise = True - - # Initialize description - self.description = f"helix (h={self.height}mm, {self.frame}, {self.trajectory_type})" - - return True, None - - except (ValueError, IndexError) as e: - return False, f"Invalid SMOOTH_HELIX parameters: {e}" - - def do_setup(self, state: 'ControllerState') -> None: - """Transform parameters if in TRF.""" - - if self.frame == 'TRF': - params = {'center': self.center} - transformed = transform_command_params_to_wrf( - 'SMOOTH_HELIX', params, 'TRF', state.Position_in - ) - self.center = transformed['center'] - self.helix_axis = transformed.get('helix_axis', [0, 0, 1]) - self.up_vector = transformed.get('up_vector', [0, 1, 0]) - - if self.specified_start_pose: - params = {'start_pose': self.specified_start_pose} - transformed = transform_command_params_to_wrf( - 'SMOOTH_HELIX', params, 'TRF', state.Position_in - ) - self.specified_start_pose = transformed.get('start_pose') - - return super().do_setup(state) - - def generate_main_trajectory(self, effective_start_pose): - """Generate helix with entry trajectory if needed and proper trajectory profile.""" - helix_gen = HelixMotion() - - # Get helix axis (default Z for WRF, transformed for TRF) - if self.helix_axis is not None: - axis = self.helix_axis - else: - axis = [0, 0, 1] # Default vertical - - # Calculate distance from start position to helix start point - start_pos = np.array(effective_start_pose[:3]) - center_np = np.array(self.center) - - # Project start position onto the helix plane (perpendicular to axis) - axis_np = np.array(axis) - axis_np = axis_np / np.linalg.norm(axis_np) - to_start = start_pos - center_np - to_start_plane = to_start - np.dot(to_start, axis_np) * axis_np - dist_to_center = np.linalg.norm(to_start_plane) - - # Check if entry trajectory is needed - entry_trajectory = None - distance_from_perimeter = abs(dist_to_center - self.radius) - - if distance_from_perimeter > self.radius * 0.05: # More than 5% off the perimeter - logger.info(f" Generating helix entry trajectory (distance from perimeter: {distance_from_perimeter:.1f}mm)") - - # Calculate entry duration based on distance (0.5s min, 2.0s max) - entry_duration = min(2.0, max(0.5, distance_from_perimeter / 50.0)) - - # Generate entry trajectory to helix start position - motion_gen = CircularMotion() - - # Calculate the target position on the helix perimeter - if dist_to_center > 0.001: - direction = to_start_plane / dist_to_center - else: - # If exactly at center, move to any point on perimeter - u = np.array([1, 0, 0]) if abs(axis_np[0]) < 0.9 else np.array([0, 1, 0]) - u = u - np.dot(u, axis_np) * axis_np - direction = u / np.linalg.norm(u) - - target_on_perimeter = center_np + direction * self.radius - # For helix, we want to start at the correct Z level - target_on_perimeter[2] = start_pos[2] # Keep same Z as start - - # Generate smooth approach trajectory - entry_trajectory = self._generate_radial_entry( - effective_start_pose, center_np, axis_np, self.radius, entry_duration - ) - - if entry_trajectory is not None and len(entry_trajectory) > 0: - logger.info(f" Helix entry trajectory generated: {len(entry_trajectory)} points over {entry_duration:.1f}s") - - # Generate main helix trajectory - trajectory = helix_gen.generate_helix_with_profile( - center=self.center, - radius=self.radius, - pitch=self.pitch, - height=self.height, - axis=axis_np, - duration=self.duration, - trajectory_type=self.trajectory_type, - jerk_limit=self.jerk_limit, - start_point=effective_start_pose, - clockwise=self.clockwise - ) - - # Update orientations to match effective start - for i in range(len(trajectory)): - trajectory[i][3:] = effective_start_pose[3:] - - # Concatenate entry trajectory if it exists - if entry_trajectory is not None and len(entry_trajectory) > 0: - full_trajectory = np.concatenate([entry_trajectory, trajectory]) - return full_trajectory - else: - return np.array(trajectory) - - -@register_command("SMOOTH_SPLINE") -class SmoothSplineCommand(BaseSmoothMotionCommand): - """Execute smooth spline motion through waypoints.""" - - __slots__ = ( - "waypoints", - "duration", - "frame", - "trajectory_type", - "jerk_limit", - "current_position_in", - ) - def __init__(self) -> None: - super().__init__(description="smooth spline") - self.waypoints: Optional[List[List[float]]] = None - self.duration: float = 5.0 - self.frame: str = 'WRF' - self.trajectory_type: str = 'cubic' - self.jerk_limit: Optional[float] = None - self.current_position_in: Optional[Sequence[int]] = None - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """ - Parse SMOOTH_SPLINE command. - Format: SMOOTH_SPLINE|wp1_x,y,z,rx,ry,rz;wp2_x,y,z,rx,ry,rz;...|frame|start_pose|timing_type|timing_value|[trajectory_type]|[jerk_limit] - """ - if parts[0].upper() != "SMOOTH_SPLINE": - return False, None - - if len(parts) < 6: - return False, "SMOOTH_SPLINE requires at least 6 parameters" - - # Support alternate wire format: - # SMOOTH_SPLINE||||||[trajectory_type]|[jerk?]| - if len(parts) >= 7 and parts[1].isdigit(): - try: - num = int(parts[1]) - self.frame = parts[2].upper() - if self.frame not in ['WRF', 'TRF']: - return False, f"Invalid frame: {self.frame}" - self.specified_start_pose = self.parse_start_pose(parts[3]) - timing_type = parts[4].upper() - timing_value = float(parts[5]) - idx = 6 - if idx < len(parts) and parts[idx].lower() in ['cubic', 'quintic', 's_curve']: - self.trajectory_type = parts[idx].lower() - idx += 1 - if self.trajectory_type == 's_curve' and idx < len(parts): - self.jerk_limit = float(parts[idx]) - idx += 1 - needed = num * 6 - if len(parts) - idx < needed: - return False, "Insufficient waypoint values" - vals = list(map(float, parts[idx:idx + needed])) - self.waypoints = [vals[i:i + 6] for i in range(0, needed, 6)] - # Estimate path length - path_length = 0.0 - for i in range(1, len(self.waypoints)): - path_length += float(np.linalg.norm(np.array(self.waypoints[i][:3]) - np.array(self.waypoints[i - 1][:3]))) - self.duration = self.parse_timing(timing_type, timing_value, path_length) - self.description = f"spline ({len(self.waypoints)} points, {self.frame}, {self.trajectory_type})" - return True, None - except Exception as e: - return False, f"Invalid SMOOTH_SPLINE parameters: {e}" - - try: - # Parse waypoints (semicolon separated) - waypoint_strs = parts[1].split(';') - self.waypoints = [] - for wp_str in waypoint_strs: - wp = list(map(float, wp_str.split(','))) - if len(wp) != 6: - return False, f"Each waypoint must have 6 values (x,y,z,rx,ry,rz)" - self.waypoints.append(wp) - - if len(self.waypoints) < 2: - return False, "SMOOTH_SPLINE requires at least 2 waypoints" - - # Parse frame - self.frame = parts[2].upper() - if self.frame not in ['WRF', 'TRF']: - return False, f"Invalid frame: {self.frame}" - - # Parse start pose - self.specified_start_pose = self.parse_start_pose(parts[3]) - - # Parse timing - timing_type = parts[4].upper() - timing_value = float(parts[5]) - # Estimate path length from waypoints - path_length = 0 - for i in range(1, len(self.waypoints)): - path_length += np.linalg.norm( - np.array(self.waypoints[i][:3]) - np.array(self.waypoints[i-1][:3]) - ) - self.duration = self.parse_timing(timing_type, timing_value, path_length) - - # Parse optional trajectory type and jerk limit - idx = 6 - if idx < len(parts): - self.trajectory_type, self.jerk_limit, idx = self.parse_trajectory_type(parts, idx) - - # Initialize description - self.description = f"spline ({len(self.waypoints)} points, {self.frame}, {self.trajectory_type})" - - return True, None - - except (ValueError, IndexError) as e: - return False, f"Invalid SMOOTH_SPLINE parameters: {e}" - - def do_setup(self, state: 'ControllerState') -> None: - """Transform parameters if in TRF, then prepare normally.""" - - self.current_position_in = state.Position_in - - if self.frame == 'TRF': - # Transform waypoints to WRF - params = {'waypoints': self.waypoints} - transformed = transform_command_params_to_wrf( - 'SMOOTH_SPLINE', params, 'TRF', state.Position_in - ) - - # Update with transformed values - self.waypoints = transformed['waypoints'] - - logger.info(f" -> TRF Spline: transformed {len(self.waypoints)} waypoints to WRF") - - # Transform start_pose if specified - self.specified_start_pose = transform_start_pose_if_needed( - self.specified_start_pose, self.frame, state.Position_in - ) - - return super().do_setup(state) - - def generate_main_trajectory(self, effective_start_pose): - """Generate spline starting from actual position.""" - motion_gen = SplineMotion() - - # Always start from the effective start pose - first_wp_error = np.linalg.norm( - np.array(self.waypoints[0][:3]) - np.array(effective_start_pose[:3]) - ) - - if first_wp_error > 5.0: - # First waypoint is far, prepend the start position - modified_waypoints = [effective_start_pose] + self.waypoints - logger.info(f" Added start position as first waypoint (distance: {first_wp_error:.1f}mm)") - else: - # Replace first waypoint with actual start to ensure continuity - modified_waypoints = [effective_start_pose] + self.waypoints[1:] - logger.info(" Replaced first waypoint with actual start position") - - timestamps = np.linspace(0, self.duration, len(modified_waypoints)) - - # Generate the spline trajectory based on type - if self.trajectory_type == 'quintic': - trajectory = motion_gen.generate_quintic_spline( - modified_waypoints, timestamps=None - ) - elif self.trajectory_type == 's_curve': - trajectory = motion_gen.generate_scurve_spline( - modified_waypoints, duration=self.duration, - jerk_limit=self.jerk_limit if self.jerk_limit else 1000 - ) - else: # cubic (default) - trajectory = motion_gen.generate_cubic_spline(modified_waypoints, timestamps) - - logger.debug(f" Generated spline with {len(trajectory)} points") - - return trajectory - - -@register_command("SMOOTH_BLEND") -class SmoothBlendCommand(BaseSmoothMotionCommand): - """Execute smooth blended trajectory through multiple segments.""" - - __slots__ = ( - "segment_definitions", - "blend_time", - "frame", - "trajectory_type", - "jerk_limit", - "current_position_in", - ) - def __init__(self) -> None: - super().__init__(description="smooth blend") - self.segment_definitions: Optional[List[dict]] = None - self.blend_time: float = 0.5 - self.frame: str = 'WRF' - self.trajectory_type: str = 'cubic' - self.jerk_limit: Optional[float] = None - self.current_position_in: Optional[Sequence[int]] = None - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """ - Parse SMOOTH_BLEND command. - Format: SMOOTH_BLEND|segments_json|blend_time|frame|start_pose|[trajectory_type]|[jerk_limit] - """ - if parts[0].upper() != "SMOOTH_BLEND": - return False, None - - if len(parts) < 5: - return False, "SMOOTH_BLEND requires at least 5 parameters" - - # New wire format: SMOOTH_BLEND|num_segments|blend_time|frame|start_pose|timing|SEG1||SEG2||... - if parts[1].isdigit(): - try: - num_segments = int(parts[1]) - self.blend_time = float(parts[2]) - self.frame = parts[3].upper() - if self.frame not in ['WRF', 'TRF']: - return False, f"Invalid frame: {self.frame}" - self.specified_start_pose = self.parse_start_pose(parts[4]) - # parts[5] timing token (DEFAULT/DURATION/SPEED) not strictly needed for segments - # Reconstruct remainder and split by '||' to obtain segments - remainder = "|".join(parts[6:]) - raw_segments = [s for s in remainder.split("||") if s.strip() != ""] - seg_defs = [] - for seg_str in raw_segments: - tokens = [t for t in seg_str.split("|") if t != ""] - if not tokens: - continue - seg_type = tokens[0].upper() - if seg_type == "LINE": - if len(tokens) < 3: - return False, "LINE segment requires end pose and duration" - end = list(map(float, tokens[1].split(","))) - duration = float(tokens[2]) - seg_defs.append({"type": "LINE", "end": end, "duration": duration}) - elif seg_type == "CIRCLE": - if len(tokens) < 6: - return False, "CIRCLE segment requires center,radius,plane,duration,clockwise" - center = list(map(float, tokens[1].split(","))) - radius = float(tokens[2]) - plane = tokens[3].upper() - duration = float(tokens[4]) - clockwise = tokens[5] in ("1", "TRUE", "True", "true", "CW", "CLOCKWISE") - seg_defs.append({"type": "CIRCLE", "center": center, "radius": radius, "plane": plane, "duration": duration, "clockwise": clockwise}) - elif seg_type == "ARC": - if len(tokens) < 5: - return False, "ARC segment requires end,center,duration,clockwise" - end = list(map(float, tokens[1].split(","))) - center = list(map(float, tokens[2].split(","))) - duration = float(tokens[3]) - clockwise = tokens[4] in ("1", "TRUE", "True", "true", "CW", "CLOCKWISE") - seg_defs.append({"type": "ARC", "end": end, "center": center, "duration": duration, "clockwise": clockwise}) - elif seg_type == "SPLINE": - if len(tokens) < 4: - return False, "SPLINE segment requires count,waypoints,duration" - count = int(tokens[1]) - wp_tokens = tokens[2].split(";") - if len(wp_tokens) != count: - return False, "SPLINE waypoint count mismatch" - waypoints = [list(map(float, wp.split(","))) for wp in wp_tokens] - duration = float(tokens[3]) - seg_defs.append({"type": "SPLINE", "waypoints": waypoints, "duration": duration}) - else: - return False, f"Invalid segment type: {seg_type}" - self.segment_definitions = seg_defs - self.description = f"blended ({len(self.segment_definitions)} segments, {self.frame}, {self.trajectory_type})" - return True, None - except Exception as e: - return False, f"Invalid SMOOTH_BLEND parameters: {e}" - - try: - # Parse segment definitions (JSON format) - self.segment_definitions = json.loads(parts[1]) - - # Validate segment definitions - if not isinstance(self.segment_definitions, list): - return False, "Segments must be a list" - - for seg in self.segment_definitions: - if 'type' not in seg: - return False, "Each segment must have a 'type' field" - if seg['type'] not in ['LINE', 'ARC', 'CIRCLE', 'SPLINE']: - return False, f"Invalid segment type: {seg['type']}" - - # Parse blend time - self.blend_time = float(parts[2]) - - # Parse frame - self.frame = parts[3].upper() - if self.frame not in ['WRF', 'TRF']: - return False, f"Invalid frame: {self.frame}" - - # Parse start pose - self.specified_start_pose = self.parse_start_pose(parts[4]) - - # Parse optional trajectory type and jerk limit - idx = 5 - if idx < len(parts): - self.trajectory_type, self.jerk_limit, idx = self.parse_trajectory_type(parts, idx) - - # Initialize description - self.description = f"blended ({len(self.segment_definitions)} segments, {self.frame}, {self.trajectory_type})" - - return True, None - - except (ValueError, IndexError, json.JSONDecodeError) as e: - return False, f"Invalid SMOOTH_BLEND parameters: {e}" - - def do_setup(self, state: 'ControllerState') -> None: - """Transform parameters if in TRF, then prepare normally.""" - - self.current_position_in = state.Position_in - - if self.frame == 'TRF': - # Transform all segment definitions to WRF - params = {'segments': self.segment_definitions} - transformed = transform_command_params_to_wrf( - 'SMOOTH_BLEND', params, 'TRF', state.Position_in - ) - - # Update with transformed values - self.segment_definitions = transformed['segments'] - - logger.info(f" -> TRF Blend: transformed {len(self.segment_definitions)} segments to WRF") - - # Transform start_pose if specified - self.specified_start_pose = transform_start_pose_if_needed( - self.specified_start_pose, self.frame, state.Position_in - ) - - return super().do_setup(state) - - def generate_main_trajectory(self, effective_start_pose): - """Generate blended trajectory starting from actual position.""" - trajectories = [] - motion_gen_circle = CircularMotion() - motion_gen_spline = SplineMotion() - - # Always start from effective start pose - last_end_pose = effective_start_pose - - for i, seg_def in enumerate(self.segment_definitions): - seg_type = seg_def['type'] - - # First segment always starts from effective_start_pose - segment_start = effective_start_pose if i == 0 else last_end_pose - - if seg_type == 'LINE': - end = seg_def['end'] - duration = seg_def['duration'] - - # Generate line segment from actual position - num_points = int(duration * 100) - timestamps = np.linspace(0, duration, num_points) - - traj = [] - for t in timestamps: - s = t / duration if duration > 0 else 1 - # Interpolate position - pos = [ - segment_start[j] * (1-s) + end[j] * s - for j in range(3) - ] - # Interpolate orientation - orient = [ - segment_start[j+3] * (1-s) + end[j+3] * s - for j in range(3) - ] - traj.append(pos + orient) - - trajectories.append(np.array(traj)) - last_end_pose = end - - elif seg_type == 'ARC': - end = seg_def['end'] - center = seg_def['center'] - duration = seg_def['duration'] - clockwise = seg_def.get('clockwise', False) - - # Check if we have a transformed normal (from TRF) - normal = seg_def.get('normal_vector', None) - - traj = motion_gen_circle.generate_arc_3d( - segment_start, end, center, - normal=normal, # Use transformed normal if available - clockwise=clockwise, duration=duration - ) - trajectories.append(traj) - last_end_pose = end - - elif seg_type == 'CIRCLE': - center = seg_def['center'] - radius = seg_def['radius'] - plane = seg_def.get('plane', 'XY') - duration = seg_def['duration'] - clockwise = seg_def.get('clockwise', False) - - # Use transformed normal if available (from TRF) - if 'normal_vector' in seg_def: - normal = seg_def['normal_vector'] - else: - plane_normals = {'XY': [0, 0, 1], 'XZ': [0, 1, 0], 'YZ': [1, 0, 0]} - normal = plane_normals.get(plane, [0, 0, 1]) - - traj = motion_gen_circle.generate_circle_3d( - center, radius, normal, - start_point=segment_start[:3], - duration=duration - ) - - if clockwise: - traj = traj[::-1] - - # Update orientations - for j in range(len(traj)): - traj[j][3:] = segment_start[3:] - - trajectories.append(traj) - # Circle returns to start, so last pose is last point of trajectory - last_end_pose = traj[-1] - - elif seg_type == 'SPLINE': - waypoints = seg_def['waypoints'] - duration = seg_def['duration'] - - # Check if first waypoint is close to segment start - wp_error = np.linalg.norm( - np.array(waypoints[0][:3]) - np.array(segment_start[:3]) - ) - - if wp_error > 5.0: - full_waypoints = [segment_start] + waypoints - else: - full_waypoints = [segment_start] + waypoints[1:] - - timestamps = np.linspace(0, duration, len(full_waypoints)) - traj = motion_gen_spline.generate_cubic_spline(full_waypoints, timestamps) - trajectories.append(traj) - last_end_pose = waypoints[-1] - - # Blend all trajectories with advanced blending - if len(trajectories) > 1: - # Select blend method based on trajectory type - if self.trajectory_type == 'quintic': - blend_method = 'quintic' - elif self.trajectory_type == 's_curve': - blend_method = 's_curve' - elif self.trajectory_type == 'cubic': - blend_method = 'cubic' - else: - blend_method = 'smoothstep' # Legacy compatibility - - # Create advanced blender - advanced_blender = AdvancedMotionBlender(sample_rate=100.0) - blended = trajectories[0] - - # Use auto sizing if blend_time is small, otherwise use specified time - if self.blend_time < 0.1: - auto_size = True - blend_samples = None - else: - auto_size = False - blend_samples = int(self.blend_time * 100) - - for i in range(1, len(trajectories)): - blended = advanced_blender.blend_trajectories( - blended, trajectories[i], - method=blend_method, - blend_samples=blend_samples, - auto_size=auto_size - ) - - logger.info(f" Blended {len(trajectories)} segments into {len(blended)} points using {blend_method}") - return blended - elif trajectories: - return trajectories[0] - else: - raise ValueError("No trajectories generated in blend") - - -@register_command("SMOOTH_WAYPOINTS") -class SmoothWaypointsCommand(BaseSmoothMotionCommand): - """Execute waypoint trajectory with corner cutting.""" - - __slots__ = ( - "waypoints", - "blend_radii", - "blend_mode", - "via_modes", - "max_velocity", - "max_acceleration", - "trajectory_type", - "frame", - "duration", - ) - def __init__(self) -> None: - super().__init__(description="smooth waypoints") - self.waypoints: Optional[List[List[float]]] = None - self.blend_radii: Any = 'auto' - self.blend_mode: str = 'parabolic' - self.via_modes: Optional[List[str]] = None - self.max_velocity: float = 100.0 - self.max_acceleration: float = 500.0 - self.trajectory_type: str = 'quintic' - self.frame: str = 'WRF' - self.duration: Optional[float] = None - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """ - Parse SMOOTH_WAYPOINTS command. - Format: SMOOTH_WAYPOINTS|wp1;wp2;...|blend_radii|blend_mode|via_modes|max_vel|max_accel|frame|[trajectory_type]|[duration] - """ - if parts[0].upper() != "SMOOTH_WAYPOINTS": - return False, None - - if len(parts) < 8: - return False, "SMOOTH_WAYPOINTS requires at least 8 parameters" - - try: - # Parse waypoints (semicolon separated) - waypoint_strs = parts[1].split(';') - self.waypoints = [] - for wp_str in waypoint_strs: - wp = list(map(float, wp_str.split(','))) - if len(wp) != 6: - return False, f"Each waypoint must have 6 values (x,y,z,rx,ry,rz)" - self.waypoints.append(wp) - - if len(self.waypoints) < 2: - return False, "SMOOTH_WAYPOINTS requires at least 2 waypoints" - - # Parse blend radii - if parts[2].upper() == 'AUTO': - self.blend_radii = 'auto' - else: - self.blend_radii = list(map(float, parts[2].split(','))) - if len(self.blend_radii) != len(self.waypoints) - 2: - return False, f"Blend radii count must be {len(self.waypoints) - 2}" - - # Parse blend mode - self.blend_mode = parts[3].lower() - if self.blend_mode not in ['parabolic', 'circular', 'none']: - return False, f"Invalid blend mode: {self.blend_mode}" - - # Parse via modes - via_mode_strs = parts[4].split(',') - self.via_modes = [] - for vm in via_mode_strs: - vm = vm.lower() - if vm not in ['via', 'stop']: - return False, f"Invalid via mode: {vm}" - self.via_modes.append(vm) - - if len(self.via_modes) != len(self.waypoints): - return False, f"Via modes count must match waypoint count" - - # Parse velocity and acceleration constraints - self.max_velocity = float(parts[5]) - self.max_acceleration = float(parts[6]) - - # Parse frame - self.frame = parts[7].upper() - if self.frame not in ['WRF', 'TRF']: - return False, f"Invalid frame: {self.frame}" - - # Parse optional trajectory type - idx = 8 - if idx < len(parts): - self.trajectory_type = parts[idx].lower() - if self.trajectory_type not in ['cubic', 'quintic', 's_curve']: - self.trajectory_type = 'quintic' - idx += 1 - - # Parse optional duration - if idx < len(parts): - self.duration = float(parts[idx]) - - # Initialize description - self.description = f"waypoints ({len(self.waypoints)} points, {self.frame}, {self.blend_mode})" - - return True, None - - except (ValueError, IndexError) as e: - return False, f"Invalid SMOOTH_WAYPOINTS parameters: {e}" - - def do_setup(self, state: 'ControllerState') -> None: - """Transform waypoints if in TRF.""" - - if self.frame == 'TRF': - # Transform all waypoints to WRF - transformed_waypoints = [] - current_q = PAROL6_ROBOT.ops.steps_to_rad(state.Position_in) - tool_pose = PAROL6_ROBOT.robot.fkine(current_q) - - for wp in self.waypoints: - transformed_wp = pose6_trf_to_wrf(wp, tool_pose) - transformed_waypoints.append(transformed_wp) - - self.waypoints = transformed_waypoints - logger.info(f" -> TRF Waypoints: transformed {len(self.waypoints)} points to WRF") - - # Basic validation - if len(self.waypoints) < 2: - self.fail("At least 2 waypoints required") - return - - return super().do_setup(state) - - def generate_main_trajectory(self, effective_start_pose): - """Generate waypoint trajectory with corner cutting.""" - - # Ensure first waypoint matches effective start pose - first_wp_error = np.linalg.norm( - np.array(self.waypoints[0][:3]) - np.array(effective_start_pose[:3]) - ) - - if first_wp_error > 10.0: - # Prepend effective start pose as first waypoint - full_waypoints = [effective_start_pose] + self.waypoints - if self.blend_radii != 'auto' and isinstance(self.blend_radii, list): - # Prepend 0 blend radius for start - full_blend_radii = [0] + self.blend_radii - else: - full_blend_radii = self.blend_radii - full_via_modes = ['via'] + self.via_modes - else: - # Replace first waypoint with effective start pose - full_waypoints = [effective_start_pose] + self.waypoints[1:] - full_blend_radii = self.blend_radii - full_via_modes = self.via_modes - - # Set up constraints - constraints = { - 'max_velocity': self.max_velocity, - 'max_acceleration': self.max_acceleration, - 'max_jerk': 5000.0 # Default jerk limit - } - - # Create planner - planner = WaypointTrajectoryPlanner( - full_waypoints, - constraints=constraints, - sample_rate=100.0 - ) - - # Determine blend mode for planner - if self.blend_mode == 'none': - planner_blend_mode = 'none' - elif self.blend_radii == 'auto': - planner_blend_mode = 'auto' - else: - planner_blend_mode = 'manual' - - # Generate trajectory with direct profile support - if planner_blend_mode == 'manual' and isinstance(full_blend_radii, list): - opt_radii = [float(r) for r in full_blend_radii] - else: - opt_radii = None - trajectory = planner.plan_trajectory( - blend_mode=planner_blend_mode, - blend_radii=opt_radii, - via_modes=full_via_modes, - trajectory_type=self.trajectory_type, - jerk_limit=constraints['max_jerk'] if self.trajectory_type == 's_curve' else None - ) - - # Apply duration scaling if specified - if self.duration and self.duration > 0: - current_duration = len(trajectory) / 100.0 - if current_duration > 0: - scale_factor = self.duration / current_duration - if scale_factor != 1.0: - # Resample trajectory for desired duration - new_length = int(self.duration * 100) - old_indices = np.linspace(0, len(trajectory) - 1, new_length) - resampled = [] - for idx in old_indices: - if idx < len(trajectory) - 1: - i = int(idx) - alpha = idx - i - pose = trajectory[i] * (1 - alpha) + trajectory[i + 1] * alpha - else: - pose = trajectory[-1] - resampled.append(pose) - trajectory = np.array(resampled) - - logger.info(f" Generated waypoint trajectory with {len(trajectory)} points") - return trajectory +""" +Smooth Motion Commands +Contains all smooth trajectory generation commands for advanced robot movements +""" + +import json +import logging +from collections.abc import Sequence +from typing import TYPE_CHECKING, Any, Optional, cast + +import numpy as np +from numpy.typing import NDArray +from spatialmath import SE3 + +import parol6.PAROL6_ROBOT as PAROL6_ROBOT +from parol6.commands.base import ExecutionStatus, ExecutionStatusCode, MotionCommand +from parol6.commands.cartesian_commands import MovePoseCommand +from parol6.config import ENTRY_MM_TOL_MM, INTERVAL_S, NEAR_MM_TOL_MM +from parol6.protocol.wire import CommandCode +from parol6.server.command_registry import register_command +from parol6.server.state import get_fkine_se3 +from parol6.smooth_motion import ( + CircularMotion, + HelixMotion, + SplineMotion, + WaypointTrajectoryPlanner, +) +from parol6.smooth_motion.advanced import AdvancedMotionBlender +from parol6.utils.errors import IKError +from parol6.utils.frames import ( + pose6_trf_to_wrf, + transform_command_params_to_wrf, + transform_start_pose_if_needed, +) +from parol6.utils.ik import solve_ik + +if TYPE_CHECKING: + from parol6.server.state import ControllerState + +logger = logging.getLogger(__name__) + + +class BaseSmoothMotionCommand(MotionCommand): + """ + Base class for all smooth motion commands with proper error tracking. + """ + + __slots__ = ( + "description", + "trajectory", + "trajectory_command", + "transition_command", + "specified_start_pose", + "transition_complete", + "trajectory_prepared", + "trajectory_generated", + ) + + def __init__(self, description="smooth motion"): + super().__init__() + self.description = description + self.trajectory: np.ndarray | None = None + self.trajectory_command: SmoothTrajectoryCommand | None = None + self.transition_command: MovePoseCommand | None = None + self.specified_start_pose: NDArray[np.floating] | None = None + self.transition_complete = False + self.trajectory_prepared = False + self.trajectory_generated = False + + # Parsing utility methods + @staticmethod + def parse_start_pose(start_str: str) -> NDArray[np.floating] | None: + """ + Parse start pose from string. + + Args: + start_str: Either 'CURRENT', 'NONE', or comma-separated pose values + + Returns: + None for CURRENT/NONE, or numpy array of floats for specified pose + """ + if start_str.upper() in ("CURRENT", "NONE"): + return None + else: + try: + return np.asarray( + list(map(float, start_str.split(","))), dtype=np.float64 + ) + except Exception: + raise ValueError(f"Invalid start pose format: {start_str}") + + @staticmethod + def parse_timing( + timing_type: str, timing_value: float, path_length: float + ) -> float: + """ + Convert timing specification to duration. + + Args: + timing_type: 'DURATION' or 'SPEED' + timing_value: Duration in seconds or speed in mm/s + path_length: Estimated path length in mm + + Returns: + Duration in seconds + """ + if timing_type.upper() == "DURATION": + return timing_value + elif timing_type.upper() == "SPEED": + if timing_value <= 0: + raise ValueError(f"Speed must be positive, got {timing_value}") + return path_length / timing_value + else: + raise ValueError(f"Unknown timing type: {timing_type}") + + @staticmethod + def calculate_path_length(command_type: str, params: dict) -> float: + """ + Estimate trajectory path length for timing calculations. + + Args: + command_type: Type of smooth motion command + params: Command parameters + + Returns: + Estimated path length in mm + """ + if command_type == "SMOOTH_CIRCLE": + # Full circle circumference + return 2 * np.pi * params.get("radius", 100) + elif command_type in ["SMOOTH_ARC_CENTER", "SMOOTH_ARC_PARAM"]: + # Estimate arc length (approximate) + radius = params.get("radius", 100) + angle = params.get("arc_angle", 90) # degrees + return radius * np.radians(angle) + elif command_type == "SMOOTH_HELIX": + # Helix path length + radius = params.get("radius", 100) + height = params.get("height", 100) + turns = ( + height / params.get("pitch", 10) if params.get("pitch", 10) > 0 else 1 + ) + return np.sqrt((2 * np.pi * radius * turns) ** 2 + height**2) + else: + # Default estimate + return 300 # mm + + @staticmethod + def parse_trajectory_type( + parts: list[str], index: int + ) -> tuple[str, float | None, int]: + """ + Parse trajectory type and optional jerk limit. + + Args: + parts: Command parts + index: Current index in parts + + Returns: + Tuple of (trajectory_type, jerk_limit, next_index) + """ + if index >= len(parts): + return "cubic", None, index + + traj_type = parts[index].lower() + index += 1 + + if traj_type not in ["cubic", "quintic", "s_curve"]: + # Not a valid trajectory type, use default + return "cubic", None, index - 1 + + # Check for jerk limit if s_curve + jerk_limit = None + if traj_type == "s_curve" and index < len(parts): + try: + jerk_limit = float(parts[index]) + index += 1 + except (ValueError, IndexError): + jerk_limit = 1000 # Default jerk limit + + return traj_type, jerk_limit, index + + def create_transition_command( + self, current_pose: np.ndarray, target_pose: NDArray[np.floating] + ) -> Optional["MovePoseCommand"]: + """Create a MovePose command for smooth transition to start position.""" + pos_error = np.linalg.norm(target_pose[:3] - current_pose[:3]) + + if pos_error < NEAR_MM_TOL_MM: # proximity threshold + self.log_info(" -> Already near start position (error: %.1fmm)", pos_error) + return None + + self.log_info( + " -> Creating smooth transition to start (%.1fmm away)", pos_error + ) + + # Calculate transition speed based on distance + if pos_error < 10: + transition_speed = 20.0 # mm/s for short distances + elif pos_error < 30: + transition_speed = 30.0 # mm/s for medium distances + else: + transition_speed = 40.0 # mm/s for long distances + + transition_duration = max(pos_error / transition_speed, 0.5) # Minimum 0.5s + + # MovePoseCommand expects a list, so convert array to list here + transition_cmd: MovePoseCommand = MovePoseCommand( + target_pose.tolist(), transition_duration + ) + + return transition_cmd + + def get_current_pose_from_position(self, position_in): + """Convert current position to pose [x,y,z,rx,ry,rz]""" + current_pose_se3 = get_fkine_se3() + + current_xyz = current_pose_se3.t * 1000 # Convert to mm + current_rpy = current_pose_se3.rpy(unit="deg", order="xyz") + return np.concatenate([current_xyz, current_rpy]) + + def do_setup(self, state: "ControllerState") -> None: + """Minimal preparation - just check if we need a transition.""" + self.log_debug(" -> Preparing %s...", self.description) + + # If there's a specified start pose, prepare transition + if self.specified_start_pose is not None: + actual_current_pose = self.get_current_pose_from_position(state.Position_in) + self.transition_command = self.create_transition_command( + actual_current_pose, self.specified_start_pose + ) + + if self.transition_command: + self.transition_command.setup(state) + if not self.transition_command.is_valid: + self.log_error(" -> ERROR: Cannot reach specified start position") + self.fail("Cannot reach specified start position") + return + else: + self.transition_command = None + + # DON'T generate trajectory yet - wait until execution + self.trajectory_generated = False + self.trajectory_prepared = False + self.log_debug( + " -> %s preparation complete (trajectory will be generated at execution)", + self.description, + ) + + def generate_main_trajectory(self, effective_start_pose): + """Override this in subclasses to generate the specific motion trajectory.""" + raise NotImplementedError("Subclasses must implement generate_main_trajectory") + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + """Execute transition first if needed, then generate and execute trajectory.""" + if self.is_finished or not self.is_valid: + return ( + ExecutionStatus.completed("Already finished") + if self.is_finished + else ExecutionStatus.failed("Invalid smooth command") + ) + + # Execute transition first if needed + if self.transition_command and not self.transition_complete: + status = self.transition_command.execute_step(state) + if status.code == ExecutionStatusCode.COMPLETED: + self.log_info(" -> Transition complete") + self.transition_complete = True + # Continue to main trajectory generation next tick + return ExecutionStatus.executing("Transition completed") + elif status.code == ExecutionStatusCode.FAILED: + self.fail( + getattr( + self.transition_command, "error_message", "Transition failed" + ) + ) + self.finish() + self.stop_and_idle(state) + return ExecutionStatus.failed("Transition failed") + else: + return ExecutionStatus.executing("Transitioning") + + # Generate trajectory on first execution step (not during preparation!) + if not self.trajectory_generated: + # Get ACTUAL current position NOW + actual_current_pose = self.get_current_pose_from_position(state.Position_in) + self.log_info( + " -> Generating %s from ACTUAL position: %s", + self.description, + [round(p, 1) for p in actual_current_pose[:3]], + ) + + # Generate trajectory from where we ACTUALLY are + self.trajectory = self.generate_main_trajectory(actual_current_pose) + if self.trajectory is None: + raise RuntimeError("Smooth trajectory generator returned None") + self.trajectory_command = SmoothTrajectoryCommand( + self.trajectory, self.description + ) + + # Quick validation of first point only + current_q = np.asarray( + PAROL6_ROBOT.ops.steps_to_rad(state.Position_in), dtype=float + ) + first_pose = self.trajectory[0] + target_se3 = SE3( + first_pose[0] / 1000, first_pose[1] / 1000, first_pose[2] / 1000 + ) * SE3.RPY( + [float(first_pose[3]), float(first_pose[4]), float(first_pose[5])], + unit="deg", + order="xyz", + ) + + ik_result = solve_ik( + cast(Any, PAROL6_ROBOT.robot), target_se3, current_q, jogging=False + ) + + if not ik_result.success: + self.log_error(" -> ERROR: Cannot reach first trajectory point") + self.finish() + self.fail("Cannot reach trajectory start") + self.stop_and_idle(state) + return ExecutionStatus.failed( + "Cannot reach trajectory start", + error=IKError("Cannot reach trajectory start"), + ) + + self.trajectory_generated = True + self.trajectory_prepared = True + + # Verify first point is close to current + distance = np.linalg.norm( + first_pose[:3] - np.array(actual_current_pose[:3]) + ) + if distance > 5.0: + self.log_warning( + " -> WARNING: First trajectory point %.1fmm from current!", + distance, + ) + + # Execute main trajectory + if self.trajectory_command and self.trajectory_prepared: + status = self.trajectory_command.execute_step(state) + + # Check for errors in trajectory execution + if ( + hasattr(self.trajectory_command, "error_state") + and self.trajectory_command.error_state + ): + self.fail(self.trajectory_command.error_message) + + if status.code == ExecutionStatusCode.COMPLETED: + self.finish() + return ExecutionStatus.completed(f"Smooth {self.description} complete") + elif status.code == ExecutionStatusCode.FAILED: + self.finish() + return status + else: + return ExecutionStatus.executing(f"Smooth {self.description}") + + self.finish() + return ExecutionStatus.completed(f"Smooth {self.description} complete") + + def _generate_radial_entry( + self, + start_pose: Sequence[float], + center: Sequence[float], + normal: Sequence[float], + radius: float, + duration: float, + sample_rate: float = 100.0, + ) -> np.ndarray: + """ + Generate a simple radial entry trajectory from the current pose to the circle/helix perimeter. + Produces a straight-line interpolation in Cartesian space. + """ + start_pose_arr = np.array(start_pose, dtype=float) + center_arr = np.array(center, dtype=float) + normal_arr = np.array(normal, dtype=float) + if np.linalg.norm(normal_arr) > 0: + normal_arr = normal_arr / np.linalg.norm(normal_arr) + + start_pos = start_pose_arr[:3] + to_start = start_pos - center_arr + # Project onto plane + to_plane = to_start - np.dot(to_start, normal_arr) * normal_arr + dist = float(np.linalg.norm(to_plane)) + + if dist < 1e-6: + # Choose arbitrary direction perpendicular to normal + axis = np.array([1.0, 0.0, 0.0]) + if abs(np.dot(axis, normal_arr)) > 0.9: + axis = np.array([0.0, 1.0, 0.0]) + direction = np.cross(normal_arr, axis) + direction = direction / np.linalg.norm(direction) + else: + direction = to_plane / dist + + target_pos = center_arr + direction * float(radius) + # Keep orientation constant + target_orient = start_pose_arr[3:] + + # Number of samples + n = max(2, int(max(0.05, float(duration)) * float(sample_rate))) + ts = np.linspace(0.0, 1.0, n) + traj = [] + for s in ts: + pos = (1.0 - s) * start_pos + s * target_pos + traj.append(np.concatenate([pos, target_orient])) + return np.array(traj) + + +class SmoothTrajectoryCommand: + """Command class for executing pre-generated smooth trajectories.""" + + def __init__(self, trajectory, description="smooth motion"): + self.trajectory = np.array(trajectory) + self.description = description + self.trajectory_index = 0 + self.is_valid = True + self.is_finished = False + self.first_step = True + self.error_state = False + self.error_message = "" + + logger.debug(f"Initializing smooth {description} with {len(trajectory)} points") + + def setup(self, state: "ControllerState") -> None: + """Skip validation - trajectory is already generated from correct position""" + self.is_valid = True + return + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + """Execute one step of the smooth trajectory""" + if self.is_finished or not self.is_valid: + return ( + ExecutionStatus.completed("Already finished") + if self.is_finished + else ExecutionStatus.failed("Invalid smooth trajectory") + ) + + if self.trajectory_index >= len(self.trajectory): + logger.info(f"Smooth {self.description} finished.") + self.is_finished = True + state.Command_out = CommandCode.IDLE + state.Speed_out.fill(0) + return ExecutionStatus.completed(f"Smooth {self.description} complete") + + # Get target pose for this step + target_pose = self.trajectory[self.trajectory_index] + + # Convert to SE3 + target_se3 = SE3( + target_pose[0] / 1000, target_pose[1] / 1000, target_pose[2] / 1000 + ) * SE3.RPY( + [float(target_pose[3]), float(target_pose[4]), float(target_pose[5])], + unit="deg", + order="xyz", + ) + + # Get current joint configuration + current_q = np.asarray( + PAROL6_ROBOT.ops.steps_to_rad(state.Position_in), dtype=float + ) + + # Solve IK + ik_result = solve_ik( + cast(Any, PAROL6_ROBOT.robot), target_se3, current_q, jogging=False + ) + + if not ik_result.success: + logger.error(f" -> IK failed at trajectory point {self.trajectory_index}") + self.is_finished = True + self.error_state = True + self.error_message = ( + f"IK failed at point {self.trajectory_index}/{len(self.trajectory)}" + ) + state.Command_out = CommandCode.IDLE + state.Speed_out.fill(0) + return ExecutionStatus.failed( + self.error_message, error=IKError(self.error_message) + ) + + # Convert to steps + target_steps = PAROL6_ROBOT.ops.rad_to_steps(ik_result.q) + + # ADD VELOCITY LIMITING - This prevents violent movements + if self.trajectory_index > 0: + # Vectorized per-tick clamping with 1.2x safety margin + target_steps = PAROL6_ROBOT.ops.clamp_steps_delta( + state.Position_in, + np.asarray(target_steps, dtype=np.int32), + INTERVAL_S, + 1.2, + ) + + # Send position command (inline to avoid instance-method binding) + np.copyto( + state.Position_out, np.asarray(target_steps, dtype=np.int32), casting="no" + ) + state.Speed_out.fill(0) + state.Command_out = CommandCode.MOVE + + # Advance to next point + self.trajectory_index += 1 + + return ExecutionStatus.executing(f"Smooth {self.description}") + + +@register_command("SMOOTH_CIRCLE") +class SmoothCircleCommand(BaseSmoothMotionCommand): + """Execute smooth circular motion.""" + + __slots__ = ( + "center", + "radius", + "plane", + "duration", + "clockwise", + "frame", + "trajectory_type", + "jerk_limit", + "center_mode", + "entry_mode", + "normal_vector", + "current_position_in", + ) + + def __init__(self) -> None: + super().__init__(description="smooth circle") + self.center: NDArray[np.floating] | None = None + self.radius: float = 100.0 + self.plane: str = "XY" + self.duration: float = 5.0 + self.clockwise: bool = False + self.frame: str = "WRF" + self.trajectory_type: str = "cubic" + self.jerk_limit: float | None = None + self.center_mode: str = "ABSOLUTE" + self.entry_mode: str = "NONE" + self.normal_vector: NDArray | None = None + self.current_position_in: NDArray[np.int32] | None = None + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse SMOOTH_CIRCLE command. + Format: SMOOTH_CIRCLE|center_x,y,z|radius|plane|frame|start_pose|timing_type|timing_value|[trajectory_type]|[jerk_limit]|[clockwise] + """ + if parts[0].upper() != "SMOOTH_CIRCLE": + return False, None + + if len(parts) < 8: + return False, "SMOOTH_CIRCLE requires at least 8 parameters" + + try: + # Parse center + center_list = list(map(float, parts[1].split(","))) + if len(center_list) != 3: + return False, "Center must have 3 coordinates" + self.center = np.asarray(center_list, dtype=np.float64) + + # Parse radius + self.radius = float(parts[2]) + + # Parse plane + self.plane = parts[3].upper() + if self.plane not in ["XY", "XZ", "YZ"]: + return False, f"Invalid plane: {self.plane}" + + # Parse frame + self.frame = parts[4].upper() + if self.frame not in ["WRF", "TRF"]: + return False, f"Invalid frame: {self.frame}" + + # Parse start pose + self.specified_start_pose = self.parse_start_pose(parts[5]) + + # Parse timing + timing_type = parts[6].upper() + timing_value = float(parts[7]) + path_length = 2 * np.pi * self.radius + self.duration = self.parse_timing(timing_type, timing_value, path_length) + + # Parse optional trajectory type and jerk limit + idx = 8 + if idx < len(parts): + self.trajectory_type, self.jerk_limit, idx = self.parse_trajectory_type( + parts, idx + ) + + # Parse optional clockwise flag + if idx < len(parts) and parts[idx].upper() in ["CW", "CLOCKWISE", "TRUE"]: + self.clockwise = True + idx += 1 + + # Parse optional center mode + if idx < len(parts): + self.center_mode = parts[idx].upper() + if self.center_mode not in ["ABSOLUTE", "TOOL", "RELATIVE"]: + self.center_mode = "ABSOLUTE" + idx += 1 + + # Parse optional entry mode + if idx < len(parts): + self.entry_mode = parts[idx].upper() + if self.entry_mode not in ["AUTO", "TANGENT", "DIRECT", "NONE"]: + self.entry_mode = "NONE" + + # Initialize description + self.description = ( + f"circle (r={self.radius}mm, {self.frame}, {self.trajectory_type})" + ) + + return True, None + + except (ValueError, IndexError) as e: + return False, f"Invalid SMOOTH_CIRCLE parameters: {e}" + + def do_setup(self, state: "ControllerState") -> None: + """Transform parameters if in TRF, then prepare normally.""" + + # Store current position for potential use in generate_main_trajectory + self.current_position_in = np.asarray(state.Position_in, dtype=np.int32) + + if self.frame == "TRF": + # Transform parameters to WRF + params = {"center": self.center, "plane": self.plane} + transformed = transform_command_params_to_wrf( + "SMOOTH_CIRCLE", params, "TRF", state.Position_in + ) + + # Update with transformed values + self.center = transformed["center"] + self.normal_vector = transformed.get("normal_vector") + + if self.center is not None: + logger.info( + f" -> TRF Circle: center {self.center[:3].tolist()} (WRF), normal {self.normal_vector}" + ) + + # Transform start_pose if specified - convert array to list for the API + if self.specified_start_pose is not None: + result = transform_start_pose_if_needed( + self.specified_start_pose.tolist(), self.frame, state.Position_in + ) + if result is not None: + self.specified_start_pose = np.asarray(result, dtype=np.float64) + + # Now do normal preparation with transformed parameters + return super().do_setup(state) + + def generate_main_trajectory(self, effective_start_pose): + """Generate circle starting from the actual start position.""" + motion_gen = CircularMotion() + + # Use transformed normal for TRF, or standard for WRF + if self.normal_vector is not None: + # TRF - use the transformed normal vector + normal = np.array(self.normal_vector) + logger.info(f" Using transformed normal: {normal.round(3)}") + else: + # WRF - use standard plane definition + plane_normals = {"XY": [0, 0, 1], "XZ": [0, 1, 0], "YZ": [1, 0, 0]} + normal = np.array(plane_normals.get(self.plane, [0, 0, 1])) + logger.info(f" Using WRF plane {self.plane} with normal: {normal}") + + logger.info( + f" Generating circle from position: {[round(p, 1) for p in effective_start_pose[:3]]}" + ) + if self.center is not None: + logger.info(f" Circle center: {[round(c, 1) for c in self.center]}") + + # Handle center_mode + if self.center is not None: + actual_center = self.center.copy() + else: + actual_center = np.array([0.0, 0.0, 0.0]) + if self.center_mode == "TOOL": + # Center at current tool position + actual_center = np.array(effective_start_pose[:3]) + logger.info( + f" Center mode: TOOL - centering at current position {actual_center}" + ) + elif self.center_mode == "RELATIVE": + # Center relative to current position + center_np = ( + np.asarray(self.center, dtype=float) + if self.center is not None + else np.zeros(3) + ) + actual_center = np.array( + [effective_start_pose[i] + center_np[i] for i in range(3)] + ) + logger.info( + f" Center mode: RELATIVE - center offset from current position to {actual_center}" + ) + else: + # ABSOLUTE mode uses provided center as-is + actual_center = np.array(actual_center) + + # Check if entry trajectory might be needed + distance_to_center = np.linalg.norm( + np.array(effective_start_pose[:3]) - np.array(actual_center) + ) + distance_from_perimeter = float(abs(distance_to_center - self.radius)) + + # Automatically generate entry trajectory if needed + entry_trajectory = None + if distance_from_perimeter > ENTRY_MM_TOL_MM: # perimeter tolerance + effective_entry_mode = self.entry_mode + + # Auto-detect need for entry if not specified + if ( + self.entry_mode == "NONE" and distance_from_perimeter > 5.0 + ): # Auto-enable for > 5mm + logger.warning( + f" Robot is {distance_from_perimeter:.1f}mm from circle perimeter - auto-enabling entry trajectory" + ) + effective_entry_mode = "AUTO" + + if effective_entry_mode != "NONE": + logger.info( + f" Generating {effective_entry_mode} entry trajectory (distance: {distance_from_perimeter:.1f}mm)" + ) + + # Calculate entry duration based on distance (0.5s min, 2.0s max) + entry_duration = float( + min(2.0, max(0.5, float(distance_from_perimeter) / 50.0)) + ) + + # Generate entry trajectory (radial approach) + entry_trajectory = self._generate_radial_entry( + effective_start_pose.tolist() + if hasattr(effective_start_pose, "tolist") + else list(effective_start_pose), + actual_center.tolist() + if hasattr(actual_center, "tolist") + else list(actual_center), + normal.tolist() if hasattr(normal, "tolist") else list(normal), + float(self.radius), + entry_duration, + ) + + if entry_trajectory is not None and len(entry_trajectory) > 0: + logger.info( + f" Entry trajectory generated: {len(entry_trajectory)} points over {entry_duration:.1f}s" + ) + + # Generate circle with specified trajectory profile + trajectory = motion_gen.generate_circle_with_profile( + center=actual_center, # Use adjusted center + radius=self.radius, + normal=normal, # This normal now correctly represents the plane + start_point=effective_start_pose, # Pass full pose including orientation + duration=self.duration, + trajectory_type=self.trajectory_type, + jerk_limit=self.jerk_limit, + ) + + if self.clockwise: + trajectory = trajectory[::-1] + + # Update orientations to match start pose + for i in range(len(trajectory)): + trajectory[i][3:] = effective_start_pose[3:] + + # Concatenate entry trajectory if it exists + if entry_trajectory is not None and len(entry_trajectory) > 0: + full_trajectory = np.concatenate([entry_trajectory, trajectory]) + return full_trajectory + else: + return trajectory + + +@register_command("SMOOTH_ARC_CENTER") +class SmoothArcCenterCommand(BaseSmoothMotionCommand): + """Execute smooth arc motion defined by center point.""" + + __slots__ = ( + "end_pose", + "center", + "duration", + "clockwise", + "frame", + "trajectory_type", + "jerk_limit", + "normal_vector", + ) + + def __init__(self) -> None: + super().__init__(description="smooth arc (center)") + self.end_pose: list[float] | None = None + self.center: list[float] | None = None + self.duration: float = 5.0 + self.clockwise: bool = False + self.frame: str = "WRF" + self.trajectory_type: str = "cubic" + self.jerk_limit: float | None = None + self.normal_vector: NDArray | None = None + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse SMOOTH_ARC_CENTER command. + Format: SMOOTH_ARC_CENTER|end_x,y,z,rx,ry,rz|center_x,y,z|frame|start_pose|timing_type|timing_value|[trajectory_type]|[jerk_limit]|[clockwise] + """ + if parts[0].upper() != "SMOOTH_ARC_CENTER": + return False, None + + if len(parts) < 7: + return False, "SMOOTH_ARC_CENTER requires at least 7 parameters" + + try: + # Parse end pose + self.end_pose = list(map(float, parts[1].split(","))) + if len(self.end_pose) != 6: + return False, "End pose must have 6 values (x,y,z,rx,ry,rz)" + + # Parse center + self.center = list(map(float, parts[2].split(","))) + if len(self.center) != 3: + return False, "Center must have 3 coordinates" + + # Parse frame + self.frame = parts[3].upper() + if self.frame not in ["WRF", "TRF"]: + return False, f"Invalid frame: {self.frame}" + + # Parse start pose + self.specified_start_pose = self.parse_start_pose(parts[4]) + + # Parse timing + timing_type = parts[5].upper() + timing_value = float(parts[6]) + # Estimate arc length + path_length = 300 # Default estimate, could be improved + self.duration = self.parse_timing(timing_type, timing_value, path_length) + + # Parse optional trajectory type and jerk limit + idx = 7 + if idx < len(parts): + self.trajectory_type, self.jerk_limit, idx = self.parse_trajectory_type( + parts, idx + ) + + # Parse optional clockwise flag + if idx < len(parts) and parts[idx].upper() in ["CW", "CLOCKWISE", "TRUE"]: + self.clockwise = True + + # Initialize description + self.description = ( + f"arc (center-based, {self.frame}, {self.trajectory_type})" + ) + + return True, None + + except (ValueError, IndexError) as e: + return False, f"Invalid SMOOTH_ARC_CENTER parameters: {e}" + + def do_setup(self, state: "ControllerState") -> None: + """Transform parameters if in TRF.""" + + if self.frame == "TRF": + params = {"end_pose": self.end_pose, "center": self.center} + transformed = transform_command_params_to_wrf( + "SMOOTH_ARC_CENTER", params, "TRF", state.Position_in + ) + self.end_pose = transformed["end_pose"] + self.center = transformed["center"] + self.normal_vector = transformed.get("normal_vector") + + # Transform start_pose if specified + _sp = transform_start_pose_if_needed( + self.specified_start_pose.tolist() + if self.specified_start_pose is not None + else None, + self.frame, + state.Position_in, + ) + self.specified_start_pose = ( + np.asarray(_sp, dtype=np.float64) if _sp is not None else None + ) + + return super().do_setup(state) + + def generate_main_trajectory(self, effective_start_pose): + """Generate arc from actual start to end with direct velocity profile.""" + motion_gen = CircularMotion() + + # Ensure required parameters are present for typing + assert self.end_pose is not None + assert self.center is not None + + # Use new direct profile generation method + trajectory = motion_gen.generate_arc_with_profile( + effective_start_pose, + self.end_pose, + self.center, + normal=self.normal_vector, # Use transformed normal if TRF + clockwise=self.clockwise, + duration=self.duration, + trajectory_type=self.trajectory_type, + jerk_limit=self.jerk_limit, + ) + + return trajectory + + +@register_command("SMOOTH_ARC_PARAM") +class SmoothArcParamCommand(BaseSmoothMotionCommand): + """Execute smooth arc motion defined by radius and angle.""" + + __slots__ = ( + "end_pose", + "radius", + "arc_angle", + "duration", + "clockwise", + "frame", + "trajectory_type", + "jerk_limit", + "normal_vector", + "current_position_in", + ) + + def __init__(self) -> None: + super().__init__(description="smooth arc (param)") + self.end_pose: list[float] | None = None + self.radius: float = 100.0 + self.arc_angle: float = 90.0 + self.duration: float = 5.0 + self.clockwise: bool = False + self.frame: str = "WRF" + self.trajectory_type: str = "cubic" + self.jerk_limit: float | None = None + self.normal_vector: NDArray | None = None + self.current_position_in: NDArray[np.int32] | None = None + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse SMOOTH_ARC_PARAM command. + Format: SMOOTH_ARC_PARAM|end_x,y,z,rx,ry,rz|radius|arc_angle|frame|start_pose|timing_type|timing_value|[trajectory_type]|[jerk_limit]|[clockwise] + """ + if parts[0].upper() != "SMOOTH_ARC_PARAM": + return False, None + + if len(parts) < 8: + return False, "SMOOTH_ARC_PARAM requires at least 8 parameters" + + try: + # Parse end pose + self.end_pose = list(map(float, parts[1].split(","))) + if len(self.end_pose) != 6: + return False, "End pose must have 6 values (x,y,z,rx,ry,rz)" + + # Parse radius and arc angle + self.radius = float(parts[2]) + self.arc_angle = float(parts[3]) + + # Parse frame + self.frame = parts[4].upper() + if self.frame not in ["WRF", "TRF"]: + return False, f"Invalid frame: {self.frame}" + + # Parse start pose + self.specified_start_pose = self.parse_start_pose(parts[5]) + + # Parse timing + timing_type = parts[6].upper() + timing_value = float(parts[7]) + path_length = self.radius * np.radians(self.arc_angle) + self.duration = self.parse_timing(timing_type, timing_value, path_length) + + # Parse optional trajectory type and jerk limit + idx = 8 + if idx < len(parts): + self.trajectory_type, self.jerk_limit, idx = self.parse_trajectory_type( + parts, idx + ) + + # Parse optional clockwise flag + if idx < len(parts) and parts[idx].upper() in ["CW", "CLOCKWISE", "TRUE"]: + self.clockwise = True + + # Initialize description + self.description = f"parametric arc (r={self.radius}mm, θ={self.arc_angle}°, {self.frame}, {self.trajectory_type})" + + return True, None + + except (ValueError, IndexError) as e: + return False, f"Invalid SMOOTH_ARC_PARAM parameters: {e}" + + def do_setup(self, state: "ControllerState") -> None: + """Transform parameters if in TRF, then prepare normally.""" + + self.current_position_in = state.Position_in + + if self.frame == "TRF": + # Transform parameters to WRF + params = { + "end_pose": self.end_pose, + "plane": "XY", # Default plane for parametric arc + } + transformed = transform_command_params_to_wrf( + "SMOOTH_ARC_PARAM", params, "TRF", state.Position_in + ) + + # Update with transformed values + self.end_pose = transformed["end_pose"] + self.normal_vector = transformed.get("normal_vector") + + if self.end_pose is not None: + logger.info(f" -> TRF Parametric Arc: end {self.end_pose[:3]} (WRF)") + else: + logger.info( + " -> TRF Parametric Arc: end pose unavailable after transform" + ) + + # Transform start_pose if specified + _sp = transform_start_pose_if_needed( + self.specified_start_pose.tolist() + if self.specified_start_pose is not None + else None, + self.frame, + state.Position_in, + ) + self.specified_start_pose = ( + np.asarray(_sp, dtype=np.float64) if _sp is not None else None + ) + + return super().do_setup(state) + + def generate_main_trajectory(self, effective_start_pose): + """Generate arc based on radius and angle from actual start.""" + assert self.end_pose is not None + # Get start and end positions + start_xyz = np.array(effective_start_pose[:3]) + end_xyz = np.array(self.end_pose[:3]) + + # If we have a transformed normal (TRF), use it to define the arc plane + if self.normal_vector is not None: + normal = np.array(self.normal_vector) + + # Find center of arc using radius and angle + chord_vec = end_xyz - start_xyz + chord_length = np.linalg.norm(chord_vec) + + if chord_length > 2 * self.radius: + logger.warning( + f" -> Warning: Points too far apart ({chord_length:.1f}mm) for radius {self.radius}mm" + ) + self.radius = float(chord_length) / 2 + 1 + + # Calculate center position using the normal vector + chord_mid = (start_xyz + end_xyz) / 2 + + # Height from chord midpoint to arc center + if chord_length > 0: + _hval = max(0.0, float(self.radius**2 - (chord_length / 2) ** 2)) + h = float(np.sqrt(_hval)) + + # Find perpendicular in the plane defined by normal + chord_dir = chord_vec / chord_length + perp_in_plane = np.cross(normal, chord_dir) + if np.linalg.norm(perp_in_plane) > 0.001: + perp_in_plane = perp_in_plane / np.linalg.norm(perp_in_plane) + else: + # Chord is parallel to normal (shouldn't happen) + perp_in_plane = np.array([1, 0, 0]) + + # Arc center + if self.clockwise: + center_3d = chord_mid - h * perp_in_plane + else: + center_3d = chord_mid + h * perp_in_plane + else: + center_3d = start_xyz + + else: + # WRF - use XY plane (standard behavior) + normal = np.array([0, 0, 1]) + + # Calculate arc center in XY plane + start_xy = start_xyz[:2] + end_xy = end_xyz[:2] + + # Midpoint + mid = (start_xy + end_xy) / 2 + + # Distance between points + d = np.linalg.norm(end_xy - start_xy) + + if d > 2 * self.radius: + logger.warning( + f" -> Warning: Points too far apart ({d:.1f}mm) for radius {self.radius}mm" + ) + self.radius = float(d) / 2 + 1 + + # Height of arc center from midpoint + _hval2 = max(0.0, float(self.radius**2 - (d / 2) ** 2)) + h = float(np.sqrt(_hval2)) + + # Perpendicular direction + if d > 0: + perp = np.array([-(end_xy[1] - start_xy[1]), end_xy[0] - start_xy[0]]) + perp = perp / np.linalg.norm(perp) + else: + perp = np.array([1, 0]) + + # Arc center (choose based on clockwise) + if self.clockwise: + center_2d = mid - h * perp + else: + center_2d = mid + h * perp + + # Use average Z for center + center_3d = [center_2d[0], center_2d[1], (start_xyz[2] + end_xyz[2]) / 2] + + # Generate arc trajectory with direct velocity profile + motion_gen = CircularMotion() + + # Use new direct profile generation method + trajectory = motion_gen.generate_arc_with_profile( + effective_start_pose, + self.end_pose, + center_3d, + normal=normal if self.normal_vector is not None else None, + clockwise=self.clockwise, + duration=self.duration, + trajectory_type=self.trajectory_type, + jerk_limit=self.jerk_limit, + ) + + return trajectory + + +@register_command("SMOOTH_HELIX") +class SmoothHelixCommand(BaseSmoothMotionCommand): + """Execute smooth helical motion.""" + + __slots__ = ( + "center", + "radius", + "pitch", + "height", + "duration", + "clockwise", + "frame", + "trajectory_type", + "jerk_limit", + "helix_axis", + "up_vector", + ) + + def __init__(self) -> None: + super().__init__(description="smooth helix") + self.center: list[float] | None = None + self.radius: float = 100.0 + self.pitch: float = 10.0 + self.height: float = 100.0 + self.duration: float = 5.0 + self.clockwise: bool = False + self.frame: str = "WRF" + self.trajectory_type: str = "cubic" + self.jerk_limit: float | None = None + self.helix_axis: NDArray | None = None + self.up_vector: NDArray | None = None + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse SMOOTH_HELIX command. + Format: SMOOTH_HELIX|center_x,y,z|radius|pitch|height|frame|start_pose|timing_type|timing_value|[trajectory_type]|[jerk_limit]|[clockwise] + """ + if parts[0].upper() != "SMOOTH_HELIX": + return False, None + + if len(parts) < 9: + return False, "SMOOTH_HELIX requires at least 9 parameters" + + try: + # Parse center + self.center = list(map(float, parts[1].split(","))) + if len(self.center) != 3: + return False, "Center must have 3 coordinates" + + # Parse radius, pitch, height + self.radius = float(parts[2]) + self.pitch = float(parts[3]) + self.height = float(parts[4]) + + # Parse frame + self.frame = parts[5].upper() + if self.frame not in ["WRF", "TRF"]: + return False, f"Invalid frame: {self.frame}" + + # Parse start pose + self.specified_start_pose = self.parse_start_pose(parts[6]) + + # Parse timing + timing_type = parts[7].upper() + timing_value = float(parts[8]) + turns = self.height / self.pitch if self.pitch > 0 else 1 + path_length = np.sqrt( + (2 * np.pi * self.radius * turns) ** 2 + self.height**2 + ) + self.duration = self.parse_timing(timing_type, timing_value, path_length) + + # Parse optional trajectory type and jerk limit + idx = 9 + if idx < len(parts): + self.trajectory_type, self.jerk_limit, idx = self.parse_trajectory_type( + parts, idx + ) + + # Parse optional clockwise flag + if idx < len(parts) and parts[idx].upper() in ["CW", "CLOCKWISE", "TRUE"]: + self.clockwise = True + + # Initialize description + self.description = ( + f"helix (h={self.height}mm, {self.frame}, {self.trajectory_type})" + ) + + return True, None + + except (ValueError, IndexError) as e: + return False, f"Invalid SMOOTH_HELIX parameters: {e}" + + def do_setup(self, state: "ControllerState") -> None: + """Transform parameters if in TRF.""" + + if self.frame == "TRF": + params = {"center": self.center} + transformed = transform_command_params_to_wrf( + "SMOOTH_HELIX", params, "TRF", state.Position_in + ) + self.center = transformed["center"] + self.helix_axis = transformed.get("helix_axis", [0, 0, 1]) + self.up_vector = transformed.get("up_vector", [0, 1, 0]) + + if self.specified_start_pose is not None: + params = {"start_pose": self.specified_start_pose.tolist()} + transformed = transform_command_params_to_wrf( + "SMOOTH_HELIX", params, "TRF", state.Position_in + ) + _sp = transformed.get("start_pose") + self.specified_start_pose = ( + np.asarray(_sp, dtype=np.float64) if _sp is not None else None + ) + + return super().do_setup(state) + + def generate_main_trajectory(self, effective_start_pose): + """Generate helix with entry trajectory if needed and proper trajectory profile.""" + helix_gen = HelixMotion() + + # Get helix axis (default Z for WRF, transformed for TRF) + if self.helix_axis is not None: + axis = self.helix_axis + else: + axis = np.array([0, 0, 1], dtype=float) # Default vertical + + # Calculate distance from start position to helix start point + start_pos = np.array(effective_start_pose[:3]) + center_np = np.array(self.center) + + # Project start position onto the helix plane (perpendicular to axis) + axis_np = np.array(axis) + axis_np = axis_np / np.linalg.norm(axis_np) + to_start = start_pos - center_np + to_start_plane = to_start - np.dot(to_start, axis_np) * axis_np + dist_to_center = np.linalg.norm(to_start_plane) + + # Check if entry trajectory is needed + entry_trajectory = None + distance_from_perimeter = abs(dist_to_center - self.radius) + + if ( + distance_from_perimeter > self.radius * 0.05 + ): # More than 5% off the perimeter + logger.info( + f" Generating helix entry trajectory (distance from perimeter: {distance_from_perimeter:.1f}mm)" + ) + + # Calculate entry duration based on distance (0.5s min, 2.0s max) + entry_duration = float( + min(2.0, max(0.5, float(distance_from_perimeter) / 50.0)) + ) + + # Generate entry trajectory to helix start position + CircularMotion() + + # Calculate the target position on the helix perimeter + if dist_to_center > 0.001: + direction = to_start_plane / dist_to_center + else: + # If exactly at center, move to any point on perimeter + u = ( + np.array([1, 0, 0]) + if abs(axis_np[0]) < 0.9 + else np.array([0, 1, 0]) + ) + u = u - np.dot(u, axis_np) * axis_np + direction = u / np.linalg.norm(u) + + target_on_perimeter = center_np + direction * self.radius + # For helix, we want to start at the correct Z level + target_on_perimeter[2] = start_pos[2] # Keep same Z as start + + # Generate smooth approach trajectory + entry_trajectory = self._generate_radial_entry( + effective_start_pose.tolist() + if hasattr(effective_start_pose, "tolist") + else list(effective_start_pose), + center_np.tolist(), + axis_np.tolist(), + float(self.radius), + entry_duration, + ) + + if entry_trajectory is not None and len(entry_trajectory) > 0: + logger.info( + f" Helix entry trajectory generated: {len(entry_trajectory)} points over {entry_duration:.1f}s" + ) + + # Generate main helix trajectory + trajectory = helix_gen.generate_helix_with_profile( + center=center_np, + radius=self.radius, + pitch=self.pitch, + height=self.height, + axis=axis_np, + duration=self.duration, + trajectory_type=self.trajectory_type, + jerk_limit=self.jerk_limit, + start_point=effective_start_pose, + clockwise=self.clockwise, + ) + + # Update orientations to match effective start + for i in range(len(trajectory)): + trajectory[i][3:] = effective_start_pose[3:] + + # Concatenate entry trajectory if it exists + if entry_trajectory is not None and len(entry_trajectory) > 0: + full_trajectory = np.concatenate([entry_trajectory, trajectory]) + return full_trajectory + else: + return np.array(trajectory) + + +@register_command("SMOOTH_SPLINE") +class SmoothSplineCommand(BaseSmoothMotionCommand): + """Execute smooth spline motion through waypoints.""" + + __slots__ = ( + "waypoints", + "duration", + "frame", + "trajectory_type", + "jerk_limit", + "current_position_in", + ) + + def __init__(self) -> None: + super().__init__(description="smooth spline") + self.waypoints: list[list[float]] | None = None + self.duration: float = 5.0 + self.frame: str = "WRF" + self.trajectory_type: str = "cubic" + self.jerk_limit: float | None = None + self.current_position_in: NDArray[np.int32] | None = None + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse SMOOTH_SPLINE command. + Format: SMOOTH_SPLINE|wp1_x,y,z,rx,ry,rz;wp2_x,y,z,rx,ry,rz;...|frame|start_pose|timing_type|timing_value|[trajectory_type]|[jerk_limit] + """ + if parts[0].upper() != "SMOOTH_SPLINE": + return False, None + + if len(parts) < 6: + return False, "SMOOTH_SPLINE requires at least 6 parameters" + + # Support alternate wire format: + # SMOOTH_SPLINE||||||[trajectory_type]|[jerk?]| + if len(parts) >= 7 and parts[1].isdigit(): + try: + num = int(parts[1]) + self.frame = parts[2].upper() + if self.frame not in ["WRF", "TRF"]: + return False, f"Invalid frame: {self.frame}" + self.specified_start_pose = self.parse_start_pose(parts[3]) + timing_type = parts[4].upper() + timing_value = float(parts[5]) + idx = 6 + if idx < len(parts) and parts[idx].lower() in [ + "cubic", + "quintic", + "s_curve", + ]: + self.trajectory_type = parts[idx].lower() + idx += 1 + if self.trajectory_type == "s_curve" and idx < len(parts): + self.jerk_limit = float(parts[idx]) + idx += 1 + needed = num * 6 + if len(parts) - idx < needed: + return False, "Insufficient waypoint values" + vals = list(map(float, parts[idx : idx + needed])) + self.waypoints = [vals[i : i + 6] for i in range(0, needed, 6)] + # Estimate path length + path_length = 0.0 + for i in range(1, len(self.waypoints)): + path_length += float( + np.linalg.norm( + np.array(self.waypoints[i][:3]) + - np.array(self.waypoints[i - 1][:3]) + ) + ) + self.duration = self.parse_timing( + timing_type, timing_value, path_length + ) + self.description = f"spline ({len(self.waypoints or [])} points, {self.frame}, {self.trajectory_type})" + return True, None + except Exception as e: + return False, f"Invalid SMOOTH_SPLINE parameters: {e}" + + try: + # Parse waypoints (semicolon separated) + waypoint_strs = parts[1].split(";") + self.waypoints = [] + for wp_str in waypoint_strs: + wp = list(map(float, wp_str.split(","))) + if len(wp) != 6: + return False, "Each waypoint must have 6 values (x,y,z,rx,ry,rz)" + self.waypoints.append(wp) + + if len(self.waypoints) < 2: + return False, "SMOOTH_SPLINE requires at least 2 waypoints" + + # Parse frame + self.frame = parts[2].upper() + if self.frame not in ["WRF", "TRF"]: + return False, f"Invalid frame: {self.frame}" + + # Parse start pose + self.specified_start_pose = self.parse_start_pose(parts[3]) + + # Parse timing + timing_type = parts[4].upper() + timing_value = float(parts[5]) + # Estimate path length from waypoints + path_length = 0.0 + for i in range(1, len(self.waypoints)): + path_length += float( + np.linalg.norm( + np.array(self.waypoints[i][:3]) + - np.array(self.waypoints[i - 1][:3]) + ) + ) + self.duration = self.parse_timing( + timing_type, timing_value, float(path_length) + ) + + # Parse optional trajectory type and jerk limit + idx = 6 + if idx < len(parts): + self.trajectory_type, self.jerk_limit, idx = self.parse_trajectory_type( + parts, idx + ) + + # Initialize description + self.description = f"spline ({len(self.waypoints or [])} points, {self.frame}, {self.trajectory_type})" + + return True, None + + except (ValueError, IndexError) as e: + return False, f"Invalid SMOOTH_SPLINE parameters: {e}" + + def do_setup(self, state: "ControllerState") -> None: + """Transform parameters if in TRF, then prepare normally.""" + + self.current_position_in = state.Position_in + + if self.frame == "TRF": + # Transform waypoints to WRF + params = {"waypoints": self.waypoints} + transformed = transform_command_params_to_wrf( + "SMOOTH_SPLINE", params, "TRF", state.Position_in + ) + + # Update with transformed values + self.waypoints = transformed["waypoints"] + + logger.info( + f" -> TRF Spline: transformed {len(self.waypoints or [])} waypoints to WRF" + ) + + # Transform start_pose if specified + _sp = transform_start_pose_if_needed( + self.specified_start_pose.tolist() + if self.specified_start_pose is not None + else None, + self.frame, + state.Position_in, + ) + self.specified_start_pose = ( + np.asarray(_sp, dtype=np.float64) if _sp is not None else None + ) + + return super().do_setup(state) + + def generate_main_trajectory(self, effective_start_pose): + """Generate spline starting from actual position.""" + assert self.waypoints is not None + wps = self.waypoints + motion_gen = SplineMotion() + + # Always start from the effective start pose + first_wp_error = np.linalg.norm( + np.array(wps[0][:3]) - np.array(effective_start_pose[:3]) + ) + + if first_wp_error > 5.0: + # First waypoint is far, prepend the start position + modified_waypoints = [effective_start_pose] + wps + logger.info( + f" Added start position as first waypoint (distance: {first_wp_error:.1f}mm)" + ) + else: + # Replace first waypoint with actual start to ensure continuity + modified_waypoints = [effective_start_pose] + wps[1:] + logger.info(" Replaced first waypoint with actual start position") + + timestamps = np.linspace(0, self.duration, len(modified_waypoints)) + + # Generate the spline trajectory based on type + if self.trajectory_type == "quintic": + trajectory = motion_gen.generate_quintic_spline( + modified_waypoints, timestamps=None + ) + elif self.trajectory_type == "s_curve": + trajectory = motion_gen.generate_scurve_spline( + modified_waypoints, + duration=self.duration, + jerk_limit=self.jerk_limit if self.jerk_limit else 1000, + ) + else: # cubic (default) + trajectory = motion_gen.generate_cubic_spline( + modified_waypoints, timestamps.tolist() + ) + + logger.debug(f" Generated spline with {len(trajectory)} points") + + return trajectory + + +@register_command("SMOOTH_BLEND") +class SmoothBlendCommand(BaseSmoothMotionCommand): + """Execute smooth blended trajectory through multiple segments.""" + + __slots__ = ( + "segment_definitions", + "blend_time", + "frame", + "trajectory_type", + "jerk_limit", + "current_position_in", + ) + + def __init__(self) -> None: + super().__init__(description="smooth blend") + self.segment_definitions: list[dict] | None = None + self.blend_time: float = 0.5 + self.frame: str = "WRF" + self.trajectory_type: str = "cubic" + self.jerk_limit: float | None = None + self.current_position_in: NDArray[np.int32] | None = None + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse SMOOTH_BLEND command. + Format: SMOOTH_BLEND|segments_json|blend_time|frame|start_pose|[trajectory_type]|[jerk_limit] + """ + if parts[0].upper() != "SMOOTH_BLEND": + return False, None + + if len(parts) < 5: + return False, "SMOOTH_BLEND requires at least 5 parameters" + + # New wire format: SMOOTH_BLEND|num_segments|blend_time|frame|start_pose|timing|SEG1||SEG2||... + if parts[1].isdigit(): + try: + int(parts[1]) + self.blend_time = float(parts[2]) + self.frame = parts[3].upper() + if self.frame not in ["WRF", "TRF"]: + return False, f"Invalid frame: {self.frame}" + self.specified_start_pose = self.parse_start_pose(parts[4]) + # parts[5] timing token (DEFAULT/DURATION/SPEED) not strictly needed for segments + # Reconstruct remainder and split by '||' to obtain segments + remainder = "|".join(parts[6:]) + raw_segments = [s for s in remainder.split("||") if s.strip() != ""] + seg_defs = [] + for seg_str in raw_segments: + tokens = [t for t in seg_str.split("|") if t != ""] + if not tokens: + continue + seg_type = tokens[0].upper() + if seg_type == "LINE": + if len(tokens) < 3: + return False, "LINE segment requires end pose and duration" + end = list(map(float, tokens[1].split(","))) + duration = float(tokens[2]) + seg_defs.append( + {"type": "LINE", "end": end, "duration": duration} + ) + elif seg_type == "CIRCLE": + if len(tokens) < 6: + return ( + False, + "CIRCLE segment requires center,radius,plane,duration,clockwise", + ) + center = list(map(float, tokens[1].split(","))) + radius = float(tokens[2]) + plane = tokens[3].upper() + duration = float(tokens[4]) + clockwise = tokens[5] in ( + "1", + "TRUE", + "True", + "true", + "CW", + "CLOCKWISE", + ) + seg_defs.append( + { + "type": "CIRCLE", + "center": center, + "radius": radius, + "plane": plane, + "duration": duration, + "clockwise": clockwise, + } + ) + elif seg_type == "ARC": + if len(tokens) < 5: + return ( + False, + "ARC segment requires end,center,duration,clockwise", + ) + end = list(map(float, tokens[1].split(","))) + center = list(map(float, tokens[2].split(","))) + duration = float(tokens[3]) + clockwise = tokens[4] in ( + "1", + "TRUE", + "True", + "true", + "CW", + "CLOCKWISE", + ) + seg_defs.append( + { + "type": "ARC", + "end": end, + "center": center, + "duration": duration, + "clockwise": clockwise, + } + ) + elif seg_type == "SPLINE": + if len(tokens) < 4: + return ( + False, + "SPLINE segment requires count,waypoints,duration", + ) + count = int(tokens[1]) + wp_tokens = tokens[2].split(";") + if len(wp_tokens) != count: + return False, "SPLINE waypoint count mismatch" + waypoints = [ + list(map(float, wp.split(","))) for wp in wp_tokens + ] + duration = float(tokens[3]) + seg_defs.append( + { + "type": "SPLINE", + "waypoints": waypoints, + "duration": duration, + } + ) + else: + return False, f"Invalid segment type: {seg_type}" + self.segment_definitions = seg_defs + self.description = f"blended ({len(self.segment_definitions or [])} segments, {self.frame}, {self.trajectory_type})" + return True, None + except Exception as e: + return False, f"Invalid SMOOTH_BLEND parameters: {e}" + + try: + # Parse segment definitions (JSON format) + self.segment_definitions = json.loads(parts[1]) + + # Validate segment definitions + if not isinstance(self.segment_definitions, list): + return False, "Segments must be a list" + + for seg in self.segment_definitions: + if "type" not in seg: + return False, "Each segment must have a 'type' field" + if seg["type"] not in ["LINE", "ARC", "CIRCLE", "SPLINE"]: + return False, f"Invalid segment type: {seg['type']}" + + # Parse blend time + self.blend_time = float(parts[2]) + + # Parse frame + self.frame = parts[3].upper() + if self.frame not in ["WRF", "TRF"]: + return False, f"Invalid frame: {self.frame}" + + # Parse start pose + self.specified_start_pose = self.parse_start_pose(parts[4]) + + # Parse optional trajectory type and jerk limit + idx = 5 + if idx < len(parts): + self.trajectory_type, self.jerk_limit, idx = self.parse_trajectory_type( + parts, idx + ) + + # Initialize description + self.description = f"blended ({len(self.segment_definitions)} segments, {self.frame}, {self.trajectory_type})" + + return True, None + + except (ValueError, IndexError, json.JSONDecodeError) as e: + return False, f"Invalid SMOOTH_BLEND parameters: {e}" + + def do_setup(self, state: "ControllerState") -> None: + """Transform parameters if in TRF, then prepare normally.""" + + self.current_position_in = state.Position_in + + if self.frame == "TRF": + # Transform all segment definitions to WRF + params = {"segments": self.segment_definitions} + transformed = transform_command_params_to_wrf( + "SMOOTH_BLEND", params, "TRF", state.Position_in + ) + + # Update with transformed values + self.segment_definitions = transformed["segments"] + + logger.info( + f" -> TRF Blend: transformed {len(self.segment_definitions or [])} segments to WRF" + ) + + # Transform start_pose if specified + _sp = transform_start_pose_if_needed( + self.specified_start_pose.tolist() + if self.specified_start_pose is not None + else None, + self.frame, + state.Position_in, + ) + self.specified_start_pose = ( + np.asarray(_sp, dtype=np.float64) if _sp is not None else None + ) + + return super().do_setup(state) + + def generate_main_trajectory(self, effective_start_pose): + """Generate blended trajectory starting from actual position.""" + assert self.segment_definitions is not None + trajectories: list[np.ndarray] = [] + motion_gen_circle = CircularMotion() + motion_gen_spline = SplineMotion() + + # Always start from effective start pose + last_end_pose = effective_start_pose + + for i, seg_def in enumerate(self.segment_definitions): + seg_type = seg_def["type"] + + # First segment always starts from effective_start_pose + segment_start = effective_start_pose if i == 0 else last_end_pose + + if seg_type == "LINE": + end = seg_def["end"] + duration = seg_def["duration"] + + # Generate line segment from actual position + num_points = int(duration * 100) + timestamps = np.linspace(0, duration, num_points) + + points: list[list[float]] = [] + for t in timestamps: + s = t / duration if duration > 0 else 1 + # Interpolate position + pos = [segment_start[j] * (1 - s) + end[j] * s for j in range(3)] + # Interpolate orientation + orient = [ + segment_start[j + 3] * (1 - s) + end[j + 3] * s + for j in range(3) + ] + points.append(pos + orient) + + traj_arr = np.array(points, dtype=float) + trajectories.append(traj_arr) + last_end_pose = end + + elif seg_type == "ARC": + end = seg_def["end"] + center = seg_def["center"] + duration = seg_def["duration"] + clockwise = seg_def.get("clockwise", False) + + # Check if we have a transformed normal (from TRF) + normal = seg_def.get("normal_vector", None) + + traj_arr = np.array( + motion_gen_circle.generate_arc_3d( + segment_start, + end, + center, + normal=normal, # Use transformed normal if available + clockwise=clockwise, + duration=duration, + ) + ) + trajectories.append(traj_arr) + last_end_pose = end + + elif seg_type == "CIRCLE": + center = seg_def["center"] + radius = seg_def["radius"] + plane = seg_def.get("plane", "XY") + duration = seg_def["duration"] + clockwise = seg_def.get("clockwise", False) + + # Use transformed normal if available (from TRF) + if "normal_vector" in seg_def: + normal = seg_def["normal_vector"] + else: + plane_normals = {"XY": [0, 0, 1], "XZ": [0, 1, 0], "YZ": [1, 0, 0]} + normal = plane_normals.get(plane, [0, 0, 1]) + + traj_arr = np.array( + motion_gen_circle.generate_circle_3d( + center, + radius, + normal, + start_point=segment_start[:3], + duration=duration, + ) + ) + + if clockwise: + traj_arr = traj_arr[::-1] + + # Update orientations + for j in range(len(traj_arr)): + traj_arr[j][3:] = segment_start[3:] + + trajectories.append(traj_arr) + # Circle returns to start, so last pose is last point of trajectory + last_end_pose = traj_arr[-1] + + elif seg_type == "SPLINE": + waypoints = seg_def["waypoints"] + duration = seg_def["duration"] + + # Check if first waypoint is close to segment start + wp_error = np.linalg.norm( + np.array(waypoints[0][:3]) - np.array(segment_start[:3]) + ) + + if wp_error > 5.0: + full_waypoints = [segment_start] + waypoints + else: + full_waypoints = [segment_start] + waypoints[1:] + + timestamps = np.linspace(0, duration, len(full_waypoints)) + traj_arr = np.array( + motion_gen_spline.generate_cubic_spline( + full_waypoints, timestamps.tolist() + ) + ) + trajectories.append(traj_arr) + last_end_pose = waypoints[-1] + + # Blend all trajectories with advanced blending + if len(trajectories) > 1: + # Select blend method based on trajectory type + if self.trajectory_type == "quintic": + blend_method = "quintic" + elif self.trajectory_type == "s_curve": + blend_method = "s_curve" + elif self.trajectory_type == "cubic": + blend_method = "cubic" + else: + blend_method = "smoothstep" # Legacy compatibility + + # Create advanced blender + advanced_blender = AdvancedMotionBlender(sample_rate=100.0) + blended = trajectories[0] + + # Use auto sizing if blend_time is small, otherwise use specified time + if self.blend_time < 0.1: + auto_size = True + blend_samples = None + else: + auto_size = False + blend_samples = int(self.blend_time * 100) + + for i in range(1, len(trajectories)): + blended = advanced_blender.blend_trajectories( + blended, + trajectories[i], + method=blend_method, + blend_samples=blend_samples, + auto_size=auto_size, + ) + + logger.info( + f" Blended {len(trajectories)} segments into {len(blended)} points using {blend_method}" + ) + return blended + elif trajectories: + return trajectories[0] + else: + raise ValueError("No trajectories generated in blend") + + +@register_command("SMOOTH_WAYPOINTS") +class SmoothWaypointsCommand(BaseSmoothMotionCommand): + """Execute waypoint trajectory with corner cutting.""" + + __slots__ = ( + "waypoints", + "blend_radii", + "blend_mode", + "via_modes", + "max_velocity", + "max_acceleration", + "trajectory_type", + "frame", + "duration", + ) + + def __init__(self) -> None: + super().__init__(description="smooth waypoints") + self.waypoints: list[list[float]] | None = None + self.blend_radii: Any = "auto" + self.blend_mode: str = "parabolic" + self.via_modes: list[str] | None = None + self.max_velocity: float = 100.0 + self.max_acceleration: float = 500.0 + self.trajectory_type: str = "quintic" + self.frame: str = "WRF" + self.duration: float | None = None + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse SMOOTH_WAYPOINTS command. + Format: SMOOTH_WAYPOINTS|wp1;wp2;...|blend_radii|blend_mode|via_modes|max_vel|max_accel|frame|[trajectory_type]|[duration] + """ + if parts[0].upper() != "SMOOTH_WAYPOINTS": + return False, None + + if len(parts) < 8: + return False, "SMOOTH_WAYPOINTS requires at least 8 parameters" + + try: + # Parse waypoints (semicolon separated) + waypoint_strs = parts[1].split(";") + self.waypoints = [] + for wp_str in waypoint_strs: + wp = list(map(float, wp_str.split(","))) + if len(wp) != 6: + return False, "Each waypoint must have 6 values (x,y,z,rx,ry,rz)" + self.waypoints.append(wp) + + if len(self.waypoints) < 2: + return False, "SMOOTH_WAYPOINTS requires at least 2 waypoints" + + # Parse blend radii + if parts[2].upper() == "AUTO": + self.blend_radii = "auto" + else: + self.blend_radii = list(map(float, parts[2].split(","))) + if len(self.blend_radii) != len(self.waypoints) - 2: + return False, f"Blend radii count must be {len(self.waypoints) - 2}" + + # Parse blend mode + self.blend_mode = parts[3].lower() + if self.blend_mode not in ["parabolic", "circular", "none"]: + return False, f"Invalid blend mode: {self.blend_mode}" + + # Parse via modes + via_mode_strs = parts[4].split(",") + self.via_modes = [] + for vm in via_mode_strs: + vm = vm.lower() + if vm not in ["via", "stop"]: + return False, f"Invalid via mode: {vm}" + self.via_modes.append(vm) + + if len(self.via_modes) != len(self.waypoints): + return False, "Via modes count must match waypoint count" + + # Parse velocity and acceleration constraints + self.max_velocity = float(parts[5]) + self.max_acceleration = float(parts[6]) + + # Parse frame + self.frame = parts[7].upper() + if self.frame not in ["WRF", "TRF"]: + return False, f"Invalid frame: {self.frame}" + + # Parse optional trajectory type + idx = 8 + if idx < len(parts): + self.trajectory_type = parts[idx].lower() + if self.trajectory_type not in ["cubic", "quintic", "s_curve"]: + self.trajectory_type = "quintic" + idx += 1 + + # Parse optional duration + if idx < len(parts): + self.duration = float(parts[idx]) + + # Initialize description + self.description = f"waypoints ({len(self.waypoints or [])} points, {self.frame}, {self.blend_mode})" + + return True, None + + except (ValueError, IndexError) as e: + return False, f"Invalid SMOOTH_WAYPOINTS parameters: {e}" + + def do_setup(self, state: "ControllerState") -> None: + """Transform waypoints if in TRF.""" + # Ensure waypoints present for typing + if self.waypoints is None: + self.fail("At least 2 waypoints required") + return + + if self.frame == "TRF": + # Transform all waypoints to WRF + tool_pose = get_fkine_se3() + transformed_waypoints = [] + for wp in self.waypoints: + transformed_wp = pose6_trf_to_wrf(wp, tool_pose) + transformed_waypoints.append(transformed_wp) + + self.waypoints = transformed_waypoints + logger.info( + f" -> TRF Waypoints: transformed {len(self.waypoints)} points to WRF" + ) + + # Basic validation + if len(self.waypoints) < 2: + self.fail("At least 2 waypoints required") + return + + return super().do_setup(state) + + def generate_main_trajectory(self, effective_start_pose): + """Generate waypoint trajectory with corner cutting.""" + assert self.waypoints is not None + assert self.via_modes is not None + wps = self.waypoints + vms = self.via_modes + + # Ensure first waypoint matches effective start pose + first_wp_error = np.linalg.norm( + np.array(wps[0][:3]) - np.array(effective_start_pose[:3]) + ) + + if first_wp_error > 10.0: + # Prepend effective start pose as first waypoint + full_waypoints = [effective_start_pose] + wps + if self.blend_radii != "auto" and isinstance(self.blend_radii, list): + # Prepend 0 blend radius for start + full_blend_radii = [0] + self.blend_radii + else: + full_blend_radii = self.blend_radii + full_via_modes = ["via"] + vms + else: + # Replace first waypoint with effective start pose + full_waypoints = [effective_start_pose] + wps[1:] + full_blend_radii = self.blend_radii + full_via_modes = vms + + # Set up constraints + constraints = { + "max_velocity": self.max_velocity, + "max_acceleration": self.max_acceleration, + "max_jerk": 5000.0, # Default jerk limit + } + + # Create planner + planner = WaypointTrajectoryPlanner( + full_waypoints, constraints=constraints, sample_rate=100.0 + ) + + # Determine blend mode for planner + if self.blend_mode == "none": + planner_blend_mode = "none" + elif self.blend_radii == "auto": + planner_blend_mode = "auto" + else: + planner_blend_mode = "manual" + + # Generate trajectory with direct profile support + if planner_blend_mode == "manual" and isinstance(full_blend_radii, list): + opt_radii = [float(r) for r in full_blend_radii] + else: + opt_radii = None + trajectory = planner.plan_trajectory( + blend_mode=planner_blend_mode, + blend_radii=opt_radii, + via_modes=full_via_modes, + trajectory_type=self.trajectory_type, + jerk_limit=constraints["max_jerk"] + if self.trajectory_type == "s_curve" + else None, + ) + + # Apply duration scaling if specified + if self.duration and self.duration > 0: + current_duration = len(trajectory) / 100.0 + if current_duration > 0: + scale_factor = self.duration / current_duration + if scale_factor != 1.0: + # Resample trajectory for desired duration + new_length = int(self.duration * 100) + old_indices = np.linspace(0, len(trajectory) - 1, new_length) + resampled = [] + for idx in old_indices: + if idx < len(trajectory) - 1: + i = int(idx) + alpha = idx - i + pose = ( + trajectory[i] * (1 - alpha) + trajectory[i + 1] * alpha + ) + else: + pose = trajectory[-1] + resampled.append(pose) + trajectory = np.array(resampled) + + logger.info(f" Generated waypoint trajectory with {len(trajectory)} points") + return trajectory diff --git a/parol6/commands/system_commands.py b/parol6/commands/system_commands.py index 8dca124..0c1ac22 100644 --- a/parol6/commands/system_commands.py +++ b/parol6/commands/system_commands.py @@ -9,12 +9,12 @@ import logging import os -from typing import Tuple, Optional, List, TYPE_CHECKING +from typing import TYPE_CHECKING -from parol6.commands.base import SystemCommand, ExecutionStatus, parse_int, parse_bool -from parol6.server.command_registry import register_command -from parol6.protocol.wire import CommandCode +from parol6.commands.base import ExecutionStatus, SystemCommand, parse_bool, parse_int from parol6.config import save_com_port +from parol6.protocol.wire import CommandCode +from parol6.server.command_registry import register_command if TYPE_CHECKING: from parol6.server.state import ControllerState @@ -25,25 +25,26 @@ @register_command("STOP") class StopCommand(SystemCommand): """Emergency stop command - immediately stops all motion.""" + __slots__ = () - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """Check if this is a STOP command.""" if parts[0].upper() == "STOP": if len(parts) != 1: return False, "STOP command takes no parameters" return True, None return False, None - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + + def execute_step(self, state: ControllerState) -> ExecutionStatus: """Execute stop - set all speeds to zero and command to IDLE.""" logger.info("STOP command executed") state.Speed_out.fill(0) state.Command_out = CommandCode.IDLE - + # Clear any active commands in the controller # This will be handled by the controller when it sees this command - + self.finish() return ExecutionStatus.completed("Robot stopped") @@ -51,23 +52,24 @@ def execute_step(self, state: 'ControllerState') -> ExecutionStatus: @register_command("ENABLE") class EnableCommand(SystemCommand): """Enable the robot controller.""" + __slots__ = () - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """Check if this is an ENABLE command.""" if parts[0].upper() == "ENABLE": if len(parts) != 1: return False, "ENABLE command takes no parameters" return True, None return False, None - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + + def execute_step(self, state: ControllerState) -> ExecutionStatus: """Execute enable - set controller to enabled state.""" logger.info("ENABLE command executed") state.enabled = True state.disabled_reason = "" state.Command_out = CommandCode.ENABLE - + self.finish() return ExecutionStatus.completed("Controller enabled") @@ -75,111 +77,89 @@ def execute_step(self, state: 'ControllerState') -> ExecutionStatus: @register_command("DISABLE") class DisableCommand(SystemCommand): """Disable the robot controller.""" + __slots__ = () - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """Check if this is a DISABLE command.""" if parts[0].upper() == "DISABLE": if len(parts) != 1: return False, "DISABLE command takes no parameters" return True, None return False, None - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: - """Execute disable - set controller to disabled state.""" + + def execute_step(self, state: ControllerState) -> ExecutionStatus: + """Execute disable - zero speeds and set controller to disabled state.""" logger.info("DISABLE command executed") + state.Speed_out.fill(0) # Zero all speeds first state.enabled = False state.disabled_reason = "User requested disable" state.Command_out = CommandCode.DISABLE - state.Speed_out.fill(0) - - self.finish() - return ExecutionStatus.completed("Controller disabled") - -@register_command("CLEAR_ERROR") -class ClearErrorCommand(SystemCommand): - """Clear any error states in the controller.""" - __slots__ = () - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """Check if this is a CLEAR_ERROR command.""" - if parts[0].upper() == "CLEAR_ERROR": - if len(parts) != 1: - return False, "CLEAR_ERROR command takes no parameters" - return True, None - return False, None - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: - """Execute clear error - reset error states.""" - logger.info("CLEAR_ERROR command executed") - - # Clear any error states - # The actual error clearing logic depends on what errors are tracked - # For now, we'll just ensure the controller is in a clean state - state.Command_out = CommandCode.IDLE # No specific CLEAR_ERROR code - self.finish() - return ExecutionStatus.completed("Errors cleared") + return ExecutionStatus.completed("Controller disabled") @register_command("SET_IO") class SetIOCommand(SystemCommand): """Set a digital I/O port state.""" - + __slots__ = ("port_index", "port_value") - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """ Parse SET_IO command. - + Format: SET_IO|port_index|value Example: SET_IO|0|1 """ if parts[0].upper() != "SET_IO": return False, None - + if len(parts) != 3: return False, "SET_IO requires 2 parameters: port_index, value" - + self.port_index = parse_int(parts[1]) self.port_value = parse_int(parts[2]) - + if self.port_index is None or self.port_value is None: return False, "Port index and value must be integers" - + # Validate port index (0-7 for 8 I/O ports) if not 0 <= self.port_index <= 7: return False, f"Port index must be 0-7, got {self.port_index}" - + # Validate port value (0 or 1) if self.port_value not in (0, 1): return False, f"Port value must be 0 or 1, got {self.port_value}" - + logger.info(f"Parsed SET_IO: port {self.port_index} = {self.port_value}") return True, None - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + + def execute_step(self, state: ControllerState) -> ExecutionStatus: """Execute set port - update I/O port state.""" if self.port_index is None or self.port_value is None: self.fail("Port index or value not set") return ExecutionStatus.failed("Port parameters not set") - + logger.info(f"SET_IO: Setting port {self.port_index} to {self.port_value}") - + # Update the output port state state.InOut_out[self.port_index] = self.port_value - + self.finish() - return ExecutionStatus.completed(f"Port {self.port_index} set to {self.port_value}") + return ExecutionStatus.completed( + f"Port {self.port_index} set to {self.port_value}" + ) @register_command("SET_PORT") class SetSerialPortCommand(SystemCommand): """Set the serial COM port used by the controller.""" + __slots__ = ("port_str",) - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """ Parse SET_PORT command. @@ -200,7 +180,7 @@ def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: logger.info(f"Parsed SET_PORT: serial_port={self.port_str}") return True, None - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + def execute_step(self, state: ControllerState) -> ExecutionStatus: """Persist the serial port selection; controller may reconnect based on this.""" if not self.port_str: self.fail("No serial port provided") @@ -213,49 +193,53 @@ def execute_step(self, state: 'ControllerState') -> ExecutionStatus: self.finish() # Include details so the controller can reconnect immediately - return ExecutionStatus.completed("Serial port saved", details={"serial_port": self.port_str}) + return ExecutionStatus.completed( + "Serial port saved", details={"serial_port": self.port_str} + ) @register_command("STREAM") class StreamCommand(SystemCommand): """Toggle stream mode for real-time jogging.""" - + __slots__ = ("stream_mode",) - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """ Parse STREAM command. - + Format: STREAM|on/off Example: STREAM|on """ if parts[0].upper() != "STREAM": return False, None - + if len(parts) != 2: return False, "STREAM requires 1 parameter: on/off" - + self.stream_mode = parse_bool(parts[1]) - if parts[1].lower() not in ('on', 'off', '1', '0', 'true', 'false'): + if parts[1].lower() not in ("on", "off", "1", "0", "true", "false"): return False, f"STREAM mode must be 'on' or 'off', got '{parts[1]}'" - + logger.info(f"Parsed STREAM: mode = {self.stream_mode}") return True, None - - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + + def execute_step(self, state: ControllerState) -> ExecutionStatus: """Execute stream mode toggle.""" if self.stream_mode is None: self.fail("Stream mode not set") return ExecutionStatus.failed("Stream mode not set") - + # The controller will handle the actual stream mode setting # This is just a placeholder that sets a flag logger.info(f"STREAM: Setting stream mode to {self.stream_mode}") - + state.stream_mode = self.stream_mode - + self.finish() - return ExecutionStatus.completed(f"Stream mode {'enabled' if self.stream_mode else 'disabled'}") + return ExecutionStatus.completed( + f"Stream mode {'enabled' if self.stream_mode else 'disabled'}" + ) @register_command("SIMULATOR") @@ -264,7 +248,7 @@ class SimulatorCommand(SystemCommand): __slots__ = ("mode_on",) - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: """ Parse SIMULATOR command. @@ -278,13 +262,22 @@ def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: return False, "SIMULATOR requires 1 parameter: on/off" self.mode_on = parse_bool(parts[1]) - if parts[1].lower() not in ('on', 'off', '1', '0', 'true', 'false', 'yes', 'no'): + if parts[1].lower() not in ( + "on", + "off", + "1", + "0", + "true", + "false", + "yes", + "no", + ): return False, "SIMULATOR parameter must be 'on' or 'off'" logger.info(f"Parsed SIMULATOR: mode_on={self.mode_on}") return True, None - def execute_step(self, state: 'ControllerState') -> ExecutionStatus: + def execute_step(self, state: ControllerState) -> ExecutionStatus: """Execute simulator toggle by setting env var and returning details to trigger reconfiguration.""" if self.mode_on is None: self.fail("Simulator mode not set") diff --git a/parol6/commands/utility_commands.py b/parol6/commands/utility_commands.py index ea4a0e1..4c54b49 100644 --- a/parol6/commands/utility_commands.py +++ b/parol6/commands/utility_commands.py @@ -1,71 +1,78 @@ -""" -Utility Commands -Contains utility commands like Delay -""" - -import logging -from typing import List, Tuple, Optional -from parol6.commands.base import CommandBase, ExecutionStatus, parse_float -from parol6.protocol.wire import CommandCode -from parol6.server.command_registry import register_command - -logger = logging.getLogger(__name__) - - -@register_command("DELAY") -class DelayCommand(CommandBase): - """ - A non-blocking command that pauses execution for a specified duration. - During the delay, it ensures the robot remains idle by sending the - appropriate commands. - """ - __slots__ = ("duration",) - def __init__(self): - """ - Initializes the Delay command. - Parameters are parsed in do_match() method. - """ - super().__init__() - self.duration = None - - def do_match(self, parts: List[str]) -> Tuple[bool, Optional[str]]: - """ - Parse DELAY command parameters. - - Format: DELAY|duration - Example: DELAY|2.5 - """ - if len(parts) != 2: - return (False, "DELAY requires 1 parameter: duration") - - self.duration = parse_float(parts[1]) - if self.duration is None or self.duration <= 0: - return (False, f"Delay duration must be positive, got {parts[1]}") - logger.info(f"Parsed Delay command for {self.duration} seconds") - self.is_valid = True - return (True, None) - - def setup(self, state): - """Start the delay timer.""" - if self.duration: - self.start_timer(self.duration) - logger.info(f" -> Delay starting for {self.duration} seconds...") - - def execute_step(self, state) -> ExecutionStatus: - """ - Keep the robot idle during the delay and report status via ExecutionStatus. - """ - if self.is_finished or not self.is_valid: - return ExecutionStatus.completed("Already finished") if self.is_finished else ExecutionStatus.failed("Invalid command") - - # Keep the robot idle during the delay - state.Command_out = CommandCode.IDLE - state.Speed_out.fill(0) - - # Check for completion - if self.timer_expired(): - logger.info(f"Delay finished after {self.duration} seconds.") - self.is_finished = True - return ExecutionStatus.completed("Delay complete") - - return ExecutionStatus.executing("Delaying") +""" +Utility Commands +Contains utility commands like Delay +""" + +import logging + +from parol6.commands.base import CommandBase, ExecutionStatus, parse_float +from parol6.protocol.wire import CommandCode +from parol6.server.command_registry import register_command +from parol6.server.state import ControllerState + +logger = logging.getLogger(__name__) + + +@register_command("DELAY") +class DelayCommand(CommandBase): + """ + A non-blocking command that pauses execution for a specified duration. + During the delay, it ensures the robot remains idle by sending the + appropriate commands. + """ + + __slots__ = ("duration",) + + def __init__(self): + """ + Initializes the Delay command. + Parameters are parsed in do_match() method. + """ + super().__init__() + self.duration: float | None = None + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse DELAY command parameters. + + Format: DELAY|duration + Example: DELAY|2.5 + """ + if len(parts) != 2: + return (False, "DELAY requires 1 parameter: duration") + + self.duration = parse_float(parts[1]) + if self.duration is None or self.duration <= 0: + return (False, f"Delay duration must be positive, got {parts[1]}") + logger.info(f"Parsed Delay command for {self.duration} seconds") + self.is_valid = True + return (True, None) + + def setup(self, state: "ControllerState") -> None: + """Start the delay timer.""" + if self.duration: + self.start_timer(self.duration) + logger.info(f" -> Delay starting for {self.duration} seconds...") + + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + """ + Keep the robot idle during the delay and report status via ExecutionStatus. + """ + if self.is_finished or not self.is_valid: + return ( + ExecutionStatus.completed("Already finished") + if self.is_finished + else ExecutionStatus.failed("Invalid command") + ) + + # Keep the robot idle during the delay + state.Command_out = CommandCode.IDLE + state.Speed_out.fill(0) + + # Check for completion + if self.timer_expired(): + logger.info(f"Delay finished after {self.duration} seconds.") + self.is_finished = True + return ExecutionStatus.completed("Delay complete") + + return ExecutionStatus.executing("Delaying") diff --git a/parol6/config.py b/parol6/config.py index 9ebc43a..f42db81 100644 --- a/parol6/config.py +++ b/parol6/config.py @@ -2,29 +2,26 @@ Central configuration for PAROL6 tunables and shared constants. """ -import os import logging +import os from pathlib import Path -from typing import Optional TRACE: int = 5 logging.addLevelName(TRACE, "TRACE") # Add Logger.trace if missing if not hasattr(logging.Logger, "trace"): + def _trace(self, msg, *args, **kwargs): if self.isEnabledFor(TRACE): self._log(TRACE, msg, args, **kwargs) + logging.Logger.trace = _trace # type: ignore[attr-defined] - logging.TRACE = TRACE # type: ignore[attr-defined] + logging.TRACE = TRACE # type: ignore[attr-defined] TRACE_ENABLED = str(os.getenv("PAROL_TRACE", "0")).lower() in ("1", "true", "yes", "on") logger = logging.getLogger(__name__) -# IK / motion planning -# Iteration limit for jogging IK solves (kept conservative for speed while jogging) -JOG_IK_ILIMIT: int = 20 - # Default control/sample rates (Hz) CONTROL_RATE_HZ: float = float(os.getenv("PAROL6_CONTROL_RATE_HZ", "250")) @@ -60,10 +57,18 @@ def _trace(self, msg, *args, **kwargs): MCAST_TTL: int = int(os.getenv("PAROL6_MCAST_TTL", "1")) MCAST_IF: str = os.getenv("PAROL6_MCAST_IF", "127.0.0.1") +# Transport selection for status updates. Default MULTICAST; set to UNICAST on CI if multicast is not available. +STATUS_TRANSPORT: str = ( + os.getenv("PAROL6_STATUS_TRANSPORT", "MULTICAST").strip().upper() +) +# Host to use for unicast fallback (defaults to loopback) +STATUS_UNICAST_HOST: str = os.getenv("PAROL6_STATUS_UNICAST_HOST", "127.0.0.1") + # Status update/broadcast rates STATUS_RATE_HZ: float = float(os.getenv("PAROL6_STATUS_RATE_HZ", "50")) STATUS_STALE_S: float = float(os.getenv("PAROL6_STATUS_STALE_S", "0.2")) + # Homing posture (degrees) for simulation/tests; can be overridden via env "PAROL6_HOME_ANGLES_DEG" (CSV) def _parse_home_angles() -> list[float]: raw = os.getenv("PAROL6_HOME_ANGLES_DEG") @@ -79,10 +84,12 @@ def _parse_home_angles() -> list[float]: except Exception: return [90.0, -90.0, 180.0, 0.0, 0.0, 180.0] + HOME_ANGLES_DEG: list[float] = _parse_home_angles() + # Ack/Tracking policy toggles -def _env_bool_optional(name: str) -> Optional[bool]: +def _env_bool_optional(name: str) -> bool | None: raw = os.getenv(name) if raw is None: return None @@ -93,16 +100,17 @@ def _env_bool_optional(name: str) -> Optional[bool]: return False return None -FORCE_ACK: Optional[bool] = _env_bool_optional("PAROL6_FORCE_ACK") + +FORCE_ACK: bool | None = _env_bool_optional("PAROL6_FORCE_ACK") def save_com_port(port: str) -> bool: """ Save COM port to persistent file. - + Args: port: COM port string to save - + Returns: True if successful, False otherwise """ @@ -117,10 +125,10 @@ def save_com_port(port: str) -> bool: return False -def load_com_port() -> Optional[str]: +def load_com_port() -> str | None: """ Load saved COM port from file. - + Returns: COM port string if found, None otherwise """ diff --git a/parol6/gcode/__init__.py b/parol6/gcode/__init__.py index 3253ede..60b1676 100644 --- a/parol6/gcode/__init__.py +++ b/parol6/gcode/__init__.py @@ -1,22 +1,28 @@ -""" -GCODE Implementation for PAROL6 Robot - -This module provides GCODE parsing and execution capabilities for the PAROL6 robot. -It translates standard GCODE commands into robot motion commands. - -Main components: -- parser.py: GCODE tokenization and parsing -- state.py: Modal state tracking for GCODE execution -- commands.py: Command mapping from GCODE to robot commands -- coordinates.py: Work coordinate system management -- interpreter.py: Main GCODE interpreter -- utils.py: Utility functions for conversions and calculations -""" - -from .parser import GcodeParser, GcodeToken -from .state import GcodeState -from .interpreter import GcodeInterpreter -from .coordinates import WorkCoordinateSystem - -__version__ = "0.1.0" -__all__ = ['GcodeParser', 'GcodeToken', 'GcodeState', 'GcodeInterpreter', 'WorkCoordinateSystem'] \ No newline at end of file +""" +GCODE Implementation for PAROL6 Robot + +This module provides GCODE parsing and execution capabilities for the PAROL6 robot. +It translates standard GCODE commands into robot motion commands. + +Main components: +- parser.py: GCODE tokenization and parsing +- state.py: Modal state tracking for GCODE execution +- commands.py: Command mapping from GCODE to robot commands +- coordinates.py: Work coordinate system management +- interpreter.py: Main GCODE interpreter +- utils.py: Utility functions for conversions and calculations +""" + +from .coordinates import WorkCoordinateSystem +from .interpreter import GcodeInterpreter +from .parser import GcodeParser, GcodeToken +from .state import GcodeState + +__version__ = "0.1.0" +__all__ = [ + "GcodeParser", + "GcodeToken", + "GcodeState", + "GcodeInterpreter", + "WorkCoordinateSystem", +] diff --git a/parol6/gcode/commands.py b/parol6/gcode/commands.py index a34d615..774accb 100644 --- a/parol6/gcode/commands.py +++ b/parol6/gcode/commands.py @@ -1,621 +1,643 @@ -""" -GCODE Command Mappings for PAROL6 Robot - -Maps GCODE commands to robot motion commands. -Implements command objects that interface with the existing robot API. -""" - -import numpy as np -from typing import Dict, Optional -from parol6.PAROL6_ROBOT import cart -from .state import GcodeState -from .coordinates import WorkCoordinateSystem -from .utils import ijk_to_center, radius_to_center, validate_arc -from parol6.commands.base import CommandBase - -class GcodeCommand(CommandBase): - """Base class for GCODE commands""" - - def __init__(self): - super().__init__() - - def to_robot_command(self) -> str: - """ - Convert to robot API command string - - Returns: - Command string for robot API - """ - return "" - - -class G0Command(GcodeCommand): - """G0 - Rapid positioning command""" - - def __init__(self, target_position: Dict[str, float], state: GcodeState, coord_system: WorkCoordinateSystem): - """ - Initialize G0 rapid move command - - Args: - target_position: Target position in work coordinates - state: Current GCODE state - coord_system: Work coordinate system - """ - super().__init__() - self.target_position = target_position - self.state = state - self.coord_system = coord_system - - # Convert to machine coordinates - self.machine_position = coord_system.work_to_machine(target_position) - - # Convert to robot coordinates [X, Y, Z, RX, RY, RZ] - self.robot_position = coord_system.gcode_to_robot_coords(self.machine_position) - - def to_robot_command(self) -> str: - """ - Convert to MovePose command for robot API - - G0 uses rapid movement (100% speed) - """ - # Format: MOVEPOSE|X|Y|Z|RX|RY|RZ|duration|speed - # Where duration="None" for speed-based, speed="None" for duration-based - x, y, z = self.robot_position[0:3] - rx, ry, rz = self.robot_position[3:6] if len(self.robot_position) >= 6 else [0, 0, 0] - - # G0 uses rapid speed (100%) - speed_percentage = 100 - duration = "None" # Speed-based movement - - command = f"MOVEPOSE|{x:.3f}|{y:.3f}|{z:.3f}|{rx:.3f}|{ry:.3f}|{rz:.3f}|{duration}|{speed_percentage}" - return command - - -class G1Command(GcodeCommand): - """G1 - Linear interpolation command""" - - def __init__(self, target_position: Dict[str, float], state: GcodeState, coord_system: WorkCoordinateSystem): - """ - Initialize G1 linear move command - - Args: - target_position: Target position in work coordinates - state: Current GCODE state - coord_system: Work coordinate system - """ - super().__init__() - self.target_position = target_position - self.state = state - self.coord_system = coord_system - - # Convert to machine coordinates - self.machine_position = coord_system.work_to_machine(target_position) - - # Convert to robot coordinates - self.robot_position = coord_system.gcode_to_robot_coords(self.machine_position) - - # Get feed rate from state (mm/min) - self.feed_rate = state.feed_rate - - def to_robot_command(self) -> str: - """ - Convert to MoveCart command for robot API - - G1 uses linear interpolation with specified feed rate - """ - # Format: MOVECART|X|Y|Z|RX|RY|RZ|duration|speed - x, y, z = self.robot_position[0:3] - rx, ry, rz = self.robot_position[3:6] if len(self.robot_position) >= 6 else [0, 0, 0] - - # Convert feed rate (mm/min) to speed percentage - # Import robot speed limits from configuration - # Values are in m/s, convert to mm/min - max_speed_mm_min = cart.vel.linear.max * 1000 * 60 # m/s to mm/min - min_speed_mm_min = cart.vel.linear.min * 1000 * 60 # m/s to mm/min - - # Map feed rate to percentage (0-100) - speed_percentage = np.interp( - self.feed_rate, - [min_speed_mm_min, max_speed_mm_min], - [0, 100] - ) - speed_percentage = np.clip(speed_percentage, 0, 100) - - duration = "None" # Speed-based movement - - command = f"MOVECART|{x:.3f}|{y:.3f}|{z:.3f}|{rx:.3f}|{ry:.3f}|{rz:.3f}|{duration}|{speed_percentage:.1f}" - return command - - -class G2Command(GcodeCommand): - """G2 - Clockwise circular interpolation command""" - - def __init__(self, target_position: Dict[str, float], - arc_params: Dict[str, float], - state: GcodeState, - coord_system: WorkCoordinateSystem): - """ - Initialize G2 clockwise arc command - - Args: - target_position: Target (end) position in work coordinates - arc_params: Arc parameters (I, J, K offsets or R radius) - state: Current GCODE state - coord_system: Work coordinate system - """ - super().__init__() - self.target_position = target_position - self.arc_params = arc_params - self.state = state - self.coord_system = coord_system - - # Get current position - self.start_position = state.current_position.copy() - - # Determine arc center based on parameters - if 'R' in arc_params: - # Radius format - self.center = radius_to_center( - self.start_position, - target_position, - arc_params['R'], - clockwise=True, - plane=state.plane - ) - else: - # IJK offset format - ijk = {} - for key in ['I', 'J', 'K']: - if key in arc_params: - ijk[key] = arc_params[key] - self.center = ijk_to_center( - self.start_position, - ijk, - plane=state.plane - ) - - # Validate arc - if not validate_arc(self.start_position, target_position, self.center, state.plane): - self.is_valid = False - self.error_message = "Invalid arc: start and end radii don't match" - - # Convert positions to machine coordinates - self.machine_start = coord_system.work_to_machine(self.start_position) - self.machine_end = coord_system.work_to_machine(target_position) - self.machine_center = coord_system.work_to_machine(self.center) - - # Convert to robot coordinates - self.robot_start = coord_system.gcode_to_robot_coords(self.machine_start) - self.robot_end = coord_system.gcode_to_robot_coords(self.machine_end) - self.robot_center = coord_system.gcode_to_robot_coords(self.machine_center) - - # Get feed rate from state - self.feed_rate = state.feed_rate - - def to_robot_command(self) -> str: - """ - Convert to SMOOTH_ARC_CENTER command for robot API - - G2 uses clockwise arc interpolation with specified feed rate - """ - # Format: SMOOTH_ARC_CENTER|end_x|end_y|end_z|end_rx|end_ry|end_rz|center_x|center_y|center_z|frame|start_x|start_y|start_z|start_rx|start_ry|start_rz|duration|speed|clockwise - - # Extract positions - end_x, end_y, end_z = self.robot_end[0:3] - end_rx, end_ry, end_rz = self.robot_end[3:6] if len(self.robot_end) >= 6 else [0, 0, 0] - - center_x, center_y, center_z = self.robot_center[0:3] - - start_x, start_y, start_z = self.robot_start[0:3] - start_rx, start_ry, start_rz = self.robot_start[3:6] if len(self.robot_start) >= 6 else [0, 0, 0] - - # Convert feed rate to speed percentage - max_speed_mm_min = cart.vel.linear.max * 1000 * 60 - min_speed_mm_min = cart.vel.linear.min * 1000 * 60 - - speed_percentage = np.interp( - self.feed_rate, - [min_speed_mm_min, max_speed_mm_min], - [0, 100] - ) - speed_percentage = np.clip(speed_percentage, 0, 100) - - # Build command string - end_str = f"{end_x:.3f}|{end_y:.3f}|{end_z:.3f}|{end_rx:.3f}|{end_ry:.3f}|{end_rz:.3f}" - center_str = f"{center_x:.3f}|{center_y:.3f}|{center_z:.3f}" - start_str = f"{start_x:.3f}|{start_y:.3f}|{start_z:.3f}|{start_rx:.3f}|{start_ry:.3f}|{start_rz:.3f}" - - # Use speed-based movement - duration = "None" - frame = "0" # World frame - clockwise = "True" # G2 is clockwise - - command = f"SMOOTH_ARC_CENTER|{end_str}|{center_str}|{frame}|{start_str}|{duration}|{speed_percentage:.1f}|{clockwise}" - return command - - -class G3Command(GcodeCommand): - """G3 - Counter-clockwise circular interpolation command""" - - def __init__(self, target_position: Dict[str, float], - arc_params: Dict[str, float], - state: GcodeState, - coord_system: WorkCoordinateSystem): - """ - Initialize G3 counter-clockwise arc command - - Args: - target_position: Target (end) position in work coordinates - arc_params: Arc parameters (I, J, K offsets or R radius) - state: Current GCODE state - coord_system: Work coordinate system - """ - super().__init__() - self.target_position = target_position - self.arc_params = arc_params - self.state = state - self.coord_system = coord_system - - # Get current position - self.start_position = state.current_position.copy() - - # Determine arc center based on parameters - if 'R' in arc_params: - # Radius format - self.center = radius_to_center( - self.start_position, - target_position, - arc_params['R'], - clockwise=False, # G3 is counter-clockwise - plane=state.plane - ) - else: - # IJK offset format - ijk = {} - for key in ['I', 'J', 'K']: - if key in arc_params: - ijk[key] = arc_params[key] - self.center = ijk_to_center( - self.start_position, - ijk, - plane=state.plane - ) - - # Validate arc - if not validate_arc(self.start_position, target_position, self.center, state.plane): - self.is_valid = False - self.error_message = "Invalid arc: start and end radii don't match" - - # Convert positions to machine coordinates - self.machine_start = coord_system.work_to_machine(self.start_position) - self.machine_end = coord_system.work_to_machine(target_position) - self.machine_center = coord_system.work_to_machine(self.center) - - # Convert to robot coordinates - self.robot_start = coord_system.gcode_to_robot_coords(self.machine_start) - self.robot_end = coord_system.gcode_to_robot_coords(self.machine_end) - self.robot_center = coord_system.gcode_to_robot_coords(self.machine_center) - - # Get feed rate from state - self.feed_rate = state.feed_rate - - def to_robot_command(self) -> str: - """ - Convert to SMOOTH_ARC_CENTER command for robot API - - G3 uses counter-clockwise arc interpolation with specified feed rate - """ - # Format: SMOOTH_ARC_CENTER|end_x|end_y|end_z|end_rx|end_ry|end_rz|center_x|center_y|center_z|frame|start_x|start_y|start_z|start_rx|start_ry|start_rz|duration|speed|clockwise - - # Extract positions - end_x, end_y, end_z = self.robot_end[0:3] - end_rx, end_ry, end_rz = self.robot_end[3:6] if len(self.robot_end) >= 6 else [0, 0, 0] - - center_x, center_y, center_z = self.robot_center[0:3] - - start_x, start_y, start_z = self.robot_start[0:3] - start_rx, start_ry, start_rz = self.robot_start[3:6] if len(self.robot_start) >= 6 else [0, 0, 0] - - # Convert feed rate to speed percentage - max_speed_mm_min = cart.vel.linear.max * 1000 * 60 - min_speed_mm_min = cart.vel.linear.min * 1000 * 60 - - speed_percentage = np.interp( - self.feed_rate, - [min_speed_mm_min, max_speed_mm_min], - [0, 100] - ) - speed_percentage = np.clip(speed_percentage, 0, 100) - - # Build command string - end_str = f"{end_x:.3f}|{end_y:.3f}|{end_z:.3f}|{end_rx:.3f}|{end_ry:.3f}|{end_rz:.3f}" - center_str = f"{center_x:.3f}|{center_y:.3f}|{center_z:.3f}" - start_str = f"{start_x:.3f}|{start_y:.3f}|{start_z:.3f}|{start_rx:.3f}|{start_ry:.3f}|{start_rz:.3f}" - - # Use speed-based movement - duration = "None" - frame = "0" # World frame - clockwise = "False" # G3 is counter-clockwise - - command = f"SMOOTH_ARC_CENTER|{end_str}|{center_str}|{frame}|{start_str}|{duration}|{speed_percentage:.1f}|{clockwise}" - return command - - -class G4Command(GcodeCommand): - """G4 - Dwell command""" - - def __init__(self, dwell_time: float): - """ - Initialize G4 dwell command - - Args: - dwell_time: Dwell time in seconds - """ - super().__init__() - # Validate and clamp dwell time - if dwell_time < 0.0: - self.dwell_time = 0.0 - elif dwell_time > 300.0: # Max 5 minutes - self.dwell_time = 300.0 - else: - self.dwell_time = dwell_time - - def to_robot_command(self) -> str: - """ - Convert to Delay command for robot API - """ - # Format: DELAY|seconds - command = f"DELAY|{self.dwell_time:.3f}" - return command - - -class G28Command(GcodeCommand): - """G28 - Return to home command""" - - def __init__(self): - """Initialize G28 home command""" - super().__init__() - - def to_robot_command(self) -> str: - """ - Convert to Home command for robot API - """ - # Format: HOME - command = "HOME" - return command - - -class M3Command(GcodeCommand): - """M3 - Spindle/Gripper on CW (close gripper)""" - - def __init__(self, gripper_port: int = 1): - """Initialize M3 gripper close command""" - super().__init__() - # Validate gripper port - if gripper_port not in [1, 2]: - self.is_valid = False - self.error_message = f"Invalid gripper port {gripper_port}. Must be 1 or 2" - self.gripper_port = 1 # Default to port 1 - else: - self.gripper_port = gripper_port - - def to_robot_command(self) -> str: - """ - Convert to Gripper command for robot API - """ - # Format: PNEUMATICGRIPPER|action|port - # M3 maps to gripper close - command = f"PNEUMATICGRIPPER|close|{self.gripper_port}" - return command - - -class M5Command(GcodeCommand): - """M5 - Spindle/Gripper off (open gripper)""" - - def __init__(self, gripper_port: int = 1): - """Initialize M5 gripper open command""" - super().__init__() - # Validate gripper port - if gripper_port not in [1, 2]: - self.is_valid = False - self.error_message = f"Invalid gripper port {gripper_port}. Must be 1 or 2" - self.gripper_port = 1 # Default to port 1 - else: - self.gripper_port = gripper_port - - def to_robot_command(self) -> str: - """ - Convert to Gripper command for robot API - """ - # Format: PNEUMATICGRIPPER|action|port - # M5 maps to gripper open - command = f"PNEUMATICGRIPPER|open|{self.gripper_port}" - return command - - -class M0Command(GcodeCommand): - """M0 - Program stop""" - - def __init__(self): - """Initialize M0 stop command""" - super().__init__() - # This command will need special handling in the interpreter - self.requires_resume = True - - def to_robot_command(self) -> str: - """ - M0 doesn't directly map to a robot command - It's handled by the interpreter - """ - return "" - - -class M1Command(GcodeCommand): - """M1 - Optional program stop""" - - def __init__(self): - """Initialize M1 optional stop command""" - super().__init__() - # This command will need special handling in the interpreter - # It only stops if optional_stop is enabled - self.requires_resume = True - self.is_optional = True - - def to_robot_command(self) -> str: - """ - M1 doesn't directly map to a robot command - It's handled by the interpreter based on optional_stop setting - """ - return "" - - -class M4Command(GcodeCommand): - """M4 - Spindle/Gripper on CCW (alternative gripper action)""" - - def __init__(self, gripper_port: int = 1): - """Initialize M4 gripper CCW command""" - super().__init__() - # Validate gripper port - if gripper_port not in [1, 2]: - self.is_valid = False - self.error_message = f"Invalid gripper port {gripper_port}. Must be 1 or 2" - self.gripper_port = 1 # Default to port 1 - else: - self.gripper_port = gripper_port - - def to_robot_command(self) -> str: - """ - Convert to Gripper command for robot API - - Note: M4 typically means counter-clockwise spindle rotation. - For a gripper, this could map to a different action or be unsupported. - Currently mapping to gripper toggle or alternative action. - """ - # For PAROL6, M4 might not have a direct gripper equivalent - # Could be used for electric gripper with different mode - # For now, we'll treat it similar to M3 but document the difference - command = f"PNEUMATICGRIPPER|close|{self.gripper_port}" - return command - - -class M30Command(GcodeCommand): - """M30 - Program end""" - - def __init__(self): - """Initialize M30 end command""" - super().__init__() - self.is_program_end = True - - def to_robot_command(self) -> str: - """ - M30 doesn't directly map to a robot command - It signals the end of the program - """ - return "" - - -def create_command_from_token(token, state: GcodeState, coord_system: WorkCoordinateSystem) -> Optional[GcodeCommand]: - """ - Create a command object from a GCODE token - - Args: - token: GcodeToken object - state: Current GCODE state - coord_system: Work coordinate system - - Returns: - GcodeCommand object or None - """ - if token.code_type == 'G': - code = int(token.code_number) - - if code == 0: # Rapid positioning - # Calculate target position - target = state.calculate_target_position(token.parameters) - return G0Command(target, state, coord_system) - - elif code == 1: # Linear interpolation - # Calculate target position - target = state.calculate_target_position(token.parameters) - return G1Command(target, state, coord_system) - - elif code == 2: # Clockwise circular interpolation - # Calculate target position - target = state.calculate_target_position(token.parameters) - # Extract arc parameters (I, J, K or R) - arc_params = {} - for key in ['I', 'J', 'K', 'R']: - if key in token.parameters: - arc_params[key] = token.parameters[key] - - if not arc_params: - # No arc parameters provided, treat as linear move - return G1Command(target, state, coord_system) - - return G2Command(target, arc_params, state, coord_system) - - elif code == 3: # Counter-clockwise circular interpolation - # Calculate target position - target = state.calculate_target_position(token.parameters) - # Extract arc parameters (I, J, K or R) - arc_params = {} - for key in ['I', 'J', 'K', 'R']: - if key in token.parameters: - arc_params[key] = token.parameters[key] - - if not arc_params: - # No arc parameters provided, treat as linear move - return G1Command(target, state, coord_system) - - return G3Command(target, arc_params, state, coord_system) - - elif code == 4: # Dwell - # Get dwell time from P (milliseconds) or S (seconds) - if 'P' in token.parameters: - dwell_time = token.parameters['P'] / 1000.0 # Convert ms to seconds - elif 'S' in token.parameters: - dwell_time = token.parameters['S'] - else: - dwell_time = 0 - return G4Command(dwell_time) - - elif code == 28: # Home - return G28Command() - - # Modal commands that change state but don't generate movement - elif code in [17, 18, 19, 20, 21, 54, 55, 56, 57, 58, 59, 90, 91]: - # These are handled by state updates - return None - - elif token.code_type == 'M': - code = int(token.code_number) - - if code == 0: # Program stop - return M0Command() - - elif code == 1: # Optional program stop - return M1Command() - - elif code == 3: # Gripper close - # Check for P parameter for port number (default 1) - port = int(token.parameters.get('P', 1)) - return M3Command(gripper_port=port) - - elif code == 4: # Gripper CCW / alternative action - # Check for P parameter for port number (default 1) - port = int(token.parameters.get('P', 1)) - return M4Command(gripper_port=port) - - elif code == 5: # Gripper open - # Check for P parameter for port number (default 1) - port = int(token.parameters.get('P', 1)) - return M5Command(gripper_port=port) - - elif code == 30: # Program end - return M30Command() - - elif token.code_type in ['F', 'S', 'T', 'COMMENT']: - # These don't generate commands, just update state - return None - - return None +""" +GCODE Command Mappings for PAROL6 Robot + +Maps GCODE commands to robot motion commands. +Implements command objects that interface with the existing robot API. +""" + +import numpy as np + +from parol6.PAROL6_ROBOT import cart + +from .coordinates import WorkCoordinateSystem +from .parser import GcodeToken +from .state import GcodeState +from .utils import ijk_to_center, radius_to_center, validate_arc + + +class GcodeCommand: + """Base class for GCODE commands""" + + def __init__(self): + super().__init__() + + def to_robot_command(self) -> str: + """ + Convert to robot API command string + + Returns: + Command string for robot API + """ + return "" + + +class G0Command(GcodeCommand): + """G0 - Rapid positioning command""" + + def __init__( + self, + target_position: dict[str, float], + state: GcodeState, + coord_system: WorkCoordinateSystem, + ): + """ + Initialize G0 rapid move command + + Args: + target_position: Target position in work coordinates + state: Current GCODE state + coord_system: Work coordinate system + """ + super().__init__() + self.target_position = target_position + self.state = state + self.coord_system = coord_system + + # Convert to machine coordinates + self.machine_position = coord_system.work_to_machine(target_position) + + # Convert to robot coordinates [X, Y, Z, RX, RY, RZ] + self.robot_position = coord_system.gcode_to_robot_coords(self.machine_position) + + def to_robot_command(self) -> str: + """ + Convert to MovePose command for robot API + + G0 uses rapid movement (100% speed) + """ + # Format: MOVEPOSE|X|Y|Z|RX|RY|RZ|duration|speed + # Where duration="None" for speed-based, speed="None" for duration-based + x, y, z = self.robot_position[0:3] + rx, ry, rz = ( + self.robot_position[3:6] if len(self.robot_position) >= 6 else [0, 0, 0] + ) + + # G0 uses rapid speed (100%) + speed_percentage = 100 + duration = "None" # Speed-based movement + + command = f"MOVEPOSE|{x:.3f}|{y:.3f}|{z:.3f}|{rx:.3f}|{ry:.3f}|{rz:.3f}|{duration}|{speed_percentage}" + return command + + +class G1Command(GcodeCommand): + """G1 - Linear interpolation command""" + + def __init__( + self, + target_position: dict[str, float], + state: GcodeState, + coord_system: WorkCoordinateSystem, + ): + """ + Initialize G1 linear move command + + Args: + target_position: Target position in work coordinates + state: Current GCODE state + coord_system: Work coordinate system + """ + super().__init__() + self.target_position = target_position + self.state = state + self.coord_system = coord_system + + # Convert to machine coordinates + self.machine_position = coord_system.work_to_machine(target_position) + + # Convert to robot coordinates + self.robot_position = coord_system.gcode_to_robot_coords(self.machine_position) + + # Get feed rate from state (mm/min) + self.feed_rate = state.feed_rate + + def to_robot_command(self) -> str: + """ + Convert to MoveCart command for robot API + + G1 uses linear interpolation with specified feed rate + """ + # Format: MOVECART|X|Y|Z|RX|RY|RZ|duration|speed + x, y, z = self.robot_position[0:3] + rx, ry, rz = ( + self.robot_position[3:6] if len(self.robot_position) >= 6 else [0, 0, 0] + ) + + # Convert feed rate (mm/min) to speed percentage + # Import robot speed limits from configuration + # Values are in m/s, convert to mm/min + max_speed_mm_min = cart.vel.linear.max * 1000 * 60 # m/s to mm/min + min_speed_mm_min = cart.vel.linear.min * 1000 * 60 # m/s to mm/min + + # Map feed rate to percentage (0-100) + speed_percentage = np.interp( + self.feed_rate, [min_speed_mm_min, max_speed_mm_min], [0, 100] + ) + speed_percentage = np.clip(speed_percentage, 0, 100) + + duration = "None" # Speed-based movement + + command = f"MOVECART|{x:.3f}|{y:.3f}|{z:.3f}|{rx:.3f}|{ry:.3f}|{rz:.3f}|{duration}|{speed_percentage:.1f}" + return command + + +class G2Command(GcodeCommand): + """G2 - Clockwise circular interpolation command""" + + def __init__( + self, + target_position: dict[str, float], + arc_params: dict[str, float], + state: GcodeState, + coord_system: WorkCoordinateSystem, + ): + """ + Initialize G2 clockwise arc command + + Args: + target_position: Target (end) position in work coordinates + arc_params: Arc parameters (I, J, K offsets or R radius) + state: Current GCODE state + coord_system: Work coordinate system + """ + super().__init__() + self.target_position = target_position + self.arc_params = arc_params + self.state = state + self.coord_system = coord_system + + # Get current position + self.start_position = state.current_position.copy() + + # Determine arc center based on parameters + if "R" in arc_params: + # Radius format + self.center = radius_to_center( + self.start_position, + target_position, + arc_params["R"], + clockwise=True, + plane=state.plane, + ) + else: + # IJK offset format + ijk = {} + for key in ["I", "J", "K"]: + if key in arc_params: + ijk[key] = arc_params[key] + self.center = ijk_to_center(self.start_position, ijk, plane=state.plane) + + # Validate arc + if not validate_arc( + self.start_position, target_position, self.center, state.plane + ): + self.is_valid = False + self.error_message = "Invalid arc: start and end radii don't match" + + # Convert positions to machine coordinates + self.machine_start = coord_system.work_to_machine(self.start_position) + self.machine_end = coord_system.work_to_machine(target_position) + self.machine_center = coord_system.work_to_machine(self.center) + + # Convert to robot coordinates + self.robot_start = coord_system.gcode_to_robot_coords(self.machine_start) + self.robot_end = coord_system.gcode_to_robot_coords(self.machine_end) + self.robot_center = coord_system.gcode_to_robot_coords(self.machine_center) + + # Get feed rate from state + self.feed_rate = state.feed_rate + + def to_robot_command(self) -> str: + """ + Convert to SMOOTH_ARC_CENTER command for robot API + + G2 uses clockwise arc interpolation with specified feed rate + """ + # Format: SMOOTH_ARC_CENTER|end_x|end_y|end_z|end_rx|end_ry|end_rz|center_x|center_y|center_z|frame|start_x|start_y|start_z|start_rx|start_ry|start_rz|duration|speed|clockwise + + # Extract positions + end_x, end_y, end_z = self.robot_end[0:3] + end_rx, end_ry, end_rz = ( + self.robot_end[3:6] if len(self.robot_end) >= 6 else [0, 0, 0] + ) + + center_x, center_y, center_z = self.robot_center[0:3] + + start_x, start_y, start_z = self.robot_start[0:3] + start_rx, start_ry, start_rz = ( + self.robot_start[3:6] if len(self.robot_start) >= 6 else [0, 0, 0] + ) + + # Convert feed rate to speed percentage + max_speed_mm_min = cart.vel.linear.max * 1000 * 60 + min_speed_mm_min = cart.vel.linear.min * 1000 * 60 + + speed_percentage = np.interp( + self.feed_rate, [min_speed_mm_min, max_speed_mm_min], [0, 100] + ) + speed_percentage = np.clip(speed_percentage, 0, 100) + + # Build command string + end_str = f"{end_x:.3f}|{end_y:.3f}|{end_z:.3f}|{end_rx:.3f}|{end_ry:.3f}|{end_rz:.3f}" + center_str = f"{center_x:.3f}|{center_y:.3f}|{center_z:.3f}" + start_str = f"{start_x:.3f}|{start_y:.3f}|{start_z:.3f}|{start_rx:.3f}|{start_ry:.3f}|{start_rz:.3f}" + + # Use speed-based movement + duration = "None" + frame = "0" # World frame + clockwise = "True" # G2 is clockwise + + command = f"SMOOTH_ARC_CENTER|{end_str}|{center_str}|{frame}|{start_str}|{duration}|{speed_percentage:.1f}|{clockwise}" + return command + + +class G3Command(GcodeCommand): + """G3 - Counter-clockwise circular interpolation command""" + + def __init__( + self, + target_position: dict[str, float], + arc_params: dict[str, float], + state: GcodeState, + coord_system: WorkCoordinateSystem, + ): + """ + Initialize G3 counter-clockwise arc command + + Args: + target_position: Target (end) position in work coordinates + arc_params: Arc parameters (I, J, K offsets or R radius) + state: Current GCODE state + coord_system: Work coordinate system + """ + super().__init__() + self.target_position = target_position + self.arc_params = arc_params + self.state = state + self.coord_system = coord_system + + # Get current position + self.start_position = state.current_position.copy() + + # Determine arc center based on parameters + if "R" in arc_params: + # Radius format + self.center = radius_to_center( + self.start_position, + target_position, + arc_params["R"], + clockwise=False, # G3 is counter-clockwise + plane=state.plane, + ) + else: + # IJK offset format + ijk = {} + for key in ["I", "J", "K"]: + if key in arc_params: + ijk[key] = arc_params[key] + self.center = ijk_to_center(self.start_position, ijk, plane=state.plane) + + # Validate arc + if not validate_arc( + self.start_position, target_position, self.center, state.plane + ): + self.is_valid = False + self.error_message = "Invalid arc: start and end radii don't match" + + # Convert positions to machine coordinates + self.machine_start = coord_system.work_to_machine(self.start_position) + self.machine_end = coord_system.work_to_machine(target_position) + self.machine_center = coord_system.work_to_machine(self.center) + + # Convert to robot coordinates + self.robot_start = coord_system.gcode_to_robot_coords(self.machine_start) + self.robot_end = coord_system.gcode_to_robot_coords(self.machine_end) + self.robot_center = coord_system.gcode_to_robot_coords(self.machine_center) + + # Get feed rate from state + self.feed_rate = state.feed_rate + + def to_robot_command(self) -> str: + """ + Convert to SMOOTH_ARC_CENTER command for robot API + + G3 uses counter-clockwise arc interpolation with specified feed rate + """ + # Format: SMOOTH_ARC_CENTER|end_x|end_y|end_z|end_rx|end_ry|end_rz|center_x|center_y|center_z|frame|start_x|start_y|start_z|start_rx|start_ry|start_rz|duration|speed|clockwise + + # Extract positions + end_x, end_y, end_z = self.robot_end[0:3] + end_rx, end_ry, end_rz = ( + self.robot_end[3:6] if len(self.robot_end) >= 6 else [0, 0, 0] + ) + + center_x, center_y, center_z = self.robot_center[0:3] + + start_x, start_y, start_z = self.robot_start[0:3] + start_rx, start_ry, start_rz = ( + self.robot_start[3:6] if len(self.robot_start) >= 6 else [0, 0, 0] + ) + + # Convert feed rate to speed percentage + max_speed_mm_min = cart.vel.linear.max * 1000 * 60 + min_speed_mm_min = cart.vel.linear.min * 1000 * 60 + + speed_percentage = np.interp( + self.feed_rate, [min_speed_mm_min, max_speed_mm_min], [0, 100] + ) + speed_percentage = np.clip(speed_percentage, 0, 100) + + # Build command string + end_str = f"{end_x:.3f}|{end_y:.3f}|{end_z:.3f}|{end_rx:.3f}|{end_ry:.3f}|{end_rz:.3f}" + center_str = f"{center_x:.3f}|{center_y:.3f}|{center_z:.3f}" + start_str = f"{start_x:.3f}|{start_y:.3f}|{start_z:.3f}|{start_rx:.3f}|{start_ry:.3f}|{start_rz:.3f}" + + # Use speed-based movement + duration = "None" + frame = "0" # World frame + clockwise = "False" # G3 is counter-clockwise + + command = f"SMOOTH_ARC_CENTER|{end_str}|{center_str}|{frame}|{start_str}|{duration}|{speed_percentage:.1f}|{clockwise}" + return command + + +class G4Command(GcodeCommand): + """G4 - Dwell command""" + + def __init__(self, dwell_time: float): + """ + Initialize G4 dwell command + + Args: + dwell_time: Dwell time in seconds + """ + super().__init__() + # Validate and clamp dwell time + if dwell_time < 0.0: + self.dwell_time = 0.0 + elif dwell_time > 300.0: # Max 5 minutes + self.dwell_time = 300.0 + else: + self.dwell_time = dwell_time + + def to_robot_command(self) -> str: + """ + Convert to Delay command for robot API + """ + # Format: DELAY|seconds + command = f"DELAY|{self.dwell_time:.3f}" + return command + + +class G28Command(GcodeCommand): + """G28 - Return to home command""" + + def __init__(self): + """Initialize G28 home command""" + super().__init__() + + def to_robot_command(self) -> str: + """ + Convert to Home command for robot API + """ + # Format: HOME + command = "HOME" + return command + + +class M3Command(GcodeCommand): + """M3 - Spindle/Gripper on CW (close gripper)""" + + def __init__(self, gripper_port: int = 1): + """Initialize M3 gripper close command""" + super().__init__() + # Validate gripper port + if gripper_port not in [1, 2]: + self.is_valid = False + self.error_message = f"Invalid gripper port {gripper_port}. Must be 1 or 2" + self.gripper_port = 1 # Default to port 1 + else: + self.gripper_port = gripper_port + + def to_robot_command(self) -> str: + """ + Convert to Gripper command for robot API + """ + # Format: PNEUMATICGRIPPER|action|port + # M3 maps to gripper close + command = f"PNEUMATICGRIPPER|close|{self.gripper_port}" + return command + + +class M5Command(GcodeCommand): + """M5 - Spindle/Gripper off (open gripper)""" + + def __init__(self, gripper_port: int = 1): + """Initialize M5 gripper open command""" + super().__init__() + # Validate gripper port + if gripper_port not in [1, 2]: + self.is_valid = False + self.error_message = f"Invalid gripper port {gripper_port}. Must be 1 or 2" + self.gripper_port = 1 # Default to port 1 + else: + self.gripper_port = gripper_port + + def to_robot_command(self) -> str: + """ + Convert to Gripper command for robot API + """ + # Format: PNEUMATICGRIPPER|action|port + # M5 maps to gripper open + command = f"PNEUMATICGRIPPER|open|{self.gripper_port}" + return command + + +class M0Command(GcodeCommand): + """M0 - Program stop""" + + def __init__(self): + """Initialize M0 stop command""" + super().__init__() + # This command will need special handling in the interpreter + self.requires_resume = True + + def to_robot_command(self) -> str: + """ + M0 doesn't directly map to a robot command + It's handled by the interpreter + """ + return "" + + +class M1Command(GcodeCommand): + """M1 - Optional program stop""" + + def __init__(self): + """Initialize M1 optional stop command""" + super().__init__() + # This command will need special handling in the interpreter + # It only stops if optional_stop is enabled + self.requires_resume = True + self.is_optional = True + + def to_robot_command(self) -> str: + """ + M1 doesn't directly map to a robot command + It's handled by the interpreter based on optional_stop setting + """ + return "" + + +class M4Command(GcodeCommand): + """M4 - Spindle/Gripper on CCW (alternative gripper action)""" + + def __init__(self, gripper_port: int = 1): + """Initialize M4 gripper CCW command""" + super().__init__() + # Validate gripper port + if gripper_port not in [1, 2]: + self.is_valid = False + self.error_message = f"Invalid gripper port {gripper_port}. Must be 1 or 2" + self.gripper_port = 1 # Default to port 1 + else: + self.gripper_port = gripper_port + + def to_robot_command(self) -> str: + """ + Convert to Gripper command for robot API + + Note: M4 typically means counter-clockwise spindle rotation. + For a gripper, this could map to a different action or be unsupported. + Currently mapping to gripper toggle or alternative action. + """ + # For PAROL6, M4 might not have a direct gripper equivalent + # Could be used for electric gripper with different mode + # For now, we'll treat it similar to M3 but document the difference + command = f"PNEUMATICGRIPPER|close|{self.gripper_port}" + return command + + +class M30Command(GcodeCommand): + """M30 - Program end""" + + def __init__(self): + """Initialize M30 end command""" + super().__init__() + self.is_program_end = True + + def to_robot_command(self) -> str: + """ + M30 doesn't directly map to a robot command + It signals the end of the program + """ + return "" + + +def create_command_from_token( + token: GcodeToken, state: GcodeState, coord_system: WorkCoordinateSystem +) -> GcodeCommand | None: + """ + Create a command object from a GCODE token + + Args: + token: GcodeToken object + state: Current GCODE state + coord_system: Work coordinate system + + Returns: + GcodeCommand object or None + """ + if token.code_type == "G": + code = int(token.code_number) + + if code == 0: # Rapid positioning + # Calculate target position + target = state.calculate_target_position(token.parameters) + return G0Command(target, state, coord_system) + + elif code == 1: # Linear interpolation + # Calculate target position + target = state.calculate_target_position(token.parameters) + return G1Command(target, state, coord_system) + + elif code == 2: # Clockwise circular interpolation + # Calculate target position + target = state.calculate_target_position(token.parameters) + # Extract arc parameters (I, J, K or R) + arc_params = {} + for key in ["I", "J", "K", "R"]: + if key in token.parameters: + arc_params[key] = token.parameters[key] + + if not arc_params: + # No arc parameters provided, treat as linear move + return G1Command(target, state, coord_system) + + return G2Command(target, arc_params, state, coord_system) + + elif code == 3: # Counter-clockwise circular interpolation + # Calculate target position + target = state.calculate_target_position(token.parameters) + # Extract arc parameters (I, J, K or R) + arc_params = {} + for key in ["I", "J", "K", "R"]: + if key in token.parameters: + arc_params[key] = token.parameters[key] + + if not arc_params: + # No arc parameters provided, treat as linear move + return G1Command(target, state, coord_system) + + return G3Command(target, arc_params, state, coord_system) + + elif code == 4: # Dwell + # Get dwell time from P (milliseconds) or S (seconds) + if "P" in token.parameters: + dwell_time = token.parameters["P"] / 1000.0 # Convert ms to seconds + elif "S" in token.parameters: + dwell_time = token.parameters["S"] + else: + dwell_time = 0 + return G4Command(dwell_time) + + elif code == 28: # Home + return G28Command() + + # Modal commands that change state but don't generate movement + elif code in [17, 18, 19, 20, 21, 54, 55, 56, 57, 58, 59, 90, 91]: + # These are handled by state updates + return None + + elif token.code_type == "M": + code = int(token.code_number) + + if code == 0: # Program stop + return M0Command() + + elif code == 1: # Optional program stop + return M1Command() + + elif code == 3: # Gripper close + # Check for P parameter for port number (default 1) + port = int(token.parameters.get("P", 1)) + return M3Command(gripper_port=port) + + elif code == 4: # Gripper CCW / alternative action + # Check for P parameter for port number (default 1) + port = int(token.parameters.get("P", 1)) + return M4Command(gripper_port=port) + + elif code == 5: # Gripper open + # Check for P parameter for port number (default 1) + port = int(token.parameters.get("P", 1)) + return M5Command(gripper_port=port) + + elif code == 30: # Program end + return M30Command() + + elif token.code_type in ["F", "S", "T", "COMMENT"]: + # These don't generate commands, just update state + return None + + return None diff --git a/parol6/gcode/coordinates.py b/parol6/gcode/coordinates.py index 9c08da5..2be00ad 100644 --- a/parol6/gcode/coordinates.py +++ b/parol6/gcode/coordinates.py @@ -1,321 +1,336 @@ -""" -Work Coordinate System Implementation for GCODE - -Manages G54-G59 work coordinate systems and transformations between -work coordinates, machine coordinates, and robot coordinates. -""" - -import json -import os -from typing import Dict, List, Optional - - -class WorkCoordinateSystem: - """Manages work coordinate systems and transformations""" - - def __init__(self, config_file: Optional[str] = None): - """ - Initialize work coordinate system - - Args: - config_file: Path to JSON file for persistent storage - """ - self.config_file = config_file or os.path.join( - os.path.dirname(__file__), 'work_coordinates.json' - ) - - # Initialize default work coordinate offsets (in mm) - self.offsets = { - 'G54': {'X': 0.0, 'Y': 0.0, 'Z': 0.0, 'A': 0.0, 'B': 0.0, 'C': 0.0}, - 'G55': {'X': 0.0, 'Y': 0.0, 'Z': 0.0, 'A': 0.0, 'B': 0.0, 'C': 0.0}, - 'G56': {'X': 0.0, 'Y': 0.0, 'Z': 0.0, 'A': 0.0, 'B': 0.0, 'C': 0.0}, - 'G57': {'X': 0.0, 'Y': 0.0, 'Z': 0.0, 'A': 0.0, 'B': 0.0, 'C': 0.0}, - 'G58': {'X': 0.0, 'Y': 0.0, 'Z': 0.0, 'A': 0.0, 'B': 0.0, 'C': 0.0}, - 'G59': {'X': 0.0, 'Y': 0.0, 'Z': 0.0, 'A': 0.0, 'B': 0.0, 'C': 0.0} - } - - # Tool offsets - self.tool_offsets = { - 0: {'Z': 0.0}, # No tool - 1: {'Z': 0.0}, # Tool 1 - 2: {'Z': 0.0}, # Tool 2 - # Add more tools as needed - } - - # Current active coordinate system - self.active_system = 'G54' - - # Current tool number - self.active_tool = 0 - - # Load saved offsets if they exist - self.load_offsets() - - def set_offset(self, coord_system: str, axis: str, value: float) -> bool: - """ - Set work coordinate offset for a specific axis - - Args: - coord_system: Work coordinate system (G54-G59) - axis: Axis name (X, Y, Z, A, B, C) - value: Offset value in mm - - Returns: - True if successful, False otherwise - """ - if coord_system not in self.offsets: - return False - - if axis not in self.offsets[coord_system]: - return False - - self.offsets[coord_system][axis] = value - self.save_offsets() - return True - - def get_offset(self, coord_system: Optional[str] = None) -> Dict[str, float]: - """ - Get work coordinate offset - - Args: - coord_system: Work coordinate system (G54-G59) or None for active - - Returns: - Dictionary of axis offsets - """ - system = coord_system or self.active_system - return self.offsets.get(system, {}).copy() - - def set_active_system(self, coord_system: str) -> bool: - """ - Set the active work coordinate system - - Args: - coord_system: Work coordinate system (G54-G59) - - Returns: - True if successful, False otherwise - """ - if coord_system in self.offsets: - self.active_system = coord_system - return True - return False - - def set_tool_offset(self, tool_number: int, z_offset: float) -> None: - """ - Set tool length offset - - Args: - tool_number: Tool number - z_offset: Z-axis offset in mm - """ - if tool_number not in self.tool_offsets: - self.tool_offsets[tool_number] = {} - self.tool_offsets[tool_number]['Z'] = z_offset - self.save_offsets() - - def get_tool_offset(self, tool_number: Optional[int] = None) -> float: - """ - Get tool length offset - - Args: - tool_number: Tool number or None for active tool - - Returns: - Z-axis offset in mm - """ - tool = tool_number if tool_number is not None else self.active_tool - return self.tool_offsets.get(tool, {}).get('Z', 0.0) - - def work_to_machine(self, work_pos: Dict[str, float], - coord_system: Optional[str] = None, - apply_tool_offset: bool = True) -> Dict[str, float]: - """ - Convert work coordinates to machine coordinates - - Args: - work_pos: Position in work coordinates - coord_system: Work coordinate system or None for active - apply_tool_offset: Whether to apply tool offset - - Returns: - Position in machine coordinates - """ - system = coord_system or self.active_system - offset = self.get_offset(system) - machine_pos = {} - - for axis in ['X', 'Y', 'Z', 'A', 'B', 'C']: - if axis in work_pos: - machine_pos[axis] = work_pos[axis] + offset.get(axis, 0.0) - - # Apply tool offset to Z axis - if axis == 'Z' and apply_tool_offset: - machine_pos[axis] += self.get_tool_offset() - - return machine_pos - - def machine_to_work(self, machine_pos: Dict[str, float], - coord_system: Optional[str] = None, - apply_tool_offset: bool = True) -> Dict[str, float]: - """ - Convert machine coordinates to work coordinates - - Args: - machine_pos: Position in machine coordinates - coord_system: Work coordinate system or None for active - apply_tool_offset: Whether to apply tool offset - - Returns: - Position in work coordinates - """ - system = coord_system or self.active_system - offset = self.get_offset(system) - work_pos = {} - - for axis in ['X', 'Y', 'Z', 'A', 'B', 'C']: - if axis in machine_pos: - work_pos[axis] = machine_pos[axis] - offset.get(axis, 0.0) - - # Remove tool offset from Z axis - if axis == 'Z' and apply_tool_offset: - work_pos[axis] -= self.get_tool_offset() - - return work_pos - - def apply_incremental(self, current_pos: Dict[str, float], - incremental: Dict[str, float]) -> Dict[str, float]: - """ - Apply incremental movement to current position - - Args: - current_pos: Current position - incremental: Incremental movement values - - Returns: - New position after incremental movement - """ - new_pos = current_pos.copy() - - for axis in ['X', 'Y', 'Z', 'A', 'B', 'C']: - if axis in incremental: - new_pos[axis] = current_pos.get(axis, 0.0) + incremental[axis] - - return new_pos - - def robot_to_gcode_coords(self, robot_pos: List[float]) -> Dict[str, float]: - """ - Convert robot Cartesian position to GCODE coordinates - - Args: - robot_pos: Robot position [X, Y, Z, RX, RY, RZ] in mm and degrees - - Returns: - GCODE coordinates {X, Y, Z, A, B, C} - """ - # For PAROL6, the mapping is straightforward - # X, Y, Z are Cartesian positions - # A, B, C are rotational axes (RX, RY, RZ) - - gcode_coords = {} - - if len(robot_pos) >= 3: - gcode_coords['X'] = robot_pos[0] - gcode_coords['Y'] = robot_pos[1] - gcode_coords['Z'] = robot_pos[2] - - if len(robot_pos) >= 6: - gcode_coords['A'] = robot_pos[3] # RX - gcode_coords['B'] = robot_pos[4] # RY - gcode_coords['C'] = robot_pos[5] # RZ - - return gcode_coords - - def gcode_to_robot_coords(self, gcode_pos: Dict[str, float]) -> List[float]: - """ - Convert GCODE coordinates to robot Cartesian position - - Args: - gcode_pos: GCODE coordinates {X, Y, Z, A, B, C} - - Returns: - Robot position [X, Y, Z, RX, RY, RZ] in mm and degrees - """ - robot_pos = [ - gcode_pos.get('X', 0.0), - gcode_pos.get('Y', 0.0), - gcode_pos.get('Z', 0.0), - gcode_pos.get('A', 0.0), # RX - gcode_pos.get('B', 0.0), # RY - gcode_pos.get('C', 0.0) # RZ - ] - - return robot_pos - - def save_offsets(self) -> None: - """Save offsets to JSON file""" - data = { - 'work_offsets': self.offsets, - 'tool_offsets': self.tool_offsets, - 'active_system': self.active_system, - 'active_tool': self.active_tool - } - - os.makedirs(os.path.dirname(self.config_file), exist_ok=True) - with open(self.config_file, 'w') as f: - json.dump(data, f, indent=2) - - def load_offsets(self) -> None: - """Load offsets from JSON file""" - if os.path.exists(self.config_file): - try: - with open(self.config_file, 'r') as f: - data = json.load(f) - - self.offsets = data.get('work_offsets', self.offsets) - self.tool_offsets = data.get('tool_offsets', self.tool_offsets) - self.active_system = data.get('active_system', 'G54') - self.active_tool = data.get('active_tool', 0) - except Exception as e: - print(f"Error loading work coordinate offsets: {e}") - - def reset_offset(self, coord_system: Optional[str] = None) -> None: - """ - Reset work coordinate offset to zero - - Args: - coord_system: System to reset, or None to reset all - """ - if coord_system: - if coord_system in self.offsets: - self.offsets[coord_system] = { - 'X': 0.0, 'Y': 0.0, 'Z': 0.0, - 'A': 0.0, 'B': 0.0, 'C': 0.0 - } - else: - # Reset all systems - for system in self.offsets: - self.offsets[system] = { - 'X': 0.0, 'Y': 0.0, 'Z': 0.0, - 'A': 0.0, 'B': 0.0, 'C': 0.0 - } - - self.save_offsets() - - def set_current_as_zero(self, machine_pos: Dict[str, float], - coord_system: Optional[str] = None) -> None: - """ - Set current machine position as zero in work coordinates - - Args: - machine_pos: Current machine position - coord_system: Work coordinate system or None for active - """ - system = coord_system or self.active_system - - # Set offsets such that current machine position becomes 0,0,0 - for axis in ['X', 'Y', 'Z', 'A', 'B', 'C']: - if axis in machine_pos: - self.offsets[system][axis] = machine_pos[axis] - - self.save_offsets() \ No newline at end of file +""" +Work Coordinate System Implementation for GCODE + +Manages G54-G59 work coordinate systems and transformations between +work coordinates, machine coordinates, and robot coordinates. +""" + +import json +import os + + +class WorkCoordinateSystem: + """Manages work coordinate systems and transformations""" + + def __init__(self, config_file: str | None = None): + """ + Initialize work coordinate system + + Args: + config_file: Path to JSON file for persistent storage + """ + self.config_file = config_file or os.path.join( + os.path.dirname(__file__), "work_coordinates.json" + ) + + # Initialize default work coordinate offsets (in mm) + self.offsets = { + "G54": {"X": 0.0, "Y": 0.0, "Z": 0.0, "A": 0.0, "B": 0.0, "C": 0.0}, + "G55": {"X": 0.0, "Y": 0.0, "Z": 0.0, "A": 0.0, "B": 0.0, "C": 0.0}, + "G56": {"X": 0.0, "Y": 0.0, "Z": 0.0, "A": 0.0, "B": 0.0, "C": 0.0}, + "G57": {"X": 0.0, "Y": 0.0, "Z": 0.0, "A": 0.0, "B": 0.0, "C": 0.0}, + "G58": {"X": 0.0, "Y": 0.0, "Z": 0.0, "A": 0.0, "B": 0.0, "C": 0.0}, + "G59": {"X": 0.0, "Y": 0.0, "Z": 0.0, "A": 0.0, "B": 0.0, "C": 0.0}, + } + + # Tool offsets + self.tool_offsets = { + 0: {"Z": 0.0}, # No tool + 1: {"Z": 0.0}, # Tool 1 + 2: {"Z": 0.0}, # Tool 2 + # Add more tools as needed + } + + # Current active coordinate system + self.active_system = "G54" + + # Current tool number + self.active_tool = 0 + + # Load saved offsets if they exist + self.load_offsets() + + def set_offset(self, coord_system: str, axis: str, value: float) -> bool: + """ + Set work coordinate offset for a specific axis + + Args: + coord_system: Work coordinate system (G54-G59) + axis: Axis name (X, Y, Z, A, B, C) + value: Offset value in mm + + Returns: + True if successful, False otherwise + """ + if coord_system not in self.offsets: + return False + + if axis not in self.offsets[coord_system]: + return False + + self.offsets[coord_system][axis] = value + self.save_offsets() + return True + + def get_offset(self, coord_system: str | None = None) -> dict[str, float]: + """ + Get work coordinate offset + + Args: + coord_system: Work coordinate system (G54-G59) or None for active + + Returns: + Dictionary of axis offsets + """ + system = coord_system or self.active_system + return self.offsets.get(system, {}).copy() + + def set_active_system(self, coord_system: str) -> bool: + """ + Set the active work coordinate system + + Args: + coord_system: Work coordinate system (G54-G59) + + Returns: + True if successful, False otherwise + """ + if coord_system in self.offsets: + self.active_system = coord_system + return True + return False + + def set_tool_offset(self, tool_number: int, z_offset: float) -> None: + """ + Set tool length offset + + Args: + tool_number: Tool number + z_offset: Z-axis offset in mm + """ + if tool_number not in self.tool_offsets: + self.tool_offsets[tool_number] = {} + self.tool_offsets[tool_number]["Z"] = z_offset + self.save_offsets() + + def get_tool_offset(self, tool_number: int | None = None) -> float: + """ + Get tool length offset + + Args: + tool_number: Tool number or None for active tool + + Returns: + Z-axis offset in mm + """ + tool = tool_number if tool_number is not None else self.active_tool + return self.tool_offsets.get(tool, {}).get("Z", 0.0) + + def work_to_machine( + self, + work_pos: dict[str, float], + coord_system: str | None = None, + apply_tool_offset: bool = True, + ) -> dict[str, float]: + """ + Convert work coordinates to machine coordinates + + Args: + work_pos: Position in work coordinates + coord_system: Work coordinate system or None for active + apply_tool_offset: Whether to apply tool offset + + Returns: + Position in machine coordinates + """ + system = coord_system or self.active_system + offset = self.get_offset(system) + machine_pos = {} + + for axis in ["X", "Y", "Z", "A", "B", "C"]: + if axis in work_pos: + machine_pos[axis] = work_pos[axis] + offset.get(axis, 0.0) + + # Apply tool offset to Z axis + if axis == "Z" and apply_tool_offset: + machine_pos[axis] += self.get_tool_offset() + + return machine_pos + + def machine_to_work( + self, + machine_pos: dict[str, float], + coord_system: str | None = None, + apply_tool_offset: bool = True, + ) -> dict[str, float]: + """ + Convert machine coordinates to work coordinates + + Args: + machine_pos: Position in machine coordinates + coord_system: Work coordinate system or None for active + apply_tool_offset: Whether to apply tool offset + + Returns: + Position in work coordinates + """ + system = coord_system or self.active_system + offset = self.get_offset(system) + work_pos = {} + + for axis in ["X", "Y", "Z", "A", "B", "C"]: + if axis in machine_pos: + work_pos[axis] = machine_pos[axis] - offset.get(axis, 0.0) + + # Remove tool offset from Z axis + if axis == "Z" and apply_tool_offset: + work_pos[axis] -= self.get_tool_offset() + + return work_pos + + def apply_incremental( + self, current_pos: dict[str, float], incremental: dict[str, float] + ) -> dict[str, float]: + """ + Apply incremental movement to current position + + Args: + current_pos: Current position + incremental: Incremental movement values + + Returns: + New position after incremental movement + """ + new_pos = current_pos.copy() + + for axis in ["X", "Y", "Z", "A", "B", "C"]: + if axis in incremental: + new_pos[axis] = current_pos.get(axis, 0.0) + incremental[axis] + + return new_pos + + def robot_to_gcode_coords(self, robot_pos: list[float]) -> dict[str, float]: + """ + Convert robot Cartesian position to GCODE coordinates + + Args: + robot_pos: Robot position [X, Y, Z, RX, RY, RZ] in mm and degrees + + Returns: + GCODE coordinates {X, Y, Z, A, B, C} + """ + # For PAROL6, the mapping is straightforward + # X, Y, Z are Cartesian positions + # A, B, C are rotational axes (RX, RY, RZ) + + gcode_coords = {} + + if len(robot_pos) >= 3: + gcode_coords["X"] = robot_pos[0] + gcode_coords["Y"] = robot_pos[1] + gcode_coords["Z"] = robot_pos[2] + + if len(robot_pos) >= 6: + gcode_coords["A"] = robot_pos[3] # RX + gcode_coords["B"] = robot_pos[4] # RY + gcode_coords["C"] = robot_pos[5] # RZ + + return gcode_coords + + def gcode_to_robot_coords(self, gcode_pos: dict[str, float]) -> list[float]: + """ + Convert GCODE coordinates to robot Cartesian position + + Args: + gcode_pos: GCODE coordinates {X, Y, Z, A, B, C} + + Returns: + Robot position [X, Y, Z, RX, RY, RZ] in mm and degrees + """ + robot_pos = [ + gcode_pos.get("X", 0.0), + gcode_pos.get("Y", 0.0), + gcode_pos.get("Z", 0.0), + gcode_pos.get("A", 0.0), # RX + gcode_pos.get("B", 0.0), # RY + gcode_pos.get("C", 0.0), # RZ + ] + + return robot_pos + + def save_offsets(self) -> None: + """Save offsets to JSON file""" + data = { + "work_offsets": self.offsets, + "tool_offsets": self.tool_offsets, + "active_system": self.active_system, + "active_tool": self.active_tool, + } + + os.makedirs(os.path.dirname(self.config_file), exist_ok=True) + with open(self.config_file, "w") as f: + json.dump(data, f, indent=2) + + def load_offsets(self) -> None: + """Load offsets from JSON file""" + if os.path.exists(self.config_file): + try: + with open(self.config_file) as f: + data = json.load(f) + + self.offsets = data.get("work_offsets", self.offsets) + self.tool_offsets = data.get("tool_offsets", self.tool_offsets) + self.active_system = data.get("active_system", "G54") + self.active_tool = data.get("active_tool", 0) + except Exception as e: + print(f"Error loading work coordinate offsets: {e}") + + def reset_offset(self, coord_system: str | None = None) -> None: + """ + Reset work coordinate offset to zero + + Args: + coord_system: System to reset, or None to reset all + """ + if coord_system: + if coord_system in self.offsets: + self.offsets[coord_system] = { + "X": 0.0, + "Y": 0.0, + "Z": 0.0, + "A": 0.0, + "B": 0.0, + "C": 0.0, + } + else: + # Reset all systems + for system in self.offsets: + self.offsets[system] = { + "X": 0.0, + "Y": 0.0, + "Z": 0.0, + "A": 0.0, + "B": 0.0, + "C": 0.0, + } + + self.save_offsets() + + def set_current_as_zero( + self, machine_pos: dict[str, float], coord_system: str | None = None + ) -> None: + """ + Set current machine position as zero in work coordinates + + Args: + machine_pos: Current machine position + coord_system: Work coordinate system or None for active + """ + system = coord_system or self.active_system + + # Set offsets such that current machine position becomes 0,0,0 + for axis in ["X", "Y", "Z", "A", "B", "C"]: + if axis in machine_pos: + self.offsets[system][axis] = machine_pos[axis] + + self.save_offsets() diff --git a/parol6/gcode/interpreter.py b/parol6/gcode/interpreter.py index 36e5437..befb078 100644 --- a/parol6/gcode/interpreter.py +++ b/parol6/gcode/interpreter.py @@ -1,357 +1,369 @@ -""" -Main GCODE Interpreter for PAROL6 Robot - -Processes GCODE programs and converts them to robot commands. -Manages state, coordinates, and command execution. -""" - -import os -import numpy as np -from typing import List, Dict, Optional, Union -from .parser import GcodeParser -from .state import GcodeState -from .coordinates import WorkCoordinateSystem -from .commands import create_command_from_token - - -class GcodeInterpreter: - """Main GCODE interpreter that processes GCODE into robot commands""" - - def __init__(self, state_file: Optional[str] = None, coord_file: Optional[str] = None): - """ - Initialize GCODE interpreter - - Args: - state_file: Path to state persistence file - coord_file: Path to coordinate system persistence file - """ - # Initialize components - self.parser = GcodeParser() - - # Use default paths if not provided - if state_file is None: - state_file = os.path.join(os.path.dirname(__file__), 'gcode_state.json') - if coord_file is None: - coord_file = os.path.join(os.path.dirname(__file__), 'work_coordinates.json') - - self.state = GcodeState(state_file) - self.coord_system = WorkCoordinateSystem(coord_file) - - # Program execution state - self.program_lines = [] - self.current_line = 0 - self.is_running = False - self.is_paused = False - self.single_block = False - - # Command queue - self.command_queue = [] - - # Error tracking - self.errors = [] - - def parse_line(self, gcode_line: str) -> List[str]: - """ - Parse a single GCODE line and return robot commands - - Args: - gcode_line: Single line of GCODE - - Returns: - List of robot command strings - """ - robot_commands = [] - - # Parse the line into tokens - tokens = self.parser.parse_line(gcode_line) - - for token in tokens: - # Validate token - is_valid, error_msg = self.parser.validate_token(token) - if not is_valid: - self.errors.append(error_msg) - if self.state.program_running: - # Stop on error during program execution - self.stop_program() - continue - - # Update state with modal commands - self.state.update_from_token(token) - - # Handle work coordinate changes - if token.code_type == 'G' and int(token.code_number) in [54, 55, 56, 57, 58, 59]: - self.coord_system.set_active_system(f'G{int(token.code_number)}') - - # Create command object - command = create_command_from_token(token, self.state, self.coord_system) - - if command: - # Get robot command string - robot_cmd = command.to_robot_command() - if robot_cmd: - robot_commands.append(robot_cmd) - - # Update position after movement commands - if hasattr(command, 'target_position'): - self.state.update_position(command.target_position) - - # Handle special commands - if hasattr(command, 'is_program_end') and command.is_program_end: - self.stop_program() - elif hasattr(command, 'requires_resume') and command.requires_resume: - # Check if this is an optional stop (M1) - if hasattr(command, 'is_optional') and command.is_optional: - # Only pause if optional_stop is enabled - if self.state.optional_stop: - self.pause_program() - else: - # M0 always pauses - self.pause_program() - - return robot_commands - - def parse_program(self, program: Union[str, List[str]]) -> List[str]: - """ - Parse a complete GCODE program - - Args: - program: GCODE program as string or list of lines - - Returns: - List of all robot commands - """ - if isinstance(program, str): - lines = program.split('\n') - else: - lines = program - - all_commands = [] - self.errors = [] - - for line in lines: - commands = self.parse_line(line) - all_commands.extend(commands) - - # Stop if errors encountered - if self.errors and not self.state.program_running: - break - - return all_commands - - def load_program(self, program: Union[str, List[str]]) -> bool: - """ - Load a GCODE program for execution - - Args: - program: GCODE program as string or list of lines - - Returns: - True if program loaded successfully - """ - if isinstance(program, str): - self.program_lines = program.split('\n') - else: - self.program_lines = program - - self.current_line = 0 - self.errors = [] - self.command_queue = [] - - # Validate program with proper line number tracking - for line_num, line in enumerate(self.program_lines, 1): - tokens = self.parser.parse_line(line) - for token in tokens: - is_valid, error_msg = self.parser.validate_token(token) - if not is_valid: - self.errors.append(f"Line {line_num}: {error_msg}") - - return len(self.errors) == 0 - - def load_file(self, filepath: str) -> bool: - """ - Load GCODE program from file - - Args: - filepath: Path to GCODE file - - Returns: - True if file loaded successfully - """ - try: - with open(filepath, 'r') as f: - program = f.read() - return self.load_program(program) - except Exception as e: - self.errors.append(f"Error loading file: {e}") - return False - - def start_program(self) -> bool: - """ - Start or resume program execution - - Returns: - True if program started successfully - """ - if not self.program_lines: - self.errors.append("No program loaded") - return False - - self.is_running = True - self.is_paused = False - self.state.program_running = True - return True - - def pause_program(self) -> None: - """Pause program execution""" - self.is_paused = True - self.state.program_running = False - # Note: Command queue is not cleared so position in program is maintained - # Commands already in queue can be optionally processed or discarded by the caller - - def stop_program(self) -> None: - """Stop program execution and reset""" - self.is_running = False - self.is_paused = False - self.current_line = 0 - self.state.program_running = False - self.command_queue = [] - - def set_optional_stop(self, enabled: bool) -> None: - """ - Enable or disable optional stop (M1) - - Args: - enabled: True to enable optional stop, False to disable - """ - self.state.optional_stop = enabled - - def get_next_command(self) -> Optional[str]: - """ - Get next robot command from the program - - Returns: - Robot command string or None if no more commands - """ - # Return queued commands first - if self.command_queue: - return self.command_queue.pop(0) - - # Check if program is running - if not self.is_running or self.is_paused: - return None - - # Process lines until we get a command or reach end - while self.current_line < len(self.program_lines): - line = self.program_lines[self.current_line] - self.current_line += 1 - - # Parse line and get commands - commands = self.parse_line(line) - - if commands: - # Add to queue - self.command_queue.extend(commands) - - # Return first command - if self.command_queue: - command = self.command_queue.pop(0) - - # Check for single block mode - if self.single_block: - self.pause_program() - - return command - - # Check for errors - if self.errors: - self.stop_program() - return None - - # End of program - self.stop_program() - return None - - def set_work_offset(self, coord_system: str, axis: str, value: float) -> bool: - """ - Set work coordinate offset - - Args: - coord_system: Work coordinate system (G54-G59) - axis: Axis (X, Y, Z, A, B, C) - value: Offset value in mm - - Returns: - True if successful - """ - # Validate coordinate system - if coord_system not in ['G54', 'G55', 'G56', 'G57', 'G58', 'G59']: - self.errors.append(f"Invalid coordinate system: {coord_system}") - return False - - # Validate axis - if axis not in ['X', 'Y', 'Z', 'A', 'B', 'C']: - self.errors.append(f"Invalid axis: {axis}") - return False - - return self.coord_system.set_offset(coord_system, axis, value) - - def set_current_as_zero(self, machine_position: List[float]) -> None: - """ - Set current position as zero in active work coordinate system - - Args: - machine_position: Current machine position [X, Y, Z, RX, RY, RZ] - """ - # Convert to dictionary - pos_dict = { - 'X': machine_position[0], - 'Y': machine_position[1], - 'Z': machine_position[2], - 'A': machine_position[3] if len(machine_position) > 3 else 0, - 'B': machine_position[4] if len(machine_position) > 4 else 0, - 'C': machine_position[5] if len(machine_position) > 5 else 0 - } - - self.coord_system.set_current_as_zero(pos_dict) - - def get_status(self) -> Dict: - """ - Get interpreter status - - Returns: - Dictionary with status information - """ - return { - 'state': self.state.get_status(), - 'coord_system': self.coord_system.active_system, - 'program_running': self.is_running, - 'program_paused': self.is_paused, - 'current_line': self.current_line, - 'total_lines': len(self.program_lines), - 'errors': self.errors[-5:] if self.errors else [] # Last 5 errors - } - - def reset(self) -> None: - """Reset interpreter to initial state""" - self.state.reset() - self.stop_program() - self.errors = [] - - def set_feed_override(self, percentage: float) -> None: - """ - Set feed rate override percentage - - Args: - percentage: Override percentage (0-200) - """ - self.state.feed_override = np.clip(percentage, 0, 200) - - def set_single_block(self, enabled: bool) -> None: - """ - Enable/disable single block mode - - Args: - enabled: True to enable single block mode - """ - self.single_block = enabled - self.state.single_block = enabled \ No newline at end of file +""" +Main GCODE Interpreter for PAROL6 Robot + +Processes GCODE programs and converts them to robot commands. +Manages state, coordinates, and command execution. +""" + +import os + +import numpy as np + +from .commands import create_command_from_token +from .coordinates import WorkCoordinateSystem +from .parser import GcodeParser +from .state import GcodeState + + +class GcodeInterpreter: + """Main GCODE interpreter that processes GCODE into robot commands""" + + def __init__(self, state_file: str | None = None, coord_file: str | None = None): + """ + Initialize GCODE interpreter + + Args: + state_file: Path to state persistence file + coord_file: Path to coordinate system persistence file + """ + # Initialize components + self.parser = GcodeParser() + + # Use default paths if not provided + if state_file is None: + state_file = os.path.join(os.path.dirname(__file__), "gcode_state.json") + if coord_file is None: + coord_file = os.path.join( + os.path.dirname(__file__), "work_coordinates.json" + ) + + self.state = GcodeState(state_file) + self.coord_system = WorkCoordinateSystem(coord_file) + + # Program execution state + self.program_lines: list[str] = [] + self.current_line: int = 0 + self.is_running: bool = False + self.is_paused: bool = False + self.single_block: bool = False + + # Command queue + self.command_queue: list[str] = [] + + # Error tracking + self.errors: list[str] = [] + + def parse_line(self, gcode_line: str) -> list[str]: + """ + Parse a single GCODE line and return robot commands + + Args: + gcode_line: Single line of GCODE + + Returns: + List of robot command strings + """ + robot_commands = [] + + # Parse the line into tokens + tokens = self.parser.parse_line(gcode_line) + + for token in tokens: + # Validate token + is_valid, error_msg = self.parser.validate_token(token) + if not is_valid: + self.errors.append(error_msg or "Invalid token") + if self.state.program_running: + # Stop on error during program execution + self.stop_program() + continue + + # Update state with modal commands + self.state.update_from_token(token) + + # Handle work coordinate changes + if token.code_type == "G" and int(token.code_number) in [ + 54, + 55, + 56, + 57, + 58, + 59, + ]: + self.coord_system.set_active_system(f"G{int(token.code_number)}") + + # Create command object + command = create_command_from_token(token, self.state, self.coord_system) + + if command: + # Get robot command string + robot_cmd = command.to_robot_command() + if robot_cmd: + robot_commands.append(robot_cmd) + + # Update position after movement commands + if hasattr(command, "target_position"): + self.state.update_position(command.target_position) + + # Handle special commands + if hasattr(command, "is_program_end") and command.is_program_end: + self.stop_program() + elif hasattr(command, "requires_resume") and command.requires_resume: + # Check if this is an optional stop (M1) + if hasattr(command, "is_optional") and command.is_optional: + # Only pause if optional_stop is enabled + if self.state.optional_stop: + self.pause_program() + else: + # M0 always pauses + self.pause_program() + + return robot_commands + + def parse_program(self, program: str | list[str]) -> list[str]: + """ + Parse a complete GCODE program + + Args: + program: GCODE program as string or list of lines + + Returns: + List of all robot commands + """ + if isinstance(program, str): + lines = program.split("\n") + else: + lines = program + + all_commands = [] + self.errors = [] + + for line in lines: + commands = self.parse_line(line) + all_commands.extend(commands) + + # Stop if errors encountered + if self.errors and not self.state.program_running: + break + + return all_commands + + def load_program(self, program: str | list[str]) -> bool: + """ + Load a GCODE program for execution + + Args: + program: GCODE program as string or list of lines + + Returns: + True if program loaded successfully + """ + if isinstance(program, str): + self.program_lines = program.split("\n") + else: + self.program_lines = program + + self.current_line = 0 + self.errors = [] + self.command_queue = [] + + # Validate program with proper line number tracking + for line_num, line in enumerate(self.program_lines, 1): + tokens = self.parser.parse_line(line) + for token in tokens: + is_valid, error_msg = self.parser.validate_token(token) + if not is_valid: + self.errors.append( + f"Line {line_num}: {error_msg or 'Invalid token'}" + ) + + return len(self.errors) == 0 + + def load_file(self, filepath: str) -> bool: + """ + Load GCODE program from file + + Args: + filepath: Path to GCODE file + + Returns: + True if file loaded successfully + """ + try: + with open(filepath) as f: + program = f.read() + return self.load_program(program) + except Exception as e: + self.errors.append(f"Error loading file: {e}") + return False + + def start_program(self) -> bool: + """ + Start or resume program execution + + Returns: + True if program started successfully + """ + if not self.program_lines: + self.errors.append("No program loaded") + return False + + self.is_running = True + self.is_paused = False + self.state.program_running = True + return True + + def pause_program(self) -> None: + """Pause program execution""" + self.is_paused = True + self.state.program_running = False + # Note: Command queue is not cleared so position in program is maintained + # Commands already in queue can be optionally processed or discarded by the caller + + def stop_program(self) -> None: + """Stop program execution and reset""" + self.is_running = False + self.is_paused = False + self.current_line = 0 + self.state.program_running = False + self.command_queue = [] + + def set_optional_stop(self, enabled: bool) -> None: + """ + Enable or disable optional stop (M1) + + Args: + enabled: True to enable optional stop, False to disable + """ + self.state.optional_stop = enabled + + def get_next_command(self) -> str | None: + """ + Get next robot command from the program + + Returns: + Robot command string or None if no more commands + """ + # Return queued commands first + if self.command_queue: + return self.command_queue.pop(0) + + # Check if program is running + if not self.is_running or self.is_paused: + return None + + # Process lines until we get a command or reach end + while self.current_line < len(self.program_lines): + line = self.program_lines[self.current_line] + self.current_line += 1 + + # Parse line and get commands + commands = self.parse_line(line) + + if commands: + # Add to queue + self.command_queue.extend(commands) + + # Return first command + if self.command_queue: + command = self.command_queue.pop(0) + + # Check for single block mode + if self.single_block: + self.pause_program() + + return command + + # Check for errors + if self.errors: + self.stop_program() + return None + + # End of program + self.stop_program() + return None + + def set_work_offset(self, coord_system: str, axis: str, value: float) -> bool: + """ + Set work coordinate offset + + Args: + coord_system: Work coordinate system (G54-G59) + axis: Axis (X, Y, Z, A, B, C) + value: Offset value in mm + + Returns: + True if successful + """ + # Validate coordinate system + if coord_system not in ["G54", "G55", "G56", "G57", "G58", "G59"]: + self.errors.append(f"Invalid coordinate system: {coord_system}") + return False + + # Validate axis + if axis not in ["X", "Y", "Z", "A", "B", "C"]: + self.errors.append(f"Invalid axis: {axis}") + return False + + return self.coord_system.set_offset(coord_system, axis, value) + + def set_current_as_zero(self, machine_position: list[float]) -> None: + """ + Set current position as zero in active work coordinate system + + Args: + machine_position: Current machine position [X, Y, Z, RX, RY, RZ] + """ + # Convert to dictionary + pos_dict = { + "X": machine_position[0], + "Y": machine_position[1], + "Z": machine_position[2], + "A": machine_position[3] if len(machine_position) > 3 else 0, + "B": machine_position[4] if len(machine_position) > 4 else 0, + "C": machine_position[5] if len(machine_position) > 5 else 0, + } + + self.coord_system.set_current_as_zero(pos_dict) + + def get_status(self) -> dict: + """ + Get interpreter status + + Returns: + Dictionary with status information + """ + return { + "state": self.state.get_status(), + "coord_system": self.coord_system.active_system, + "program_running": self.is_running, + "program_paused": self.is_paused, + "current_line": self.current_line, + "total_lines": len(self.program_lines), + "errors": self.errors[-5:] if self.errors else [], # Last 5 errors + } + + def reset(self) -> None: + """Reset interpreter to initial state""" + self.state.reset() + self.stop_program() + self.errors = [] + + def set_feed_override(self, percentage: float) -> None: + """ + Set feed rate override percentage + + Args: + percentage: Override percentage (0-200) + """ + self.state.feed_override = np.clip(percentage, 0, 200) + + def set_single_block(self, enabled: bool) -> None: + """ + Enable/disable single block mode + + Args: + enabled: True to enable single block mode + """ + self.single_block = enabled + self.state.single_block = enabled diff --git a/parol6/gcode/parser.py b/parol6/gcode/parser.py index 260638d..b2316d1 100644 --- a/parol6/gcode/parser.py +++ b/parol6/gcode/parser.py @@ -1,268 +1,307 @@ -""" -GCODE Parser for PAROL6 Robot - -Tokenizes and parses GCODE lines into structured data. -Supports standard GCODE syntax including G-codes, M-codes, and parameters. -""" - -import re -from typing import Dict, List, Optional, Tuple, Union -from dataclasses import dataclass - - -@dataclass -class GcodeToken: - """Represents a parsed GCODE token""" - code_type: str # 'G', 'M', 'T', 'N', etc. - code_number: float # The numeric value - parameters: Dict[str, float] # Associated parameters (X, Y, Z, F, etc.) - comment: Optional[str] = None - line_number: Optional[int] = None - raw_line: str = "" - - def __str__(self): - result = f"{self.code_type}{self.code_number:.10g}".rstrip('0').rstrip('.') - for key, val in self.parameters.items(): - result += f" {key}{val:.10g}".rstrip('0').rstrip('.') - if self.comment: - result += f" ; {self.comment}" - return result - - -class GcodeParser: - """GCODE parser that tokenizes and validates GCODE lines""" - - # Regex patterns for parsing - COMMENT_PATTERN = re.compile(r'\((.*?)\)|;(.*)$') - LINE_NUMBER_PATTERN = re.compile(r'^N(\d+)', re.IGNORECASE) - CODE_PATTERN = re.compile(r'([GMT])(\d+(?:\.\d+)?)', re.IGNORECASE) - PARAM_PATTERN = re.compile(r'([XYZABCIJKRFSPQD])([+-]?\d*\.?\d+)', re.IGNORECASE) - - # Valid GCODE commands we support - SUPPORTED_G_CODES = { - 0: "Rapid positioning", - 1: "Linear interpolation", - 2: "Clockwise arc", - 3: "Counter-clockwise arc", - 4: "Dwell", - 17: "XY plane selection", - 18: "XZ plane selection", - 19: "YZ plane selection", - 20: "Inch units", - 21: "Millimeter units", - 28: "Return to home", - 54: "Work coordinate 1", - 55: "Work coordinate 2", - 56: "Work coordinate 3", - 57: "Work coordinate 4", - 58: "Work coordinate 5", - 59: "Work coordinate 6", - 90: "Absolute positioning", - 91: "Incremental positioning" - } - - SUPPORTED_M_CODES = { - 0: "Program stop", - 1: "Optional stop", - 3: "Spindle/Gripper on CW", - 4: "Spindle/Gripper on CCW", - 5: "Spindle/Gripper off", - 30: "Program end" - } - - def __init__(self): - self.line_count = 0 - self.errors = [] - - def parse_line(self, line: str) -> List[GcodeToken]: - """ - Parse a single line of GCODE into tokens - - Args: - line: Raw GCODE line - - Returns: - List of GcodeToken objects parsed from the line - """ - self.line_count += 1 - tokens = [] - - # Store original line - original_line = line.strip() - if not original_line: - return tokens - - # Extract and remove comments - comment = None - comment_match = self.COMMENT_PATTERN.search(line) - if comment_match: - comment = comment_match.group(1) or comment_match.group(2) - line = self.COMMENT_PATTERN.sub('', line) - - # Extract line number if present - line_number = None - line_num_match = self.LINE_NUMBER_PATTERN.match(line) - if line_num_match: - line_number = int(line_num_match.group(1)) - line = line[line_num_match.end():] - - # Convert to uppercase for parsing - line = line.upper().strip() - - # Parse all parameters first - parameters = {} - for match in self.PARAM_PATTERN.finditer(line): - param_letter = match.group(1) - try: - param_value = float(match.group(2)) - # Validate feed rate - if param_letter == 'F' and param_value <= 0: - self.errors.append(f"Line {self.line_count}: Invalid feed rate: {param_value}") - continue - # Validate spindle speed - if param_letter == 'S' and param_value < 0: - self.errors.append(f"Line {self.line_count}: Invalid spindle speed: {param_value}") - continue - parameters[param_letter] = param_value - except ValueError: - self.errors.append(f"Line {self.line_count}: Invalid numeric value for {param_letter}: {match.group(2)}") - - # Parse G and M codes - codes_found = [] - for match in self.CODE_PATTERN.finditer(line): - code_type = match.group(1) - code_number = float(match.group(2)) - codes_found.append((code_type, code_number)) - - # Create tokens for each code found - if codes_found: - for code_type, code_number in codes_found: - # For motion commands, include coordinate parameters - if code_type == 'G' and code_number in [0, 1, 2, 3]: - motion_params = {k: v for k, v in parameters.items() - if k in ['X', 'Y', 'Z', 'A', 'B', 'C', 'I', 'J', 'K', 'R', 'F']} - token = GcodeToken( - code_type=code_type, - code_number=code_number, - parameters=motion_params, - comment=comment, - line_number=line_number, - raw_line=original_line - ) - else: - # Other codes get remaining parameters - token = GcodeToken( - code_type=code_type, - code_number=code_number, - parameters={k: v for k, v in parameters.items() - if k not in ['X', 'Y', 'Z', 'A', 'B', 'C', 'I', 'J', 'K', 'R', 'F']}, - comment=comment, - line_number=line_number, - raw_line=original_line - ) - tokens.append(token) - - # Handle standalone feed rate - elif 'F' in parameters and not codes_found: - token = GcodeToken( - code_type='F', - code_number=parameters['F'], - parameters={}, - comment=comment, - line_number=line_number, - raw_line=original_line - ) - tokens.append(token) - - # Handle comment-only lines - elif comment: - token = GcodeToken( - code_type='COMMENT', - code_number=0, - parameters={}, - comment=comment, - line_number=line_number, - raw_line=original_line - ) - tokens.append(token) - - return tokens - - def validate_token(self, token: GcodeToken) -> Tuple[bool, Optional[str]]: - """ - Validate a GCODE token - - Args: - token: GcodeToken to validate - - Returns: - Tuple of (is_valid, error_message) - """ - if token.code_type == 'G': - if token.code_number not in self.SUPPORTED_G_CODES: - return False, f"Unsupported G-code: G{token.code_number}" - - # Validate required parameters for motion commands - if token.code_number in [0, 1]: # Linear motion - if not any(k in token.parameters for k in ['X', 'Y', 'Z', 'A', 'B', 'C']): - return False, f"G{token.code_number} requires at least one coordinate" - - elif token.code_number in [2, 3]: # Arc motion - if not any(k in token.parameters for k in ['X', 'Y', 'Z']): - return False, f"G{token.code_number} requires endpoint coordinates" - if not (('I' in token.parameters or 'J' in token.parameters) or 'R' in token.parameters): - return False, f"G{token.code_number} requires either IJK offsets or R radius" - - elif token.code_number == 4: # Dwell - if 'P' not in token.parameters and 'S' not in token.parameters: - return False, "G4 dwell requires P (milliseconds) or S (seconds) parameter" - - elif token.code_type == 'M': - if token.code_number not in self.SUPPORTED_M_CODES: - return False, f"Unsupported M-code: M{token.code_number}" - - elif token.code_type in ['F', 'T', 'S', 'COMMENT']: - # These are always valid if parsed - pass - - else: - return False, f"Unknown code type: {token.code_type}" - - return True, None - - def parse_program(self, program: Union[str, List[str]]) -> List[GcodeToken]: - """ - Parse a complete GCODE program - - Args: - program: Either a string with newlines or a list of lines - - Returns: - List of all tokens in the program - """ - if isinstance(program, str): - lines = program.split('\n') - else: - lines = program - - all_tokens = [] - self.errors = [] - self.line_count = 0 - - for line in lines: - try: - tokens = self.parse_line(line) - for token in tokens: - is_valid, error_msg = self.validate_token(token) - if not is_valid: - self.errors.append(f"Line {self.line_count}: {error_msg}") - else: - all_tokens.append(token) - except Exception as e: - self.errors.append(f"Line {self.line_count}: Parse error - {str(e)}") - - return all_tokens - - def get_errors(self) -> List[str]: - """Get list of parsing errors""" - return self.errors \ No newline at end of file +""" +GCODE Parser for PAROL6 Robot + +Tokenizes and parses GCODE lines into structured data. +Supports standard GCODE syntax including G-codes, M-codes, and parameters. +""" + +import re +from dataclasses import dataclass + + +@dataclass +class GcodeToken: + """Represents a parsed GCODE token""" + + code_type: str # 'G', 'M', 'T', 'N', etc. + code_number: float # The numeric value + parameters: dict[str, float] # Associated parameters (X, Y, Z, F, etc.) + comment: str | None = None + line_number: int | None = None + raw_line: str = "" + + def __str__(self): + result = f"{self.code_type}{self.code_number:.10g}".rstrip("0").rstrip(".") + for key, val in self.parameters.items(): + result += f" {key}{val:.10g}".rstrip("0").rstrip(".") + if self.comment: + result += f" ; {self.comment}" + return result + + +class GcodeParser: + """GCODE parser that tokenizes and validates GCODE lines""" + + # Regex patterns for parsing + COMMENT_PATTERN = re.compile(r"\((.*?)\)|;(.*)$") + LINE_NUMBER_PATTERN = re.compile(r"^N(\d+)", re.IGNORECASE) + CODE_PATTERN = re.compile(r"([GMT])(\d+(?:\.\d+)?)", re.IGNORECASE) + PARAM_PATTERN = re.compile(r"([XYZABCIJKRFSPQD])([+-]?\d*\.?\d+)", re.IGNORECASE) + + # Valid GCODE commands we support + SUPPORTED_G_CODES = { + 0: "Rapid positioning", + 1: "Linear interpolation", + 2: "Clockwise arc", + 3: "Counter-clockwise arc", + 4: "Dwell", + 17: "XY plane selection", + 18: "XZ plane selection", + 19: "YZ plane selection", + 20: "Inch units", + 21: "Millimeter units", + 28: "Return to home", + 54: "Work coordinate 1", + 55: "Work coordinate 2", + 56: "Work coordinate 3", + 57: "Work coordinate 4", + 58: "Work coordinate 5", + 59: "Work coordinate 6", + 90: "Absolute positioning", + 91: "Incremental positioning", + } + + SUPPORTED_M_CODES = { + 0: "Program stop", + 1: "Optional stop", + 3: "Spindle/Gripper on CW", + 4: "Spindle/Gripper on CCW", + 5: "Spindle/Gripper off", + 30: "Program end", + } + + def __init__(self): + self.line_count = 0 + self.errors = [] + + def parse_line(self, line: str) -> list[GcodeToken]: + """ + Parse a single line of GCODE into tokens + + Args: + line: Raw GCODE line + + Returns: + List of GcodeToken objects parsed from the line + """ + self.line_count += 1 + tokens: list[GcodeToken] = [] + + # Store original line + original_line = line.strip() + if not original_line: + return tokens + + # Extract and remove comments + comment = None + comment_match = self.COMMENT_PATTERN.search(line) + if comment_match: + comment = comment_match.group(1) or comment_match.group(2) + line = self.COMMENT_PATTERN.sub("", line) + + # Extract line number if present + line_number = None + line_num_match = self.LINE_NUMBER_PATTERN.match(line) + if line_num_match: + line_number = int(line_num_match.group(1)) + line = line[line_num_match.end() :] + + # Convert to uppercase for parsing + line = line.upper().strip() + + # Parse all parameters first + parameters: dict[str, float] = {} + for match in self.PARAM_PATTERN.finditer(line): + param_letter = match.group(1) + try: + param_value = float(match.group(2)) + # Validate feed rate + if param_letter == "F" and param_value <= 0: + self.errors.append( + f"Line {self.line_count}: Invalid feed rate: {param_value}" + ) + continue + # Validate spindle speed + if param_letter == "S" and param_value < 0: + self.errors.append( + f"Line {self.line_count}: Invalid spindle speed: {param_value}" + ) + continue + parameters[param_letter] = param_value + except ValueError: + self.errors.append( + f"Line {self.line_count}: Invalid numeric value for {param_letter}: {match.group(2)}" + ) + + # Parse G and M codes + codes_found: list[tuple[str, float]] = [] + for match in self.CODE_PATTERN.finditer(line): + code_type = match.group(1) + code_number = float(match.group(2)) + codes_found.append((code_type, code_number)) + + # Create tokens for each code found + if codes_found: + for code_type, code_number in codes_found: + # For motion commands, include coordinate parameters + if code_type == "G" and code_number in [0, 1, 2, 3]: + motion_params = { + k: v + for k, v in parameters.items() + if k in ["X", "Y", "Z", "A", "B", "C", "I", "J", "K", "R", "F"] + } + token = GcodeToken( + code_type=code_type, + code_number=code_number, + parameters=motion_params, + comment=comment, + line_number=line_number, + raw_line=original_line, + ) + else: + # Other codes get remaining parameters + token = GcodeToken( + code_type=code_type, + code_number=code_number, + parameters={ + k: v + for k, v in parameters.items() + if k + not in [ + "X", + "Y", + "Z", + "A", + "B", + "C", + "I", + "J", + "K", + "R", + "F", + ] + }, + comment=comment, + line_number=line_number, + raw_line=original_line, + ) + tokens.append(token) + + # Handle standalone feed rate + elif "F" in parameters and not codes_found: + token = GcodeToken( + code_type="F", + code_number=parameters["F"], + parameters={}, + comment=comment, + line_number=line_number, + raw_line=original_line, + ) + tokens.append(token) + + # Handle comment-only lines + elif comment: + token = GcodeToken( + code_type="COMMENT", + code_number=0, + parameters={}, + comment=comment, + line_number=line_number, + raw_line=original_line, + ) + tokens.append(token) + + return tokens + + def validate_token(self, token: GcodeToken) -> tuple[bool, str | None]: + """ + Validate a GCODE token + + Args: + token: GcodeToken to validate + + Returns: + Tuple of (is_valid, error_message) + """ + if token.code_type == "G": + if token.code_number not in self.SUPPORTED_G_CODES: + return False, f"Unsupported G-code: G{token.code_number}" + + # Validate required parameters for motion commands + if token.code_number in [0, 1]: # Linear motion + if not any( + k in token.parameters for k in ["X", "Y", "Z", "A", "B", "C"] + ): + return ( + False, + f"G{token.code_number} requires at least one coordinate", + ) + + elif token.code_number in [2, 3]: # Arc motion + if not any(k in token.parameters for k in ["X", "Y", "Z"]): + return False, f"G{token.code_number} requires endpoint coordinates" + if not ( + ("I" in token.parameters or "J" in token.parameters) + or "R" in token.parameters + ): + return ( + False, + f"G{token.code_number} requires either IJK offsets or R radius", + ) + + elif token.code_number == 4: # Dwell + if "P" not in token.parameters and "S" not in token.parameters: + return ( + False, + "G4 dwell requires P (milliseconds) or S (seconds) parameter", + ) + + elif token.code_type == "M": + if token.code_number not in self.SUPPORTED_M_CODES: + return False, f"Unsupported M-code: M{token.code_number}" + + elif token.code_type in ["F", "T", "S", "COMMENT"]: + # These are always valid if parsed + pass + + else: + return False, f"Unknown code type: {token.code_type}" + + return True, None + + def parse_program(self, program: str | list[str]) -> list[GcodeToken]: + """ + Parse a complete GCODE program + + Args: + program: Either a string with newlines or a list of lines + + Returns: + List of all tokens in the program + """ + if isinstance(program, str): + lines = program.split("\n") + else: + lines = program + + all_tokens = [] + self.errors = [] + self.line_count = 0 + + for line in lines: + try: + tokens = self.parse_line(line) + for token in tokens: + is_valid, error_msg = self.validate_token(token) + if not is_valid: + self.errors.append(f"Line {self.line_count}: {error_msg}") + else: + all_tokens.append(token) + except Exception as e: + self.errors.append(f"Line {self.line_count}: Parse error - {e!s}") + + return all_tokens + + def get_errors(self) -> list[str]: + """Get list of parsing errors""" + return self.errors diff --git a/parol6/gcode/state.py b/parol6/gcode/state.py index 8cc3ae6..a31be41 100644 --- a/parol6/gcode/state.py +++ b/parol6/gcode/state.py @@ -1,337 +1,346 @@ -""" -GCODE State Management for PAROL6 Robot - -Tracks modal states during GCODE execution including: -- Coordinate systems (G54-G59) -- Positioning modes (G90/G91) -- Units (G20/G21) -- Feed rates and spindle speeds -- Active plane (G17/G18/G19) -""" - -import json -import os -from typing import Dict, Optional -from dataclasses import dataclass, field - - -@dataclass -class GcodeState: - """Tracks modal GCODE state during execution""" - - # Motion modes - motion_mode: str = 'G0' # G0, G1, G2, G3 - positioning_mode: str = 'G90' # G90 (absolute) or G91 (incremental) - - # Coordinate system - work_coordinate: str = 'G54' # G54-G59 - work_offsets: Dict[str, Dict[str, float]] = field(default_factory=lambda: { - 'G54': {'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0}, - 'G55': {'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0}, - 'G56': {'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0}, - 'G57': {'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0}, - 'G58': {'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0}, - 'G59': {'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0} - }) - - # Current position (in work coordinates) - current_position: Dict[str, float] = field(default_factory=lambda: { - 'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0 - }) - - # Machine position (absolute, no offsets) - machine_position: Dict[str, float] = field(default_factory=lambda: { - 'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0 - }) - - # Units and scaling - units: str = 'G21' # G20 (inches) or G21 (mm) - units_scale: float = 1.0 # Multiplier to convert to mm - - # Feed and speed - feed_rate: float = 100.0 # mm/min - spindle_speed: float = 0.0 # RPM - - # Plane selection for arcs - plane: str = 'G17' # G17 (XY), G18 (XZ), G19 (YZ) - - # Tool - tool_number: int = 0 - tool_length_offset: float = 0.0 - - # Program control - program_running: bool = False - single_block: bool = False - optional_stop: bool = False # M1 optional stop enable - feed_override: float = 100.0 # Percentage - - def __init__(self, state_file: Optional[str] = None): - """ - Initialize GCODE state, optionally loading from file - - Args: - state_file: Path to JSON file for persistent state - """ - # Initialize all dataclass fields with their defaults first - self.motion_mode = 'G0' - self.positioning_mode = 'G90' - self.work_coordinate = 'G54' - self.work_offsets = { - 'G54': {'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0}, - 'G55': {'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0}, - 'G56': {'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0}, - 'G57': {'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0}, - 'G58': {'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0}, - 'G59': {'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0} - } - self.current_position = {'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0} - self.machine_position = {'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0} - self.units = 'G21' - self.units_scale = 1.0 - self.feed_rate = 100.0 - self.spindle_speed = 0.0 - self.plane = 'G17' - self.tool_number = 0 - self.tool_length_offset = 0.0 - self.program_running = False - self.single_block = False - self.optional_stop = False - self.feed_override = 100.0 - - # Now handle the state file - self.state_file = state_file - if state_file and os.path.exists(state_file): - self.load_state() - - def update_from_token(self, token) -> None: - """ - Update state based on a GCODE token - - Args: - token: GcodeToken object - """ - if token.code_type == 'G': - code = int(token.code_number) - - # Motion modes - if code in [0, 1, 2, 3]: - self.motion_mode = f'G{code}' - - # Plane selection - elif code in [17, 18, 19]: - self.plane = f'G{code}' - - # Units - elif code == 20: # Inches - self.units = 'G20' - self.units_scale = 25.4 # Convert inches to mm - elif code == 21: # Millimeters - self.units = 'G21' - self.units_scale = 1.0 - - # Work coordinates - elif code in [54, 55, 56, 57, 58, 59]: - self.work_coordinate = f'G{code}' - - # Positioning mode - elif code == 90: - self.positioning_mode = 'G90' - elif code == 91: - self.positioning_mode = 'G91' - - elif token.code_type == 'F': - # Feed rate - self.feed_rate = token.code_number * self.units_scale - - elif token.code_type == 'S': - # Spindle speed - self.spindle_speed = token.code_number - - elif token.code_type == 'T': - # Tool number - self.tool_number = int(token.code_number) - - def get_work_offset(self) -> Dict[str, float]: - """Get current work coordinate offset""" - return self.work_offsets[self.work_coordinate] - - def set_work_offset(self, axis: str, value: float) -> None: - """ - Set work coordinate offset for an axis - - Args: - axis: Axis name (X, Y, Z, A, B, C) - value: Offset value in mm - """ - if axis in self.work_offsets[self.work_coordinate]: - self.work_offsets[self.work_coordinate][axis] = value - if self.state_file: - self.save_state() - - def work_to_machine(self, work_coords: Dict[str, float]) -> Dict[str, float]: - """ - Convert work coordinates to machine coordinates - - Args: - work_coords: Dictionary of work coordinates - - Returns: - Dictionary of machine coordinates - """ - machine_coords = {} - offset = self.get_work_offset() - - for axis in ['X', 'Y', 'Z', 'A', 'B', 'C']: - if axis in work_coords: - # Apply work offset and tool offset (for Z) - machine_coords[axis] = work_coords[axis] + offset.get(axis, 0) - if axis == 'Z': - machine_coords[axis] += self.tool_length_offset - - return machine_coords - - def machine_to_work(self, machine_coords: Dict[str, float]) -> Dict[str, float]: - """ - Convert machine coordinates to work coordinates - - Args: - machine_coords: Dictionary of machine coordinates - - Returns: - Dictionary of work coordinates - """ - work_coords = {} - offset = self.get_work_offset() - - for axis in ['X', 'Y', 'Z', 'A', 'B', 'C']: - if axis in machine_coords: - # Remove work offset and tool offset (for Z) - work_coords[axis] = machine_coords[axis] - offset.get(axis, 0) - if axis == 'Z': - work_coords[axis] -= self.tool_length_offset - - return work_coords - - def calculate_target_position(self, parameters: Dict[str, float]) -> Dict[str, float]: - """ - Calculate target position based on positioning mode and parameters - - Args: - parameters: Dictionary of axis values from GCODE - - Returns: - Dictionary of target positions in work coordinates - """ - target = self.current_position.copy() - - for axis in ['X', 'Y', 'Z', 'A', 'B', 'C']: - if axis in parameters: - value = parameters[axis] * self.units_scale - - if self.positioning_mode == 'G90': # Absolute - target[axis] = value - else: # G91 - Incremental - target[axis] = self.current_position[axis] + value - - return target - - def update_position(self, new_position: Dict[str, float]) -> None: - """ - Update current position - - Args: - new_position: New position in work coordinates - """ - self.current_position.update(new_position) - # Update machine position - machine_pos = self.work_to_machine(new_position) - self.machine_position.update(machine_pos) - - def reset(self) -> None: - """Reset state to defaults""" - self.motion_mode = 'G0' - self.positioning_mode = 'G90' - self.work_coordinate = 'G54' - self.units = 'G21' - self.units_scale = 1.0 - self.feed_rate = 100.0 - self.spindle_speed = 0.0 - self.plane = 'G17' - self.tool_number = 0 - self.tool_length_offset = 0.0 - self.program_running = False - self.single_block = False - self.feed_override = 100.0 - - # Keep work offsets but reset positions - self.current_position = {'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0} - self.machine_position = {'X': 0, 'Y': 0, 'Z': 0, 'A': 0, 'B': 0, 'C': 0} - - def save_state(self) -> None: - """Save state to JSON file""" - if self.state_file: - # Save complete modal state - state_dict = { - 'work_offsets': self.work_offsets, - 'units': self.units, - 'work_coordinate': self.work_coordinate, - 'tool_length_offset': self.tool_length_offset, - 'motion_mode': self.motion_mode, - 'positioning_mode': self.positioning_mode, - 'plane': self.plane, - 'feed_rate': self.feed_rate, - 'spindle_speed': self.spindle_speed, - 'current_position': self.current_position, - 'machine_position': self.machine_position - } - - # Create directory if needed (only if path has a directory component) - dir_name = os.path.dirname(self.state_file) - if dir_name: - os.makedirs(dir_name, exist_ok=True) - with open(self.state_file, 'w') as f: - json.dump(state_dict, f, indent=2) - - def load_state(self) -> None: - """Load state from JSON file""" - if self.state_file and os.path.exists(self.state_file): - try: - with open(self.state_file, 'r') as f: - state_dict = json.load(f) - - # Load complete modal state - self.work_offsets = state_dict.get('work_offsets', self.work_offsets) - self.units = state_dict.get('units', self.units) - self.work_coordinate = state_dict.get('work_coordinate', self.work_coordinate) - self.tool_length_offset = state_dict.get('tool_length_offset', 0.0) - self.motion_mode = state_dict.get('motion_mode', self.motion_mode) - self.positioning_mode = state_dict.get('positioning_mode', self.positioning_mode) - self.plane = state_dict.get('plane', self.plane) - self.feed_rate = state_dict.get('feed_rate', self.feed_rate) - self.spindle_speed = state_dict.get('spindle_speed', self.spindle_speed) - if 'current_position' in state_dict: - self.current_position = state_dict['current_position'] - if 'machine_position' in state_dict: - self.machine_position = state_dict['machine_position'] - - # Update units scale - self.units_scale = 25.4 if self.units == 'G20' else 1.0 - except Exception as e: - print(f"Error loading state file: {e}") - - def get_status(self) -> Dict: - """Get current state as dictionary for status reporting""" - return { - 'motion_mode': self.motion_mode, - 'positioning_mode': self.positioning_mode, - 'work_coordinate': self.work_coordinate, - 'units': self.units, - 'feed_rate': self.feed_rate, - 'spindle_speed': self.spindle_speed, - 'plane': self.plane, - 'tool_number': self.tool_number, - 'current_position': self.current_position.copy(), - 'machine_position': self.machine_position.copy(), - 'program_running': self.program_running, - 'optional_stop': self.optional_stop - } \ No newline at end of file +""" +GCODE State Management for PAROL6 Robot + +Tracks modal states during GCODE execution including: +- Coordinate systems (G54-G59) +- Positioning modes (G90/G91) +- Units (G20/G21) +- Feed rates and spindle speeds +- Active plane (G17/G18/G19) +""" + +import json +import os +from dataclasses import dataclass, field + +from parol6.gcode.parser import GcodeToken + + +@dataclass +class GcodeState: + """Tracks modal GCODE state during execution""" + + # Motion modes + motion_mode: str = "G0" # G0, G1, G2, G3 + positioning_mode: str = "G90" # G90 (absolute) or G91 (incremental) + + # Coordinate system + work_coordinate: str = "G54" # G54-G59 + work_offsets: dict[str, dict[str, float]] = field( + default_factory=lambda: { + "G54": {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0}, + "G55": {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0}, + "G56": {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0}, + "G57": {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0}, + "G58": {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0}, + "G59": {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0}, + } + ) + + # Current position (in work coordinates) + current_position: dict[str, float] = field( + default_factory=lambda: {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0} + ) + + # Machine position (absolute, no offsets) + machine_position: dict[str, float] = field( + default_factory=lambda: {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0} + ) + + # Units and scaling + units: str = "G21" # G20 (inches) or G21 (mm) + units_scale: float = 1.0 # Multiplier to convert to mm + + # Feed and speed + feed_rate: float = 100.0 # mm/min + spindle_speed: float = 0.0 # RPM + + # Plane selection for arcs + plane: str = "G17" # G17 (XY), G18 (XZ), G19 (YZ) + + # Tool + tool_number: int = 0 + tool_length_offset: float = 0.0 + + # Program control + program_running: bool = False + single_block: bool = False + optional_stop: bool = False # M1 optional stop enable + feed_override: float = 100.0 # Percentage + + def __init__(self, state_file: str | None = None): + """ + Initialize GCODE state, optionally loading from file + + Args: + state_file: Path to JSON file for persistent state + """ + # Initialize all dataclass fields with their defaults first + self.motion_mode = "G0" + self.positioning_mode = "G90" + self.work_coordinate = "G54" + self.work_offsets = { + "G54": {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0}, + "G55": {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0}, + "G56": {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0}, + "G57": {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0}, + "G58": {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0}, + "G59": {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0}, + } + self.current_position = {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0} + self.machine_position = {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0} + self.units = "G21" + self.units_scale = 1.0 + self.feed_rate = 100.0 + self.spindle_speed = 0.0 + self.plane = "G17" + self.tool_number = 0 + self.tool_length_offset = 0.0 + self.program_running = False + self.single_block = False + self.optional_stop = False + self.feed_override = 100.0 + + # Now handle the state file + self.state_file = state_file + if state_file and os.path.exists(state_file): + self.load_state() + + def update_from_token(self, token: GcodeToken) -> None: + """ + Update state based on a GCODE token + + Args: + token: GcodeToken object + """ + if token.code_type == "G": + code = int(token.code_number) + + # Motion modes + if code in [0, 1, 2, 3]: + self.motion_mode = f"G{code}" + + # Plane selection + elif code in [17, 18, 19]: + self.plane = f"G{code}" + + # Units + elif code == 20: # Inches + self.units = "G20" + self.units_scale = 25.4 # Convert inches to mm + elif code == 21: # Millimeters + self.units = "G21" + self.units_scale = 1.0 + + # Work coordinates + elif code in [54, 55, 56, 57, 58, 59]: + self.work_coordinate = f"G{code}" + + # Positioning mode + elif code == 90: + self.positioning_mode = "G90" + elif code == 91: + self.positioning_mode = "G91" + + elif token.code_type == "F": + # Feed rate + self.feed_rate = token.code_number * self.units_scale + + elif token.code_type == "S": + # Spindle speed + self.spindle_speed = token.code_number + + elif token.code_type == "T": + # Tool number + self.tool_number = int(token.code_number) + + def get_work_offset(self) -> dict[str, float]: + """Get current work coordinate offset""" + return self.work_offsets[self.work_coordinate] + + def set_work_offset(self, axis: str, value: float) -> None: + """ + Set work coordinate offset for an axis + + Args: + axis: Axis name (X, Y, Z, A, B, C) + value: Offset value in mm + """ + if axis in self.work_offsets[self.work_coordinate]: + self.work_offsets[self.work_coordinate][axis] = value + if self.state_file: + self.save_state() + + def work_to_machine(self, work_coords: dict[str, float]) -> dict[str, float]: + """ + Convert work coordinates to machine coordinates + + Args: + work_coords: Dictionary of work coordinates + + Returns: + Dictionary of machine coordinates + """ + machine_coords = {} + offset = self.get_work_offset() + + for axis in ["X", "Y", "Z", "A", "B", "C"]: + if axis in work_coords: + # Apply work offset and tool offset (for Z) + machine_coords[axis] = work_coords[axis] + offset.get(axis, 0) + if axis == "Z": + machine_coords[axis] += self.tool_length_offset + + return machine_coords + + def machine_to_work(self, machine_coords: dict[str, float]) -> dict[str, float]: + """ + Convert machine coordinates to work coordinates + + Args: + machine_coords: Dictionary of machine coordinates + + Returns: + Dictionary of work coordinates + """ + work_coords = {} + offset = self.get_work_offset() + + for axis in ["X", "Y", "Z", "A", "B", "C"]: + if axis in machine_coords: + # Remove work offset and tool offset (for Z) + work_coords[axis] = machine_coords[axis] - offset.get(axis, 0) + if axis == "Z": + work_coords[axis] -= self.tool_length_offset + + return work_coords + + def calculate_target_position( + self, parameters: dict[str, float] + ) -> dict[str, float]: + """ + Calculate target position based on positioning mode and parameters + + Args: + parameters: Dictionary of axis values from GCODE + + Returns: + Dictionary of target positions in work coordinates + """ + target = self.current_position.copy() + + for axis in ["X", "Y", "Z", "A", "B", "C"]: + if axis in parameters: + value = parameters[axis] * self.units_scale + + if self.positioning_mode == "G90": # Absolute + target[axis] = value + else: # G91 - Incremental + target[axis] = self.current_position[axis] + value + + return target + + def update_position(self, new_position: dict[str, float]) -> None: + """ + Update current position + + Args: + new_position: New position in work coordinates + """ + self.current_position.update(new_position) + # Update machine position + machine_pos = self.work_to_machine(new_position) + self.machine_position.update(machine_pos) + + def reset(self) -> None: + """Reset state to defaults""" + self.motion_mode = "G0" + self.positioning_mode = "G90" + self.work_coordinate = "G54" + self.units = "G21" + self.units_scale = 1.0 + self.feed_rate = 100.0 + self.spindle_speed = 0.0 + self.plane = "G17" + self.tool_number = 0 + self.tool_length_offset = 0.0 + self.program_running = False + self.single_block = False + self.feed_override = 100.0 + + # Keep work offsets but reset positions + self.current_position = {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0} + self.machine_position = {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0} + + def save_state(self) -> None: + """Save state to JSON file""" + if self.state_file: + # Save complete modal state + state_dict = { + "work_offsets": self.work_offsets, + "units": self.units, + "work_coordinate": self.work_coordinate, + "tool_length_offset": self.tool_length_offset, + "motion_mode": self.motion_mode, + "positioning_mode": self.positioning_mode, + "plane": self.plane, + "feed_rate": self.feed_rate, + "spindle_speed": self.spindle_speed, + "current_position": self.current_position, + "machine_position": self.machine_position, + } + + # Create directory if needed (only if path has a directory component) + dir_name = os.path.dirname(self.state_file) + if dir_name: + os.makedirs(dir_name, exist_ok=True) + with open(self.state_file, "w") as f: + json.dump(state_dict, f, indent=2) + + def load_state(self) -> None: + """Load state from JSON file""" + if self.state_file and os.path.exists(self.state_file): + try: + with open(self.state_file) as f: + state_dict = json.load(f) + + # Load complete modal state + self.work_offsets = state_dict.get("work_offsets", self.work_offsets) + self.units = state_dict.get("units", self.units) + self.work_coordinate = state_dict.get( + "work_coordinate", self.work_coordinate + ) + self.tool_length_offset = state_dict.get("tool_length_offset", 0.0) + self.motion_mode = state_dict.get("motion_mode", self.motion_mode) + self.positioning_mode = state_dict.get( + "positioning_mode", self.positioning_mode + ) + self.plane = state_dict.get("plane", self.plane) + self.feed_rate = state_dict.get("feed_rate", self.feed_rate) + self.spindle_speed = state_dict.get("spindle_speed", self.spindle_speed) + if "current_position" in state_dict: + self.current_position = state_dict["current_position"] + if "machine_position" in state_dict: + self.machine_position = state_dict["machine_position"] + + # Update units scale + self.units_scale = 25.4 if self.units == "G20" else 1.0 + except Exception as e: + print(f"Error loading state file: {e}") + + def get_status(self) -> dict: + """Get current state as dictionary for status reporting""" + return { + "motion_mode": self.motion_mode, + "positioning_mode": self.positioning_mode, + "work_coordinate": self.work_coordinate, + "units": self.units, + "feed_rate": self.feed_rate, + "spindle_speed": self.spindle_speed, + "plane": self.plane, + "tool_number": self.tool_number, + "current_position": self.current_position.copy(), + "machine_position": self.machine_position.copy(), + "program_running": self.program_running, + "optional_stop": self.optional_stop, + } diff --git a/parol6/gcode/utils.py b/parol6/gcode/utils.py index 0dab484..a79cfd9 100644 --- a/parol6/gcode/utils.py +++ b/parol6/gcode/utils.py @@ -1,346 +1,360 @@ -""" -Utility functions for GCODE processing - -Provides conversion functions, calculations, and helpers for GCODE implementation. -""" - -import math -import numpy as np -from typing import Dict, List - - -def feed_rate_to_duration(distance: float, feed_rate: float) -> float: - """ - Convert feed rate to duration for a given distance - - Args: - distance: Distance to travel in mm - feed_rate: Feed rate in mm/min - - Returns: - Duration in seconds - """ - if feed_rate <= 0: - return 0 - - # Convert mm/min to mm/s - feed_rate_mm_s = feed_rate / 60.0 - - # Calculate duration - duration = distance / feed_rate_mm_s - - return duration - - -def feed_rate_to_speed_percentage(feed_rate: float, - min_speed: float = 120.0, - max_speed: float = 3600.0) -> float: - """ - Convert feed rate to speed percentage - - Args: - feed_rate: Feed rate in mm/min - min_speed: Minimum speed in mm/min (default 120 = 2 mm/s) - max_speed: Maximum speed in mm/min (default 3600 = 60 mm/s) - - Returns: - Speed percentage (0-100) - """ - # Clamp feed rate to valid range - feed_rate = np.clip(feed_rate, min_speed, max_speed) - - # Map to percentage - speed_percentage = np.interp(feed_rate, [min_speed, max_speed], [0, 100]) - - return speed_percentage - - -def speed_percentage_to_feed_rate(speed_percentage: float, - min_speed: float = 120.0, - max_speed: float = 3600.0) -> float: - """ - Convert speed percentage to feed rate - - Args: - speed_percentage: Speed percentage (0-100) - min_speed: Minimum speed in mm/min - max_speed: Maximum speed in mm/min - - Returns: - Feed rate in mm/min - """ - # Clamp percentage - speed_percentage = np.clip(speed_percentage, 0, 100) - - # Map to feed rate - feed_rate = np.interp(speed_percentage, [0, 100], [min_speed, max_speed]) - - return feed_rate - - -def calculate_distance(start: Dict[str, float], end: Dict[str, float]) -> float: - """ - Calculate Euclidean distance between two points - - Args: - start: Starting position {X, Y, Z, ...} - end: Ending position {X, Y, Z, ...} - - Returns: - Distance in mm - """ - distance = 0 - for axis in ['X', 'Y', 'Z']: - if axis in start and axis in end: - distance += (end[axis] - start[axis]) ** 2 - - return math.sqrt(distance) - - -def ijk_to_center(start: Dict[str, float], ijk: Dict[str, float], - plane: str = 'G17') -> Dict[str, float]: - """ - Convert IJK offsets to arc center point - - Args: - start: Starting position - ijk: IJK offset values (relative to start) - plane: Active plane (G17=XY, G18=XZ, G19=YZ) - - Returns: - Center point coordinates - """ - center = start.copy() - - if plane == 'G17': # XY plane - if 'I' in ijk: - center['X'] = start.get('X', 0) + ijk['I'] - if 'J' in ijk: - center['Y'] = start.get('Y', 0) + ijk['J'] - elif plane == 'G18': # XZ plane - if 'I' in ijk: - center['X'] = start.get('X', 0) + ijk['I'] - if 'K' in ijk: - center['Z'] = start.get('Z', 0) + ijk['K'] - elif plane == 'G19': # YZ plane - if 'J' in ijk: - center['Y'] = start.get('Y', 0) + ijk['J'] - if 'K' in ijk: - center['Z'] = start.get('Z', 0) + ijk['K'] - - return center - - -def radius_to_center(start: Dict[str, float], end: Dict[str, float], - radius: float, clockwise: bool = True, - plane: str = 'G17') -> Dict[str, float]: - """ - Calculate arc center from radius - - Args: - start: Starting position - end: Ending position - radius: Arc radius (positive for <180°, negative for >180°) - clockwise: True for G2, False for G3 - plane: Active plane - - Returns: - Center point coordinates - """ - # Get the two axes involved in the arc based on plane - if plane == 'G17': # XY plane - axis1, axis2 = 'X', 'Y' - elif plane == 'G18': # XZ plane - axis1, axis2 = 'X', 'Z' - else: # G19 - YZ plane - axis1, axis2 = 'Y', 'Z' - - # Get start and end coordinates - x1 = start.get(axis1, 0) - y1 = start.get(axis2, 0) - x2 = end.get(axis1, 0) - y2 = end.get(axis2, 0) - - # Calculate midpoint - mx = (x1 + x2) / 2 - my = (y1 + y2) / 2 - - # Calculate distance between points - dx = x2 - x1 - dy = y2 - y1 - d = math.sqrt(dx**2 + dy**2) - - # Check if arc is possible - if d > 2 * abs(radius): - raise ValueError(f"Arc radius {radius} too small for distance {d}") - - # Calculate distance from midpoint to center - if abs(d) < 1e-10: # Points are the same - return start.copy() - - h = math.sqrt(radius**2 - (d/2)**2) - - # Calculate perpendicular direction - px = -dy / d - py = dx / d - - # Determine direction based on radius sign and clockwise flag - if radius < 0: - h = -h - if not clockwise: - h = -h - - # Calculate center - center = start.copy() - center[axis1] = mx + h * px - center[axis2] = my + h * py - - return center - - -def validate_arc(start: Dict[str, float], end: Dict[str, float], - center: Dict[str, float], plane: str = 'G17') -> bool: - """ - Validate arc parameters - - Args: - start: Starting position - end: Ending position - center: Arc center - plane: Active plane - - Returns: - True if arc is valid - """ - # Get the two axes involved - if plane == 'G17': - axis1, axis2 = 'X', 'Y' - elif plane == 'G18': - axis1, axis2 = 'X', 'Z' - else: - axis1, axis2 = 'Y', 'Z' - - # Calculate radii from center to start and end - r_start = math.sqrt( - (start.get(axis1, 0) - center.get(axis1, 0))**2 + - (start.get(axis2, 0) - center.get(axis2, 0))**2 - ) - - r_end = math.sqrt( - (end.get(axis1, 0) - center.get(axis1, 0))**2 + - (end.get(axis2, 0) - center.get(axis2, 0))**2 - ) - - # Check if radii are approximately equal (within 0.01mm) - return abs(r_start - r_end) < 0.01 - - -def estimate_motion_time(start: Dict[str, float], end: Dict[str, float], - feed_rate: float, is_rapid: bool = False) -> float: - """ - Estimate time for a motion command - - Args: - start: Starting position - end: Ending position - feed_rate: Feed rate in mm/min - is_rapid: True for G0 rapid moves - - Returns: - Estimated time in seconds - """ - if is_rapid: - # Rapid moves use maximum speed - # Use robot's actual max linear velocity (60 mm/s) - rapid_speed = 60.0 # mm/s (from PAROL6_ROBOT.Cartesian_linear_velocity_max) - distance = calculate_distance(start, end) - return distance / rapid_speed - else: - # Regular moves use feed rate - distance = calculate_distance(start, end) - return feed_rate_to_duration(distance, feed_rate) - - -def parse_gcode_file(filepath: str) -> List[str]: - """ - Parse GCODE file and return list of lines - - Args: - filepath: Path to GCODE file - - Returns: - List of GCODE lines - """ - lines = [] - try: - with open(filepath, 'r') as f: - for line in f: - # Remove whitespace and convert to uppercase - line = line.strip() - if line and not line.startswith('%'): # Skip empty lines and % markers - lines.append(line) - except Exception as e: - print(f"Error reading GCODE file: {e}") - - return lines - - -def format_gcode_number(value: float, decimals: int = 3) -> str: - """ - Format number for GCODE output - - Args: - value: Numeric value - decimals: Number of decimal places - - Returns: - Formatted string without trailing zeros - """ - formatted = f"{value:.{decimals}f}" - # Remove trailing zeros and decimal point if not needed - formatted = formatted.rstrip('0').rstrip('.') - return formatted - - -def split_into_segments(start: Dict[str, float], end: Dict[str, float], - max_segment_length: float = 10.0) -> List[Dict[str, float]]: - """ - Split long moves into smaller segments - - Args: - start: Starting position - end: Ending position - max_segment_length: Maximum segment length in mm - - Returns: - List of intermediate positions - """ - distance = calculate_distance(start, end) - - if distance <= max_segment_length: - return [end] - - # Calculate number of segments - num_segments = int(math.ceil(distance / max_segment_length)) - - # Generate intermediate points - points = [] - for i in range(1, num_segments + 1): - t = i / num_segments - point = {} - for axis in ['X', 'Y', 'Z', 'A', 'B', 'C']: - if axis in start and axis in end: - point[axis] = start[axis] + t * (end[axis] - start[axis]) - points.append(point) - - return points - - -def inches_to_mm(value: float) -> float: - """Convert inches to millimeters""" - return value * 25.4 - - -def mm_to_inches(value: float) -> float: - """Convert millimeters to inches""" - return value / 25.4 \ No newline at end of file +""" +Utility functions for GCODE processing + +Provides conversion functions, calculations, and helpers for GCODE implementation. +""" + +import math + +import numpy as np + + +def feed_rate_to_duration(distance: float, feed_rate: float) -> float: + """ + Convert feed rate to duration for a given distance + + Args: + distance: Distance to travel in mm + feed_rate: Feed rate in mm/min + + Returns: + Duration in seconds + """ + if feed_rate <= 0: + return 0 + + # Convert mm/min to mm/s + feed_rate_mm_s = feed_rate / 60.0 + + # Calculate duration + duration = distance / feed_rate_mm_s + + return duration + + +def feed_rate_to_speed_percentage( + feed_rate: float, min_speed: float = 120.0, max_speed: float = 3600.0 +) -> float: + """ + Convert feed rate to speed percentage + + Args: + feed_rate: Feed rate in mm/min + min_speed: Minimum speed in mm/min (default 120 = 2 mm/s) + max_speed: Maximum speed in mm/min (default 3600 = 60 mm/s) + + Returns: + Speed percentage (0-100) + """ + # Clamp feed rate to valid range + feed_rate = float(np.clip(feed_rate, min_speed, max_speed)) + + # Map to percentage + speed_percentage = float(np.interp(feed_rate, [min_speed, max_speed], [0.0, 100.0])) + + return speed_percentage + + +def speed_percentage_to_feed_rate( + speed_percentage: float, min_speed: float = 120.0, max_speed: float = 3600.0 +) -> float: + """ + Convert speed percentage to feed rate + + Args: + speed_percentage: Speed percentage (0-100) + min_speed: Minimum speed in mm/min + max_speed: Maximum speed in mm/min + + Returns: + Feed rate in mm/min + """ + # Clamp percentage + speed_percentage = float(np.clip(speed_percentage, 0.0, 100.0)) + + # Map to feed rate + feed_rate = float(np.interp(speed_percentage, [0.0, 100.0], [min_speed, max_speed])) + + return feed_rate + + +def calculate_distance(start: dict[str, float], end: dict[str, float]) -> float: + """ + Calculate Euclidean distance between two points + + Args: + start: Starting position {X, Y, Z, ...} + end: Ending position {X, Y, Z, ...} + + Returns: + Distance in mm + """ + distance: float = 0.0 + for axis in ["X", "Y", "Z"]: + if axis in start and axis in end: + distance += (end[axis] - start[axis]) ** 2 + + return float(math.sqrt(distance)) + + +def ijk_to_center( + start: dict[str, float], ijk: dict[str, float], plane: str = "G17" +) -> dict[str, float]: + """ + Convert IJK offsets to arc center point + + Args: + start: Starting position + ijk: IJK offset values (relative to start) + plane: Active plane (G17=XY, G18=XZ, G19=YZ) + + Returns: + Center point coordinates + """ + center = start.copy() + + if plane == "G17": # XY plane + if "I" in ijk: + center["X"] = start.get("X", 0) + ijk["I"] + if "J" in ijk: + center["Y"] = start.get("Y", 0) + ijk["J"] + elif plane == "G18": # XZ plane + if "I" in ijk: + center["X"] = start.get("X", 0) + ijk["I"] + if "K" in ijk: + center["Z"] = start.get("Z", 0) + ijk["K"] + elif plane == "G19": # YZ plane + if "J" in ijk: + center["Y"] = start.get("Y", 0) + ijk["J"] + if "K" in ijk: + center["Z"] = start.get("Z", 0) + ijk["K"] + + return center + + +def radius_to_center( + start: dict[str, float], + end: dict[str, float], + radius: float, + clockwise: bool = True, + plane: str = "G17", +) -> dict[str, float]: + """ + Calculate arc center from radius + + Args: + start: Starting position + end: Ending position + radius: Arc radius (positive for <180°, negative for >180°) + clockwise: True for G2, False for G3 + plane: Active plane + + Returns: + Center point coordinates + """ + # Get the two axes involved in the arc based on plane + if plane == "G17": # XY plane + axis1, axis2 = "X", "Y" + elif plane == "G18": # XZ plane + axis1, axis2 = "X", "Z" + else: # G19 - YZ plane + axis1, axis2 = "Y", "Z" + + # Get start and end coordinates + x1 = start.get(axis1, 0) + y1 = start.get(axis2, 0) + x2 = end.get(axis1, 0) + y2 = end.get(axis2, 0) + + # Calculate midpoint + mx = (x1 + x2) / 2 + my = (y1 + y2) / 2 + + # Calculate distance between points + dx = x2 - x1 + dy = y2 - y1 + d = math.sqrt(dx**2 + dy**2) + + # Check if arc is possible + if d > 2 * abs(radius): + raise ValueError(f"Arc radius {radius} too small for distance {d}") + + # Calculate distance from midpoint to center + if abs(d) < 1e-10: # Points are the same + return start.copy() + + h = math.sqrt(radius**2 - (d / 2) ** 2) + + # Calculate perpendicular direction + px = -dy / d + py = dx / d + + # Determine direction based on radius sign and clockwise flag + if radius < 0: + h = -h + if not clockwise: + h = -h + + # Calculate center + center = start.copy() + center[axis1] = mx + h * px + center[axis2] = my + h * py + + return center + + +def validate_arc( + start: dict[str, float], + end: dict[str, float], + center: dict[str, float], + plane: str = "G17", +) -> bool: + """ + Validate arc parameters + + Args: + start: Starting position + end: Ending position + center: Arc center + plane: Active plane + + Returns: + True if arc is valid + """ + # Get the two axes involved + if plane == "G17": + axis1, axis2 = "X", "Y" + elif plane == "G18": + axis1, axis2 = "X", "Z" + else: + axis1, axis2 = "Y", "Z" + + # Calculate radii from center to start and end + r_start = math.sqrt( + (start.get(axis1, 0) - center.get(axis1, 0)) ** 2 + + (start.get(axis2, 0) - center.get(axis2, 0)) ** 2 + ) + + r_end = math.sqrt( + (end.get(axis1, 0) - center.get(axis1, 0)) ** 2 + + (end.get(axis2, 0) - center.get(axis2, 0)) ** 2 + ) + + # Check if radii are approximately equal (within 0.01mm) + return abs(r_start - r_end) < 0.01 + + +def estimate_motion_time( + start: dict[str, float], + end: dict[str, float], + feed_rate: float, + is_rapid: bool = False, +) -> float: + """ + Estimate time for a motion command + + Args: + start: Starting position + end: Ending position + feed_rate: Feed rate in mm/min + is_rapid: True for G0 rapid moves + + Returns: + Estimated time in seconds + """ + if is_rapid: + # Rapid moves use maximum speed + # Use robot's actual max linear velocity (60 mm/s) + rapid_speed = 60.0 # mm/s (from PAROL6_ROBOT.Cartesian_linear_velocity_max) + distance = calculate_distance(start, end) + return distance / rapid_speed + else: + # Regular moves use feed rate + distance = calculate_distance(start, end) + return feed_rate_to_duration(distance, feed_rate) + + +def parse_gcode_file(filepath: str) -> list[str]: + """ + Parse GCODE file and return list of lines + + Args: + filepath: Path to GCODE file + + Returns: + List of GCODE lines + """ + lines = [] + try: + with open(filepath) as f: + for line in f: + # Remove whitespace and convert to uppercase + line = line.strip() + if line and not line.startswith("%"): # Skip empty lines and % markers + lines.append(line) + except Exception as e: + print(f"Error reading GCODE file: {e}") + + return lines + + +def format_gcode_number(value: float, decimals: int = 3) -> str: + """ + Format number for GCODE output + + Args: + value: Numeric value + decimals: Number of decimal places + + Returns: + Formatted string without trailing zeros + """ + formatted = f"{value:.{decimals}f}" + # Remove trailing zeros and decimal point if not needed + formatted = formatted.rstrip("0").rstrip(".") + return formatted + + +def split_into_segments( + start: dict[str, float], end: dict[str, float], max_segment_length: float = 10.0 +) -> list[dict[str, float]]: + """ + Split long moves into smaller segments + + Args: + start: Starting position + end: Ending position + max_segment_length: Maximum segment length in mm + + Returns: + List of intermediate positions + """ + distance = calculate_distance(start, end) + + if distance <= max_segment_length: + return [end] + + # Calculate number of segments + num_segments = int(math.ceil(distance / max_segment_length)) + + # Generate intermediate points + points = [] + for i in range(1, num_segments + 1): + t = i / num_segments + point = {} + for axis in ["X", "Y", "Z", "A", "B", "C"]: + if axis in start and axis in end: + point[axis] = start[axis] + t * (end[axis] - start[axis]) + points.append(point) + + return points + + +def inches_to_mm(value: float) -> float: + """Convert inches to millimeters""" + return value * 25.4 + + +def mm_to_inches(value: float) -> float: + """Convert millimeters to inches""" + return value / 25.4 diff --git a/parol6/protocol/types.py b/parol6/protocol/types.py index 76ce3c7..2f098f7 100644 --- a/parol6/protocol/types.py +++ b/parol6/protocol/types.py @@ -12,22 +12,36 @@ # Stream mode state enum class StreamModeState(Enum): """Stream mode state for jog commands.""" + OFF = 0 # Stream mode disabled (default FIFO queueing) - ON = 1 # Stream mode enabled (latest-wins for jog commands) + ON = 1 # Stream mode enabled (latest-wins for jog commands) # Frame literals -Frame = Literal['WRF', 'TRF'] +Frame = Literal["WRF", "TRF"] -# Axis literals -Axis = Literal['X+', 'X-', 'Y+', 'Y-', 'Z+', 'Z-', 'RX+', 'RX-', 'RY+', 'RY-', 'RZ+', 'RZ-'] +# Axis literals +Axis = Literal[ + "X+", "X-", "Y+", "Y-", "Z+", "Z-", "RX+", "RX-", "RY+", "RY-", "RZ+", "RZ-" +] # Acknowledgment status literals -AckStatus = Literal['SENT', 'QUEUED', 'EXECUTING', 'COMPLETED', 'FAILED', 'INVALID', 'CANCELLED', 'TIMEOUT', 'NO_TRACKING'] +AckStatus = Literal[ + "SENT", + "QUEUED", + "EXECUTING", + "COMPLETED", + "FAILED", + "INVALID", + "CANCELLED", + "TIMEOUT", + "NO_TRACKING", +] class IOStatus(TypedDict): """Digital I/O status.""" + in1: int in2: int out1: int @@ -37,6 +51,7 @@ class IOStatus(TypedDict): class GripperStatus(TypedDict): """Electric gripper status.""" + id: int position: int speed: int @@ -45,16 +60,21 @@ class GripperStatus(TypedDict): object_detect: int -class StatusAggregate(TypedDict): +class StatusAggregate(TypedDict, total=False): """Aggregate robot status.""" + pose: list[float] # 4x4 transformation matrix flattened (len=16) angles: list[float] # 6 joint angles in degrees io: IOStatus | list[int] # Back-compat with existing server format gripper: GripperStatus | list[int] + action_current: str # Current executing action/command name + action_state: str # Current action state (IDLE, EXECUTING, etc) + action_next: str # Next non-streamable action in queue class TrackingStatus(TypedDict): """Command tracking status.""" + command_id: str | None status: AckStatus details: str @@ -64,6 +84,7 @@ class TrackingStatus(TypedDict): class SendResult(TypedDict): """Standardized result for command-sending APIs.""" + command_id: str | None status: AckStatus details: str @@ -73,5 +94,16 @@ class SendResult(TypedDict): class WireResponse(TypedDict): """Typed wrapper for parsed wire responses.""" - type: Literal['PONG','POSE','ANGLES','IO','GRIPPER','SPEEDS','STATUS','GCODE_STATUS','SERVER_STATE'] + + type: Literal[ + "PONG", + "POSE", + "ANGLES", + "IO", + "GRIPPER", + "SPEEDS", + "STATUS", + "GCODE_STATUS", + "SERVER_STATE", + ] payload: dict | list | str diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py index f311aa3..b95dbd0 100644 --- a/parol6/protocol/wire.py +++ b/parol6/protocol/wire.py @@ -4,19 +4,25 @@ This module centralizes encoding of command strings and decoding of common response payloads used by the headless controller. """ + import logging -from typing import List, Literal, Sequence, cast, Union -import numpy as np +from collections.abc import Sequence -from .types import Frame, Axis, StatusAggregate # Centralized binary wire protocol helpers (pack/unpack + codes) from enum import IntEnum +from typing import Literal, cast + +import numpy as np + +from .types import Axis, Frame, StatusAggregate logger = logging.getLogger(__name__) # Precomputed bit-unpack lookup table for 0..255 (MSB..LSB) # Using NumPy ensures fast vectorized selection without per-call allocations. -_BIT_UNPACK = np.unpackbits(np.arange(256, dtype=np.uint8)[:, None], axis=1, bitorder="big") +_BIT_UNPACK = np.unpackbits( + np.arange(256, dtype=np.uint8)[:, None], axis=1, bitorder="big" +) START = b"\xff\xff\xff" END = b"\x01\x02" PAYLOAD_LEN = 52 # matches existing firmware expectation @@ -43,6 +49,7 @@ class CommandCode(IntEnum): """Unified command codes for firmware interface.""" + HOME = 100 ENABLE = 101 DISABLE = 102 @@ -56,7 +63,7 @@ def split_bitfield(byte_val: int) -> list[int]: return [(byte_val >> i) & 1 for i in range(7, -1, -1)] -def fuse_bitfield_2_bytearray(bits: Union[list[int], Sequence[int]]) -> bytes: +def fuse_bitfield_2_bytearray(bits: list[int] | Sequence[int]) -> bytes: """ Fuse a big-endian list of 8 bits (MSB..LSB) into a single byte. Any truthy value is treated as 1. @@ -91,7 +98,7 @@ def fuse_2_bytes(b0: int, b1: int) -> int: return val - 0x10000 if (val & 0x8000) else val -def _get_array_value(arr: Union[np.ndarray, memoryview], index: int, default: int = 0) -> int: +def _get_array_value(arr: np.ndarray | memoryview, index: int, default: int = 0) -> int: """ Safely get value from array-like object with bounds checking. Optimized for zero-copy access when possible. @@ -108,7 +115,7 @@ def pack_tx_frame_into( out: memoryview, position_out: np.ndarray, speed_out: np.ndarray, - command_code: Union[int, CommandCode], + command_code: int | CommandCode, affected_joint_out: np.ndarray, inout_out: np.ndarray, timeout_out: int, @@ -168,7 +175,7 @@ def pack_tx_frame_into( bitfield_val = 0 for i in range(8): if _get_array_value(affected_joint_out, i, 0): - bitfield_val |= (1 << (7 - i)) + bitfield_val |= 1 << (7 - i) out[offset] = bitfield_val offset += 1 @@ -176,7 +183,7 @@ def pack_tx_frame_into( bitfield_val = 0 for i in range(8): if _get_array_value(inout_out, i, 0): - bitfield_val |= (1 << (7 - i)) + bitfield_val |= 1 << (7 - i) out[offset] = bitfield_val offset += 1 @@ -184,7 +191,6 @@ def pack_tx_frame_into( out[offset] = int(timeout_out) & 0xFF offset += 1 - # Gripper: position, speed, current as 2 bytes each (big-endian) for idx in range(3): v = _get_array_value(gripper_data_out, idx, 0) & 0xFFFF @@ -210,14 +216,14 @@ def pack_tx_frame_into( def unpack_rx_frame_into( data: memoryview, *, - pos_out, - spd_out, - homed_out, - io_out, - temp_out, - poserr_out, - timing_out, - grip_out, + pos_out: np.ndarray, + spd_out: np.ndarray, + homed_out: np.ndarray, + io_out: np.ndarray, + temp_out: np.ndarray, + poserr_out: np.ndarray, + timing_out: np.ndarray, + grip_out: np.ndarray, ) -> bool: """ Zero-allocation decode of a 52-byte RX frame payload (memoryview) directly into numpy arrays. @@ -229,7 +235,9 @@ def unpack_rx_frame_into( """ try: if len(data) < 52: - logger.warning(f"unpack_rx_frame_into: payload too short ({len(data)} bytes)") + logger.warning( + f"unpack_rx_frame_into: payload too short ({len(data)} bytes)" + ) return False mv = memoryview(data) @@ -239,12 +247,20 @@ def unpack_rx_frame_into( pos3 = b[:6] spd3 = b[6:] - pos = (pos3[:, 0].astype(np.int32) << 16) | (pos3[:, 1].astype(np.int32) << 8) | pos3[:, 2].astype(np.int32) - spd = (spd3[:, 0].astype(np.int32) << 16) | (spd3[:, 1].astype(np.int32) << 8) | spd3[:, 2].astype(np.int32) + pos = ( + (pos3[:, 0].astype(np.int32) << 16) + | (pos3[:, 1].astype(np.int32) << 8) + | pos3[:, 2].astype(np.int32) + ) + spd = ( + (spd3[:, 0].astype(np.int32) << 16) + | (spd3[:, 1].astype(np.int32) << 8) + | spd3[:, 2].astype(np.int32) + ) # Sign-correct 24-bit to int32 - pos[pos >= (1 << 23)] -= (1 << 24) - spd[spd >= (1 << 23)] -= (1 << 24) + pos[pos >= (1 << 23)] -= 1 << 24 + spd[spd >= (1 << 23)] -= 1 << 24 np.copyto(pos_out, pos, casting="no") np.copyto(spd_out, spd, casting="no") @@ -298,6 +314,7 @@ def unpack_rx_frame_into( # Encoding helpers # ========================= + def _opt(value: object | None, none_token: str = "NONE") -> str: """Format an optional value as a string, using none_token for None.""" return none_token if value is None else str(value) @@ -409,7 +426,9 @@ def encode_gcode_program_inline(lines: Sequence[str]) -> str: # ========================= # Decoding helpers # ========================= -def decode_simple(resp: str, expected_prefix: Literal["ANGLES", "IO", "GRIPPER", "SPEEDS", "POSE"]) -> List[float] | List[int] | None: +def decode_simple( + resp: str, expected_prefix: Literal["ANGLES", "IO", "GRIPPER", "SPEEDS", "POSE"] +) -> list[float] | list[int] | None: """ Decode simple prefixed payloads like: ANGLES|a0,a1,a2,a3,a4,a5 @@ -421,11 +440,15 @@ def decode_simple(resp: str, expected_prefix: Literal["ANGLES", "IO", "GRIPPER", Returns list[float] or list[int] depending on the expected_prefix. """ if not resp: - logger.debug(f"decode_simple: Empty response for expected prefix '{expected_prefix}'") + logger.debug( + f"decode_simple: Empty response for expected prefix '{expected_prefix}'" + ) return None parts = resp.strip().split("|", 1) if len(parts) != 2 or parts[0] != expected_prefix: - logger.warning(f"decode_simple: Invalid response format. Expected '{expected_prefix}|...' but got '{resp}'") + logger.warning( + f"decode_simple: Invalid response format. Expected '{expected_prefix}|...' but got '{resp}'" + ) return None payload = parts[1] tokens = [t for t in payload.split(",") if t != ""] @@ -435,20 +458,25 @@ def decode_simple(resp: str, expected_prefix: Literal["ANGLES", "IO", "GRIPPER", try: return [int(t) for t in tokens] except ValueError as e: - logger.error(f"decode_simple: Failed to parse integers for {expected_prefix}. Payload: '{payload}', Error: {e}") + logger.error( + f"decode_simple: Failed to parse integers for {expected_prefix}. Payload: '{payload}', Error: {e}" + ) return None else: try: return [float(t) for t in tokens] except ValueError as e: - logger.error(f"decode_simple: Failed to parse floats for {expected_prefix}. Payload: '{payload}', Error: {e}") + logger.error( + f"decode_simple: Failed to parse floats for {expected_prefix}. Payload: '{payload}', Error: {e}" + ) return None def decode_status(resp: str) -> StatusAggregate | None: """ Decode aggregate status: - STATUS|POSE=p0,p1,...,p15|ANGLES=a0,...,a5|SPEEDS=s0,...,s5|IO=in1,in2,out1,out2,estop|GRIPPER=id,pos,spd,cur,status,obj + STATUS|POSE=p0,p1,...,p15|ANGLES=a0,...,a5|SPEEDS=s0,...,s5|IO=in1,in2,out1,out2,estop|GRIPPER=id,pos,spd,cur,status,obj| + ACTION_CURRENT=...|ACTION_STATE=... Returns a dict matching StatusAggregate or None on parse failure. """ @@ -463,26 +491,50 @@ def decode_status(resp: str) -> StatusAggregate | None: "speeds": None, "io": None, "gripper": None, + "action_current": None, + "action_state": None, + "joint_en": None, + "cart_en_wrf": None, + "cart_en_trf": None, } for sec in sections: if sec.startswith("POSE="): - vals = [float(x) for x in sec[len("POSE="):].split(",") if x] + vals = [float(x) for x in sec[len("POSE=") :].split(",") if x] result["pose"] = vals elif sec.startswith("ANGLES="): - vals = [float(x) for x in sec[len("ANGLES="):].split(",") if x] + vals = [float(x) for x in sec[len("ANGLES=") :].split(",") if x] result["angles"] = vals elif sec.startswith("SPEEDS="): - vals = [float(x) for x in sec[len("SPEEDS="):].split(",") if x] + vals = [float(x) for x in sec[len("SPEEDS=") :].split(",") if x] result["speeds"] = vals elif sec.startswith("IO="): - vals = [int(x) for x in sec[len("IO="):].split(",") if x] + vals = [int(x) for x in sec[len("IO=") :].split(",") if x] result["io"] = vals elif sec.startswith("GRIPPER="): - vals = [int(x) for x in sec[len("GRIPPER="):].split(",") if x] + vals = [int(x) for x in sec[len("GRIPPER=") :].split(",") if x] result["gripper"] = vals - - # Basic validation - if result["pose"] is None and result["angles"] is None and result["io"] is None and result["gripper"] is None: + elif sec.startswith("ACTION_CURRENT="): + result["action_current"] = sec[len("ACTION_CURRENT=") :] + elif sec.startswith("ACTION_STATE="): + result["action_state"] = sec[len("ACTION_STATE=") :] + elif sec.startswith("JOINT_EN="): + vals = [int(x) for x in sec[len("JOINT_EN=") :].split(",") if x] + result["joint_en"] = vals + elif sec.startswith("CART_EN_WRF="): + vals = [int(x) for x in sec[len("CART_EN_WRF=") :].split(",") if x] + result["cart_en_wrf"] = vals + elif sec.startswith("CART_EN_TRF="): + vals = [int(x) for x in sec[len("CART_EN_TRF=") :].split(",") if x] + result["cart_en_trf"] = vals + + # Basic validation: accept if at least one of the core groups is present + if ( + result["pose"] is None + and result["angles"] is None + and result["io"] is None + and result["gripper"] is None + and result["action_current"] is None + ): return None return cast(StatusAggregate, result) diff --git a/parol6/server/command_registry.py b/parol6/server/command_registry.py index aaff3b8..7641a11 100644 --- a/parol6/server/command_registry.py +++ b/parol6/server/command_registry.py @@ -9,10 +9,10 @@ from __future__ import annotations import logging -from typing import Dict, Type, Optional, List, Callable, Any, Tuple -from importlib import import_module import pkgutil import time +from collections.abc import Callable +from importlib import import_module from parol6.commands.base import CommandBase from parol6.config import TRACE @@ -23,39 +23,39 @@ class CommandRegistry: """ Singleton registry for command classes. - + Commands register themselves using the @register_command decorator. The registry supports auto-discovery of decorated commands and provides a centralized lookup mechanism. """ - - _instance: Optional[CommandRegistry] = None - _commands: Dict[str, Type[CommandBase]] = {} - _class_to_name: Dict[Type[CommandBase], str] = {} + + _instance: CommandRegistry | None = None + _commands: dict[str, type[CommandBase]] = {} + _class_to_name: dict[type[CommandBase], str] = {} _discovered: bool = False - + def __new__(cls) -> CommandRegistry: """Ensure singleton pattern.""" if cls._instance is None: cls._instance = super().__new__(cls) return cls._instance - + def __init__(self): """Initialize the registry (only runs once due to singleton).""" - if not hasattr(self, '_initialized'): + if not hasattr(self, "_initialized"): self._commands = {} self._class_to_name = {} self._discovered = False self._initialized = True - - def register(self, name: str, command_class: Type[CommandBase]) -> None: + + def register(self, name: str, command_class: type[CommandBase]) -> None: """ Register a command class with the given name. - + Args: name: The command name/identifier command_class: The command class to register - + Raises: ValueError: If a command with the same name is already registered """ @@ -71,24 +71,24 @@ def register(self, name: str, command_class: Type[CommandBase]) -> None: # Maintain reverse mapping for fast class -> name lookup self._class_to_name[command_class] = name logger.debug(f"Registered command '{name}' -> {command_class.__name__}") - - def get_command_class(self, name: str) -> Optional[Type[CommandBase]]: + + def get_command_class(self, name: str) -> type[CommandBase] | None: """ Retrieve a command class by name. - + Args: name: The command name to look up - + Returns: The command class if found, None otherwise """ # Ensure commands are discovered if not self._discovered: self.discover_commands() - + return self._commands.get(name) - - def get_name_for_class(self, cls: Type[CommandBase]) -> Optional[str]: + + def get_name_for_class(self, cls: type[CommandBase]) -> str | None: """ Retrieve the registered command name for a given command class. Returns None if the class is not registered. @@ -99,50 +99,50 @@ def get_name_for_class(self, cls: Type[CommandBase]) -> Optional[str]: # Prefer explicit reverse map; fall back to class attribute set by decorator return self._class_to_name.get(cls) or getattr(cls, "_registered_name", None) - def list_registered_commands(self) -> List[str]: + def list_registered_commands(self) -> list[str]: """ Return a list of all registered command names. - + Returns: List of command names (sorted) """ # Ensure commands are discovered if not self._discovered: self.discover_commands() - + return sorted(self._commands.keys()) - + def discover_commands(self) -> None: """ Auto-discover and register all decorated commands. - + This method imports all modules in the parol6.commands package to trigger the @register_command decorators. """ if self._discovered: return - + logger.info("Discovering commands...") - + # Import parol6.commands package try: - commands_package = import_module('parol6.commands') + commands_package = import_module("parol6.commands") except ImportError as e: logger.error(f"Failed to import parol6.commands: {e}") return - + # Iterate through all modules in the commands package package_path = commands_package.__path__ for importer, modname, ispkg in pkgutil.iter_modules(package_path): if ispkg: continue # Skip subpackages - - full_module_name = f'parol6.commands.{modname}' - + + full_module_name = f"parol6.commands.{modname}" + # Skip the base module - if modname == 'base': + if modname == "base": continue - + try: # Import the module (this triggers decorators) import_module(full_module_name) @@ -151,17 +151,21 @@ def discover_commands(self) -> None: logger.warning(f"Failed to import {full_module_name}: {e}") except Exception as e: logger.error(f"Error loading {full_module_name}: {e}") - + self._discovered = True - logger.info(f"Command discovery complete. {len(self._commands)} commands registered.") - - def create_command_from_parts(self, parts: List[str]) -> Tuple[Optional[CommandBase], Optional[str]]: + logger.info( + f"Command discovery complete. {len(self._commands)} commands registered." + ) + + def create_command_from_parts( + self, parts: list[str] + ) -> tuple[CommandBase | None, str | None]: """ Create a command instance from pre-split message parts. - + Args: parts: Pre-split message parts - + Returns: A tuple of (command, error_message): - (command, None) if successful @@ -171,50 +175,58 @@ def create_command_from_parts(self, parts: List[str]) -> Tuple[Optional[CommandB # Ensure commands are discovered if not self._discovered: self.discover_commands() - + if not parts: logger.debug("Empty message parts") return None, None - + command_name = parts[0].upper() start_t = time.perf_counter() logger.log(TRACE, "match_start name=%s parts=%d", command_name, len(parts)) - + # Direct O(1) lookup of command class command_class = self._commands.get(command_name) - + if command_class is None: logger.log(TRACE, "match_unknown name=%s", command_name) logger.debug(f"No command registered for: {command_name}") return None, None - + try: # Create instance and let it parse parameters command = command_class() can_handle, error = command.match(parts) # Pass pre-split parts - + if can_handle: dur_ms = (time.perf_counter() - start_t) * 1000.0 logger.log(TRACE, "match_ok name=%s dur_ms=%.2f", command_name, dur_ms) return command, None elif error: dur_ms = (time.perf_counter() - start_t) * 1000.0 - logger.log(TRACE, "match_error name=%s dur_ms=%.2f err=%s", command_name, dur_ms, error) + logger.log( + TRACE, + "match_error name=%s dur_ms=%.2f err=%s", + command_name, + dur_ms, + error, + ) logger.warning(f"Command '{command_name}' rejected: {error}") return None, error - + except Exception as e: dur_ms = (time.perf_counter() - start_t) * 1000.0 - logger.log(TRACE, "match_error name=%s dur_ms=%.2f exc=%s", command_name, dur_ms, e) + logger.log( + TRACE, "match_error name=%s dur_ms=%.2f exc=%s", command_name, dur_ms, e + ) logger.error(f"Error creating command '{command_name}': {e}") return None, str(e) - + return None, "Command validation failed" - + def clear(self) -> None: """ Clear all registered commands. - + This is mainly useful for testing. """ self._commands.clear() @@ -226,36 +238,38 @@ def clear(self) -> None: _registry = CommandRegistry() -def register_command(name: str) -> Callable[[Type[CommandBase]], Type[CommandBase]]: +def register_command(name: str) -> Callable[[type[CommandBase]], type[CommandBase]]: """ Decorator to register a command class. - + Usage: @register_command("MoveJ") class MoveJointCommand(CommandBase): ... - + Args: name: The command name/identifier - + Returns: Decorator function that registers the class """ - def decorator(cls: Type[CommandBase]) -> Type[CommandBase]: + + def decorator(cls: type[CommandBase]) -> type[CommandBase]: # Verify it's a CommandBase subclass if not issubclass(cls, CommandBase): raise TypeError(f"Class {cls.__name__} must inherit from CommandBase") - + # Register with the global registry _registry.register(name, cls) - + # Add the command name as a class attribute for reference cls._registered_name = name - + return cls - + return decorator + # Module-level convenience functions that delegate to the registry singleton get_command_class = _registry.get_command_class list_registered_commands = _registry.list_registered_commands diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 2a52db8..33b54e1 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -2,31 +2,50 @@ Main controller for PAROL6 robot server. """ +import argparse import logging +import os import socket -import time import threading -import argparse -import os -from typing import Optional, Dict, Any, List, Tuple, Deque, Union, Sequence, cast -from dataclasses import dataclass, field +import time from collections import deque +from dataclasses import dataclass, field +from typing import Any import numpy as np -from parol6.server.state import StateManager, ControllerState -from parol6.server.transports.udp_transport import UDPTransport -from parol6.server.transports.serial_transport import SerialTransport -from parol6.server.transports.mock_serial_transport import MockSerialTransport -from parol6.server.transports import create_and_connect_transport, is_simulation_mode -from parol6.server.command_registry import discover_commands, create_command_from_parts +from parol6.ack_policy import AckPolicy +from parol6.commands.base import ( + CommandBase, + CommandContext, + ExecutionStatus, + ExecutionStatusCode, + MotionCommand, + QueryCommand, + SystemCommand, +) +from parol6.gcode import GcodeInterpreter +from parol6.protocol.wire import CommandCode, unpack_rx_frame_into +from parol6.server.command_registry import create_command_from_parts, discover_commands +from parol6.server.state import ControllerState, StateManager from parol6.server.status_broadcast import StatusBroadcaster from parol6.server.status_cache import get_cache -from parol6.protocol.wire import CommandCode, unpack_rx_frame_into -from parol6.gcode import GcodeInterpreter -from parol6.config import * -from parol6.commands.base import CommandBase, ExecutionStatus, ExecutionStatusCode, QueryCommand, MotionCommand, SystemCommand, CommandContext -from parol6.ack_policy import AckPolicy +from parol6.server.transports import create_and_connect_transport, is_simulation_mode +from parol6.server.transports.mock_serial_transport import MockSerialTransport +from parol6.server.transports.serial_transport import SerialTransport +from parol6.server.transports.udp_transport import UDPTransport +import parol6.config as cfg +from parol6.config import ( + TRACE, + INTERVAL_S, + MCAST_GROUP, + MCAST_PORT, + MCAST_IF, + MCAST_TTL, + STATUS_RATE_HZ, + STATUS_STALE_S, + get_com_port_with_fallback, +) logger = logging.getLogger("parol6.server.controller") @@ -34,19 +53,21 @@ @dataclass class ExecutionContext: """Context passed to commands during execution.""" - udp_transport: Optional[UDPTransport] - serial_transport: Optional[Union[SerialTransport, MockSerialTransport]] - gcode_interpreter: Optional[GcodeInterpreter] - addr: Optional[Tuple[str, int]] + + udp_transport: UDPTransport | None + serial_transport: SerialTransport | MockSerialTransport | None + gcode_interpreter: GcodeInterpreter | None + addr: tuple[str, int] | None state: ControllerState @dataclass class QueuedCommand: """Represents a command in the queue with metadata.""" + command: CommandBase - command_id: Optional[str] = None - address: Optional[Tuple[str, int]] = None + command_id: str | None = None + address: tuple[str, int] | None = None queued_time: float = field(default_factory=time.time) activated: bool = False initialized: bool = False @@ -56,9 +77,10 @@ class QueuedCommand: @dataclass class ControllerConfig: """Configuration for the controller.""" - udp_host: str = '0.0.0.0' + + udp_host: str = "0.0.0.0" udp_port: int = 5001 - serial_port: Optional[str] = None + serial_port: str | None = None serial_baudrate: int = 3000000 loop_interval: float = INTERVAL_S estop_recovery_delay: float = 1.0 @@ -68,18 +90,18 @@ class ControllerConfig: class Controller: """ Main controller that orchestrates all components of the PAROL6 server. - + This replaces the monolithic controller.py with a modular design: - State management via StateManager singleton - Transport abstraction for UDP and Serial - Command execution via CommandExecutor - Automatic command discovery and registration """ - + def __init__(self, config: ControllerConfig): """ Initialize the controller with configuration. - + Args: config: Configuration object for the controller """ @@ -87,49 +109,57 @@ def __init__(self, config: ControllerConfig): self.running = False self.shutdown_event = threading.Event() self._initialized = False - + # Core components self.state_manager = StateManager() - self.udp_transport: Optional[UDPTransport] = None - self.serial_transport: Optional[Union[SerialTransport, MockSerialTransport]] = None - + self.udp_transport: UDPTransport | None = None + self.serial_transport: SerialTransport | MockSerialTransport | None = None + # ACK management - self.ack_socket = None + self.ack_socket: socket.socket | None = None try: self.ack_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) except Exception as e: logger.error(f"Failed to create ACK socket: {e}") - + # Command queue and tracking (merged from CommandExecutor) - self.command_queue: Deque[QueuedCommand] = deque(maxlen=100) - self.active_command: Optional[QueuedCommand] = None - + self.command_queue: deque[QueuedCommand] = deque(maxlen=100) + self.active_command: QueuedCommand | None = None + # Command tracking - self.current_command = None - self.command_id_map: Dict[str, Any] = {} - + self.current_command: CommandBase | None = None + self.command_id_map: dict[str, Any] = {} + # E-stop recovery - self.estop_active = None # None = unknown, True = active, False = released + self.estop_active: bool | None = ( + None # None = unknown, True = active, False = released + ) self.first_frame_received = False # Track if we've received data from robot self._serial_last_version = 0 # Version of last decoded serial frame - + + # TX coalescing state (reduce redundant serial writes) + self._last_tx: dict[str, Any] | None = None + self._tx_keepalive_s = float(os.getenv("PAROL6_TX_KEEPALIVE_S", "0.2")) + # Thread for command processing - self.command_thread = None - + self.command_thread: threading.Thread | None = None + # GCODE interpreter self.gcode_interpreter = GcodeInterpreter() # Status services (updater + multicast broadcaster) - self._status_updater: Optional[Any] = None - self._status_broadcaster: Optional[Any] = None - + self._status_updater: Any | None = None + self._status_broadcaster: Any | None = None + # Initialize components on construction self._initialize_components() - - def _send_ack(self, cmd_id: str, status: str, details: str, addr: Tuple[str, int]) -> None: + + def _send_ack( + self, cmd_id: str, status: str, details: str, addr: tuple[str, int] + ) -> None: """ Send an acknowledgment message. - + Args: cmd_id: Command ID to acknowledge status: Status (QUEUED, EXECUTING, COMPLETED, FAILED, CANCELLED) @@ -138,34 +168,45 @@ def _send_ack(self, cmd_id: str, status: str, details: str, addr: Tuple[str, int """ if not cmd_id or not self.ack_socket: return - + # Debug/Trace log all outgoing ACKs - logger.log(TRACE, "ack_send id=%s status=%s details=%s addr=%s", cmd_id, status, details, addr) - + logger.log( + TRACE, + "ack_send id=%s status=%s details=%s addr=%s", + cmd_id, + status, + details, + addr, + ) + message = f"ACK|{cmd_id}|{status}|{details}".encode("ascii") - + try: self.ack_socket.sendto(message, addr) except Exception as e: logger.error(f"Failed to send ACK to {addr[0]}:{addr[1]} - {e}") - + def _initialize_components(self) -> None: """ Initialize all components during construction. - + Raises: RuntimeError: If critical components fail to initialize """ try: # Discover and register all commands discover_commands() - + # Initialize UDP transport - logger.info(f"Starting UDP server on {self.config.udp_host}:{self.config.udp_port}") - self.udp_transport = UDPTransport(self.config.udp_host, self.config.udp_port) + logger.info( + f"Starting UDP server on {self.config.udp_host}:{self.config.udp_port}" + ) + self.udp_transport = UDPTransport( + self.config.udp_host, self.config.udp_port + ) if not self.udp_transport.create_socket(): raise RuntimeError("Failed to create UDP socket") - + # Load persisted COM port if not provided try: if not self.config.serial_port: @@ -181,26 +222,46 @@ def _initialize_components(self) -> None: self.serial_transport = create_and_connect_transport( port=self.config.serial_port, baudrate=self.config.serial_baudrate, - auto_find_port=True + auto_find_port=True, ) - + if self.serial_transport: # Only announce connected and start reader if actually connected if self.serial_transport.is_connected(): - logger.info("Connected to transport: %s", self.serial_transport.port) + logger.info( + "Connected to transport: %s", self.serial_transport.port + ) try: self.serial_transport.start_reader(self.shutdown_event) logger.info("Serial reader thread started") except Exception as e: logger.warning("Failed to start serial reader: %s", e) else: - logger.warning("Serial transport not connected at startup (port=%s)", self.config.serial_port) + logger.warning( + "Serial transport not connected at startup (port=%s)", + self.config.serial_port, + ) else: - logger.warning("No serial port configured. Waiting for SET_PORT via UDP or set --serial/PAROL6_COM_PORT/com_port.txt before connecting.") - + logger.warning( + "No serial port configured. Waiting for SET_PORT via UDP or set --serial/PAROL6_COM_PORT/com_port.txt before connecting." + ) + # Initialize robot state self.state_manager.reset_state() + # Initialize TX coalescing state + state = self.state_manager.get_state() + self._last_tx = { + "pos": np.empty_like(state.Position_out), + "spd": np.empty_like(state.Speed_out), + "cmd": None, + "aff": np.empty_like(state.Affected_joint_out), + "io": np.empty_like(state.InOut_out), + "tout": None, + "grip": np.empty_like(state.Gripper_data_out), + "last_sent": 0.0, + } + # Optionally queue auto-home per policy (default OFF) if self.config.auto_home: try: @@ -211,10 +272,12 @@ def _initialize_components(self) -> None: logger.info("Auto-home queued") except Exception as e: logger.warning(f"Failed to queue auto-home: {e}") - + # Start status updater and broadcaster (ASCII multicast at configured rate) try: - logger.info(f"StatusBroadcaster config: group={MCAST_GROUP} port={MCAST_PORT} ttl={MCAST_TTL} iface={MCAST_IF} rate_hz={STATUS_RATE_HZ} stale_s={STATUS_STALE_S}") + logger.info( + f"StatusBroadcaster config: group={MCAST_GROUP} port={MCAST_PORT} ttl={MCAST_TTL} iface={MCAST_IF} rate_hz={STATUS_RATE_HZ} stale_s={STATUS_STALE_S}" + ) broadcaster = StatusBroadcaster( state_mgr=self.state_manager, group=MCAST_GROUP, @@ -222,7 +285,8 @@ def _initialize_components(self) -> None: ttl=MCAST_TTL, iface_ip=MCAST_IF, rate_hz=STATUS_RATE_HZ, - stale_s=STATUS_STALE_S) + stale_s=STATUS_STALE_S, + ) broadcaster.start() logger.info("Status updater and broadcaster started") @@ -231,42 +295,42 @@ def _initialize_components(self) -> None: self._initialized = True logger.info("Controller initialized successfully") - + except Exception as e: logger.error(f"Failed to initialize controller: {e}") self._initialized = False raise RuntimeError(f"Controller initialization failed: {e}") - + def is_initialized(self) -> bool: """Check if controller is properly initialized.""" return self._initialized - + def start(self): """Start the main control loop.""" if self.running: logger.warning("Controller already running") return - + self.running = True - + # Start command processing thread self.command_thread = threading.Thread(target=self._command_processing_loop) self.command_thread.start() - + # Start main control loop logger.info("Starting main control loop") self._main_control_loop() - + def stop(self): """Stop the controller and clean up resources.""" logger.info("Stopping controller...") self.running = False self.shutdown_event.set() - + # Wait for threads to finish if self.command_thread and self.command_thread.is_alive(): self.command_thread.join(timeout=2.0) - + # Stop status services try: if self._status_broadcaster: @@ -279,12 +343,12 @@ def stop(self): # Clean up transports if self.udp_transport: self.udp_transport.close_socket() - + if self.serial_transport: self.serial_transport.disconnect() - + logger.info("Controller stopped") - + def _main_control_loop(self): """ Main control loop that: @@ -294,7 +358,7 @@ def _main_control_loop(self): 4. Writes to firmware (serial) 5. Maintains timing """ - + tick = self.config.loop_interval next_t = time.perf_counter() prev_t = next_t # for period measurement @@ -303,7 +367,7 @@ def _main_control_loop(self): while self.running: try: state = self.state_manager.get_state() - + # 1. Read from firmware if self.serial_transport and self.serial_transport.is_connected(): try: @@ -328,21 +392,34 @@ def _main_control_loop(self): self._serial_last_version = ver except Exception as e: logger.warning(f"Error decoding latest serial frame: {e}") - + # Serial auto-reconnect when a port is known if self.serial_transport and not self.serial_transport.is_connected(): if self.serial_transport.auto_reconnect(): try: self.serial_transport.start_reader(self.shutdown_event) self.first_frame_received = False - logger.info("Serial reader thread started (after reconnect)") + # Reset TX keepalive to force prompt write after reconnect + if self._last_tx is not None: + self._last_tx["last_sent"] = 0.0 + logger.info( + "Serial reader thread started (after reconnect)" + ) except Exception as e: - logger.warning("Failed to start serial reader after reconnect: %s", e) + logger.warning( + "Failed to start serial reader after reconnect: %s", e + ) # 2. Handle E-stop (only check when connected to robot and received first frame) - if self.serial_transport and self.serial_transport.is_connected() and self.first_frame_received: - if state.InOut_in[4] == 0: # E-stop pressed (0 = pressed, 1 = released) - if self.estop_active != True: # Not already in E-stop state + if ( + self.serial_transport + and self.serial_transport.is_connected() + and self.first_frame_received + ): + if ( + state.InOut_in[4] == 0 + ): # E-stop pressed (0 = pressed, 1 = released) + if not self.estop_active: # Not already in E-stop state logger.warning("E-STOP activated") self.estop_active = True # Cancel active command @@ -354,7 +431,7 @@ def _main_control_loop(self): state.Command_out = CommandCode.DISABLE state.Speed_out.fill(0) elif state.InOut_in[4] == 1: # E-stop released (1 = released) - if self.estop_active == True: # Was in E-stop state + if self.estop_active: # Was in E-stop state # E-stop was released - automatic recovery logger.info("E-STOP released - automatic recovery") self.estop_active = False @@ -363,9 +440,11 @@ def _main_control_loop(self): state.disabled_reason = "" state.Command_out = CommandCode.IDLE state.Speed_out.fill(0) - + # 3. Execute commands if not in E-stop (or E-stop state unknown) - if self.estop_active != True: # Execute if E-stop is False or None (unknown) + if ( + not self.estop_active + ): # Execute if E-stop is False or None (unknown) # Execute active command if self.active_command or self.command_queue: self._execute_active_command() @@ -376,24 +455,63 @@ def _main_control_loop(self): # No commands - idle state.Command_out = CommandCode.IDLE state.Speed_out.fill(0) - np.copyto(state.Position_out, state.Position_in, casting='no') - + np.copyto(state.Position_out, state.Position_in, casting="no") + # 4. Write to firmware - if self.serial_transport and self.serial_transport.is_connected(): - # Optimized to pass arrays directly without creating lists - ok = self.serial_transport.write_frame( - state.Position_out, - state.Speed_out, - state.Command_out.value, - state.Affected_joint_out, - state.InOut_out, - state.Timeout_out, - state.Gripper_data_out, + if ( + self.serial_transport + and self.serial_transport.is_connected() + and self._last_tx is not None + ): + # Check if state has changed or keepalive needed + now = time.perf_counter() + dirty = ( + (state.Command_out.value != self._last_tx["cmd"]) + or ( + not np.array_equal(state.Position_out, self._last_tx["pos"]) + ) + or (not np.array_equal(state.Speed_out, self._last_tx["spd"])) + or ( + not np.array_equal( + state.Affected_joint_out, self._last_tx["aff"] + ) + ) + or (not np.array_equal(state.InOut_out, self._last_tx["io"])) + or (int(state.Timeout_out) != int(self._last_tx["tout"])) + or ( + not np.array_equal( + state.Gripper_data_out, self._last_tx["grip"] + ) + ) ) - if ok: - # Auto-reset one-shot gripper modes after successful send - if state.Gripper_data_out[4] in (1, 2): - state.Gripper_data_out[4] = 0 + + # Write if dirty or keepalive timeout reached + if dirty or ( + now - self._last_tx["last_sent"] >= self._tx_keepalive_s + ): + ok = self.serial_transport.write_frame( + state.Position_out, + state.Speed_out, + state.Command_out.value, + state.Affected_joint_out, + state.InOut_out, + state.Timeout_out, + state.Gripper_data_out, + ) + if ok: + # Update last TX snapshot + self._last_tx["cmd"] = state.Command_out.value + np.copyto(self._last_tx["pos"], state.Position_out) + np.copyto(self._last_tx["spd"], state.Speed_out) + np.copyto(self._last_tx["aff"], state.Affected_joint_out) + np.copyto(self._last_tx["io"], state.InOut_out) + self._last_tx["tout"] = int(state.Timeout_out) + np.copyto(self._last_tx["grip"], state.Gripper_data_out) + self._last_tx["last_sent"] = now + + # Auto-reset one-shot gripper modes after successful send + if state.Gripper_data_out[4] in (1, 2): + state.Gripper_data_out[4] = 0 # 5. Maintain loop timing using deadline scheduling + update loop metrics now = time.perf_counter() @@ -406,42 +524,58 @@ def _main_control_loop(self): state.ema_period_s = float(period) else: # EMA with alpha=0.1 - state.ema_period_s = 0.1 * float(period) + 0.9 * float(state.ema_period_s) + state.ema_period_s = 0.1 * float(period) + 0.9 * float( + state.ema_period_s + ) next_t += tick sleep = next_t - time.perf_counter() if sleep > 0: time.sleep(sleep) else: - # Overrun; catch up and log if severe + # Overrun; catch up state.overrun_count += 1 next_t = time.perf_counter() - if -sleep > tick * 0.5: - logger.warning(f"Control loop overrun by {-sleep:.4f}s (target: {tick:.4f}s)") - + if now - prev_print > 5: + # Warn only if short-term average period degraded >10% vs target + if state.ema_period_s > tick * 1.10: + logger.warning( + f"Control loop avg period degraded by +{((state.ema_period_s / tick) - 1.0) * 100.0:.0f}% " + f"(avg={state.ema_period_s:.4f}s target={tick:.4f}s); latest overrun={-sleep:.4f}s" + ) prev_print = now # Calculate command frequency cmd_hz = 0.0 if state.ema_command_period_s > 0.0: cmd_hz = 1.0 / state.ema_command_period_s - + # Calculate short-term command rate from recent timestamps short_term_cmd_hz = 0.0 if len(state.command_timestamps) >= 2: # Calculate rate from first and last timestamp in window - time_span = state.command_timestamps[-1] - state.command_timestamps[0] + time_span = ( + state.command_timestamps[-1] - state.command_timestamps[0] + ) if time_span > 0: - short_term_cmd_hz = (len(state.command_timestamps) - 1) / time_span - + short_term_cmd_hz = ( + len(state.command_timestamps) - 1 + ) / time_span + logger.debug( - f"loop_count: {state.loop_count}, " - f"overrun_count: {state.overrun_count}, " - f"loop_hz: {((1.0 / state.ema_period_s) if state.ema_period_s > 0.0 else 0.0):.2f}, " - f"cmd_count: {state.command_count}, " - f"cmd_hz: {cmd_hz:.2f} (short_term: {short_term_cmd_hz:.2f})" + "loop=%.2fHz cmd=%.2fHz s=%.2f/%d q=%d ov=%d", + ( + (1.0 / state.ema_period_s) + if state.ema_period_s > 0.0 + else 0.0 + ), + cmd_hz, + short_term_cmd_hz, + max(0, len(state.command_timestamps) - 1), + state.command_count, + state.overrun_count, ) - + except KeyboardInterrupt: logger.info("Keyboard interrupt received") self.stop() @@ -449,7 +583,7 @@ def _main_control_loop(self): except Exception as e: logger.error(f"Error in main control loop: {e}", exc_info=True) # Continue running despite errors - + def _command_processing_loop(self): """ Separate thread for processing incoming commands from UDP. @@ -462,30 +596,36 @@ def _command_processing_loop(self): continue cmd_str, addr = tup try: - _n = cmd_str.split("|", 1)[0].upper() if isinstance(cmd_str, str) else "UNKNOWN" + _n = ( + cmd_str.split("|", 1)[0].upper() + if isinstance(cmd_str, str) + else "UNKNOWN" + ) except Exception: _n = "UNKNOWN" logger.log(TRACE, "cmd_received name=%s from=%s", _n, addr) state = self.state_manager.get_state() - + # Track command reception for frequency metrics now = time.time() if state.last_command_time > 0: # Calculate period between commands period = now - state.last_command_time state.last_command_period_s = period - + # Update EMA of command period (alpha=0.1) if state.ema_command_period_s <= 0.0: state.ema_command_period_s = period else: - state.ema_command_period_s = 0.1 * period + 0.9 * state.ema_command_period_s - + state.ema_command_period_s = ( + 0.1 * period + 0.9 * state.ema_command_period_s + ) + state.last_command_time = now state.command_count += 1 state.command_timestamps.append(now) # No command IDs on wire; treat entire datagram as the command - cmd_parts = cmd_str.split('|') + cmd_parts = cmd_str.split("|") cmd_name = cmd_parts[0].upper() # Build server-side ack/wait policy policy = AckPolicy.from_env(lambda: state.stream_mode) @@ -493,90 +633,152 @@ def _command_processing_loop(self): # Stream fast-path: if an active streamable command of the same type exists, # reuse the instance by calling match/setup and skip object creation/queueing. if state.stream_mode and self.active_command: - logger.log(TRACE, "stream_fast_path_considered active=%s incoming=%s", type(self.active_command.command).__name__, cmd_name) + logger.log( + TRACE, + "stream_fast_path_considered active=%s incoming=%s", + type(self.active_command.command).__name__, + cmd_name, + ) active_inst = self.active_command.command - if isinstance(active_inst, MotionCommand) and active_inst.streamable: + if ( + isinstance(active_inst, MotionCommand) + and active_inst.streamable + ): active_name = active_inst._registered_name if active_name == cmd_name: can_handle, match_err = active_inst.match(cmd_parts) if can_handle: try: - active_inst.bind(CommandContext( - udp_transport=self.udp_transport, - addr=addr, - gcode_interpreter=self.gcode_interpreter, - dt=self.config.loop_interval - )) + active_inst.bind( + CommandContext( + udp_transport=self.udp_transport, + addr=addr, + gcode_interpreter=self.gcode_interpreter, + dt=self.config.loop_interval, + ) + ) active_inst.setup(state) except Exception as _e: - logger.error("Stream fast-path setup failed for %s: %s", active_name, _e) + logger.error( + "Stream fast-path setup failed for %s: %s", + active_name, + _e, + ) else: - logger.log(TRACE, "stream_fast_path_applied name=%s", active_name) + logger.log( + TRACE, + "stream_fast_path_applied name=%s", + active_name, + ) continue else: if match_err: - if self.udp_transport and policy.requires_ack(cmd_str): - self.udp_transport.send_response(f"ERROR|{match_err}", addr) - logger.log(TRACE, "Stream fast-path match failed for %s: %s", active_name, match_err) + if self.udp_transport and policy.requires_ack( + cmd_str + ): + self.udp_transport.send_response( + f"ERROR|{match_err}", addr + ) + logger.log( + TRACE, + "Stream fast-path match failed for %s: %s", + active_name, + match_err, + ) # Create command instance from message command, error = create_command_from_parts(cmd_parts) if not command: if error: # Known command but invalid parameters - logger.warning(f"Command validation failed: {cmd_str} - {error}") + logger.warning( + f"Command validation failed: {cmd_str} - {error}" + ) if self.udp_transport: self.udp_transport.send_response(f"ERROR|{error}", addr) else: # Unknown command logger.warning(f"Unknown command: {cmd_str}") if self.udp_transport: - self.udp_transport.send_response("ERROR|Unknown command", addr) + self.udp_transport.send_response( + "ERROR|Unknown command", addr + ) continue # Handle system commands (they can execute regardless of enable state) if isinstance(command, SystemCommand): # System commands execute immediately without queueing - command.bind(CommandContext( - udp_transport=self.udp_transport, - addr=addr, - gcode_interpreter=self.gcode_interpreter, - dt=self.config.loop_interval - )) - logger.log(TRACE, "syscmd_execute_start name=%s", type(command).__name__) + command.bind( + CommandContext( + udp_transport=self.udp_transport, + addr=addr, + gcode_interpreter=self.gcode_interpreter, + dt=self.config.loop_interval, + ) + ) + logger.log( + TRACE, "syscmd_execute_start name=%s", type(command).__name__ + ) command.setup(state) status = command.tick(state) - logger.log(TRACE, "syscmd_execute_%s name=%s msg=%s", - "ok" if status.code == ExecutionStatusCode.COMPLETED else "err", - type(command).__name__, status.message) + logger.log( + TRACE, + "syscmd_execute_%s name=%s msg=%s", + "ok" if status.code == ExecutionStatusCode.COMPLETED else "err", + type(command).__name__, + status.message, + ) # Handle side-effects for certain system commands (e.g., SET_PORT) try: - if status.details and isinstance(status.details, dict) and 'serial_port' in status.details: - port = status.details.get('serial_port') + if ( + status.details + and isinstance(status.details, dict) + and "serial_port" in status.details + ): + port = status.details.get("serial_port") if port: # Remember configured port self.config.serial_port = port try: # (Re)connect transport immediately using provided port - self.serial_transport = create_and_connect_transport( - port=port, - baudrate=self.config.serial_baudrate, - auto_find_port=False + self.serial_transport = ( + create_and_connect_transport( + port=port, + baudrate=self.config.serial_baudrate, + auto_find_port=False, + ) ) - if self.serial_transport and hasattr(self.serial_transport, "start_reader"): - self.serial_transport.start_reader(self.shutdown_event) + if self.serial_transport and hasattr( + self.serial_transport, "start_reader" + ): + self.serial_transport.start_reader( + self.shutdown_event + ) self.first_frame_received = False - logger.info("Serial reader thread started (after SET_PORT)") + # Reset TX keepalive to force prompt write after reconnect + if self._last_tx is not None: + self._last_tx["last_sent"] = 0.0 + logger.info( + "Serial reader thread started (after SET_PORT)" + ) except Exception as e: - logger.warning(f"Failed to (re)connect serial on SET_PORT: {e}") + logger.warning( + f"Failed to (re)connect serial on SET_PORT: {e}" + ) # Handle SIMULATOR toggle - if status.details and isinstance(status.details, dict) and 'simulator_mode' in status.details: - mode = str(status.details.get('simulator_mode', '')).lower() + if ( + status.details + and isinstance(status.details, dict) + and "simulator_mode" in status.details + ): + mode = str(status.details.get("simulator_mode", "")).lower() try: # Update env to drive transport_factory.is_simulation_mode() - os.environ["PAROL6_FAKE_SERIAL"] = "1" if mode in ("on", "1", "true", "yes") else "0" + os.environ["PAROL6_FAKE_SERIAL"] = ( + "1" if mode in ("on", "1", "true", "yes") else "0" + ) # Pre-switch safety: stop and clear motion before transport switch try: @@ -588,7 +790,10 @@ def _command_processing_loop(self): self._cancel_active_command("Simulator mode toggle") self._clear_queue("Simulator mode toggle") except Exception as _e: - logger.debug("Simulator toggle pre-switch safety failed: %s", _e) + logger.debug( + "Simulator toggle pre-switch safety failed: %s", + _e, + ) # Disconnect any existing transport if self.serial_transport: @@ -600,21 +805,42 @@ def _command_processing_loop(self): self.serial_transport = create_and_connect_transport( port=self.config.serial_port, baudrate=self.config.serial_baudrate, - auto_find_port=True + auto_find_port=True, ) # If enabled, sync simulator to last known controller state so pose continuity is preserved try: - if mode in ("on", "1", "true", "yes") and isinstance(self.serial_transport, MockSerialTransport): - self.serial_transport.sync_from_controller_state(state) + if mode in ( + "on", + "1", + "true", + "yes", + ) and isinstance( + self.serial_transport, MockSerialTransport + ): + self.serial_transport.sync_from_controller_state( + state + ) except Exception as _e: - logger.warning("Failed to sync simulator from controller state: %s", _e) + logger.warning( + "Failed to sync simulator from controller state: %s", + _e, + ) if self.serial_transport: - self.serial_transport.start_reader(self.shutdown_event) + self.serial_transport.start_reader( + self.shutdown_event + ) self.first_frame_received = False - logger.info("Serial reader thread started (after SIMULATOR toggle)") + # Reset TX keepalive to force prompt write after transport switch + if self._last_tx is not None: + self._last_tx["last_sent"] = 0.0 + logger.info( + "Serial reader thread started (after SIMULATOR toggle)" + ) except Exception as e: - logger.warning(f"Failed to (re)configure transport on SIMULATOR: {e}") + logger.warning( + f"Failed to (re)configure transport on SIMULATOR: {e}" + ) except Exception as e: logger.debug(f"System command side-effect handling failed: {e}") @@ -633,33 +859,53 @@ def _command_processing_loop(self): if self.udp_transport and policy.requires_ack(cmd_str): reason = state.disabled_reason or "Controller disabled" self.udp_transport.send_response(f"ERROR|{reason}", addr) - logger.warning(f"Motion command rejected - controller disabled: {cmd_name}") + logger.warning( + f"Motion command rejected - controller disabled: {cmd_name}" + ) continue # Query commands execute immediately (bypass queue) if isinstance(command, QueryCommand): # Execute query immediately with context - command.bind(CommandContext( - udp_transport=self.udp_transport, - addr=addr, - gcode_interpreter=self.gcode_interpreter, - dt=self.config.loop_interval - )) + command.bind( + CommandContext( + udp_transport=self.udp_transport, + addr=addr, + gcode_interpreter=self.gcode_interpreter, + dt=self.config.loop_interval, + ) + ) command.setup(state) _ = command.tick(state) # Query commands send their own responses continue # Apply stream mode logic for streamable motion commands - if state.stream_mode and isinstance(command, MotionCommand) and getattr(command, 'streamable', False): + if ( + state.stream_mode + and isinstance(command, MotionCommand) + and command.streamable + ): + # Drain UDP buffer to discard stale commands before processing new one + if self.udp_transport: + drained = self.udp_transport.drain_buffer() + if drained > 0: + logger.log(TRACE, "udp_buffer_drained count=%d", drained) + # Cancel any active streamable command and replace it (suppress per-command ACK to reduce UDP chatter) - if self.active_command and isinstance(self.active_command.command, MotionCommand) and getattr(self.active_command.command, 'streamable', False): + if ( + self.active_command + and isinstance(self.active_command.command, MotionCommand) + and getattr(self.active_command.command, "streamable", False) + ): self.active_command = None # Clear any queued streamable commands without per-command ACKs to reduce UDP chatter removed = 0 for queued_cmd in list(self.command_queue): - if isinstance(queued_cmd.command, MotionCommand) and getattr(queued_cmd.command, 'streamable', False): + if isinstance(queued_cmd.command, MotionCommand) and getattr( + queued_cmd.command, "streamable", False + ): self.command_queue.remove(queued_cmd) removed += 1 if removed: @@ -667,7 +913,9 @@ def _command_processing_loop(self): # Queue the command status = self._queue_command(addr, command, None) - logger.log(TRACE, "Command %s queued with status: %s", cmd_name, status.code) + logger.log( + TRACE, "Command %s queued with status: %s", cmd_name, status.code + ) # For motion commands: ACK acceptance only if policy requires ACK if isinstance(command, MotionCommand) and self.udp_transport: @@ -683,65 +931,86 @@ def _command_processing_loop(self): self._execute_active_command() except Exception as e: logger.error(f"Error in command processing: {e}", exc_info=True) - - - def _queue_command(self, - address: Optional[Tuple[str, int]], - command: CommandBase, - command_id: Optional[str] = None - ) -> ExecutionStatus: + + def _queue_command( + self, + address: tuple[str, int] | None, + command: CommandBase, + command_id: str | None = None, + ) -> ExecutionStatus: """ Add a command to the execution queue. - + Args: command: The command to queue command_id: Optional ID for tracking address: Optional (ip, port) for acknowledgments priority: Priority level (higher = executed first) - + Returns: ExecutionStatus indicating queue status """ # Check if queue is full if len(self.command_queue) >= 100: # max_queue_size - logger.warning(f"Command queue full (max 100)") + logger.warning("Command queue full (max 100)") if command_id and address: self._send_ack(command_id, "FAILED", "Queue full", address) return ExecutionStatus.failed("Queue full") - + # Create queued command queued_cmd = QueuedCommand( - command=command, - command_id=command_id, - address=address + command=command, command_id=command_id, address=address ) - + # Bind dynamic context so the command can reply/inspect interpreter when executed - command.bind(CommandContext( - udp_transport=self.udp_transport, - addr=address, - gcode_interpreter=self.gcode_interpreter, - dt=self.config.loop_interval - )) - + command.bind( + CommandContext( + udp_transport=self.udp_transport, + addr=address, + gcode_interpreter=self.gcode_interpreter, + dt=self.config.loop_interval, + ) + ) + self.command_queue.append(queued_cmd) - + + # Update non-streamable queue snapshot + state = self.state_manager.get_state() + state.queue_nonstreamable = [ + type(qc.command).__name__ + for qc in self.command_queue + if not ( + isinstance(qc.command, MotionCommand) + and getattr(qc.command, "streamable", False) + ) + ] + + # Update next action + if state.queue_nonstreamable: + state.action_next = state.queue_nonstreamable[0] + else: + state.action_next = "" + # Send acknowledgment if command_id and address: queue_pos = len(self.command_queue) - self._send_ack(command_id, "QUEUED", f"Position {queue_pos} in queue", address) - - logger.log(TRACE, "Queued command: %s (ID: %s)", type(command).__name__, command_id) - + self._send_ack( + command_id, "QUEUED", f"Position {queue_pos} in queue", address + ) + + logger.log( + TRACE, "Queued command: %s (ID: %s)", type(command).__name__, command_id + ) + return ExecutionStatus( code=ExecutionStatusCode.QUEUED, - message=f"Command queued at position {len(self.command_queue)}" + message=f"Command queued at position {len(self.command_queue)}", ) - - def _execute_active_command(self) -> Optional[ExecutionStatus]: + + def _execute_active_command(self) -> ExecutionStatus | None: """ Execute one step of the active command from the queue. - + Returns: ExecutionStatus of the execution, or None if no active command """ @@ -753,40 +1022,48 @@ def _execute_active_command(self) -> Optional[ExecutionStatus]: self.active_command = self.command_queue.popleft() # mark not yet activated self.active_command.activated = False - + # Execute active command step if self.active_command: ac = self.active_command try: state = self.state_manager.get_state() - ac = self.active_command - + # Check if controller is enabled if state.enabled: # Perform setup and EXECUTING ACK only once if ac and not getattr(ac, "activated", False): ac.command.setup(state) - + + # Update action tracking + state.action_current = type(ac.command).__name__ + state.action_state = "EXECUTING" + # Send executing acknowledgment once if ac.command_id and ac.address: self._send_ack( ac.command_id, "EXECUTING", f"Starting {type(ac.command).__name__}", - ac.address + ac.address, ) - + ac.activated = True - logger.log(TRACE, "Activated command: %s (id=%s)", type(ac.command).__name__, ac.command_id) + logger.log( + TRACE, + "Activated command: %s (id=%s)", + type(ac.command).__name__, + ac.command_id, + ) else: # Cancel command due to disabled controller self._cancel_active_command("Controller disabled") return ExecutionStatus( code=ExecutionStatusCode.CANCELLED, - message="Controller disabled" + message="Controller disabled", ) - + # Execute command step if not getattr(ac, "first_tick_logged", False): logger.log(TRACE, "tick_start name=%s", type(ac.command).__name__) @@ -794,100 +1071,164 @@ def _execute_active_command(self) -> Optional[ExecutionStatus]: status = ac.command.tick(state) # Enqueue any generated commands (e.g., from GCODE parsed in queued mode) - if status.details and isinstance(status.details, dict) and 'enqueue' in status.details: + if ( + status.details + and isinstance(status.details, dict) + and "enqueue" in status.details + ): try: - for robot_cmd_str in status.details['enqueue']: - cmd_obj, _ = create_command_from_parts(robot_cmd_str.split("|")) + for robot_cmd_str in status.details["enqueue"]: + cmd_obj, _ = create_command_from_parts( + robot_cmd_str.split("|") + ) if cmd_obj: # Queue without address/id for generated commands self._queue_command(("127.0.0.1", 0), cmd_obj, None) except Exception as e: logger.error(f"Error enqueuing generated commands: {e}") - + # Check if command is finished if status.code == ExecutionStatusCode.COMPLETED: # Command completed successfully name = type(ac.command).__name__ cid, addr = ac.command_id, ac.address - logger.log(TRACE, "Command completed: %s (id=%s) at t=%f", name, cid, time.time()) - + logger.log( + TRACE, + "Command completed: %s (id=%s) at t=%f", + name, + cid, + time.time(), + ) + # Send completion acknowledgment if cid and addr: - self._send_ack( - cid, - "COMPLETED", - status.message, - addr + self._send_ack(cid, "COMPLETED", status.message, addr) + + # Update action tracking to idle + state.action_current = "" + state.action_state = "IDLE" + + # Update queue snapshot and next action + state.queue_nonstreamable = [ + type(qc.command).__name__ + for qc in self.command_queue + if not ( + isinstance(qc.command, MotionCommand) + and getattr(qc.command, "streamable", False) ) - + ] + state.action_next = ( + state.queue_nonstreamable[0] + if state.queue_nonstreamable + else "" + ) + # Record and clear self.active_command = None - + elif status.code == ExecutionStatusCode.FAILED: # Command failed name = type(ac.command).__name__ cid, addr = ac.command_id, ac.address - logger.debug(f"Command failed: {name} (id={cid}) - {status.message} at t={time.time():.6f}") - + logger.debug( + f"Command failed: {name} (id={cid}) - {status.message} at t={time.time():.6f}" + ) + # Send failure acknowledgment if cid and addr: - self._send_ack( - cid, - "FAILED", - status.message, - addr + self._send_ack(cid, "FAILED", status.message, addr) + + # Update action tracking to idle + state.action_current = "" + state.action_state = "IDLE" + + # If this is a streamable command, clear all queued streamable commands + # to prevent pileup when commands fail repeatedly (e.g., IK failures at limits) + if isinstance(ac.command, MotionCommand) and getattr( + ac.command, "streamable", False + ): + removed = self._clear_streamable_commands( + f"Active streamable command failed: {status.message}" ) - + if removed > 0: + logger.info( + f"Cleared {removed} queued streamable commands due to active command failure" + ) + + # Update queue snapshot and next action + state.queue_nonstreamable = [ + type(qc.command).__name__ + for qc in self.command_queue + if not ( + isinstance(qc.command, MotionCommand) + and getattr(qc.command, "streamable", False) + ) + ] + state.action_next = ( + state.queue_nonstreamable[0] + if state.queue_nonstreamable + else "" + ) + self.active_command = None - + return status - + except Exception as e: logger.error(f"Command execution error: {e}") - + # Handle execution error - save command info before clearing cid = ac.command_id if ac else None addr = ac.address if ac else None - + if cid and addr: - self._send_ack(cid, "FAILED", f"Execution error: {str(e)}", addr) + self._send_ack(cid, "FAILED", f"Execution error: {e!s}", addr) self.active_command = None - - return ExecutionStatus.failed(f"Execution error: {str(e)}", error=e) - + + return ExecutionStatus.failed(f"Execution error: {e!s}", error=e) + return None - + def _cancel_active_command(self, reason: str = "Cancelled by user") -> None: """ Cancel the currently active command. - + Args: reason: Reason for cancellation """ if not self.active_command: return - - logger.info(f"Cancelling active command: {type(self.active_command.command).__name__} - {reason}") - + + logger.info( + f"Cancelling active command: {type(self.active_command.command).__name__} - {reason}" + ) + # Send cancellation acknowledgment if self.active_command.command_id and self.active_command.address: self._send_ack( self.active_command.command_id, "CANCELLED", reason, - self.active_command.address + self.active_command.address, ) - + + # Update action tracking to idle + state = self.state_manager.get_state() + state.action_current = "" + state.action_state = "IDLE" + # Record and clear self.active_command = None - - def _clear_queue(self, reason: str = "Queue cleared") -> List[Tuple[str, ExecutionStatus]]: + + def _clear_queue( + self, reason: str = "Queue cleared" + ) -> list[tuple[str, ExecutionStatus]]: """ Clear all queued commands. - + Args: reason: Reason for clearing the queue - + Returns: List of (command_id, status) for cleared commands """ @@ -895,30 +1236,67 @@ def _clear_queue(self, reason: str = "Queue cleared") -> List[Tuple[str, Executi # TODO: don't send out an ack for every queued command, just one signalling queues been cleared while self.command_queue: queued_cmd = self.command_queue.popleft() - + # Send cancellation acknowledgment if queued_cmd.command_id and queued_cmd.address: self._send_ack( - queued_cmd.command_id, - "CANCELLED", - reason, - queued_cmd.address + queued_cmd.command_id, "CANCELLED", reason, queued_cmd.address ) - + # Record cleared command if queued_cmd.command_id: status = ExecutionStatus( - code=ExecutionStatusCode.CANCELLED, - message=reason + code=ExecutionStatusCode.CANCELLED, message=reason ) cleared.append((queued_cmd.command_id, status)) - + logger.info(f"Cleared {len(cleared)} commands from queue: {reason}") - + + # Update action tracking + state = self.state_manager.get_state() + state.queue_nonstreamable = [] + state.action_next = "" + return cleared - - - + + def _clear_streamable_commands( + self, reason: str = "Streamable commands cleared" + ) -> int: + """ + Clear all queued streamable motion commands. + + This is used to prevent command pileup when streamable commands fail repeatedly + (e.g., IK failures when jogging at kinematic limits). + + Args: + reason: Reason for clearing streamable commands + + Returns: + Number of commands cleared + """ + removed_count = 0 + + # Iterate through a copy of the queue to safely remove items + for queued_cmd in list(self.command_queue): + if isinstance(queued_cmd.command, MotionCommand) and getattr( + queued_cmd.command, "streamable", False + ): + self.command_queue.remove(queued_cmd) + removed_count += 1 + + # Send cancellation acknowledgment (though streamable commands typically don't have IDs) + if queued_cmd.command_id and queued_cmd.address: + self._send_ack( + queued_cmd.command_id, "CANCELLED", reason, queued_cmd.address + ) + + if removed_count > 0: + logger.debug( + f"Cleared {removed_count} streamable commands from queue: {reason}" + ) + + return removed_count + def _fetch_gcode_commands(self): """ Fetch next command from GCODE interpreter if program is running. @@ -926,74 +1304,98 @@ def _fetch_gcode_commands(self): """ if not self.gcode_interpreter.is_running: return - + try: # Get next command from GCODE program next_gcode_cmd = self.gcode_interpreter.get_next_command() if not next_gcode_cmd: return - + # Use command registry to create command object command_obj, _ = create_command_from_parts(next_gcode_cmd.split("|")) - + if command_obj: # Queue without address/id for GCODE commands self._queue_command(("127.0.0.1", 0), command_obj, None) - cmd_name = next_gcode_cmd.split('|')[0] if '|' in next_gcode_cmd else next_gcode_cmd + cmd_name = ( + next_gcode_cmd.split("|")[0] + if "|" in next_gcode_cmd + else next_gcode_cmd + ) logger.debug(f"Queued GCODE command: {cmd_name}") else: logger.warning(f"Unknown GCODE command generated: {next_gcode_cmd}") - + except Exception as e: logger.error(f"Error fetching GCODE commands: {e}") def main(): - global TRACE_ENABLED - """Main entry point for the controller.""" + """Main entry point for the controller.""" # Parse arguments first to get logging level - parser = argparse.ArgumentParser(description='PAROL6 Robot Controller') - parser.add_argument('--host', default='0.0.0.0', help='UDP host address') - parser.add_argument('--port', type=int, default=5001, help='UDP port') - parser.add_argument('--serial', help='Serial port (e.g., /dev/ttyUSB0 or COM3)') - parser.add_argument('--baudrate', type=int, default=3000000, help='Serial baudrate') - parser.add_argument('--auto-home', action='store_true', - help='Queue HOME on startup (default: off)') - + parser = argparse.ArgumentParser(description="PAROL6 Robot Controller") + parser.add_argument("--host", default="0.0.0.0", help="UDP host address") + parser.add_argument("--port", type=int, default=5001, help="UDP port") + parser.add_argument("--serial", help="Serial port (e.g., /dev/ttyUSB0 or COM3)") + parser.add_argument("--baudrate", type=int, default=3000000, help="Serial baudrate") + parser.add_argument( + "--auto-home", action="store_true", help="Queue HOME on startup (default: off)" + ) + # Verbose logging options (from controller.py) - parser.add_argument('-v', '--verbose', action='count', default=0, - help='Increase verbosity; -v=INFO, -vv=DEBUG, -vvv=TRACE') - parser.add_argument('-q', '--quiet', action='store_true', - help='Enable quiet logging (WARNING level)') - parser.add_argument('--log-level', choices=['TRACE', 'DEBUG', 'INFO', 'WARNING', 'ERROR', 'CRITICAL'], - help='Set specific log level') + parser.add_argument( + "-v", + "--verbose", + action="count", + default=0, + help="Increase verbosity; -v=INFO, -vv=DEBUG, -vvv=TRACE", + ) + parser.add_argument( + "-q", + "--quiet", + action="store_true", + help="Enable quiet logging (WARNING level)", + ) + parser.add_argument( + "--log-level", + choices=["TRACE", "DEBUG", "INFO", "WARNING", "ERROR", "CRITICAL"], + help="Set specific log level", + ) args = parser.parse_args() - + # Determine log level + # Precedence: + # 1) Explicit --log-level + # 2) Verbose / quiet flags + # 3) Environment-driven TRACE (PAROL_TRACE=1 via TRACE_ENABLED) + # 4) Default INFO if args.log_level: - if args.log_level == 'TRACE': + if args.log_level == "TRACE": log_level = TRACE - TRACE_ENABLED=True + cfg.TRACE_ENABLED = True else: log_level = getattr(logging, args.log_level) elif args.verbose >= 3: log_level = TRACE - TRACE_ENABLED=True + cfg.TRACE_ENABLED = True elif args.verbose >= 2: log_level = logging.DEBUG elif args.verbose == 1: log_level = logging.INFO elif args.quiet: log_level = logging.WARNING + elif cfg.TRACE_ENABLED: + # Enable TRACE when PAROL_TRACE=1 and no CLI override is given + log_level = TRACE else: log_level = logging.INFO logging.basicConfig( level=log_level, format="%(asctime)s %(levelname)s %(name)s: %(message)s", - datefmt="%H:%M:%S" + datefmt="%H:%M:%S", ) - + # Create configuration (env vars may override defaults) env_host = os.getenv("PAROL6_CONTROLLER_IP") env_port = os.getenv("PAROL6_CONTROLLER_PORT") @@ -1010,13 +1412,13 @@ def main(): udp_port=udp_port, serial_port=args.serial, serial_baudrate=args.baudrate, - auto_home=bool(args.auto_home) + auto_home=bool(args.auto_home), ) - + # Create and run controller try: controller = Controller(config) - + if controller.is_initialized(): try: controller.start() @@ -1030,9 +1432,9 @@ def main(): except RuntimeError as e: logger.error(f"Failed to create controller: {e}") return 1 - + return 0 -if __name__ == '__main__': +if __name__ == "__main__": exit(main()) diff --git a/parol6/server/state.py b/parol6/server/state.py index ddb1773..79ad91d 100644 --- a/parol6/server/state.py +++ b/parol6/server/state.py @@ -1,18 +1,22 @@ from __future__ import annotations -from dataclasses import dataclass, field -from typing import Deque, Dict, List, Optional, Tuple, Any, Union, Sequence -from collections import deque -import threading -import time import logging +import threading +from collections import deque +from dataclasses import dataclass, field +from typing import Any, cast + import numpy as np +from spatialmath import SE3 + +import parol6.PAROL6_ROBOT as PAROL6_ROBOT from parol6.protocol.wire import CommandCode @dataclass class GripperModeResetTracker: """Tracks gripper mode for auto-reset functionality.""" + calibration_sent: bool = False # Flag for calibration mode error_clear_sent: bool = False # Flag for error clear mode @@ -24,6 +28,7 @@ class ControllerState: Buffers use preallocated NumPy ndarrays for zero-copy, in-place operations. """ + # Serial/transport ser: Any = None last_reconnect_attempt: float = 0.0 @@ -35,6 +40,9 @@ class ControllerState: e_stop_active: bool = False stream_mode: bool = False + # Tool configuration (affects kinematics and visualization) + _current_tool: str = "NONE" + # I/O buffers and protocol tracking (serial frame parsing state) input_byte: int = 0 start_cond1: int = 0 @@ -42,60 +50,116 @@ class ControllerState: start_cond3: int = 0 good_start: int = 0 data_len: int = 0 - data_buffer: List[bytes] = field(default_factory=lambda: [b""] * 255) + data_buffer: list[bytes] = field(default_factory=lambda: [b""] * 255) data_counter: int = 0 # Robot telemetry and command buffers - using ndarray for efficiency Command_out: CommandCode = CommandCode.IDLE # The command code to send to firmware # int32 joint buffers - Position_out: np.ndarray = field(default_factory=lambda: np.zeros((6,), dtype=np.int32)) - Speed_out: np.ndarray = field(default_factory=lambda: np.zeros((6,), dtype=np.int32)) - Gripper_data_out: np.ndarray = field(default_factory=lambda: np.zeros((6,), dtype=np.int32)) - - Position_in: np.ndarray = field(default_factory=lambda: np.zeros((6,), dtype=np.int32)) + Position_out: np.ndarray = field( + default_factory=lambda: np.zeros((6,), dtype=np.int32) + ) + Speed_out: np.ndarray = field( + default_factory=lambda: np.zeros((6,), dtype=np.int32) + ) + Gripper_data_out: np.ndarray = field( + default_factory=lambda: np.zeros((6,), dtype=np.int32) + ) + + Position_in: np.ndarray = field( + default_factory=lambda: np.zeros((6,), dtype=np.int32) + ) Speed_in: np.ndarray = field(default_factory=lambda: np.zeros((6,), dtype=np.int32)) - Timing_data_in: np.ndarray = field(default_factory=lambda: np.zeros((1,), dtype=np.int32)) - Gripper_data_in: np.ndarray = field(default_factory=lambda: np.zeros((6,), dtype=np.int32)) + Timing_data_in: np.ndarray = field( + default_factory=lambda: np.zeros((1,), dtype=np.int32) + ) + Gripper_data_in: np.ndarray = field( + default_factory=lambda: np.zeros((6,), dtype=np.int32) + ) # uint8 flag/bitfield buffers - Affected_joint_out: np.ndarray = field(default_factory=lambda: np.zeros((8,), dtype=np.uint8)) - InOut_out: np.ndarray = field(default_factory=lambda: np.zeros((8,), dtype=np.uint8)) + Affected_joint_out: np.ndarray = field( + default_factory=lambda: np.zeros((8,), dtype=np.uint8) + ) + InOut_out: np.ndarray = field( + default_factory=lambda: np.zeros((8,), dtype=np.uint8) + ) InOut_in: np.ndarray = field(default_factory=lambda: np.zeros((8,), dtype=np.uint8)) Homed_in: np.ndarray = field(default_factory=lambda: np.zeros((8,), dtype=np.uint8)) - Temperature_error_in: np.ndarray = field(default_factory=lambda: np.zeros((8,), dtype=np.uint8)) - Position_error_in: np.ndarray = field(default_factory=lambda: np.zeros((8,), dtype=np.uint8)) + Temperature_error_in: np.ndarray = field( + default_factory=lambda: np.zeros((8,), dtype=np.uint8) + ) + Position_error_in: np.ndarray = field( + default_factory=lambda: np.zeros((8,), dtype=np.uint8) + ) Timeout_out: int = 0 XTR_data: int = 0 # Command queueing and tracking - command_queue: Deque[Any] = field(default_factory=deque) - incoming_command_buffer: Deque[Tuple[str, Tuple[str, int]]] = field(default_factory=deque) - command_id_map: Dict[Any, Tuple[str, Tuple[str, int]]] = field(default_factory=dict) + command_queue: deque[Any] = field(default_factory=deque) + incoming_command_buffer: deque[tuple[str, tuple[str, int]]] = field( + default_factory=deque + ) + command_id_map: dict[Any, tuple[str, tuple[str, int]]] = field(default_factory=dict) active_command: Any = None - active_command_id: Optional[str] = None + active_command_id: str | None = None last_command_time: float = 0.0 + # Action tracking for status broadcast and queries + action_current: str = "" + action_state: str = "IDLE" # IDLE, EXECUTING, COMPLETED, FAILED + action_next: str = "" + queue_nonstreamable: list[str] = field(default_factory=list) + # Network setup and uptime ip: str = "127.0.0.1" port: int = 5001 start_time: float = 0.0 - gripper_mode_tracker: GripperModeResetTracker = field(default_factory=GripperModeResetTracker) + gripper_mode_tracker: GripperModeResetTracker = field( + default_factory=GripperModeResetTracker + ) # Control loop runtime metrics (used by benchmarks/monitoring) loop_count: int = 0 overrun_count: int = 0 last_period_s: float = 0.0 ema_period_s: float = 0.0 - + # Command frequency metrics command_count: int = 0 - last_command_time: float = 0.0 last_command_period_s: float = 0.0 ema_command_period_s: float = 0.0 - command_timestamps: Deque[float] = field(default_factory=lambda: deque(maxlen=10)) + command_timestamps: deque[float] = field(default_factory=lambda: deque(maxlen=10)) + + # Forward kinematics cache (invalidated when Position_in or current_tool changes) + _fkine_last_pos_in: np.ndarray = field( + default_factory=lambda: np.zeros((6,), dtype=np.int32) + ) + _fkine_last_tool: str = "" + _fkine_se3: Any = None # SE3 instance from spatialmath + _fkine_mat: np.ndarray = field(default_factory=lambda: np.eye(4, dtype=np.float64)) + _fkine_flat_mm: np.ndarray = field( + default_factory=lambda: np.zeros((16,), dtype=np.float64) + ) + + @property + def current_tool(self) -> str: + """Get the current tool name.""" + return self._current_tool + + @current_tool.setter + def current_tool(self, tool_name: str) -> None: + """Set the current tool and apply it to the robot model.""" + if tool_name != self._current_tool: + self._current_tool = tool_name + # Apply tool to robot model (rebuilds with tool as final link) + PAROL6_ROBOT.apply_tool(tool_name) + # Invalidate cache + self._fkine_se3 = None + logger.info(f"Tool changed to {tool_name}, fkine cache invalidated") logger = logging.getLogger(__name__) @@ -109,9 +173,9 @@ class StateManager: convenience methods for common state operations. """ - _instance: Optional[StateManager] = None + _instance: StateManager | None = None _lock: threading.Lock = threading.Lock() - _state: Optional[ControllerState] = None + _state: ControllerState | None = None def __new__(cls) -> StateManager: """Ensure singleton pattern with thread safety.""" @@ -124,7 +188,7 @@ def __new__(cls) -> StateManager: def __init__(self): """Initialize the state manager (only runs once due to singleton).""" - if not hasattr(self, '_initialized'): + if not hasattr(self, "_initialized"): self._state = ControllerState() self._state_lock = threading.RLock() # Use RLock for re-entrant locking self._initialized = True @@ -157,8 +221,9 @@ def reset_state(self) -> None: self._state = ControllerState() logger.info("Controller state reset") + # Global singleton instance accessor -_state_manager: Optional[StateManager] = None +_state_manager: StateManager | None = None def get_instance() -> StateManager: @@ -176,3 +241,110 @@ def get_state() -> ControllerState: Convenience function to get the current controller state. """ return get_instance().get_state() + + +# ----------------------------- +# Forward kinematics cache management +# ----------------------------- + + +def invalidate_fkine_cache() -> None: + """ + Invalidate the fkine cache, forcing recomputation on next access. + Call this when the robot model changes (e.g., tool change). + """ + state = get_state() + state._fkine_se3 = None + state._fkine_last_tool = "" + logger.debug("fkine cache invalidated") + + +def ensure_fkine_updated(state: ControllerState) -> None: + """ + Ensure the fkine cache is up to date with current Position_in and tool. + If Position_in or current_tool has changed, recalculate fkine and update cache. + + This function is thread-safe when called with state from get_state(). + + Parameters + ---------- + state : ControllerState + The controller state to update + """ + # Check if cache is valid + pos_changed = not np.array_equal(state.Position_in, state._fkine_last_pos_in) + tool_changed = state.current_tool != state._fkine_last_tool + + if pos_changed or tool_changed or state._fkine_se3 is None: + # Recompute fkine + q = PAROL6_ROBOT.ops.steps_to_rad(state.Position_in) + assert PAROL6_ROBOT.robot is not None + T = cast(SE3, cast(Any, PAROL6_ROBOT.robot).fkine(q)) + + # Cache SE3 object + state._fkine_se3 = T + + # Cache as 4x4 matrix + mat = np.asarray(T.A, dtype=np.float64).copy() + np.copyto(state._fkine_mat, mat) + + # Cache as flattened 16-vector with mm translation + flat = mat.reshape(-1).copy() + flat[3] *= 1000.0 # X translation to mm + flat[7] *= 1000.0 # Y translation to mm + flat[11] *= 1000.0 # Z translation to mm + np.copyto(state._fkine_flat_mm, flat) + + # Update cache tracking + np.copyto(state._fkine_last_pos_in, state.Position_in) + state._fkine_last_tool = state.current_tool + + +def get_fkine_se3(state: ControllerState | None = None) -> SE3: + """ + Get the current end-effector pose as an SE3 object. + Automatically updates cache if needed. + + Returns + ------- + SE3 + Current end-effector pose + """ + if state is None: + state = get_state() + ensure_fkine_updated(state) + return state._fkine_se3 + + +def get_fkine_matrix(state: ControllerState | None = None) -> np.ndarray: + """ + Get the current end-effector pose as a 4x4 homogeneous transformation matrix. + Automatically updates cache if needed. + Translation is in meters. + + Returns + ------- + np.ndarray + 4x4 transformation matrix (translation in meters) + """ + if state is None: + state = get_state() + ensure_fkine_updated(state) + return state._fkine_mat + + +def get_fkine_flat_mm(state: ControllerState | None = None) -> np.ndarray: + """ + Get the current end-effector pose as a flattened 16-element array. + Automatically updates cache if needed. + Translation components (indices 3, 7, 11) are in millimeters for compatibility. + + Returns + ------- + np.ndarray + Flattened 16-element pose array (translation in mm) + """ + if state is None: + state = get_state() + ensure_fkine_updated(state) + return state._fkine_flat_mm diff --git a/parol6/server/status_broadcast.py b/parol6/server/status_broadcast.py index fd11daa..3104acd 100644 --- a/parol6/server/status_broadcast.py +++ b/parol6/server/status_broadcast.py @@ -1,28 +1,34 @@ from __future__ import annotations -import os +import logging import socket -import struct import threading import time -import logging -from typing import Optional +from parol6 import config as cfg from parol6.server.state import StateManager from parol6.server.status_cache import get_cache -from parol6 import config as cfg logger = logging.getLogger(__name__) + class StatusBroadcaster(threading.Thread): """ - Broadcasts ASCII STATUS frames via UDP multicast. + Broadcasts ASCII STATUS frames via UDP. + + Transport: + - cfg.STATUS_TRANSPORT: "MULTICAST" (default) or "UNICAST" - Config: + Multicast Config (used when STATUS_TRANSPORT == MULTICAST): - cfg.MCAST_GROUP (default "239.255.0.101") - - cfg.MCAST_PORT (default 50510) - - cfg.MCAST_TTL (default 1) - - cfg.MCAST_IF (default "127.0.0.1") + - cfg.MCAST_PORT (default 50510) + - cfg.MCAST_TTL (default 1) + - cfg.MCAST_IF (default "127.0.0.1") + + Unicast Config (used when STATUS_TRANSPORT == UNICAST): + - cfg.STATUS_UNICAST_HOST (default "127.0.0.1") + + General: - cfg.STATUS_RATE_HZ (default 50) - cfg.STATUS_STALE_S (default 0.2) -> skip broadcast if cache is stale """ @@ -46,60 +52,160 @@ def __init__( self._period = 1.0 / max(rate_hz, 1.0) self._stale_s = stale_s - self._sock: Optional[socket.socket] = None + # Negotiated transport (can be forced via env or auto-fallback at runtime) + self._use_unicast: bool = cfg.STATUS_TRANSPORT == "UNICAST" + + self._sock: socket.socket | None = None self._running = threading.Event() self._running.set() - - # EMA rate tracking for multicast TX + + # EMA rate tracking for TX self._tx_count = 0 self._tx_last_time = time.monotonic() self._tx_ema_period = 1.0 / rate_hz # Initialize with expected period self._tx_last_log_time = time.monotonic() # For 3-second logging interval + # Failure tracking for runtime fallback + self._send_failures = 0 + self._max_send_failures = 3 + + def _detect_primary_ip(self) -> str: + tmp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + try: + tmp.connect(("1.1.1.1", 80)) + return tmp.getsockname()[0] + except Exception: + return "127.0.0.1" + finally: + try: + tmp.close() + except Exception: + pass + + def _verify_multicast_reachable( + self, sock: socket.socket, timeout: float = 0.1 + ) -> bool: + """ + Attempt a loopback reachability check for multicast by joining the group on a + temporary receiver and sending a probe. Returns True if the probe is received. + """ + recv_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + try: + recv_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + try: + recv_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) + except Exception: + pass + recv_sock.bind(("", self.port)) + mreq = socket.inet_aton(self.group) + socket.inet_aton(self.iface_ip) + recv_sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq) + recv_sock.settimeout(timeout) + + token = b"PAROL6_MCAST_PROBE" + try: + sock.sendto(token, (self.group, self.port)) + except OSError as e: + logger.debug(f"Multicast probe send failed: {e}") + return False + try: + data, _ = recv_sock.recvfrom(2048) + return data == token + except Exception: + return False + finally: + try: + recv_sock.close() + except Exception: + pass + + def _switch_to_unicast(self) -> None: + """Close current socket and switch to unicast transport.""" + try: + if self._sock: + self._sock.close() + except Exception as e: + logger.debug(f"Error closing multicast socket during fallback: {e}") + usock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + usock.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF, 1 << 20) + self._sock = usock + self._use_unicast = True + self._send_failures = 0 + logger.info( + f"StatusBroadcaster (UNICAST-FALLBACK) -> dest={cfg.STATUS_UNICAST_HOST}:{self.port}" + ) + def _setup_socket(self) -> None: + # UNICAST: simple UDP socket without multicast options + if self._use_unicast: + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF, 1 << 20) + self._sock = sock + logger.info( + f"StatusBroadcaster (UNICAST) -> dest={cfg.STATUS_UNICAST_HOST}:{self.port}" + ) + return + + # MULTICAST: configure multicast TTL/IF with verification and fallback sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, self.ttl) sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1) - # Prefer loopback interface for multicast; if that fails, fall back to primary NIC IP - def _detect_primary_ip() -> str: - tmp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + try: + sock.setsockopt( + socket.IPPROTO_IP, + socket.IP_MULTICAST_IF, + socket.inet_aton(self.iface_ip), + ) + if not self._verify_multicast_reachable(sock): + raise RuntimeError( + f"Initial multicast reachability check failed on iface {self.iface_ip}" + ) + except Exception as e: + logger.warning( + f"StatusBroadcaster: interface {self.iface_ip} failed verification: {e}" + ) try: - tmp.connect(("1.1.1.1", 80)) - return tmp.getsockname()[0] - except Exception: - return "127.0.0.1" - finally: + primary_ip = self._detect_primary_ip() + sock.setsockopt( + socket.IPPROTO_IP, + socket.IP_MULTICAST_IF, + socket.inet_aton(primary_ip), + ) + logger.info( + f"StatusBroadcaster: fallback IP_MULTICAST_IF to {primary_ip}" + ) + if not self._verify_multicast_reachable(sock): + raise RuntimeError("Fallback multicast reachability failed") + except Exception as e2: + logger.warning( + f"StatusBroadcaster: failed to set IP_MULTICAST_IF: {e2}" + ) + # As a last resort, switch to UNICAST try: - tmp.close() + sock.close() except Exception: pass - - try: - sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(self.iface_ip)) - except Exception: - try: - primary_ip = _detect_primary_ip() - sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(primary_ip)) - logger.info(f"StatusBroadcaster: fallback IP_MULTICAST_IF to {primary_ip}") - except Exception as e: - logger.warning(f"StatusBroadcaster: failed to set IP_MULTICAST_IF: {e}") + self._switch_to_unicast() + return sock.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF, 1 << 20) self._sock = sock + logger.info( + f"StatusBroadcaster (MULTICAST) -> group={self.group} port={self.port} iface={self.iface_ip} ttl={self.ttl}" + ) def run(self) -> None: self._setup_socket() cache = get_cache() - dest = (self.group, self.port) - sock = self._sock - if sock is None: + + # Validate socket exists + if self._sock is None: logger.error("StatusBroadcaster socket not initialized") return # Deadline-based timing to maintain consistent rate next_deadline = time.monotonic() + self._period - + while self._running.is_set(): # Always refresh cache from latest state before considering broadcast try: @@ -111,25 +217,57 @@ def run(self) -> None: # Skip broadcast if cache is stale (e.g., serial disconnected) if cache.age_s() <= self._stale_s: payload = cache.to_ascii().encode("ascii", errors="ignore") - # memoryview avoids an extra copy in some implementations - sock.sendto(memoryview(payload), dest) - - # Track multicast TX rate with EMA - now = time.monotonic() - if self._tx_count > 0: # Skip first sample for period calculation - period = now - self._tx_last_time - if period > 0: - # EMA update: 0.1 * new + 0.9 * old - self._tx_ema_period = 0.1 * period + 0.9 * self._tx_ema_period - self._tx_last_time = now - self._tx_count += 1 - - # Log rate every 3 seconds - if now - self._tx_last_log_time >= 3.0 and self._tx_ema_period > 0: - tx_hz = 1.0 / self._tx_ema_period - logger.debug(f"Multicast TX: {tx_hz:.1f} Hz (count={self._tx_count})") - self._tx_last_log_time = now - + # Refresh socket and destination each loop in case we switched transports + sock = self._sock + if sock is None: + # Socket disappeared unexpectedly; try to switch to unicast and continue + self._switch_to_unicast() + sock = self._sock + dest = ( + (cfg.STATUS_UNICAST_HOST, self.port) + if self._use_unicast + else (self.group, self.port) + ) + try: + sock.sendto(memoryview(payload), dest) # type: ignore[arg-type] + except OSError as e: + self._send_failures += 1 + # Log occasionally to avoid flooding + if time.monotonic() - self._tx_last_log_time >= 5.0: + logger.warning(f"StatusBroadcaster send failed: {e}") + self._tx_last_log_time = time.monotonic() + # If too many failures and we are on multicast, fall back to unicast + if ( + not self._use_unicast + and self._send_failures >= self._max_send_failures + ): + logger.info( + f"StatusBroadcaster: {self._send_failures} consecutive send errors; switching to UNICAST" + ) + self._switch_to_unicast() + else: + # Reset failure count on success + self._send_failures = 0 + # Track TX rate with EMA + now = time.monotonic() + if self._tx_count > 0: # Skip first sample for period calculation + period = now - self._tx_last_time + if period > 0: + # EMA update: 0.1 * new + 0.9 * old + self._tx_ema_period = ( + 0.1 * period + 0.9 * self._tx_ema_period + ) + self._tx_last_time = now + self._tx_count += 1 + + # Log rate every 3 seconds + if now - self._tx_last_log_time >= 3.0 and self._tx_ema_period > 0: + tx_hz = 1.0 / self._tx_ema_period + logger.debug( + f"Status TX: {tx_hz:.1f} Hz (count={self._tx_count})" + ) + self._tx_last_log_time = now + # Sleep until next deadline (compensates for work time) sleep_time = next_deadline - time.monotonic() if sleep_time > 0: diff --git a/parol6/server/status_cache.py b/parol6/server/status_cache.py index 702a9d2..2ab6a43 100644 --- a/parol6/server/status_cache.py +++ b/parol6/server/status_cache.py @@ -1,11 +1,15 @@ import threading import time -from typing import List, Optional -from numpy.typing import ArrayLike + import numpy as np +from numpy.typing import ArrayLike import parol6.PAROL6_ROBOT as PAROL6_ROBOT -from parol6.server.state import ControllerState +from parol6.server.state import ControllerState, get_fkine_flat_mm, get_fkine_se3 +from parol6.utils.ik import AXIS_MAP, solve_ik +from spatialmath import SE3 +from typing import Any +import math class StatusCache: @@ -33,6 +37,7 @@ def __init__(self) -> None: self.last_update_s: float = 0.0 # last cache build (any section) self.last_serial_s: float = 0.0 # last time a fresh serial frame was observed + self._last_tool_name: str = "NONE" # Track tool changes # Cached ASCII fragments to reduce allocations self._angles_ascii: str = "0,0,0,0,0,0" @@ -40,12 +45,30 @@ def __init__(self) -> None: self._io_ascii: str = "0,0,0,0,0" self._gripper_ascii: str = "0,0,0,0,0,0" self._pose_ascii: str = ",".join("0" for _ in range(16)) + + # Action tracking fields + self._action_current: str = "" + self._action_state: str = "IDLE" + + # Enablement arrays (12 ints each) + self.joint_en = np.ones((12,), dtype=np.uint8) + self.cart_en_wrf = np.ones((12,), dtype=np.uint8) + self.cart_en_trf = np.ones((12,), dtype=np.uint8) + self._joint_en_ascii: str = ",".join(str(int(v)) for v in self.joint_en) + self._cart_en_wrf_ascii: str = ",".join(str(int(v)) for v in self.cart_en_wrf) + self._cart_en_trf_ascii: str = ",".join(str(int(v)) for v in self.cart_en_trf) + self._ascii_full: str = ( f"STATUS|POSE={self._pose_ascii}" f"|ANGLES={self._angles_ascii}" f"|SPEEDS={self._speeds_ascii}" f"|IO={self._io_ascii}" f"|GRIPPER={self._gripper_ascii}" + f"|ACTION_CURRENT={self._action_current}" + f"|ACTION_STATE={self._action_state}" + f"|JOINT_EN={self._joint_en_ascii}" + f"|CART_EN_WRF={self._cart_en_wrf_ascii}" + f"|CART_EN_TRF={self._cart_en_trf_ascii}" ) # Change-detection caches to avoid expensive recomputation when inputs unchanged @@ -53,7 +76,89 @@ def __init__(self) -> None: def _format_csv_from_list(self, vals: ArrayLike) -> str: # Using str() on each value preserves prior formatting semantics - return ",".join(str(v) for v in vals) # type: ignore + return ",".join(str(v) for v in vals) # type: ignore + + def _compute_joint_enable( + self, q_rad: np.ndarray, delta_rad: float = math.radians(0.2) + ) -> None: + """Compute per-joint +/- enable bits based on joint limits and a small delta.""" + # Be robust to uninitialized robot in type-checked context + robot: Any = getattr(PAROL6_ROBOT, "robot", None) + if robot is None: + self.joint_en[:] = 1 + return + qlim = getattr(robot, "qlim", None) + if qlim is None: + self.joint_en[:] = 1 + return + allow_plus = (q_rad + delta_rad) <= qlim[1, :] + allow_minus = (q_rad - delta_rad) >= qlim[0, :] + # Pack into [J1+,J1-,J2+,J2-,...,J6+,J6-] + bits = [] + for i in range(6): + bits.append(1 if allow_plus[i] else 0) + bits.append(1 if allow_minus[i] else 0) + self.joint_en[:] = np.asarray(bits, dtype=np.uint8) + self._joint_en_ascii = self._format_csv_from_list(self.joint_en.tolist()) + + def _compute_cart_enable( + self, + T: SE3, + frame: str, + q_rad: np.ndarray, + delta_mm: float = 0.5, + delta_deg: float = 0.5, + ) -> None: + """Compute per-axis +/- enable bits for the given frame (WRF/TRF) via small-step IK.""" + bits = [] + # Build small delta transforms + t_step_m = delta_mm / 1000.0 + r_step_rad = math.radians(delta_deg) + for axis, (v_lin, v_rot) in AXIS_MAP.items(): + # Compose delta SE3 for this axis + dT = SE3() + # Translation + dx = v_lin[0] * t_step_m + dy = v_lin[1] * t_step_m + dz = v_lin[2] * t_step_m + if abs(dx) > 0 or abs(dy) > 0 or abs(dz) > 0: + dT = dT * SE3(dx, dy, dz) + # Rotation + rx = v_rot[0] * r_step_rad + ry = v_rot[1] * r_step_rad + rz = v_rot[2] * r_step_rad + if abs(rx) > 0: + dT = dT * SE3.Rx(rx) + if abs(ry) > 0: + dT = dT * SE3.Ry(ry) + if abs(rz) > 0: + dT = dT * SE3.Rz(rz) + + # Apply in specified frame + if frame == "WRF": + T_target = dT * T + else: # TRF + T_target = T * dT + + try: + ik = solve_ik( + PAROL6_ROBOT.robot, + T_target, + q_rad, + jogging=True, + quiet_logging=True, + ) + bits.append(1 if ik.success else 0) + except Exception: + bits.append(0) + + arr = np.asarray(bits, dtype=np.uint8) + if frame == "WRF": + self.cart_en_wrf[:] = arr + self._cart_en_wrf_ascii = self._format_csv_from_list(arr.tolist()) + else: + self.cart_en_trf[:] = arr + self._cart_en_trf_ascii = self._format_csv_from_list(arr.tolist()) def update_from_state(self, state: ControllerState) -> None: """ @@ -65,23 +170,49 @@ def update_from_state(self, state: ControllerState) -> None: changed_any = False with self._lock: - if self._last_pos_in is None or not np.array_equal(state.Position_in, self._last_pos_in): # Position changed - np.copyto(self._last_pos_in, state.Position_in) + # Check if position or tool changed + tool_changed = state.current_tool != self._last_tool_name + pos_changed = self._last_pos_in is None or not np.array_equal( + state.Position_in, self._last_pos_in + ) + + if pos_changed or tool_changed: + if pos_changed: + np.copyto(self._last_pos_in, state.Position_in) + if tool_changed: + self._last_tool_name = state.current_tool + # Vectorized steps->deg - self.angles_deg = np.asarray(PAROL6_ROBOT.ops.steps_to_deg(state.Position_in)) # float64, shape (6,) + self.angles_deg = np.asarray( + PAROL6_ROBOT.ops.steps_to_deg(state.Position_in) + ) # float64, shape (6,) # Publish angles list and ASCII self._angles_ascii = self._format_csv_from_list(self.angles_deg) changed_any = True - # Vectorized steps->rad for FK - q_current = PAROL6_ROBOT.ops.steps_to_rad(state.Position_in) # float64, shape (6,) - # robot.fkine expects joint vector in radians - current_pose_matrix = PAROL6_ROBOT.robot.fkine(q_current).A # 4x4 - pose_flat = current_pose_matrix.reshape(-1) # 16 - self.pose = np.asarray(pose_flat, dtype=np.float64) + # Get cached fkine (automatically updates if needed) + pose_flat_mm = get_fkine_flat_mm(state) # Already in mm for translation + np.copyto(self.pose, pose_flat_mm) self._pose_ascii = self._format_csv_from_list(self.pose) changed_any = True + # Compute enablement arrays at 50 Hz when pose/angles change + try: + q_rad = np.asarray( + PAROL6_ROBOT.ops.steps_to_rad(state.Position_in), dtype=float + ) + except Exception: + q_rad = np.zeros((6,), dtype=float) + try: + T = get_fkine_se3(state) + except Exception: + T = SE3() + # JOINT_EN + self._compute_joint_enable(q_rad) + # CART_EN for both frames + self._compute_cart_enable(T, "WRF", q_rad) + self._compute_cart_enable(T, "TRF", q_rad) + # 2) IO (first 5) if not np.array_equal(self.io, state.InOut_in[:5]): np.copyto(self.io, state.InOut_in[:5]) @@ -100,7 +231,16 @@ def update_from_state(self, state: ControllerState) -> None: self._gripper_ascii = self._format_csv_from_list(self.gripper) changed_any = True - # 5) Assemble full ASCII only if any section changed + # 5) Action tracking + if ( + self._action_current != state.action_current + or self._action_state != state.action_state + ): + self._action_current = state.action_current + self._action_state = state.action_state + changed_any = True + + # 6) Assemble full ASCII only if any section changed if changed_any: self._ascii_full = ( f"STATUS|POSE={self._pose_ascii}" @@ -108,6 +248,11 @@ def update_from_state(self, state: ControllerState) -> None: f"|SPEEDS={self._speeds_ascii}" f"|IO={self._io_ascii}" f"|GRIPPER={self._gripper_ascii}" + f"|ACTION_CURRENT={self._action_current}" + f"|ACTION_STATE={self._action_state}" + f"|JOINT_EN={self._joint_en_ascii}" + f"|CART_EN_WRF={self._cart_en_wrf_ascii}" + f"|CART_EN_TRF={self._cart_en_trf_ascii}" ) self.last_update_s = now @@ -130,7 +275,7 @@ def age_s(self) -> float: # Module-level singleton -_status_cache: Optional[StatusCache] = None +_status_cache: StatusCache | None = None def get_cache() -> StatusCache: diff --git a/parol6/server/transports/__init__.py b/parol6/server/transports/__init__.py index 969b255..1cafe02 100644 --- a/parol6/server/transports/__init__.py +++ b/parol6/server/transports/__init__.py @@ -5,20 +5,20 @@ communicating with the robot hardware or simulation. """ -from .serial_transport import SerialTransport from .mock_serial_transport import MockSerialTransport -from .udp_transport import UDPTransport +from .serial_transport import SerialTransport from .transport_factory import ( - create_transport, create_and_connect_transport, - is_simulation_mode + create_transport, + is_simulation_mode, ) +from .udp_transport import UDPTransport __all__ = [ - 'SerialTransport', - 'MockSerialTransport', - 'UDPTransport', - 'create_transport', - 'create_and_connect_transport', - 'is_simulation_mode', + "SerialTransport", + "MockSerialTransport", + "UDPTransport", + "create_transport", + "create_and_connect_transport", + "is_simulation_mode", ] diff --git a/parol6/server/transports/mock_serial_transport.py b/parol6/server/transports/mock_serial_transport.py index b182751..0bbc7d8 100644 --- a/parol6/server/transports/mock_serial_transport.py +++ b/parol6/server/transports/mock_serial_transport.py @@ -6,17 +6,18 @@ operates at the wire protocol level, making it transparent to the controller code. """ -import time + import logging import threading +import time from dataclasses import dataclass, field -from typing import Optional, List -from parol6.protocol.wire import CommandCode, split_to_3_bytes, pack_tx_frame_into +import numpy as np + +import parol6.PAROL6_ROBOT as PAROL6_ROBOT from parol6 import config as cfg +from parol6.protocol.wire import CommandCode, split_to_3_bytes from parol6.server.state import ControllerState -import parol6.PAROL6_ROBOT as PAROL6_ROBOT -import numpy as np logger = logging.getLogger(__name__) @@ -24,32 +25,53 @@ @dataclass class MockRobotState: """Internal state of the simulated robot.""" + # Joint positions (in steps) - position_in: np.ndarray = field(default_factory=lambda: np.zeros((6,), dtype=np.int32)) + position_in: np.ndarray = field( + default_factory=lambda: np.zeros((6,), dtype=np.int32) + ) + # Floating accumulator for high-fidelity integration (steps, float) + position_f: np.ndarray = field( + default_factory=lambda: np.zeros((6,), dtype=np.float64) + ) # Joint speeds (in steps/sec) speed_in: np.ndarray = field(default_factory=lambda: np.zeros((6,), dtype=np.int32)) # Homed status per joint homed_in: np.ndarray = field(default_factory=lambda: np.zeros((8,), dtype=np.uint8)) # I/O states - io_in: np.ndarray = field(default_factory=lambda: np.zeros((8,), dtype=np.uint8)) # E-stop released + io_in: np.ndarray = field( + default_factory=lambda: np.zeros((8,), dtype=np.uint8) + ) # E-stop released # Error states - temperature_error_in: np.ndarray = field(default_factory=lambda: np.zeros((8,), dtype=np.uint8)) - position_error_in: np.ndarray = field(default_factory=lambda: np.zeros((8,), dtype=np.uint8)) + temperature_error_in: np.ndarray = field( + default_factory=lambda: np.zeros((8,), dtype=np.uint8) + ) + position_error_in: np.ndarray = field( + default_factory=lambda: np.zeros((8,), dtype=np.uint8) + ) # Gripper state - gripper_data_in: np.ndarray = field(default_factory=lambda: np.zeros((6,), dtype=np.int32)) + gripper_data_in: np.ndarray = field( + default_factory=lambda: np.zeros((6,), dtype=np.int32) + ) # Timing data - timing_data_in: np.ndarray = field(default_factory=lambda: np.zeros((1,), dtype=np.int32)) - + timing_data_in: np.ndarray = field( + default_factory=lambda: np.zeros((1,), dtype=np.int32) + ) + # Simulation parameters update_rate: float = cfg.INTERVAL_S # match control loop cadence last_update: float = field(default_factory=time.time) homing_countdown: int = 0 - + # Command state from controller command_out: int = CommandCode.IDLE - position_out: np.ndarray = field(default_factory=lambda: np.zeros((6,), dtype=np.int32)) - speed_out: np.ndarray = field(default_factory=lambda: np.zeros((6,), dtype=np.int32)) - + position_out: np.ndarray = field( + default_factory=lambda: np.zeros((6,), dtype=np.int32) + ) + speed_out: np.ndarray = field( + default_factory=lambda: np.zeros((6,), dtype=np.int32) + ) + def __post_init__(self): """Initialize robot to standby position.""" # Set initial positions to standby position for better IK @@ -57,7 +79,9 @@ def __post_init__(self): deg = float(PAROL6_ROBOT.joint.standby.deg[i]) steps = int(PAROL6_ROBOT.ops.deg_to_steps(deg, i)) self.position_in[i] = steps - + # Initialize float accumulator from integer steps + self.position_f = self.position_in.astype(np.float64) + # Ensure E-stop is not pressed (bit 4 = 1 means released) self.io_in[4] = 1 @@ -65,17 +89,19 @@ def __post_init__(self): class MockSerialTransport: """ Mock serial transport that simulates robot hardware responses. - + This class implements the exact same interface as SerialTransport, but generates simulated responses instead of communicating with real hardware. The simulation operates at the frame level, making it completely transparent to the controller. """ - - def __init__(self, port: Optional[str] = None, baudrate: int = 2000000, timeout: float = 0): + + def __init__( + self, port: str | None = None, baudrate: int = 2000000, timeout: float = 0 + ): """ Initialize the mock serial transport. - + Args: port: Ignored (for interface compatibility) baudrate: Ignored (for interface compatibility) @@ -84,18 +110,18 @@ def __init__(self, port: Optional[str] = None, baudrate: int = 2000000, timeout: self.port = port or "MOCK_SERIAL" self.baudrate = baudrate self.timeout = timeout - + # Internal robot state self._state = MockRobotState() - + # Frame generation tracking - self._frames_to_send: List[bytes] = [] + self._frames_to_send: list[bytes] = [] self._last_frame_time = time.time() self._frame_interval = cfg.INTERVAL_S # match control loop cadence - + # Connection state self._connected = False - + # Statistics self._frames_sent = 0 self._frames_received = 0 @@ -105,26 +131,40 @@ def __init__(self, port: Optional[str] = None, baudrate: int = 2000000, timeout: self._frame_mv = memoryview(self._frame_buf)[:52] self._frame_version = 0 self._frame_ts = 0.0 - self._reader_thread: Optional[threading.Thread] = None + self._reader_thread: threading.Thread | None = None self._reader_running = False - + + # Precompute motion simulation constants + self._vmax_f = PAROL6_ROBOT.joint.speed.max.astype(np.float64, copy=False) + self._vmax_i32 = PAROL6_ROBOT.joint.speed.max.astype(np.int32, copy=False) + lims = np.asarray(PAROL6_ROBOT.joint.limits.steps, dtype=np.int64) + self._jmin_f = lims[:, 0].astype(np.float64, copy=False) + self._jmax_f = lims[:, 1].astype(np.float64, copy=False) + + # Scratch buffers for motion simulation + self._prev_pos_f = np.zeros((6,), dtype=np.float64) + + self._state.last_update = time.perf_counter() + logger.info("MockSerialTransport initialized - simulation mode active") - - def connect(self, port: Optional[str] = None) -> bool: + + def connect(self, port: str | None = None) -> bool: """ Simulate serial port connection. - + Args: port: Optional port name (ignored) - + Returns: Always returns True for mock """ if port: self.port = port - + self._connected = True self._state = MockRobotState() # Reset state on connect + # Initialize time base to perf_counter for consistent scheduling + self._state.last_update = time.perf_counter() logger.info(f"MockSerialTransport connected to simulated port: {self.port}") return True @@ -136,9 +176,11 @@ def sync_from_controller_state(self, state: ControllerState) -> None: """ try: self._state.position_in = state.Position_in.copy() + # keep high-fidelity accumulator in sync + self._state.position_f = self._state.position_in.astype(np.float64) self._state.homed_in = state.Homed_in.copy() self._state.position_out = self._state.position_in.copy() - self._state.last_update = time.time() + self._state.last_update = time.perf_counter() self._state.homing_countdown = 0 # Clear speeds and hold position @@ -146,47 +188,51 @@ def sync_from_controller_state(self, state: ControllerState) -> None: self._state.command_out = CommandCode.IDLE logger.info("MockSerialTransport: state synchronized from controller") except Exception as e: - logger.warning("MockSerialTransport: failed to sync from controller state: %s", e) - + logger.warning( + "MockSerialTransport: failed to sync from controller state: %s", e + ) + def disconnect(self) -> None: """Simulate serial port disconnection.""" self._connected = False logger.info(f"MockSerialTransport disconnected from: {self.port}") - + def is_connected(self) -> bool: """ Check if mock connection is active. - + Returns: Connection state """ return self._connected - + def auto_reconnect(self) -> bool: """ Mock auto-reconnect (always succeeds). - + Returns: True if not connected, False if already connected """ if not self._connected: return self.connect(self.port) return False - - def write_frame(self, - position_out: np.ndarray, - speed_out: np.ndarray, - command_out: int, - affected_joint_out: np.ndarray, - inout_out: np.ndarray, - timeout_out: int, - gripper_data_out: np.ndarray) -> bool: + + def write_frame( + self, + position_out: np.ndarray, + speed_out: np.ndarray, + command_out: int, + affected_joint_out: np.ndarray, + inout_out: np.ndarray, + timeout_out: int, + gripper_data_out: np.ndarray, + ) -> bool: """ Process a command frame from the controller. - + Instead of writing to serial, this updates the internal simulation state. - + Args: position_out: Target positions speed_out: Speed commands @@ -195,21 +241,21 @@ def write_frame(self, inout_out: I/O commands timeout_out: Timeout value gripper_data_out: Gripper commands - + Returns: True if processed successfully """ if not self._connected: return False - + # Update simulation state with command self._state.command_out = command_out - self._state.position_out = position_out - self._state.speed_out = speed_out - + self._state.position_out = np.array(position_out, dtype=np.int32, copy=False) + self._state.speed_out = np.array(speed_out, dtype=np.float64, copy=False) + # Track frame reception self._frames_received += 1 - + # Simulate gripper state updates if gripper_data_out[4] == 1: # Calibration mode # Simulate gripper calibration @@ -218,25 +264,24 @@ def write_frame(self, elif gripper_data_out[4] == 2: # Error clear mode # Clear gripper errors self._state.gripper_data_in[4] &= ~0x20 # Clear error bit - + # Update gripper position/speed/current if commanded if gripper_data_out[3] != 0: # Gripper command active self._state.gripper_data_in[1] = gripper_data_out[0] # Position self._state.gripper_data_in[2] = gripper_data_out[1] # Speed self._state.gripper_data_in[3] = gripper_data_out[2] # Current - + return True - - + def _simulate_motion(self, dt: float) -> None: """ Simulate one step of robot motion. - + Args: dt: Time delta since last update """ state = self._state - + # Handle homing countdown if state.homing_countdown > 0: state.homing_countdown -= 1 @@ -248,13 +293,14 @@ def _simulate_motion(self, dt: float) -> None: for i in range(6): steps = int(PAROL6_ROBOT.ops.deg_to_steps(float(target_deg[i]), i)) state.position_in[i] = steps + state.position_f[i] = float(steps) state.speed_in[i] = 0 # Clear HOME command to avoid immediately restarting homing state.command_out = CommandCode.IDLE - + # Ensure E-stop stays released in simulation state.io_in[4] = 1 - + # Simulate motion based on command type if state.command_out == CommandCode.HOME: # Start homing sequence @@ -266,68 +312,82 @@ def _simulate_motion(self, dt: float) -> None: # Zero speeds during homing for i in range(6): state.speed_in[i] = 0 - + elif state.command_out == CommandCode.JOG or state.command_out == 123: - # Speed control mode - for i in range(6): - v = int(state.speed_out[i]) - # Apply speed limits - max_v = int(PAROL6_ROBOT.joint.speed.max[i]) - v = max(-max_v, min(max_v, v)) - - # Integrate position - new_pos = int(state.position_in[i] + v * dt) - - # Apply joint limits - jmin, jmax = PAROL6_ROBOT.joint.limits.steps[i] - if new_pos < jmin: - new_pos = jmin - v = 0 - elif new_pos > jmax: - new_pos = jmax - v = 0 - - state.speed_in[i] = v - state.position_in[i] = new_pos - + # Speed control mode (vectorized float-accumulated integration) + np.copyto(self._prev_pos_f, state.position_f) + + # Clip commanded speeds to joint limits + v_cmd = np.clip( + state.speed_out.astype(np.float64, copy=False), + -self._vmax_f, + self._vmax_f, + ) + + # Integrate position + new_pos_f = state.position_f + v_cmd * dt + + # Apply joint limits + np.clip(new_pos_f, self._jmin_f, self._jmax_f, out=state.position_f) + + # Report actual velocity based on realized motion + if dt > 0: + realized_v = np.rint((state.position_f - self._prev_pos_f) / dt).astype( + np.int32 + ) + np.clip(realized_v, -self._vmax_i32, self._vmax_i32, out=state.speed_in) + else: + state.speed_in.fill(0) + elif state.command_out == CommandCode.MOVE or state.command_out == 156: - # Position control mode + # Position control mode (float-accumulated and per-tick speed clamp) + prev_pos_f = state.position_f.copy() for i in range(6): - target = state.position_out[i] - current = state.position_in[i] - err = int(target - current) - - if err == 0: - state.speed_in[i] = 0 - continue - - # Calculate step size based on max speed - max_step = int(PAROL6_ROBOT.joint.speed.max[i] * dt) - if max_step < 1: - max_step = 1 - - # Move toward target - step = max(-max_step, min(max_step, err)) - new_pos = current + step - + target = float(state.position_out[i]) + current_f = float(state.position_f[i]) + err_f = target - current_f + + # Calculate max move this tick from per-joint max speed + max_step_f = float(PAROL6_ROBOT.joint.speed.max[i]) * float(dt) + if max_step_f < 1.0: + # ensure some progress at very small dt + max_step_f = 1.0 + + move = float(err_f) + if move > max_step_f: + move = max_step_f + elif move < -max_step_f: + move = -max_step_f + + new_pos_f = current_f + move + # Apply joint limits jmin, jmax = PAROL6_ROBOT.joint.limits.steps[i] - if new_pos < jmin: - new_pos = jmin - step = 0 - elif new_pos > jmax: - new_pos = jmax - step = 0 - - state.position_in[i] = int(new_pos) - state.speed_in[i] = int(step / dt) if dt > 0 else 0 - + if new_pos_f < float(jmin): + new_pos_f = float(jmin) + elif new_pos_f > float(jmax): + new_pos_f = float(jmax) + + state.position_f[i] = new_pos_f + + # Report actual velocity based on realized motion + if dt > 0: + realized_v = np.rint((state.position_f - prev_pos_f) / dt).astype( + np.int32 + ) + else: + realized_v = np.zeros(6, dtype=np.int32) + vmax = PAROL6_ROBOT.joint.speed.max.astype(np.int32) + state.speed_in[:] = np.clip(realized_v, -vmax, vmax) + else: # Idle or unknown command - hold position for i in range(6): state.speed_in[i] = 0 - - + + # Sync integer telemetry from high-fidelity accumulator + state.position_in[:] = np.rint(state.position_f).astype(np.int32) + # ================================ # Latest-frame API (reduced-copy) # ================================ @@ -340,28 +400,37 @@ def start_reader(self, shutdown_event: threading.Event) -> threading.Thread: def _run(): self._reader_running = True + period = self._frame_interval + next_deadline = time.perf_counter() + try: while not shutdown_event.is_set(): if not self._connected: time.sleep(0.05) continue - now = time.time() - if now - self._last_frame_time >= self._frame_interval: + + now = time.perf_counter() + if now >= next_deadline: # Advance simulation before publishing a new frame - try: - dt = now - self._state.last_update - if dt > 0: - self._simulate_motion(dt) - self._state.last_update = now - except Exception: - logger.exception("MockSerialTransport: simulation step failed") - - payload = self._encode_payload_from_state() - self._frame_buf[:52] = payload + dt = now - self._state.last_update + if dt > 0: + self._simulate_motion(dt) + self._state.last_update = now + + self._encode_payload_into(self._frame_mv) self._frame_version += 1 - self._frame_ts = now - self._last_frame_time = now - time.sleep(min(self._frame_interval, 0.01)) + self._frame_ts = time.time() + + # Advance deadline + next_deadline += period + # If we fell far behind, resync to avoid tight catch-up loop + if next_deadline < now - period: + next_deadline = now + period + else: + # Sleep until next deadline (or at most 2ms to stay responsive) + sleep_time = min(next_deadline - now, 0.002) + if sleep_time > 0: + time.sleep(sleep_time) finally: self._reader_running = False @@ -370,23 +439,28 @@ def _run(): t.start() return t - def _encode_payload_from_state(self) -> bytes: + def _encode_payload_into(self, out_mv: memoryview) -> None: """ - Build a 52-byte payload per firmware layout from the simulated state. + Build a 52-byte payload per firmware layout from the simulated state directly into memoryview. + Zero-allocation version for use in the reader loop. """ st = self._state - out = bytearray(52) + out = out_mv # Positions (6 * 3 bytes) off = 0 for i in range(6): b0, b1, b2 = split_to_3_bytes(int(st.position_in[i])) - out[off] = b0; out[off + 1] = b1; out[off + 2] = b2 + out[off] = b0 + out[off + 1] = b1 + out[off + 2] = b2 off += 3 # Speeds (6 * 3 bytes) off = 18 for i in range(6): b0, b1, b2 = split_to_3_bytes(int(st.speed_in[i])) - out[off] = b0; out[off + 1] = b1; out[off + 2] = b2 + out[off] = b0 + out[off + 1] = b1 + out[off + 2] = b2 off += 3 def bits_to_byte(bits: np.ndarray) -> int: @@ -419,14 +493,15 @@ def bits_to_byte(bits: np.ndarray) -> int: status = int(gd[4]) & 0xFF if gd.all() else 0 out[44] = dev_id & 0xFF - out[45] = (pos >> 8) & 0xFF; out[46] = pos & 0xFF - out[47] = (spd >> 8) & 0xFF; out[48] = spd & 0xFF - out[49] = (cur >> 8) & 0xFF; out[50] = cur & 0xFF + out[45] = (pos >> 8) & 0xFF + out[46] = pos & 0xFF + out[47] = (spd >> 8) & 0xFF + out[48] = spd & 0xFF + out[49] = (cur >> 8) & 0xFF + out[50] = cur & 0xFF out[51] = status & 0xFF - return bytes(out) - - def get_latest_frame_view(self) -> tuple[Optional[memoryview], int, float]: + def get_latest_frame_view(self) -> tuple[memoryview | None, int, float]: """ Return latest 52-byte payload memoryview, version, timestamp. """ @@ -436,31 +511,31 @@ def get_latest_frame_view(self) -> tuple[Optional[memoryview], int, float]: def get_info(self) -> dict: """ Get information about the mock transport. - + Returns: Dictionary with transport information """ return { - 'port': self.port, - 'baudrate': self.baudrate, - 'connected': self._connected, - 'timeout': self.timeout, - 'mode': 'MOCK_SERIAL', - 'frames_sent': self._frames_sent, - 'frames_received': self._frames_received, - 'simulation_rate_hz': int(1.0 / self._frame_interval), - 'robot_state': { - 'homed': all(self._state.homed_in[i] == 1 for i in range(6)), - 'estop': self._state.io_in[4] == 0, - 'command': self._state.command_out - } + "port": self.port, + "baudrate": self.baudrate, + "connected": self._connected, + "timeout": self.timeout, + "mode": "MOCK_SERIAL", + "frames_sent": self._frames_sent, + "frames_received": self._frames_received, + "simulation_rate_hz": int(1.0 / self._frame_interval), + "robot_state": { + "homed": all(self._state.homed_in[i] == 1 for i in range(6)), + "estop": self._state.io_in[4] == 0, + "command": self._state.command_out, + }, } def create_mock_serial_transport() -> MockSerialTransport: """ Factory function to create a mock serial transport. - + Returns: Configured MockSerialTransport instance """ diff --git a/parol6/server/transports/serial_transport.py b/parol6/server/transports/serial_transport.py index e6bf5ef..86169be 100644 --- a/parol6/server/transports/serial_transport.py +++ b/parol6/server/transports/serial_transport.py @@ -5,38 +5,41 @@ data exchange with the robot hardware. """ -import serial import logging -import time +import os import threading -from typing import Optional +import time + import numpy as np -import os +import serial +from parol6.config import INTERVAL_S, get_com_port_with_fallback from parol6.protocol.wire import pack_tx_frame_into -from parol6.config import get_com_port_with_fallback, INTERVAL_S logger = logging.getLogger(__name__) + class SerialTransport: """ Manages serial port communication with the robot. - + This class handles: - Serial port connection and reconnection - Frame parsing and validation - Command transmission - Telemetry reception """ - + # Frame delimiters - START_BYTES = bytes([0xff, 0xff, 0xff]) + START_BYTES = bytes([0xFF, 0xFF, 0xFF]) END_BYTES = bytes([0x01, 0x02]) - - def __init__(self, port: Optional[str] = None, baudrate: int = 2000000, timeout: float = 0): + + def __init__( + self, port: str | None = None, baudrate: int = 2000000, timeout: float = 0 + ): """ Initialize the serial transport. - + Args: port: Serial port name (e.g., 'COM5', '/dev/ttyACM0') baudrate: Baud rate for serial communication @@ -45,7 +48,7 @@ def __init__(self, port: Optional[str] = None, baudrate: int = 2000000, timeout: self.port = port self.baudrate = baudrate self.timeout = timeout - self.serial: Optional[serial.Serial] = None + self.serial: serial.Serial | None = None self.last_reconnect_attempt = 0.0 self.reconnect_interval = 1.0 # seconds between reconnect attempts @@ -62,55 +65,53 @@ def __init__(self, port: Optional[str] = None, baudrate: int = 2000000, timeout: self._frame_mv = memoryview(self._frame_buf)[:52] self._frame_version = 0 self._frame_ts = 0.0 - self._reader_thread: Optional[threading.Thread] = None + self._reader_thread: threading.Thread | None = None self._reader_running = False # Preallocated TX buffer (3 start + 1 len + 52 payload = 56 bytes) self._tx_buf = bytearray(56) self._tx_mv = memoryview(self._tx_buf) - + # Hz tracking for debug prints self._last_print_time = 0.0 self._print_interval = 3.0 # seconds self._rx_msg_count = 0 self._interval_msg_count = 0 - - def connect(self, port: Optional[str] = None) -> bool: + + def connect(self, port: str | None = None) -> bool: """ Connect to the serial port. - + Args: port: Optional port override. If not provided, uses stored port. - + Returns: True if connection successful, False otherwise """ if port: self.port = port - + if not self.port: logger.warning("No serial port specified") return False - + try: # Close existing connection if any if self.serial and self.serial.is_open: self.serial.close() - + # Create new connection self.serial = serial.Serial( - port=self.port, - baudrate=self.baudrate, - timeout=self.timeout + port=self.port, baudrate=self.baudrate, timeout=self.timeout ) - + if self.serial.is_open: logger.info(f"Connected to serial port: {self.port}") return True else: logger.error(f"Failed to open serial port: {self.port}") return False - + except serial.SerialException as e: logger.error(f"Serial connection error: {e}") self.serial = None @@ -119,7 +120,7 @@ def connect(self, port: Optional[str] = None) -> bool: logger.error(f"Unexpected error connecting to serial: {e}") self.serial = None return False - + def disconnect(self) -> None: """Disconnect from the serial port.""" if self.serial: @@ -131,53 +132,55 @@ def disconnect(self) -> None: logger.error(f"Error closing serial port: {e}") finally: self.serial = None - + def is_connected(self) -> bool: """ Check if serial connection is active. - + Returns: True if connected and open, False otherwise """ return self.serial is not None and self.serial.is_open - + def auto_reconnect(self) -> bool: """ Attempt to reconnect to the serial port if disconnected. - + This implements a rate-limited reconnection attempt. - + Returns: True if reconnection successful, False otherwise """ now = time.time() - + # Rate limit reconnection attempts if now - self.last_reconnect_attempt < self.reconnect_interval: return False - + self.last_reconnect_attempt = now - + if self.port: logger.info(f"Attempting to reconnect to {self.port}...") return self.connect(self.port) - + return False - - def write_frame(self, - position_out: np.ndarray, - speed_out: np.ndarray, - command_out: int, - affected_joint_out: np.ndarray, - inout_out: np.ndarray, - timeout_out: int, - gripper_data_out: np.ndarray) -> bool: + + def write_frame( + self, + position_out: np.ndarray, + speed_out: np.ndarray, + command_out: int, + affected_joint_out: np.ndarray, + inout_out: np.ndarray, + timeout_out: int, + gripper_data_out: np.ndarray, + ) -> bool: """ Write a command frame to the robot. - + Optimized to accept array-like objects directly without conversion. Supports lists, array.array, and other sequence types. - + Args: position_out: Target positions for joints speed_out: Speed commands for joints @@ -186,13 +189,13 @@ def write_frame(self, inout_out: I/O commands timeout_out: Timeout value gripper_data_out: Gripper commands - + Returns: True if write successful, False otherwise """ if not self.is_connected(): return False - + try: # Write to serial using preallocated buffer and zero-alloc pack ser = self.serial @@ -207,11 +210,11 @@ def write_frame(self, affected_joint_out, inout_out, timeout_out, - gripper_data_out + gripper_data_out, ) ser.write(self._tx_mv) return True - + except serial.SerialException as e: logger.error(f"Serial write error: {e}") # Mark connection as lost @@ -220,9 +223,6 @@ def write_frame(self, except Exception as e: logger.error(f"Unexpected error writing frame: {e}") return False - - - # ================================ # Latest-frame API (reduced-copy) @@ -235,7 +235,9 @@ def start_reader(self, shutdown_event: threading.Event) -> threading.Thread: Returns the started Thread object. If already running, returns the existing one. """ if not self.is_connected(): - raise RuntimeError("SerialTransport.start_reader: serial port not connected") + raise RuntimeError( + "SerialTransport.start_reader: serial port not connected" + ) if self._reader_thread and self._reader_thread.is_alive(): return self._reader_thread @@ -261,15 +263,27 @@ def _run() -> None: time.sleep(0.1) continue try: - # Read into preallocated scratch buffer - n = ser.readinto(self._scratch_mv) + # Check if data is available to avoid empty read syscalls + iw = ser.in_waiting + + if iw <= 0: + # No data available, short sleep and continue + time.sleep(min(INTERVAL_S, 0.0015)) + continue + + # Read up to available bytes into preallocated scratch buffer + k = min(iw, len(self._scratch)) + n = ser.readinto(self._scratch_mv[:k]) except serial.SerialException as e: logger.error(f"Serial reader error: {e}") self.disconnect() break except (OSError, TypeError, ValueError, AttributeError): # fd likely closed during disconnect; stop quietly - logger.info("Serial reader stopping due to disconnect/closed FD", exc_info=False) + logger.info( + "Serial reader stopping due to disconnect/closed FD", + exc_info=False, + ) try: self.disconnect() except Exception: @@ -283,21 +297,30 @@ def _run() -> None: # Timeout or no data; loop to check shutdown_event continue - # Append into ring buffer and parse + # Batch append into ring buffer and parse cap = self._r_cap head = self._r_head tail = self._r_tail rb = self._ring src = self._scratch - for i in range(n): - rb[head] = src[i] - head += 1 - if head == cap: - head = 0 - if head == tail: - tail += 1 - if tail == cap: - tail = 0 + + # Calculate overflow and adjust tail if needed + avail = (head - tail + cap) % cap + free = ( + cap - 1 - avail + ) # keep one slot empty to disambiguate full/empty + over = max(0, n - free) + if over: + tail = (tail + over) % cap + + # Batch copy into ring buffer using slices + first = min(n, cap - head) + rb[head : head + first] = src[:first] + remain = n - first + if remain: + rb[0:remain] = src[first:n] + head = (head + n) % cap + self._r_head = head self._r_tail = tail self._parse_ring_for_frames() @@ -359,16 +382,16 @@ def available(h: int, t: int) -> int: payload_len = 52 if length >= 52 else length start = (tail + 4) % cap if start + payload_len <= cap: - self._frame_buf[:payload_len] = rb[start:start + payload_len] + self._frame_buf[:payload_len] = rb[start : start + payload_len] else: first = cap - start self._frame_buf[:first] = rb[start:cap] - self._frame_buf[first:payload_len] = rb[0:payload_len - first] + self._frame_buf[first:payload_len] = rb[0 : payload_len - first] if payload_len >= 52: self._frame_version += 1 self._frame_ts = time.time() - + # Update Hz tracking if enabled self._update_hz_tracking() @@ -378,82 +401,88 @@ def available(h: int, t: int) -> int: # Publish updated tail self._r_tail = tail - def get_latest_frame_view(self) -> tuple[Optional[memoryview], int, float]: + def get_latest_frame_view(self) -> tuple[memoryview | None, int, float]: """ Return a tuple of (memoryview|None, version:int, timestamp:float). The memoryview points to a stable 52-byte buffer which is updated by the reader. """ mv = self._frame_mv if self._frame_version > 0 else None return (mv, self._frame_version, self._frame_ts) - + def get_info(self) -> dict: """ Get information about the current serial connection. - + Returns: Dictionary with connection information """ info = { - 'port': self.port, - 'baudrate': self.baudrate, - 'connected': self.is_connected(), - 'timeout': self.timeout + "port": self.port, + "baudrate": self.baudrate, + "connected": self.is_connected(), + "timeout": self.timeout, } - + if self.serial and self.serial.is_open: try: - info['in_waiting'] = self.serial.in_waiting - info['out_waiting'] = self.serial.out_waiting - except: - pass - + info["in_waiting"] = self.serial.in_waiting + info["out_waiting"] = self.serial.out_waiting + except Exception as e: + logger.debug("Failed to read serial wait counts: %s", e) + return info - + def _update_hz_tracking(self) -> None: """ Update EMA Hz tracking and print debug info periodically. - + This method calculates the instantaneous Hz based on time between messages, updates the EMA (Exponential Moving Average), tracks min/max values, and prints debug info every few seconds. """ current_time = time.perf_counter() - + # Increment message counters self._rx_msg_count += 1 self._interval_msg_count += 1 - + # Check if it's time to print debug info if self._last_print_time == 0.0: self._last_print_time = current_time elif current_time - self._last_print_time >= self._print_interval: # Print debug information if self._interval_msg_count > 0: - avg_hz = self._interval_msg_count / (current_time - self._last_print_time) - logger.debug(f"Serial RX Stats - Avg Hz: {avg_hz:.2f} (Total: {self._rx_msg_count})" + avg_hz = self._interval_msg_count / ( + current_time - self._last_print_time + ) + logger.debug( + f"Serial RX Stats - Avg Hz: {avg_hz:.2f} (Total: {self._rx_msg_count})" ) else: - logger.debug(f"Serial RX Stats - No messages received in last {self._print_interval:.1f}s") - + logger.debug( + f"Serial RX Stats - No messages received in last {self._print_interval:.1f}s" + ) + # Reset interval statistics self._last_print_time = current_time self._interval_msg_count = 0 -def create_serial_transport(port: Optional[str] = None, - baudrate: int = 2000000) -> SerialTransport: +def create_serial_transport( + port: str | None = None, baudrate: int = 2000000 +) -> SerialTransport: """ Factory function to create and optionally connect a serial transport. - + Args: port: Optional serial port to connect to baudrate: Baud rate for communication - + Returns: SerialTransport instance (may or may not be connected) """ transport = SerialTransport(port=port, baudrate=baudrate) - + # Try to connect if port provided if port: transport.connect(port) @@ -462,5 +491,5 @@ def create_serial_transport(port: Optional[str] = None, saved_port = get_com_port_with_fallback() if saved_port: transport.connect(saved_port) - + return transport diff --git a/parol6/server/transports/transport_factory.py b/parol6/server/transports/transport_factory.py index c912c23..f4d11b7 100644 --- a/parol6/server/transports/transport_factory.py +++ b/parol6/server/transports/transport_factory.py @@ -6,13 +6,13 @@ real serial, mock serial, or other transport types. """ -import os import logging -from typing import Optional, Union +import os +from typing import Any -from parol6.server.transports.serial_transport import SerialTransport -from parol6.server.transports.mock_serial_transport import MockSerialTransport from parol6.config import get_com_port_with_fallback +from parol6.server.transports.mock_serial_transport import MockSerialTransport +from parol6.server.transports.serial_transport import SerialTransport logger = logging.getLogger(__name__) @@ -20,7 +20,7 @@ def is_simulation_mode() -> bool: """ Check if simulation mode is enabled. - + Returns: True if simulation mode is enabled via environment variable """ @@ -29,24 +29,24 @@ def is_simulation_mode() -> bool: def create_transport( - transport_type: Optional[str] = None, - port: Optional[str] = None, + transport_type: str | None = None, + port: str | None = None, baudrate: int = 2000000, - **kwargs -) -> Union[SerialTransport, MockSerialTransport]: + **kwargs: Any, +) -> SerialTransport | MockSerialTransport: """ Create an appropriate transport instance based on configuration. - + The factory will automatically select the appropriate transport: - MockSerialTransport if PAROL6_FAKE_SERIAL is set - SerialTransport otherwise - + Args: transport_type: Explicit transport type ('serial', 'mock', or None for auto) port: Serial port name (for real serial) baudrate: Baud rate for serial communication **kwargs: Additional transport-specific parameters - + Returns: Transport instance (SerialTransport or MockSerialTransport) """ @@ -54,71 +54,73 @@ def create_transport( if transport_type is None: # Auto-detect based on environment if is_simulation_mode(): - transport_type = 'mock' + transport_type = "mock" else: - transport_type = 'serial' - + transport_type = "serial" + # Create appropriate transport - if transport_type == 'mock': + if transport_type == "mock": logger.info("Creating MockSerialTransport for simulation") - transport: Union[MockSerialTransport, SerialTransport] = MockSerialTransport(port=port, baudrate=baudrate, **kwargs) - - elif transport_type == 'serial': + transport: MockSerialTransport | SerialTransport = MockSerialTransport( + port=port, baudrate=baudrate, **kwargs + ) + + elif transport_type == "serial": logger.info(f"Creating SerialTransport for port: {port}") transport = SerialTransport(port=port, baudrate=baudrate, **kwargs) - + else: raise ValueError(f"Unknown transport type: {transport_type}") - + return transport def create_and_connect_transport( - transport_type: Optional[str] = None, - port: Optional[str] = None, + transport_type: str | None = None, + port: str | None = None, baudrate: int = 2000000, auto_find_port: bool = True, - **kwargs -) -> Optional[Union[SerialTransport, MockSerialTransport]]: + **kwargs: Any, +) -> SerialTransport | MockSerialTransport | None: """ Create and connect a transport instance. - + This is a convenience function that creates a transport and attempts to connect it. For real serial, it can also attempt to find and load a saved port. - + Args: transport_type: Transport type or None for auto-detect port: Serial port or None baudrate: Baud rate auto_find_port: Whether to try loading saved port for serial **kwargs: Additional parameters - + Returns: Connected transport instance or None if connection failed """ # For mock transport, port finding is not needed - if is_simulation_mode() or transport_type == 'mock': - transport = create_transport('mock', port=port, baudrate=baudrate, **kwargs) + if is_simulation_mode() or transport_type == "mock": + transport = create_transport("mock", port=port, baudrate=baudrate, **kwargs) if transport.connect(): return transport return None - + # For real serial, handle port finding if not port and auto_find_port: # Try to load saved port port = get_com_port_with_fallback() if port: logger.info(f"Using saved serial port: {port}") - + # Create transport transport = create_transport(transport_type, port=port, baudrate=baudrate, **kwargs) - + # Attempt connection if port is known if port: if transport.connect(port): return transport else: logger.warning(f"Failed to connect to port: {port}") - + return transport diff --git a/parol6/server/transports/udp_transport.py b/parol6/server/transports/udp_transport.py index e6700a3..1baef3f 100644 --- a/parol6/server/transports/udp_transport.py +++ b/parol6/server/transports/udp_transport.py @@ -7,31 +7,30 @@ from __future__ import annotations -import socket import logging +import socket import time -from typing import Optional, List, Tuple logger = logging.getLogger(__name__) - - class UDPTransport: """ Manages UDP socket communication for the controller. - + This class handles: - Socket creation and binding - Non-blocking message reception - Response sending - Connection management """ - - def __init__(self, ip: str = "127.0.0.1", port: int = 5001, buffer_size: int = 1024): + + def __init__( + self, ip: str = "127.0.0.1", port: int = 5001, buffer_size: int = 1024 + ): """ Initialize the UDP transport. - + Args: ip: IP address to bind to port: Port number to listen on @@ -40,35 +39,35 @@ def __init__(self, ip: str = "127.0.0.1", port: int = 5001, buffer_size: int = 1 self.ip = ip self.port = port self.buffer_size = buffer_size - self.socket: Optional[socket.socket] = None + self.socket: socket.socket | None = None self._running = False self._rx = bytearray(self.buffer_size) self._rxv = memoryview(self._rx) - + def create_socket(self) -> bool: """ Create and bind the UDP socket. - + Returns: True if successful, False otherwise """ try: # Create UDP socket self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - + # Use blocking mode with short timeout for responsive shutdown self.socket.setblocking(True) self.socket.settimeout(0.25) - + # Allow address/port reuse for fast restarts self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) try: - self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) # type: ignore[attr-defined] + self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) except Exception: # Not available on all platforms pass self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 1 << 20) - + # Bind to address with small retry window to avoid transient EADDRINUSE _attempts = 3 _delay_s = 0.1 @@ -80,12 +79,12 @@ def create_socket(self) -> bool: if _i == _attempts - 1: raise time.sleep(_delay_s) - + self._running = True logger.info(f"UDP socket created and bound to {self.ip}:{self.port}") return True - - except socket.error as e: + + except OSError as e: logger.error(f"Failed to create UDP socket: {e}") self.socket = None return False @@ -93,11 +92,11 @@ def create_socket(self) -> bool: logger.error(f"Unexpected error creating UDP socket: {e}") self.socket = None return False - + def close_socket(self) -> None: """Close the UDP socket and clean up resources.""" self._running = False - + if self.socket: try: self.socket.close() @@ -106,10 +105,8 @@ def close_socket(self) -> None: logger.error(f"Error closing UDP socket: {e}") finally: self.socket = None - - - - def receive_one(self) -> Optional[Tuple[str, Tuple[str, int]]]: + + def receive_one(self) -> tuple[str, tuple[str, int]] | None: """ Blocking receive of a single datagram using recvfrom_into with a short timeout. Returns (message_str, address) on success, or None on timeout/error. @@ -122,116 +119,163 @@ def receive_one(self) -> Optional[Tuple[str, Tuple[str, int]]]: return None try: # Decode ASCII payload and trim only CR/LF to avoid extra copies - message_str = self._rxv[:nbytes].tobytes().decode("ascii", errors="ignore").rstrip("\r\n") + message_str = ( + self._rxv[:nbytes] + .tobytes() + .decode("ascii", errors="ignore") + .rstrip("\r\n") + ) except Exception: logger.warning(f"Failed to decode UDP datagram from {address}") return None return (message_str, address) - except socket.timeout: + except TimeoutError: return None - except socket.error as e: + except OSError as e: logger.error(f"Socket error receiving UDP message: {e}") return None except Exception as e: logger.error(f"Unexpected error in receive_one: {e}") return None - def send_response(self, message: str, address: Tuple[str, int]) -> bool: + def drain_buffer(self) -> int: + """ + Drain all pending messages from the UDP receive buffer. + + This is useful in stream mode to discard stale commands when new ones arrive. + Returns the number of messages drained. + """ + if not self.socket or not self._running: + return 0 + + drained_count = 0 + original_timeout = None + + try: + # Temporarily switch to non-blocking mode + original_timeout = self.socket.gettimeout() + self.socket.setblocking(False) + + # Read all pending messages until buffer is empty + while True: + try: + nbytes, _ = self.socket.recvfrom_into(self._rxv) + if nbytes > 0: + drained_count += 1 + except OSError: + # No more data available (expected) + break + + # Restore original timeout + self.socket.settimeout(original_timeout) + + except Exception as e: + logger.error(f"Error draining UDP buffer: {e}") + # Try to restore timeout even if draining failed + try: + if original_timeout is not None: + self.socket.settimeout(original_timeout) + except Exception as e2: + logger.debug("Failed to restore UDP socket timeout: %s", e2) + + return drained_count + + def send_response(self, message: str, address: tuple[str, int]) -> bool: """ Send a response message to a specific address. - + Args: message: The message string to send address: Tuple of (ip, port) to send to - + Returns: True if successful, False otherwise """ if not self.socket or not self._running: logger.warning("Cannot send response - socket not available") return False - + try: # Encode and send the message - data = message.encode('ascii') + data = message.encode("ascii") self.socket.sendto(data, address) return True - - except socket.error as e: + + except OSError as e: logger.error(f"Socket error sending UDP response: {e}") return False except Exception as e: logger.error(f"Unexpected error sending UDP response: {e}") return False - - def broadcast_message(self, message: str, addresses: List[Tuple[str, int]]) -> int: + + def broadcast_message(self, message: str, addresses: list[tuple[str, int]]) -> int: """ Broadcast a message to multiple addresses. - + Args: message: The message to broadcast addresses: List of (ip, port) tuples to send to - + Returns: Number of successful sends """ success_count = 0 - + for address in addresses: if self.send_response(message, address): success_count += 1 - + return success_count - + def is_running(self) -> bool: """ Check if the transport is running. - + Returns: True if running, False otherwise """ return self._running and self.socket is not None - - - + def get_socket_info(self) -> dict: """ Get information about the current socket. - + Returns: Dictionary with socket information """ info = { - 'ip': self.ip, - 'port': self.port, - 'buffer_size': self.buffer_size, - 'running': self._running, - 'socket_open': self.socket is not None, + "ip": self.ip, + "port": self.port, + "buffer_size": self.buffer_size, + "running": self._running, + "socket_open": self.socket is not None, } - + if self.socket: try: sockname = self.socket.getsockname() - info['bound_address'] = sockname - except: - pass - + info["bound_address"] = sockname + except Exception as e: + logger.debug("Failed to get UDP sockname: %s", e) + return info -def create_udp_transport(ip: str = "127.0.0.1", port: int = 5001) -> Optional[UDPTransport]: +def create_udp_transport( + ip: str = "127.0.0.1", port: int = 5001 +) -> UDPTransport | None: """ Factory function to create and initialize a UDP transport. - + Args: ip: IP address to bind to port: Port number to listen on - + Returns: Initialized UDPTransport instance, or None if initialization failed """ transport = UDPTransport(ip, port) - + if transport.create_socket(): return transport else: diff --git a/parol6/smooth_motion/__init__.py b/parol6/smooth_motion/__init__.py index 98d01f5..8b99f6f 100644 --- a/parol6/smooth_motion/__init__.py +++ b/parol6/smooth_motion/__init__.py @@ -1,9 +1,9 @@ +from .advanced import AdvancedMotionBlender from .circle import CircularMotion from .helix import HelixMotion +from .motion_blender import MotionBlender from .spline import SplineMotion from .waypoints import WaypointTrajectoryPlanner -from .motion_blender import MotionBlender -from .advanced import AdvancedMotionBlender __all__ = [ "CircularMotion", @@ -11,5 +11,5 @@ "HelixMotion", "WaypointTrajectoryPlanner", "MotionBlender", - "AdvancedMotionBlender" -] \ No newline at end of file + "AdvancedMotionBlender", +] diff --git a/parol6/smooth_motion/advanced.py b/parol6/smooth_motion/advanced.py index ab7f311..febc785 100644 --- a/parol6/smooth_motion/advanced.py +++ b/parol6/smooth_motion/advanced.py @@ -2,8 +2,6 @@ Advanced trajectory blending utilities (C2 continuity, minimum-jerk, etc.). """ -from typing import Optional, Tuple - import numpy as np @@ -34,7 +32,9 @@ def __init__(self, sample_rate: float = 100.0): "cubic": self._blend_cubic, } - def extract_trajectory_state(self, trajectory: np.ndarray, index: int) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: + def extract_trajectory_state( + self, trajectory: np.ndarray, index: int + ) -> tuple[np.ndarray, np.ndarray, np.ndarray]: """ Extract position, velocity, and acceleration at a trajectory point. @@ -74,11 +74,15 @@ def extract_trajectory_state(self, trajectory: np.ndarray, index: int) -> Tuple[ v2 = (trajectory[-1] - trajectory[-2]) / self.dt acc = (v2 - v1) / self.dt else: - acc = (trajectory[index + 1] - 2 * trajectory[index] + trajectory[index - 1]) / (self.dt**2) + acc = ( + trajectory[index + 1] - 2 * trajectory[index] + trajectory[index - 1] + ) / (self.dt**2) return pos, vel, acc - def calculate_blend_region_size(self, traj1: np.ndarray, traj2: np.ndarray, max_accel: float = 1000.0) -> int: + def calculate_blend_region_size( + self, traj1: np.ndarray, traj2: np.ndarray, max_accel: float = 1000.0 + ) -> int: """ Calculate optimal blend region size based on velocity mismatch. @@ -106,7 +110,14 @@ def calculate_blend_region_size(self, traj1: np.ndarray, traj2: np.ndarray, max_ return blend_samples def solve_quintic_coefficients( - self, p0: np.ndarray, pf: np.ndarray, v0: np.ndarray, vf: np.ndarray, a0: np.ndarray, af: np.ndarray, T: float + self, + p0: np.ndarray, + pf: np.ndarray, + v0: np.ndarray, + vf: np.ndarray, + a0: np.ndarray, + af: np.ndarray, + T: float, ) -> np.ndarray: """ Solve for quintic polynomial coefficients given boundary conditions. @@ -134,18 +145,40 @@ def solve_quintic_coefficients( return coeffs - def evaluate_quintic(self, coeffs: np.ndarray, t: float) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: + def evaluate_quintic( + self, coeffs: np.ndarray, t: float + ) -> tuple[np.ndarray, np.ndarray, np.ndarray]: """ Evaluate quintic polynomial at time t. Returns position, velocity, and acceleration. """ - pos = coeffs[0] + coeffs[1] * t + coeffs[2] * t**2 + coeffs[3] * t**3 + coeffs[4] * t**4 + coeffs[5] * t**5 - vel = coeffs[1] + 2 * coeffs[2] * t + 3 * coeffs[3] * t**2 + 4 * coeffs[4] * t**3 + 5 * coeffs[5] * t**4 - acc = 2 * coeffs[2] + 6 * coeffs[3] * t + 12 * coeffs[4] * t**2 + 20 * coeffs[5] * t**3 + pos = ( + coeffs[0] + + coeffs[1] * t + + coeffs[2] * t**2 + + coeffs[3] * t**3 + + coeffs[4] * t**4 + + coeffs[5] * t**5 + ) + vel = ( + coeffs[1] + + 2 * coeffs[2] * t + + 3 * coeffs[3] * t**2 + + 4 * coeffs[4] * t**3 + + 5 * coeffs[5] * t**4 + ) + acc = ( + 2 * coeffs[2] + + 6 * coeffs[3] * t + + 12 * coeffs[4] * t**2 + + 20 * coeffs[5] * t**3 + ) return pos, vel, acc - def _blend_quintic(self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int) -> np.ndarray: + def _blend_quintic( + self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int + ) -> np.ndarray: """ Generate quintic polynomial blend with C2 continuity. @@ -165,7 +198,9 @@ def _blend_quintic(self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: in return np.array(blend_traj) - def _blend_scurve(self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int) -> np.ndarray: + def _blend_scurve( + self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int + ) -> np.ndarray: """ Generate S-curve blend with jerk limiting. """ @@ -176,12 +211,12 @@ def _blend_scurve(self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int from .scurve import MultiAxisSCurveTrajectory scurve = MultiAxisSCurveTrajectory( - p0[:6], # assume first 6 are XYZRPY - pf[:6], - v0=v0[:6], - vf=vf[:6], - T=blend_samples * self.dt, - jerk_limit=5000, + list(map(float, p0[:6].tolist())), # assume first 6 are XYZRPY + list(map(float, pf[:6].tolist())), + v0=list(map(float, v0[:6].tolist())), + vf=list(map(float, vf[:6].tolist())), + T=float(blend_samples * self.dt), + jerk_limit=5000.0, ) points = scurve.get_trajectory_points(self.dt) @@ -190,7 +225,11 @@ def _blend_scurve(self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int pose = np.zeros_like(p0) pose[:6] = points["position"][i] if len(p0) > 6: - alpha = i / (len(points["position"]) - 1) if len(points["position"]) > 1 else 1.0 + alpha = ( + i / (len(points["position"]) - 1) + if len(points["position"]) > 1 + else 1.0 + ) pose[6:] = p0[6:] * (1 - alpha) + pf[6:] * alpha blend_traj.append(pose) @@ -198,7 +237,9 @@ def _blend_scurve(self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int except Exception: return self._blend_quintic(traj1, traj2, blend_samples) - def _blend_smoothstep(self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int) -> np.ndarray: + def _blend_smoothstep( + self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int + ) -> np.ndarray: """ Legacy smoothstep blend for backward compatibility. """ @@ -214,7 +255,9 @@ def _blend_smoothstep(self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: return np.array(blend_traj) - def _blend_minimum_jerk(self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int) -> np.ndarray: + def _blend_minimum_jerk( + self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int + ) -> np.ndarray: """ Minimum jerk trajectory blend. @@ -231,7 +274,9 @@ def _blend_minimum_jerk(self, traj1: np.ndarray, traj2: np.ndarray, blend_sample return np.array(blend_traj) - def _blend_cubic(self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int) -> np.ndarray: + def _blend_cubic( + self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int + ) -> np.ndarray: """ Cubic spline blend with C1 continuity. """ @@ -260,7 +305,7 @@ def blend_trajectories( traj1: np.ndarray, traj2: np.ndarray, method: str = "quintic", - blend_samples: Optional[int] = None, + blend_samples: int | None = None, auto_size: bool = True, ) -> np.ndarray: """ diff --git a/parol6/smooth_motion/base.py b/parol6/smooth_motion/base.py index a632a67..8e6c9d7 100644 --- a/parol6/smooth_motion/base.py +++ b/parol6/smooth_motion/base.py @@ -4,7 +4,7 @@ Provides common timing utilities and constraints for derived generators. """ -from typing import Union +from typing import Any import numpy as np @@ -23,10 +23,10 @@ def __init__(self, control_rate: float = 100.0): """ self.control_rate = control_rate self.dt = 1.0 / control_rate - self.trajectory_cache = {} + self.trajectory_cache: dict[str, Any] = {} self.constraints = MotionConstraints() # Add constraints - def generate_timestamps(self, duration: Union[float, np.floating]) -> np.ndarray: + def generate_timestamps(self, duration: float | np.floating) -> np.ndarray: """Generate evenly spaced timestamps for trajectory""" num_points = int(duration * self.control_rate) return np.linspace(0, duration, num_points) diff --git a/parol6/smooth_motion/circle.py b/parol6/smooth_motion/circle.py index 64a44af..10c9a7c 100644 --- a/parol6/smooth_motion/circle.py +++ b/parol6/smooth_motion/circle.py @@ -2,7 +2,7 @@ Circle trajectory generator. """ -from typing import Sequence, Optional, Union +from collections.abc import Sequence import numpy as np from numpy.typing import NDArray @@ -18,10 +18,10 @@ def generate_arc_3d( self, start_pose: Sequence[float], end_pose: Sequence[float], - center: Union[Sequence[float], NDArray], - normal: Optional[Union[Sequence[float], NDArray]] = None, + center: Sequence[float] | NDArray, + normal: Sequence[float] | NDArray | None = None, clockwise: bool = True, - duration: Union[float, np.floating] = 2.0, + duration: float | np.floating = 2.0, ) -> np.ndarray: """ Generate a 3D circular arc trajectory @@ -51,8 +51,8 @@ def generate_arc_3d( normal = np.cross(r1, r2) if np.linalg.norm(normal) < 1e-6: # Points are collinear normal = np.array([0, 0, 1]) # Default to XY plane - normal = np.array(normal, dtype=float) - normal = normal / np.linalg.norm(normal) + normal_np: np.ndarray = np.array(normal, dtype=float) + normal_np = normal_np / np.linalg.norm(normal_np) # Arc sweep angle calculation r1_norm = r1 / np.linalg.norm(r1) @@ -62,7 +62,7 @@ def generate_arc_3d( # Arc direction validation cross = np.cross(r1_norm, r2_norm) - if np.dot(cross, normal) < 0: + if np.dot(cross, normal_np) < 0: arc_angle = 2 * np.pi - arc_angle if clockwise: @@ -82,11 +82,15 @@ def generate_arc_3d( else: # Rotate radius vector angle = s * arc_angle - rot_matrix = self._rotation_matrix_from_axis_angle(normal, angle) + rot_matrix = self._rotation_matrix_from_axis_angle(normal_np, angle) current_pos = center_pt + rot_matrix @ r1 # Interpolate orientation (SLERP) - current_orient = self._slerp_orientation(start_pose[3:], end_pose[3:], s) + current_orient = self._slerp_orientation( + np.asarray(start_pose[3:], dtype=float), + np.asarray(end_pose[3:], dtype=float), + float(s), + ) # Combine position and orientation pose = np.concatenate([current_pos, current_orient]) @@ -98,12 +102,12 @@ def generate_arc_with_profile( self, start_pose: Sequence[float], end_pose: Sequence[float], - center: Union[Sequence[float], NDArray], - normal: Optional[Union[Sequence[float], NDArray]] = None, + center: Sequence[float] | NDArray, + normal: Sequence[float] | NDArray | None = None, clockwise: bool = True, - duration: Union[float, np.floating] = 2.0, + duration: float | np.floating = 2.0, trajectory_type: str = "cubic", - jerk_limit: Optional[float] = None, + jerk_limit: float | None = None, ) -> np.ndarray: """ Generate arc trajectory with specified velocity profile. @@ -126,7 +130,9 @@ def generate_arc_with_profile( """ if trajectory_type == "cubic": # Use existing cubic implementation - return self.generate_arc_3d(start_pose, end_pose, center, normal, clockwise, duration) + return self.generate_arc_3d( + start_pose, end_pose, center, normal, clockwise, duration + ) # Convert to numpy arrays start_pos = np.array(start_pose[:3]) @@ -142,8 +148,8 @@ def generate_arc_with_profile( normal = np.cross(r1, r2) if np.linalg.norm(normal) < 1e-6: normal = np.array([0, 0, 1]) - normal = np.array(normal, dtype=float) - normal = normal / np.linalg.norm(normal) + normal_np: np.ndarray = np.array(normal, dtype=float) + normal_np = normal_np / np.linalg.norm(normal_np) # Calculate arc angle r1_norm = r1 / np.linalg.norm(r1) @@ -153,7 +159,7 @@ def generate_arc_with_profile( # Determine arc direction cross = np.cross(r1_norm, r2_norm) - if np.dot(cross, normal) < 0: + if np.dot(cross, normal_np) < 0: arc_angle = 2 * np.pi - arc_angle if clockwise: arc_angle = -arc_angle @@ -185,11 +191,15 @@ def generate_arc_with_profile( angle = s * arc_angle # Rotation matrix for arc - rot_matrix = self._rotation_matrix_from_axis_angle(normal, angle) + rot_matrix = self._rotation_matrix_from_axis_angle(normal_np, angle) current_pos = center_pt + rot_matrix @ r1 # Interpolate orientation (SLERP) - current_orient = self._slerp_orientation(start_pose[3:], end_pose[3:], s) + current_orient = self._slerp_orientation( + np.asarray(start_pose[3:], dtype=float), + np.asarray(end_pose[3:], dtype=float), + float(s), + ) # Combine position and orientation pose = np.concatenate([current_pos, current_orient]) @@ -199,12 +209,12 @@ def generate_arc_with_profile( def generate_circle_3d( self, - center: Union[Sequence[float], NDArray], + center: Sequence[float] | NDArray, radius: float, - normal: Union[Sequence[float], NDArray] = [0, 0, 1], - start_angle: Optional[float] = None, - duration: Union[float, np.floating] = 4.0, - start_point: Optional[Sequence[float]] = None, + normal: Sequence[float] | NDArray = [0, 0, 1], + start_angle: float | None = None, + duration: float | np.floating = 4.0, + start_point: Sequence[float] | None = None, ) -> np.ndarray: """ Generate a complete circle trajectory that starts at start_point @@ -213,10 +223,10 @@ def generate_circle_3d( trajectory = [] # Circle coordinate system - normal = np.array(normal, dtype=float) - normal = normal / np.linalg.norm(normal) - u = self._get_perpendicular_vector(normal) - v = np.cross(normal, u) + normal_np: np.ndarray = np.array(normal, dtype=float) + normal_np = normal_np / np.linalg.norm(normal_np) + u = self._get_perpendicular_vector(normal_np) + v = np.cross(normal_np, u) center_np = np.array(center, dtype=float) @@ -225,14 +235,16 @@ def generate_circle_3d( start_pos = np.array(start_point[:3]) # Project start point onto the circle plane to_start = start_pos - center_np - to_start_plane = to_start - np.dot(to_start, normal) * normal + to_start_plane = to_start - np.dot(to_start, normal_np) * normal_np # Get distance from center in the plane dist_in_plane = np.linalg.norm(to_start_plane) if dist_in_plane < 0.001: # Center start point - undefined angle - print(" WARNING: Start point is at circle center, using default position") + print( + " WARNING: Start point is at circle center, using default position" + ) start_angle = 0 actual_start = center_np + radius * u else: @@ -245,9 +257,13 @@ def generate_circle_3d( # Check if entry trajectory might be needed radius_error = abs(dist_in_plane - radius) if radius_error > radius * 0.05: # More than 5% off - print(f" INFO: Starting {dist_in_plane:.1f}mm from center (radius: {radius}mm)") + print( + f" INFO: Starting {dist_in_plane:.1f}mm from center (radius: {radius}mm)" + ) if radius_error > radius * 0.3: # More than 30% off - print(" WARNING: Large distance from circle - consider using entry trajectory") + print( + " WARNING: Large distance from circle - consider using entry trajectory" + ) actual_start = start_pos else: @@ -261,7 +277,9 @@ def generate_circle_3d( pos = actual_start else: # Generate circle points - angle = start_angle + (2 * np.pi * t / duration) + angle = float(start_angle if start_angle is not None else 0.0) + ( + 2 * np.pi * t / duration + ) pos = center_np + radius * (np.cos(angle) * u + np.sin(angle) * v) # Placeholder orientation (will be overridden) @@ -272,14 +290,14 @@ def generate_circle_3d( def generate_circle_with_profile( self, - center: Union[Sequence[float], NDArray], + center: Sequence[float] | NDArray, radius: float, - normal: Union[Sequence[float], NDArray] = [0, 0, 1], - duration: Union[float, np.floating] = 4.0, + normal: Sequence[float] | NDArray = [0, 0, 1], + duration: float | np.floating = 4.0, trajectory_type: str = "cubic", - jerk_limit: Optional[float] = None, - start_angle: Optional[float] = None, - start_point: Optional[Sequence[float]] = None, + jerk_limit: float | None = None, + start_angle: float | None = None, + start_point: Sequence[float] | None = None, ) -> np.ndarray: """ Generate circle with specified trajectory profile. @@ -314,7 +332,9 @@ def generate_circle_with_profile( self.control_rate = adaptive_rate self.dt = 1.0 / adaptive_rate # Use print for debug info since logger not imported here - print(f" [ADAPTIVE] Using {adaptive_rate:.0f}Hz control rate for {radius:.0f}mm radius circle") + print( + f" [ADAPTIVE] Using {adaptive_rate:.0f}Hz control rate for {radius:.0f}mm radius circle" + ) else: original_rate = None original_dt = None @@ -322,11 +342,23 @@ def generate_circle_with_profile( try: if trajectory_type == "cubic": # Use existing implementation - result = self.generate_circle_3d(center, radius, normal, start_angle, duration, start_point) + result = self.generate_circle_3d( + center, radius, normal, start_angle, duration, start_point + ) elif trajectory_type == "quintic": - result = self.generate_quintic_circle(center, radius, normal, duration, start_angle, start_point) + result = self.generate_quintic_circle( + center, radius, normal, duration, start_angle, start_point + ) elif trajectory_type == "s_curve": - result = self.generate_scurve_circle(center, radius, normal, duration, jerk_limit, start_angle, start_point) + result = self.generate_scurve_circle( + center, + radius, + normal, + duration, + jerk_limit, + start_angle, + start_point, + ) else: raise ValueError(f"Unknown trajectory type: {trajectory_type}") finally: @@ -339,12 +371,12 @@ def generate_circle_with_profile( def generate_quintic_circle( self, - center: Union[Sequence[float], NDArray], + center: Sequence[float] | NDArray, radius: float, - normal: Union[Sequence[float], NDArray] = [0, 0, 1], - duration: Union[float, np.floating] = 4.0, - start_angle: Optional[float] = None, - start_point: Optional[Sequence[float]] = None, + normal: Sequence[float] | NDArray = [0, 0, 1], + duration: float | np.floating = 4.0, + start_angle: float | None = None, + start_point: Sequence[float] | None = None, ) -> np.ndarray: """ Generate circle trajectory using quintic polynomial velocity profile. @@ -354,17 +386,17 @@ def generate_quintic_circle( num_points = int(duration * self.control_rate) # Setup coordinate system - normal = np.array(normal, dtype=float) - normal = normal / np.linalg.norm(normal) - u = self._get_perpendicular_vector(normal) - v = np.cross(normal, u) + normal_np: np.ndarray = np.array(normal, dtype=float) + normal_np = normal_np / np.linalg.norm(normal_np) + u = self._get_perpendicular_vector(normal_np) + v = np.cross(normal_np, u) center_np = np.array(center, dtype=float) # Handle start point if provided if start_point is not None: start_pos = np.array(start_point[:3]) to_start = start_pos - center_np - to_start_plane = to_start - np.dot(to_start, normal) * normal + to_start_plane = to_start - np.dot(to_start, normal_np) * normal_np dist_in_plane = np.linalg.norm(to_start_plane) if dist_in_plane < 0.001: @@ -378,14 +410,22 @@ def generate_quintic_circle( # Check if entry trajectory might be needed radius_error = abs(dist_in_plane - radius) if radius_error > radius * 0.05: # More than 5% off - print(f" INFO: Starting {dist_in_plane:.1f}mm from center (radius: {radius}mm)") + print( + f" INFO: Starting {dist_in_plane:.1f}mm from center (radius: {radius}mm)" + ) if radius_error > radius * 0.2: # More than 20% off - print(" WARNING: Large distance from circle - consider using entry trajectory") + print( + " WARNING: Large distance from circle - consider using entry trajectory" + ) else: start_angle = 0 if start_angle is None else start_angle # Step 1: Generate uniformly spaced angular points - angles = np.linspace(start_angle, start_angle + 2 * np.pi, num_points) + angles = np.linspace( + float(start_angle if start_angle is not None else 0.0), + float((start_angle if start_angle is not None else 0.0) + 2 * np.pi), + num_points, + ) uniform_positions = [] for angle in angles: @@ -427,13 +467,13 @@ def generate_quintic_circle( def generate_scurve_circle( self, - center: Union[Sequence[float], NDArray], + center: Sequence[float] | NDArray, radius: float, - normal: Union[Sequence[float], NDArray] = [0, 0, 1], - duration: Union[float, np.floating] = 4.0, - jerk_limit: Optional[float] = 5000.0, - start_angle: Optional[float] = None, - start_point: Optional[Sequence[float]] = None, + normal: Sequence[float] | NDArray = [0, 0, 1], + duration: float | np.floating = 4.0, + jerk_limit: float | None = 5000.0, + start_angle: float | None = None, + start_point: Sequence[float] | None = None, ) -> np.ndarray: """ Generate circle trajectory using S-curve velocity profile. @@ -446,17 +486,17 @@ def generate_scurve_circle( num_points = int(duration * self.control_rate) # Setup coordinate system - normal = np.array(normal, dtype=float) - normal = normal / np.linalg.norm(normal) - u = self._get_perpendicular_vector(normal) - v = np.cross(normal, u) + normal_np: np.ndarray = np.array(normal, dtype=float) + normal_np = normal_np / np.linalg.norm(normal_np) + u = self._get_perpendicular_vector(normal_np) + v = np.cross(normal_np, u) center_np = np.array(center, dtype=float) # Handle start point if provided if start_point is not None: start_pos = np.array(start_point[:3]) to_start = start_pos - center_np - to_start_plane = to_start - np.dot(to_start, normal) * normal + to_start_plane = to_start - np.dot(to_start, normal_np) * normal_np dist_in_plane = np.linalg.norm(to_start_plane) if dist_in_plane < 0.001: @@ -470,14 +510,22 @@ def generate_scurve_circle( # Check if entry trajectory might be needed radius_error = abs(dist_in_plane - radius) if radius_error > radius * 0.05: # More than 5% off - print(f" INFO: Starting {dist_in_plane:.1f}mm from center (radius: {radius}mm)") + print( + f" INFO: Starting {dist_in_plane:.1f}mm from center (radius: {radius}mm)" + ) if radius_error > radius * 0.2: # More than 20% off - print(" WARNING: Large distance from circle - consider using entry trajectory") + print( + " WARNING: Large distance from circle - consider using entry trajectory" + ) else: start_angle = 0 if start_angle is None else start_angle # Step 1: Generate uniformly spaced angular points - angles = np.linspace(start_angle, start_angle + 2 * np.pi, num_points) + angles = np.linspace( + float(start_angle if start_angle is not None else 0.0), + float((start_angle if start_angle is not None else 0.0) + 2 * np.pi), + num_points, + ) uniform_positions = [] for angle in angles: @@ -523,14 +571,18 @@ def generate_scurve_circle( return np.array(trajectory) - def _rotation_matrix_from_axis_angle(self, axis: np.ndarray, angle: float) -> np.ndarray: + def _rotation_matrix_from_axis_angle( + self, axis: np.ndarray, angle: float + ) -> np.ndarray: """Generate rotation matrix using Rodrigues' formula""" axis = axis / np.linalg.norm(axis) cos_a = np.cos(angle) sin_a = np.sin(angle) # Cross-product matrix - K = np.array([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]], [-axis[1], axis[0], 0]]) + K = np.array( + [[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]], [-axis[1], axis[0], 0]] + ) # Rodrigues' formula R = np.eye(3) + sin_a * K + (1 - cos_a) * K @ K @@ -547,7 +599,10 @@ def _get_perpendicular_vector(self, v: np.ndarray) -> np.ndarray: return cross / np.linalg.norm(cross) def _slerp_orientation( - self, start_orient: NDArray[np.floating], end_orient: NDArray[np.floating], t: float + self, + start_orient: NDArray[np.floating], + end_orient: NDArray[np.floating], + t: float, ) -> np.ndarray: """Spherical linear interpolation for orientation""" # Convert to quaternions @@ -555,9 +610,9 @@ def _slerp_orientation( r2 = Rotation.from_euler("xyz", end_orient, degrees=True) # Quaternion interpolation setup - key_rots = Rotation.from_quat([r1.as_quat(), r2.as_quat()]) - slerp = Slerp([0, 1], key_rots) + key_rots = Rotation.from_quat(np.stack([r1.as_quat(), r2.as_quat()])) + slerp = Slerp(np.array([0.0, 1.0], dtype=float), key_rots) - # Interpolate - interp_rot = slerp(t) - return interp_rot.as_euler("xyz", degrees=True) + # Interpolate at a single time by passing a 1D array + interp_rot = slerp(np.array([t], dtype=float)) + return interp_rot.as_euler("xyz", degrees=True)[0] diff --git a/parol6/smooth_motion/helix.py b/parol6/smooth_motion/helix.py index aded610..7003986 100644 --- a/parol6/smooth_motion/helix.py +++ b/parol6/smooth_motion/helix.py @@ -2,7 +2,7 @@ Helix trajectory generator. """ -from typing import Sequence, Optional, Union +from collections.abc import Sequence import numpy as np from numpy.typing import NDArray @@ -23,15 +23,15 @@ def _get_perpendicular_vector(self, v: np.ndarray) -> np.ndarray: def generate_helix_with_profile( self, - center: Union[Sequence[float], NDArray], + center: Sequence[float] | NDArray, radius: float, pitch: float, height: float, - axis: Union[Sequence[float], NDArray] = [0, 0, 1], - duration: Union[float, np.floating] = 4.0, + axis: Sequence[float] | NDArray = [0, 0, 1], + duration: float | np.floating = 4.0, trajectory_type: str = "cubic", - jerk_limit: Optional[float] = None, - start_point: Optional[Sequence[float]] = None, + jerk_limit: float | None = None, + start_point: Sequence[float] | None = None, clockwise: bool = False, ) -> np.ndarray: """ @@ -62,20 +62,28 @@ def generate_helix_with_profile( ) if trajectory_type == "s_curve": return self.generate_scurve_helix( - center, radius, pitch, height, axis, duration, jerk_limit, start_point, clockwise + center, + radius, + pitch, + height, + axis, + duration, + jerk_limit, + start_point, + clockwise, ) raise ValueError(f"Unknown trajectory type: {trajectory_type}") def generate_cubic_helix( self, - center: Union[Sequence[float], NDArray], + center: Sequence[float] | NDArray, radius: float, pitch: float, height: float, - axis: Union[Sequence[float], NDArray] = [0, 0, 1], - duration: Union[float, np.floating] = 4.0, - start_point: Optional[Sequence[float]] = None, + axis: Sequence[float] | NDArray = [0, 0, 1], + duration: float | np.floating = 4.0, + start_point: Sequence[float] | None = None, clockwise: bool = False, ) -> np.ndarray: """ @@ -87,10 +95,10 @@ def generate_cubic_helix( total_angle = 2 * np.pi * num_revolutions # Setup coordinate system - axis = np.array(axis, dtype=float) - axis = axis / np.linalg.norm(axis) - u = self._get_perpendicular_vector(axis) - v = np.cross(axis, u) + axis_np: np.ndarray = np.array(axis, dtype=float) + axis_np = axis_np / np.linalg.norm(axis_np) + u = self._get_perpendicular_vector(axis_np) + v = np.cross(axis_np, u) center_np = np.array(center, dtype=float) # Determine starting angle if start_point provided @@ -98,11 +106,13 @@ def generate_cubic_helix( if start_point is not None: start_pos = np.array(start_point[:3]) to_start = start_pos - center_np - to_start_plane = to_start - np.dot(to_start, axis) * axis + to_start_plane = to_start - np.dot(to_start, axis_np) * axis_np if np.linalg.norm(to_start_plane) > 0.001: to_start_normalized = to_start_plane / np.linalg.norm(to_start_plane) - start_angle = np.arctan2(np.dot(to_start_normalized, v), np.dot(to_start_normalized, u)) + start_angle = np.arctan2( + np.dot(to_start_normalized, v), np.dot(to_start_normalized, u) + ) # Generate trajectory points num_points = int(duration * self.control_rate) @@ -123,7 +133,11 @@ def generate_cubic_helix( z_offset = height * progress # Calculate 3D position - pos = center_np + radius * (np.cos(theta) * u + np.sin(theta) * v) + z_offset * axis + pos = ( + center_np + + radius * (np.cos(theta) * u + np.sin(theta) * v) + + z_offset * axis_np + ) # Placeholder orientation (could be enhanced) orient = [0, 0, 0] if start_point is None else start_point[3:6] @@ -134,13 +148,13 @@ def generate_cubic_helix( def generate_quintic_helix( self, - center: Union[Sequence[float], NDArray], + center: Sequence[float] | NDArray, radius: float, pitch: float, height: float, - axis: Union[Sequence[float], NDArray] = [0, 0, 1], - duration: Union[float, np.floating] = 4.0, - start_point: Optional[Sequence[float]] = None, + axis: Sequence[float] | NDArray = [0, 0, 1], + duration: float | np.floating = 4.0, + start_point: Sequence[float] | None = None, clockwise: bool = False, ) -> np.ndarray: """ @@ -152,10 +166,10 @@ def generate_quintic_helix( total_angle = 2 * np.pi * num_revolutions # Setup coordinate system - axis = np.array(axis, dtype=float) - axis = axis / np.linalg.norm(axis) - u = self._get_perpendicular_vector(axis) - v = np.cross(axis, u) + axis_np: np.ndarray = np.array(axis, dtype=float) + axis_np = axis_np / np.linalg.norm(axis_np) + u = self._get_perpendicular_vector(axis_np) + v = np.cross(axis_np, u) center_np = np.array(center, dtype=float) # Determine starting angle @@ -163,11 +177,13 @@ def generate_quintic_helix( if start_point is not None: start_pos = np.array(start_point[:3]) to_start = start_pos - center_np - to_start_plane = to_start - np.dot(to_start, axis) * axis + to_start_plane = to_start - np.dot(to_start, axis_np) * axis_np if np.linalg.norm(to_start_plane) > 0.001: to_start_normalized = to_start_plane / np.linalg.norm(to_start_plane) - start_angle = np.arctan2(np.dot(to_start_normalized, v), np.dot(to_start_normalized, u)) + start_angle = np.arctan2( + np.dot(to_start_normalized, v), np.dot(to_start_normalized, u) + ) # Generate trajectory with quintic profile num_points = int(duration * self.control_rate) @@ -188,7 +204,11 @@ def generate_quintic_helix( z_offset = height * progress # Calculate position - pos = center_np + radius * (np.cos(theta) * u + np.sin(theta) * v) + z_offset * axis + pos = ( + center_np + + radius * (np.cos(theta) * u + np.sin(theta) * v) + + z_offset * axis_np + ) # Orientation orient = [0, 0, 0] if start_point is None else start_point[3:6] @@ -199,14 +219,14 @@ def generate_quintic_helix( def generate_scurve_helix( self, - center: Union[Sequence[float], NDArray], + center: Sequence[float] | NDArray, radius: float, pitch: float, height: float, - axis: Union[Sequence[float], NDArray] = [0, 0, 1], - duration: Union[float, np.floating] = 4.0, - jerk_limit: Optional[float] = None, - start_point: Optional[Sequence[float]] = None, + axis: Sequence[float] | NDArray = [0, 0, 1], + duration: float | np.floating = 4.0, + jerk_limit: float | None = None, + start_point: Sequence[float] | None = None, clockwise: bool = False, ) -> np.ndarray: """ @@ -218,10 +238,10 @@ def generate_scurve_helix( total_angle = 2 * np.pi * num_revolutions # Setup coordinate system - axis = np.array(axis, dtype=float) - axis = axis / np.linalg.norm(axis) - u = self._get_perpendicular_vector(axis) - v = np.cross(axis, u) + axis_np: np.ndarray = np.array(axis, dtype=float) + axis_np = axis_np / np.linalg.norm(axis_np) + u = self._get_perpendicular_vector(axis_np) + v = np.cross(axis_np, u) center_np = np.array(center, dtype=float) # Determine starting angle @@ -229,11 +249,13 @@ def generate_scurve_helix( if start_point is not None: start_pos = np.array(start_point[:3]) to_start = start_pos - center_np - to_start_plane = to_start - np.dot(to_start, axis) * axis + to_start_plane = to_start - np.dot(to_start, axis_np) * axis_np if np.linalg.norm(to_start_plane) > 0.001: to_start_normalized = to_start_plane / np.linalg.norm(to_start_plane) - start_angle = np.arctan2(np.dot(to_start_normalized, v), np.dot(to_start_normalized, u)) + start_angle = np.arctan2( + np.dot(to_start_normalized, v), np.dot(to_start_normalized, u) + ) # Generate trajectory with S-curve profile num_points = int(duration * self.control_rate) @@ -259,7 +281,11 @@ def generate_scurve_helix( z_offset = height * progress # Calculate position - pos = center_np + radius * (np.cos(theta) * u + np.sin(theta) * v) + z_offset * axis + pos = ( + center_np + + radius * (np.cos(theta) * u + np.sin(theta) * v) + + z_offset * axis_np + ) # Orientation orient = [0, 0, 0] if start_point is None else start_point[3:6] diff --git a/parol6/smooth_motion/motion_blender.py b/parol6/smooth_motion/motion_blender.py index ad416c3..0900877 100644 --- a/parol6/smooth_motion/motion_blender.py +++ b/parol6/smooth_motion/motion_blender.py @@ -4,8 +4,6 @@ from __future__ import annotations -from typing import List - import numpy as np from scipy.spatial.transform import Rotation, Slerp @@ -16,7 +14,9 @@ class MotionBlender: def __init__(self, blend_time: float = 0.5): self.blend_time = blend_time - def blend_trajectories(self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int = 50) -> np.ndarray: + def blend_trajectories( + self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int = 50 + ) -> np.ndarray: """Blend two trajectory segments with improved velocity continuity""" if blend_samples < 4: @@ -30,11 +30,13 @@ def blend_trajectories(self, traj1: np.ndarray, traj2: np.ndarray, blend_samples overlap_end = min(len(traj2), blend_samples // 3) # Extract blend region - blend_start_pose = traj1[overlap_start] if overlap_start < len(traj1) else traj1[-1] + blend_start_pose = ( + traj1[overlap_start] if overlap_start < len(traj1) else traj1[-1] + ) blend_end_pose = traj2[overlap_end] if overlap_end < len(traj2) else traj2[0] # Generate smooth transition using S-curve - blended: List[np.ndarray] = [] + blended: list[np.ndarray] = [] for i in range(blend_samples): t = i / (blend_samples - 1) # Use smoothstep function for smoother acceleration @@ -43,21 +45,21 @@ def blend_trajectories(self, traj1: np.ndarray, traj2: np.ndarray, blend_samples # Blend position pos_blend = blend_start_pose * (1 - s) + blend_end_pose * s - # Orientation interpolation via SLERP - r1 = Rotation.from_euler('xyz', blend_start_pose[3:], degrees=True) - r2 = Rotation.from_euler('xyz', blend_end_pose[3:], degrees=True) - key_rots = Rotation.from_quat([r1.as_quat(), r2.as_quat()]) - slerp = Slerp([0, 1], key_rots) - orient_blend = slerp(s).as_euler('xyz', degrees=True) + # Orientation interpolation via SLERP (pass array-like time) + r1 = Rotation.from_euler("xyz", blend_start_pose[3:], degrees=True) + r2 = Rotation.from_euler("xyz", blend_end_pose[3:], degrees=True) + key_rots = Rotation.from_quat(np.stack([r1.as_quat(), r2.as_quat()])) + slerp = Slerp(np.array([0.0, 1.0], dtype=float), key_rots) + orient_blend = slerp(np.array([float(s)], dtype=float)).as_euler( + "xyz", degrees=True + )[0] pos_blend[3:] = orient_blend blended.append(pos_blend) # Combine with better overlap handling - result = np.vstack([ - traj1[:overlap_start], - np.array(blended), - traj2[overlap_end:] - ]) + result = np.vstack( + [traj1[:overlap_start], np.array(blended), traj2[overlap_end:]] + ) return result diff --git a/parol6/smooth_motion/motion_constraints.py b/parol6/smooth_motion/motion_constraints.py index fef27b6..aa0c661 100644 --- a/parol6/smooth_motion/motion_constraints.py +++ b/parol6/smooth_motion/motion_constraints.py @@ -5,95 +5,104 @@ based on gear ratios and mechanical properties. """ -from typing import Dict, Optional, List - import numpy as np + import parol6.PAROL6_ROBOT as PAROL6_ROBOT + class MotionConstraints: """ Motion constraints for PAROL6 robot. - + Defines per-joint limits for velocity, acceleration, and jerk based on gear ratios and mechanical properties. """ - + def __init__(self): """Initialize with PAROL6-specific constraints.""" # Use gear ratios from PAROL6_ROBOT.py - self.gear_ratios: List[float] = PAROL6_ROBOT.Joint_reduction_ratio - + self.gear_ratios = [float(x) for x in PAROL6_ROBOT.joint.ratio.tolist()] + # Use jerk limits from PAROL6_ROBOT.py - self.max_jerk: List[float] = PAROL6_ROBOT.Joint_max_jerk - + self.max_jerk = [float(x) for x in PAROL6_ROBOT.joint.jerk.max.tolist()] + # Use maximum velocities from PAROL6_ROBOT.py - self.max_velocity: List[float] = PAROL6_ROBOT.Joint_max_speed - + self.max_velocity = [float(x) for x in PAROL6_ROBOT.joint.speed.max.tolist()] + # Use max acceleration from PAROL6_ROBOT.py (convert from RAD/S² to STEP/S²) - # Calculate max accelerations for each joint - self.max_acceleration: List[float] = [] - for i in range(len(self.gear_ratios)): - # Convert from RAD/S² to STEP/S² using gear ratio - acc_rad_s2 = PAROL6_ROBOT.Joint_max_acc - acc_step_s2 = PAROL6_ROBOT.SPEED_RAD2STEP(acc_rad_s2, i) - self.max_acceleration.append(acc_step_s2) - - def get_joint_constraints(self, joint_idx: int) -> Optional[Dict[str, float]]: + # steps/s² = (rad/s²) * (steps/rad) , and steps/rad = ratio / radian_per_step + max_acc_rad = float(PAROL6_ROBOT.joint.acc.max_rad) + steps_per_rad_base = 1.0 / float(PAROL6_ROBOT.conv.radian_per_step) + self.max_acceleration = [ + max_acc_rad * steps_per_rad_base * float(ratio) + for ratio in PAROL6_ROBOT.joint.ratio + ] + + def get_joint_constraints(self, joint_idx: int) -> dict[str, float] | None: """Get constraints for specific joint.""" if joint_idx >= len(self.gear_ratios): return None - + return { - 'gear_ratio': self.gear_ratios[joint_idx], - 'v_max': self.max_velocity[joint_idx], - 'a_max': self.max_acceleration[joint_idx], - 'j_max': self.max_jerk[joint_idx] + "gear_ratio": self.gear_ratios[joint_idx], + "v_max": self.max_velocity[joint_idx], + "a_max": self.max_acceleration[joint_idx], + "j_max": self.max_jerk[joint_idx], } - - def scale_for_gear_ratio(self, joint_idx: int, base_limits: Dict[str, float]) -> Dict[str, float]: + + def scale_for_gear_ratio( + self, joint_idx: int, base_limits: dict[str, float] + ) -> dict[str, float]: """Scale motion limits based on gear ratio.""" ratio = self.gear_ratios[joint_idx] - + # Higher gear ratio = lower speed but higher precision scaled = { - 'v_max': base_limits['v_max'] / ratio, - 'a_max': base_limits['a_max'] / ratio, - 'j_max': base_limits['j_max'] / ratio + "v_max": base_limits["v_max"] / ratio, + "a_max": base_limits["a_max"] / ratio, + "j_max": base_limits["j_max"] / ratio, } - + return scaled - - def validate_trajectory(self, trajectory: np.ndarray, - joint_idx: int, dt: float = 0.01) -> Dict[str, float | bool]: + + def validate_trajectory( + self, trajectory: np.ndarray, joint_idx: int, dt: float = 0.01 + ) -> dict[str, float | bool]: """ Validate that trajectory respects constraints. - + Returns: Dictionary with validation results """ constraints = self.get_joint_constraints(joint_idx) if constraints is None or len(trajectory) < 3: return { - 'velocity_ok': True, - 'acceleration_ok': True, - 'jerk_ok': True, - 'max_velocity': 0.0, - 'max_acceleration': 0.0, - 'max_jerk': 0.0 + "velocity_ok": True, + "acceleration_ok": True, + "jerk_ok": True, + "max_velocity": 0.0, + "max_acceleration": 0.0, + "max_jerk": 0.0, } - + # Calculate derivatives numerically velocities = np.diff(trajectory) / dt accelerations = np.diff(velocities) / dt jerks = np.diff(accelerations) / dt - - validation = { - 'velocity_ok': np.all(np.abs(velocities) <= constraints['v_max']), - 'acceleration_ok': np.all(np.abs(accelerations) <= constraints['a_max']), - 'jerk_ok': np.all(np.abs(jerks) <= constraints['j_max']), - 'max_velocity': float(np.max(np.abs(velocities))) if velocities.size else 0.0, - 'max_acceleration': float(np.max(np.abs(accelerations))) if accelerations.size else 0.0, - 'max_jerk': float(np.max(np.abs(jerks))) if jerks.size else 0.0 + + validation: dict[str, float | bool] = { + "velocity_ok": bool(np.all(np.abs(velocities) <= constraints["v_max"])), + "acceleration_ok": bool( + np.all(np.abs(accelerations) <= constraints["a_max"]) + ), + "jerk_ok": bool(np.all(np.abs(jerks) <= constraints["j_max"])), + "max_velocity": float(np.max(np.abs(velocities))) + if velocities.size + else 0.0, + "max_acceleration": float(np.max(np.abs(accelerations))) + if accelerations.size + else 0.0, + "max_jerk": float(np.max(np.abs(jerks))) if jerks.size else 0.0, } - + return validation diff --git a/parol6/smooth_motion/quintic.py b/parol6/smooth_motion/quintic.py index d50e5ee..6ef76b0 100644 --- a/parol6/smooth_motion/quintic.py +++ b/parol6/smooth_motion/quintic.py @@ -1,9 +1,11 @@ """ Quintic polynomial primitives and multi-axis quintic trajectory. """ -from typing import Dict, Optional, List + +from typing import Any, Optional import numpy as np + from .motion_constraints import MotionConstraints @@ -46,7 +48,14 @@ def __init__( self.qf = qf # Store boundary conditions - self.boundary_conditions = {"q0": q0, "qf": qf, "v0": v0, "vf": vf, "a0": a0, "af": af} + self.boundary_conditions = { + "q0": q0, + "qf": qf, + "v0": v0, + "vf": vf, + "a0": a0, + "af": af, + } # Solve for polynomial coefficients using analytical method self.coeffs = self._solve_coefficients_analytical(q0, qf, v0, vf, a0, af, T) @@ -78,20 +87,42 @@ def _solve_coefficients_analytical(self, q0, qf, v0, vf, a0, af, T): a2_ = a0_norm / 2.0 a3_ = 10 * (qf - q0) - 6 * v0_norm - 4 * vf_norm - (3 * a0_norm - af_norm) / 2.0 - a4_ = -15 * (qf - q0) + 8 * v0_norm + 7 * vf_norm + (3 * a0_norm - 2 * af_norm) / 2.0 + a4_ = ( + -15 * (qf - q0) + + 8 * v0_norm + + 7 * vf_norm + + (3 * a0_norm - 2 * af_norm) / 2.0 + ) a5_ = 6 * (qf - q0) - 3 * (v0_norm + vf_norm) - (a0_norm - af_norm) / 2.0 # Convert back to actual time domain - coeffs = np.array([a0_, a1_ / T, a2_ / T**2, a3_ / T**3, a4_ / T**4, a5_ / T**5]) + coeffs = np.array( + [a0_, a1_ / T, a2_ / T**2, a3_ / T**3, a4_ / T**4, a5_ / T**5] + ) return coeffs def _prepare_derivative_coeffs(self): """Pre-compute coefficients for velocity, acceleration, and jerk.""" self.vel_coeffs = np.array( - [self.coeffs[1], 2 * self.coeffs[2], 3 * self.coeffs[3], 4 * self.coeffs[4], 5 * self.coeffs[5]] + [ + self.coeffs[1], + 2 * self.coeffs[2], + 3 * self.coeffs[3], + 4 * self.coeffs[4], + 5 * self.coeffs[5], + ] + ) + self.acc_coeffs = np.array( + [ + 2 * self.coeffs[2], + 6 * self.coeffs[3], + 12 * self.coeffs[4], + 20 * self.coeffs[5], + ] + ) + self.jerk_coeffs = np.array( + [6 * self.coeffs[3], 24 * self.coeffs[4], 60 * self.coeffs[5]] ) - self.acc_coeffs = np.array([2 * self.coeffs[2], 6 * self.coeffs[3], 12 * self.coeffs[4], 20 * self.coeffs[5]]) - self.jerk_coeffs = np.array([6 * self.coeffs[3], 24 * self.coeffs[4], 60 * self.coeffs[5]]) def position(self, t: float) -> float: """Evaluate position at time t using Horner's method.""" @@ -153,7 +184,7 @@ def evaluate(self, t: float, derivative: int = 0) -> float: return self.jerk(t) raise ValueError(f"Derivative order {derivative} not supported (max is 3)") - def get_trajectory_points(self, dt: float = 0.01) -> Dict[str, np.ndarray]: + def get_trajectory_points(self, dt: float = 0.01) -> dict[str, np.ndarray]: """ Generate trajectory points at specified time interval. @@ -170,34 +201,42 @@ def get_trajectory_points(self, dt: float = 0.01) -> Dict[str, np.ndarray]: } return trajectory - def validate_continuity(self, tolerance: float = 1e-10) -> Dict[str, bool]: + def validate_continuity(self, tolerance: float = 1e-10) -> dict[str, bool]: """ Validate that boundary conditions are satisfied. """ validation = { "q0": abs(self.position(0) - self.boundary_conditions["q0"]) < tolerance, - "qf": abs(self.position(self.T) - self.boundary_conditions["qf"]) < tolerance, + "qf": abs(self.position(self.T) - self.boundary_conditions["qf"]) + < tolerance, "v0": abs(self.velocity(0) - self.boundary_conditions["v0"]) < tolerance, - "vf": abs(self.velocity(self.T) - self.boundary_conditions["vf"]) < tolerance, - "a0": abs(self.acceleration(0) - self.boundary_conditions["a0"]) < tolerance, - "af": abs(self.acceleration(self.T) - self.boundary_conditions["af"]) < tolerance, + "vf": abs(self.velocity(self.T) - self.boundary_conditions["vf"]) + < tolerance, + "a0": abs(self.acceleration(0) - self.boundary_conditions["a0"]) + < tolerance, + "af": abs(self.acceleration(self.T) - self.boundary_conditions["af"]) + < tolerance, } return validation - def validate_numerical_stability(self) -> Dict[str, object]: + def validate_numerical_stability(self) -> dict[str, Any]: """ Check for potential numerical stability issues. """ - stability: Dict[str, object] = {"is_stable": True, "warnings": [], "metrics": {}} + warnings: list[str] = [] + metrics: dict[str, float] = {} + is_stable = True # Check condition number (ratio of time to distance) distance = abs(self.qf - self.q0) if distance > 1e-6: time_distance_ratio = self.T / distance - stability["metrics"]["time_distance_ratio"] = time_distance_ratio + metrics["time_distance_ratio"] = time_distance_ratio if time_distance_ratio > 100: - stability["is_stable"] = False - stability["warnings"].append(f"Poor conditioning: T/d ratio = {time_distance_ratio:.1f}") + is_stable = False + warnings.append( + f"Poor conditioning: T/d ratio = {time_distance_ratio:.1f}" + ) # Check coefficient magnitudes coeff_magnitudes = [abs(c) for c in self.coeffs] @@ -207,18 +246,20 @@ def validate_numerical_stability(self) -> Dict[str, object]: if min_nonzero_coeff > 0: coeff_ratio = max_coeff / min_nonzero_coeff - stability["metrics"]["coefficient_ratio"] = coeff_ratio + metrics["coefficient_ratio"] = coeff_ratio if coeff_ratio > 1e6: - stability["warnings"].append(f"Large coefficient ratio: {coeff_ratio:.2e}") + warnings.append(f"Large coefficient ratio: {coeff_ratio:.2e}") if self.T < 0.001: - stability["warnings"].append(f"Very small duration T={self.T} may cause numerical issues") + warnings.append( + f"Very small duration T={self.T} may cause numerical issues" + ) max_jerk = max(abs(self.jerk(t)) for t in np.linspace(0, self.T, 10)) if max_jerk > 1e6: - stability["warnings"].append(f"Very large jerk values: {max_jerk:.2e}") + warnings.append(f"Very large jerk values: {max_jerk:.2e}") - return stability + return {"is_stable": is_stable, "warnings": warnings, "metrics": metrics} class MultiAxisQuinticTrajectory: @@ -231,13 +272,13 @@ class MultiAxisQuinticTrajectory: def __init__( self, - q0: List[float], - qf: List[float], - v0: Optional[List[float]] = None, - vf: Optional[List[float]] = None, - a0: Optional[List[float]] = None, - af: Optional[List[float]] = None, - T: Optional[float] = None, + q0: list[float], + qf: list[float], + v0: list[float] | None = None, + vf: list[float] | None = None, + a0: list[float] | None = None, + af: list[float] | None = None, + T: float | None = None, constraints: Optional["MotionConstraints"] = None, ): """ @@ -255,25 +296,34 @@ def __init__( """ self.num_axes = len(q0) - # Default boundary conditions - v0 = v0 if v0 is not None else [0] * self.num_axes - vf = vf if vf is not None else [0] * self.num_axes - a0 = a0 if a0 is not None else [0] * self.num_axes - af = af if af is not None else [0] * self.num_axes - - # Calculate minimum time if not specified - if T is None: - T = self._calculate_minimum_time(q0, qf, v0, vf, constraints) - - self.T = T + # Default boundary conditions (use floats to satisfy typing) + v0 = v0 if v0 is not None else [0.0] * self.num_axes + vf = vf if vf is not None else [0.0] * self.num_axes + a0 = a0 if a0 is not None else [0.0] * self.num_axes + af = af if af is not None else [0.0] * self.num_axes + + # Determine duration as a concrete float + T_val: float = ( + T + if T is not None + else self._calculate_minimum_time(q0, qf, v0, vf, constraints) + ) + self.T: float = T_val # Generate quintic polynomial for each axis - self.axis_trajectories: List[QuinticPolynomial] = [] + self.axis_trajectories: list[QuinticPolynomial] = [] for i in range(self.num_axes): - quintic = QuinticPolynomial(q0[i], qf[i], v0[i], vf[i], a0[i], af[i], T) + quintic = QuinticPolynomial(q0[i], qf[i], v0[i], vf[i], a0[i], af[i], T_val) self.axis_trajectories.append(quintic) - def _calculate_minimum_time(self, q0, qf, v0, vf, constraints: Optional["MotionConstraints"]) -> float: + def _calculate_minimum_time( + self, + q0: list[float], + qf: list[float], + v0: list[float], + vf: list[float], + constraints: Optional["MotionConstraints"], + ) -> float: """ Calculate minimum time based on velocity and acceleration constraints. """ @@ -282,7 +332,7 @@ def _calculate_minimum_time(self, q0, qf, v0, vf, constraints: Optional["MotionC max_distance = max(abs(qf[i] - q0[i]) for i in range(self.num_axes)) return max(2.0, max_distance / 50.0) # Assume 50 units/s default - min_times: List[float] = [] + min_times: list[float] = [] for i in range(self.num_axes): distance = abs(qf[i] - q0[i]) @@ -299,11 +349,16 @@ def _calculate_minimum_time(self, q0, qf, v0, vf, constraints: Optional["MotionC # Use maximum time to ensure all constraints are satisfied return max(min_times) * 1.2 if min_times else 2.0 - def evaluate_all(self, t: float) -> Dict[str, List[float]]: + def evaluate_all(self, t: float) -> dict[str, list[float]]: """ Evaluate all axes at time t. """ - result: Dict[str, List[float]] = {"position": [], "velocity": [], "acceleration": [], "jerk": []} + result: dict[str, list[float]] = { + "position": [], + "velocity": [], + "acceleration": [], + "jerk": [], + } for traj in self.axis_trajectories: result["position"].append(traj.position(t)) result["velocity"].append(traj.velocity(t)) @@ -311,7 +366,7 @@ def evaluate_all(self, t: float) -> Dict[str, List[float]]: result["jerk"].append(traj.jerk(t)) return result - def get_trajectory_points(self, dt: float = 0.01) -> Dict[str, np.ndarray]: + def get_trajectory_points(self, dt: float = 0.01) -> dict[str, np.ndarray]: """Generate trajectory points for all axes.""" time_points = np.arange(0, self.T + dt, dt) num_points = len(time_points) diff --git a/parol6/smooth_motion/scurve.py b/parol6/smooth_motion/scurve.py index 00613a2..cbc6b04 100644 --- a/parol6/smooth_motion/scurve.py +++ b/parol6/smooth_motion/scurve.py @@ -2,7 +2,7 @@ S-curve profile generator and multi-axis S-curve trajectory. """ -from typing import Dict, Optional, List, TYPE_CHECKING +from typing import TYPE_CHECKING, TypedDict import numpy as np @@ -13,6 +13,13 @@ from .motion_constraints import MotionConstraints +class FeasibilityInfo(TypedDict, total=False): + status: str + warnings: list[str] + achievable_a: float + achievable_v: float + + class SCurveProfile: """ Seven-segment S-curve velocity profile generator. @@ -55,6 +62,11 @@ def __init__( self.v_start = float(v_start) self.v_end = float(v_end) + # Initialize typed containers for type checkers + self.profile_type: str = "" + self.segments: dict[str, float] = {} + self.segment_boundaries: list[dict[str, float]] = [] + # Check feasibility and adjust parameters if needed self.feasibility_status = self._check_feasibility() @@ -64,7 +76,7 @@ def __init__( # Pre-calculate segment boundaries for proper evaluation self._calculate_segment_boundaries() - def _calculate_profile(self): + def _calculate_profile(self) -> tuple[str, dict[str, float]]: """ Calculate the S-curve profile type and segment durations. @@ -78,7 +90,7 @@ def _calculate_profile(self): # Asymmetric profiles for non-zero boundary velocities return self._calculate_asymmetric_profile() - def _calculate_symmetric_profile(self): + def _calculate_symmetric_profile(self) -> tuple[str, dict[str, float]]: """Calculate symmetric S-curve profile (v_start = v_end = 0).""" # Time to reach maximum acceleration from zero (jerk phase) T_j = self.a_max / self.j_max if self.j_max > 0 else 0.0 @@ -87,21 +99,29 @@ def _calculate_symmetric_profile(self): d_jerk = (self.a_max**3) / (self.j_max**2) if self.j_max > 0 else 0.0 # Check if we can reach maximum velocity - d_to_vmax = (self.v_max**2 / self.a_max + self.v_max * self.a_max / self.j_max) if self.a_max > 0 and self.j_max > 0 else float("inf") + d_to_vmax = ( + (self.v_max**2 / self.a_max + self.v_max * self.a_max / self.j_max) + if self.a_max > 0 and self.j_max > 0 + else float("inf") + ) if self.distance < 2 * d_jerk: # Case 1: Reduced acceleration profile (never reach a_max) profile_type = "reduced" - a_reached = (abs(self.distance) * self.j_max**2 / 2) ** (1 / 3) if self.j_max > 0 else 0.0 + a_reached = ( + (abs(self.distance) * self.j_max**2 / 2) ** (1 / 3) + if self.j_max > 0 + else 0.0 + ) T_j_actual = a_reached / self.j_max if self.j_max > 0 else 0.0 segments = { "T_j1": T_j_actual, # Jerk up - "T_a": 0.0, # No constant acceleration + "T_a": 0.0, # No constant acceleration "T_j2": T_j_actual, # Jerk down - "T_v": 0.0, # No cruise + "T_v": 0.0, # No cruise "T_j3": T_j_actual, # Jerk down (decel) - "T_d": 0.0, # No constant deceleration + "T_d": 0.0, # No constant deceleration "T_j4": T_j_actual, # Jerk up (decel end) "a_reached": a_reached, "v_reached": a_reached * T_j_actual, @@ -110,8 +130,14 @@ def _calculate_symmetric_profile(self): # Case 2: Triangular velocity profile (never reach v_max) profile_type = "triangular" - v_reached = np.sqrt(max(self.distance * self.a_max - 2 * self.a_max**3 / self.j_max**2, 0.0)) - T_a = (v_reached - self.a_max**2 / self.j_max) / self.a_max if self.a_max > 0 and self.j_max > 0 else 0.0 + v_reached = np.sqrt( + max(self.distance * self.a_max - 2 * self.a_max**3 / self.j_max**2, 0.0) + ) + T_a = ( + (v_reached - self.a_max**2 / self.j_max) / self.a_max + if self.a_max > 0 and self.j_max > 0 + else 0.0 + ) segments = { "T_j1": T_j, @@ -128,10 +154,18 @@ def _calculate_symmetric_profile(self): profile_type = "full" # Time at constant acceleration (after jerk phases) - T_a = (self.v_max - self.a_max**2 / self.j_max) / self.a_max if self.a_max > 0 and self.j_max > 0 else 0.0 + T_a = ( + (self.v_max - self.a_max**2 / self.j_max) / self.a_max + if self.a_max > 0 and self.j_max > 0 + else 0.0 + ) # Distance covered during acceleration/deceleration - d_accel = self.v_max**2 / self.a_max + self.v_max * self.a_max / self.j_max if self.a_max > 0 and self.j_max > 0 else 0.0 + d_accel = ( + self.v_max**2 / self.a_max + self.v_max * self.a_max / self.j_max + if self.a_max > 0 and self.j_max > 0 + else 0.0 + ) # Distance at cruise velocity d_cruise = self.distance - 2 * d_accel @@ -164,7 +198,7 @@ def _calculate_symmetric_profile(self): return profile_type, segments - def _calculate_asymmetric_profile(self): + def _calculate_asymmetric_profile(self) -> tuple[str, dict[str, float]]: """Calculate asymmetric S-curve for non-zero boundary velocities.""" v_diff = abs(self.v_end - self.v_start) v_avg = (self.v_start + self.v_end) / 2 @@ -176,7 +210,9 @@ def _calculate_asymmetric_profile(self): T_j = self.a_max / self.j_max if self.j_max > 0 else 0.0 if self.v_start < self.v_max and self.v_end < self.v_max: - v_peak = min(self.v_max, v_avg + np.sqrt(max(self.distance * self.a_max, 0.0))) + v_peak = min( + self.v_max, v_avg + np.sqrt(max(self.distance * self.a_max, 0.0)) + ) T_accel = (v_peak - self.v_start) / self.a_max if self.a_max > 0 else 0.0 T_a = max(0.0, T_accel - 2 * T_j) @@ -184,7 +220,9 @@ def _calculate_asymmetric_profile(self): T_decel = (v_peak - self.v_end) / self.a_max if self.a_max > 0 else 0.0 T_d = max(0.0, T_decel - 2 * T_j) - d_accel = self.v_start * (T_a + 2 * T_j) + 0.5 * self.a_max * (T_a + T_j) ** 2 + d_accel = ( + self.v_start * (T_a + 2 * T_j) + 0.5 * self.a_max * (T_a + T_j) ** 2 + ) d_decel = self.v_end * (T_d + 2 * T_j) + 0.5 * self.a_max * (T_d + T_j) ** 2 d_cruise = self.distance - d_accel - d_decel @@ -192,7 +230,17 @@ def _calculate_asymmetric_profile(self): T_v = d_cruise / v_peak else: T_v = 0.0 - v_peak = np.sqrt(max((2 * self.distance * self.a_max + self.v_start**2 + self.v_end**2) / 2, 0.0)) + v_peak = np.sqrt( + max( + ( + 2 * self.distance * self.a_max + + self.v_start**2 + + self.v_end**2 + ) + / 2, + 0.0, + ) + ) else: # Simple case - just decelerate T_a = 0.0 @@ -213,7 +261,7 @@ def _calculate_asymmetric_profile(self): } return "asymmetric", segments - def _check_feasibility(self) -> Dict[str, object]: + def _check_feasibility(self) -> FeasibilityInfo: """ Check if S-curve profile is achievable with given constraints. @@ -230,17 +278,24 @@ def _check_feasibility(self) -> Dict[str, object]: else float("inf") ) - feasibility: Dict[str, object] = {"status": "feasible", "warnings": []} + warnings: list[str] = [] + feasibility: FeasibilityInfo = {"status": "feasible", "warnings": warnings} if self.distance < 2 * d_min_jerk: - achievable_a = (abs(self.distance) * self.j_max**2 / 2) ** (1 / 3) if self.j_max > 0 else 0.0 + achievable_a = ( + (abs(self.distance) * self.j_max**2 / 2) ** (1 / 3) + if self.j_max > 0 + else 0.0 + ) feasibility["status"] = "reduced_acceleration" feasibility["achievable_a"] = achievable_a feasibility["warnings"].append( f"Distance too short to reach full acceleration. Max achievable: {achievable_a:.2f}" ) elif self.distance < 2 * d_min_vel: - achievable_v = np.sqrt(self.distance * self.a_max) if self.a_max > 0 else 0.0 + achievable_v = ( + np.sqrt(self.distance * self.a_max) if self.a_max > 0 else 0.0 + ) feasibility["status"] = "triangular_velocity" feasibility["achievable_v"] = achievable_v feasibility["warnings"].append( @@ -256,16 +311,18 @@ def _check_feasibility(self) -> Dict[str, object]: if self.v_max <= 0 or self.a_max <= 0 or self.j_max <= 0: feasibility["status"] = "invalid_constraints" - feasibility["warnings"].append("Invalid constraints: v_max, a_max, and j_max must all be positive") + feasibility["warnings"].append( + "Invalid constraints: v_max, a_max, and j_max must all be positive" + ) return feasibility - def _calculate_segment_boundaries(self): + def _calculate_segment_boundaries(self) -> None: """ Pre-calculate position, velocity, and acceleration at segment boundaries. This ensures proper continuity across segments. """ - self.segment_boundaries: List[Dict[str, float]] = [] + boundary_list: list[dict[str, float]] = [] # Initial state pos = 0.0 @@ -279,7 +336,7 @@ def _calculate_segment_boundaries(self): acc_end = acc + j * T_j1 vel_end = vel + acc * T_j1 + 0.5 * j * T_j1**2 pos_end = pos + vel * T_j1 + 0.5 * acc * T_j1**2 + (1 / 6) * j * T_j1**3 - self.segment_boundaries.append( + boundary_list.append( { "pos_start": pos, "vel_start": vel, @@ -300,7 +357,7 @@ def _calculate_segment_boundaries(self): acc_end = acc vel_end = vel + acc * T_a pos_end = pos + vel * T_a + 0.5 * acc * T_a**2 - self.segment_boundaries.append( + boundary_list.append( { "pos_start": pos, "vel_start": vel, @@ -321,7 +378,7 @@ def _calculate_segment_boundaries(self): acc_end = acc + j * T_j2 vel_end = vel + acc * T_j2 + 0.5 * j * T_j2**2 pos_end = pos + vel * T_j2 + 0.5 * acc * T_j2**2 + (1 / 6) * j * T_j2**3 - self.segment_boundaries.append( + boundary_list.append( { "pos_start": pos, "vel_start": vel, @@ -342,7 +399,7 @@ def _calculate_segment_boundaries(self): acc_end = 0.0 vel_end = vel pos_end = pos + vel * T_v - self.segment_boundaries.append( + boundary_list.append( { "pos_start": pos, "vel_start": vel, @@ -363,7 +420,7 @@ def _calculate_segment_boundaries(self): acc_end = j * T_j3 vel_end = vel + 0.5 * j * T_j3**2 pos_end = pos + vel * T_j3 + (1 / 6) * j * T_j3**3 - self.segment_boundaries.append( + boundary_list.append( { "pos_start": pos, "vel_start": vel, @@ -384,7 +441,7 @@ def _calculate_segment_boundaries(self): acc_end = acc vel_end = vel + acc * T_d pos_end = pos + vel * T_d + 0.5 * acc * T_d**2 - self.segment_boundaries.append( + boundary_list.append( { "pos_start": pos, "vel_start": vel, @@ -405,7 +462,7 @@ def _calculate_segment_boundaries(self): acc_end = acc + j * T_j4 vel_end = vel + acc * T_j4 + 0.5 * j * T_j4**2 pos_end = pos + vel * T_j4 + 0.5 * acc * T_j4**2 + (1 / 6) * j * T_j4**3 - self.segment_boundaries.append( + boundary_list.append( { "pos_start": pos, "vel_start": vel, @@ -418,8 +475,10 @@ def _calculate_segment_boundaries(self): } ) pos, vel, acc = pos_end, vel_end, acc_end + # finalize + self.segment_boundaries = boundary_list - def generate_trajectory_quintics(self) -> List["QuinticPolynomial"]: + def generate_trajectory_quintics(self) -> list["QuinticPolynomial"]: """ Generate quintic polynomials for each segment of the S-curve. @@ -428,7 +487,7 @@ def generate_trajectory_quintics(self) -> List["QuinticPolynomial"]: """ from .quintic import QuinticPolynomial # Local import to avoid cycle - quintics: List[QuinticPolynomial] = [] + quintics: list[QuinticPolynomial] = [] for seg in self.segment_boundaries: if seg["duration"] > 0: q = QuinticPolynomial( @@ -448,7 +507,7 @@ def get_total_time(self) -> float: """Get total time for the S-curve trajectory.""" return float(self.segments["T_total"]) - def evaluate_at_time(self, t: float) -> Dict[str, float]: + def evaluate_at_time(self, t: float) -> dict[str, float]: """ Evaluate S-curve at specific time. @@ -456,13 +515,28 @@ def evaluate_at_time(self, t: float) -> Dict[str, float]: Dictionary with position, velocity, acceleration, jerk """ if t <= 0: - return {"position": 0.0, "velocity": self.v_start, "acceleration": 0.0, "jerk": 0.0} + return { + "position": 0.0, + "velocity": self.v_start, + "acceleration": 0.0, + "jerk": 0.0, + } if t >= self.segments["T_total"]: if self.segment_boundaries: last = self.segment_boundaries[-1] - return {"position": last["pos_end"], "velocity": last["vel_end"], "acceleration": 0.0, "jerk": 0.0} - return {"position": self.distance, "velocity": self.v_end, "acceleration": 0.0, "jerk": 0.0} + return { + "position": last["pos_end"], + "velocity": last["vel_end"], + "acceleration": 0.0, + "jerk": 0.0, + } + return { + "position": self.distance, + "velocity": self.v_end, + "acceleration": 0.0, + "jerk": 0.0, + } # Find which segment we're in cumulative_time = 0.0 @@ -473,9 +547,16 @@ def evaluate_at_time(self, t: float) -> Dict[str, float]: cumulative_time += seg["duration"] # Fallback - return {"position": self.distance, "velocity": self.v_end, "acceleration": 0.0, "jerk": 0.0} + return { + "position": self.distance, + "velocity": self.v_end, + "acceleration": 0.0, + "jerk": 0.0, + } - def _evaluate_in_segment(self, segment: Dict[str, float], t: float) -> Dict[str, float]: + def _evaluate_in_segment( + self, segment: dict[str, float], t: float + ) -> dict[str, float]: """ Evaluate motion within a specific segment using proper kinematics. """ @@ -503,12 +584,12 @@ class MultiAxisSCurveTrajectory: def __init__( self, - start_pose: List[float], - end_pose: List[float], - v0: Optional[List[float]] = None, - vf: Optional[List[float]] = None, - T: Optional[float] = None, - jerk_limit: Optional[float] = None, + start_pose: list[float], + end_pose: list[float], + v0: list[float] | None = None, + vf: list[float] | None = None, + T: float | None = None, + jerk_limit: float | None = None, ): """ Create synchronized S-curve trajectories for multiple axes. @@ -525,12 +606,20 @@ def __init__( self.end_pose = np.array(end_pose, dtype=float) self.num_axes = len(start_pose) - self.v0 = np.array(v0, dtype=float) if v0 is not None else np.zeros(self.num_axes, dtype=float) - self.vf = np.array(vf, dtype=float) if vf is not None else np.zeros(self.num_axes, dtype=float) + self.v0 = ( + np.array(v0, dtype=float) + if v0 is not None + else np.zeros(self.num_axes, dtype=float) + ) + self.vf = ( + np.array(vf, dtype=float) + if vf is not None + else np.zeros(self.num_axes, dtype=float) + ) self.constraints = MotionConstraints() - self.axis_profiles: List[Optional[SCurveProfile]] = [] + self.axis_profiles: list[SCurveProfile | None] = [] self.max_time = 0.0 for i in range(self.num_axes): @@ -542,11 +631,15 @@ def __init__( joint_constraints = self.constraints.get_joint_constraints(i) if joint_constraints is None: - joint_constraints = {"v_max": 10000.0, "a_max": 20000.0, "j_max": jerk_limit if jerk_limit else 50000.0} + joint_constraints = { + "v_max": 10000.0, + "a_max": 20000.0, + "j_max": jerk_limit if jerk_limit else 50000.0, + } j_max = jerk_limit if jerk_limit is not None else joint_constraints["j_max"] - profile = SCurveProfile( + axis_profile = SCurveProfile( float(distance), float(joint_constraints["v_max"]), float(joint_constraints["a_max"]), @@ -554,21 +647,21 @@ def __init__( v_start=float(self.v0[i]), v_end=float(self.vf[i]), ) - self.axis_profiles.append(profile) - self.max_time = max(self.max_time, profile.get_total_time()) + self.axis_profiles.append(axis_profile) + self.max_time = max(self.max_time, axis_profile.get_total_time()) self.T = float(T) if T is not None else float(self.max_time) # Calculate time scaling factors for synchronization - self.time_scales: List[float] = [] - for profile in self.axis_profiles: - if profile is not None and self.T > 0: - scale = profile.get_total_time() / self.T + self.time_scales: list[float] = [] + for maybe_profile in self.axis_profiles: + if maybe_profile is not None and self.T > 0: + scale = maybe_profile.get_total_time() / self.T self.time_scales.append(scale) else: self.time_scales.append(1.0) - def get_trajectory_points(self, dt: float = 0.01) -> Dict[str, np.ndarray]: + def get_trajectory_points(self, dt: float = 0.01) -> dict[str, np.ndarray]: """ Generate synchronized trajectory points for all axes. @@ -584,20 +677,26 @@ def get_trajectory_points(self, dt: float = 0.01) -> Dict[str, np.ndarray]: for idx, t in enumerate(timestamps): for axis in range(self.num_axes): - if self.axis_profiles[axis] is None: + axis_profile = self.axis_profiles[axis] + if axis_profile is None: positions[idx, axis] = self.start_pose[axis] velocities[idx, axis] = 0.0 accelerations[idx, axis] = 0.0 else: t_scaled = t * self.time_scales[axis] - values = self.axis_profiles[axis].evaluate_at_time(t_scaled) + values = axis_profile.evaluate_at_time(t_scaled) positions[idx, axis] = self.start_pose[axis] + values["position"] velocities[idx, axis] = values["velocity"] accelerations[idx, axis] = values["acceleration"] - return {"position": positions, "velocity": velocities, "acceleration": accelerations, "timestamps": timestamps} + return { + "position": positions, + "velocity": velocities, + "acceleration": accelerations, + "timestamps": timestamps, + } - def evaluate_at_time(self, t: float) -> Dict[str, np.ndarray]: + def evaluate_at_time(self, t: float) -> dict[str, np.ndarray]: """ Evaluate trajectory at specific time. @@ -609,15 +708,22 @@ def evaluate_at_time(self, t: float) -> Dict[str, np.ndarray]: acceleration = np.zeros(self.num_axes) for axis in range(self.num_axes): - if self.axis_profiles[axis] is None: + axis_profile = self.axis_profiles[axis] + if axis_profile is None: position[axis] = self.start_pose[axis] velocity[axis] = 0.0 acceleration[axis] = 0.0 else: - t_scaled = min(t * self.time_scales[axis], self.axis_profiles[axis].get_total_time()) - values = self.axis_profiles[axis].evaluate_at_time(t_scaled) + t_scaled = min( + t * self.time_scales[axis], axis_profile.get_total_time() + ) + values = axis_profile.evaluate_at_time(t_scaled) position[axis] = self.start_pose[axis] + values["position"] velocity[axis] = values["velocity"] acceleration[axis] = values["acceleration"] - return {"position": position, "velocity": velocity, "acceleration": acceleration} + return { + "position": position, + "velocity": velocity, + "acceleration": acceleration, + } diff --git a/parol6/smooth_motion/spline.py b/parol6/smooth_motion/spline.py index f8b68ae..b26979d 100644 --- a/parol6/smooth_motion/spline.py +++ b/parol6/smooth_motion/spline.py @@ -4,10 +4,9 @@ from __future__ import annotations -from typing import List, Optional +from typing import Any import numpy as np -from numpy.typing import NDArray from scipy.interpolate import CubicSpline from scipy.spatial.transform import Rotation, Slerp @@ -21,12 +20,12 @@ class SplineMotion(TrajectoryGenerator): def generate_quintic_spline_true( self, - waypoints: List[List[float]], + waypoints: list[list[float]], waypoint_behavior: str = "stop", profile_type: str = "s_curve", optimization: str = "jerk", time_optimal: bool = False, - jerk_limit: Optional[float] = None, + jerk_limit: float | None = None, ) -> np.ndarray: """ Generate true quintic spline trajectory with optional S-curve profiles. @@ -42,7 +41,9 @@ def generate_quintic_spline_true( time_optimal: Calculate minimum time if True """ if profile_type == "s_curve": - return self._generate_scurve_waypoints(waypoints, waypoint_behavior, optimization, jerk_limit) + return self._generate_scurve_waypoints( + waypoints, waypoint_behavior, optimization, jerk_limit + ) if profile_type == "quintic": return self._generate_pure_quintic_waypoints(waypoints, waypoint_behavior) # Fall back to cubic implementation @@ -60,12 +61,12 @@ def _generate_pure_quintic_waypoints(self, waypoints, behavior): segment_times = [] for i in range(num_waypoints - 1): distance = np.linalg.norm(waypoints[i + 1, :3] - waypoints[i, :3]) - time = max(1.0, distance / 50.0) # 50 mm/s average + time = float(max(1.0, float(distance) / 50.0)) # 50 mm/s average segment_times.append(time) # Generate trajectory segments - full_trajectory = [] - prev_vf: Optional[List[float]] = None + full_trajectory: list[list[float]] = [] + prev_vf: list[float] | None = None for i in range(num_waypoints - 1): start_pose = waypoints[i] @@ -86,28 +87,39 @@ def _generate_pure_quintic_waypoints(self, waypoints, behavior): vf = [0.0] * 6 else: next_direction = waypoints[i + 2] - waypoints[i + 1] - next_segment_time = segment_times[i + 1] if (i + 1) < len(segment_times) else segment_times[i] + next_segment_time = ( + segment_times[i + 1] + if (i + 1) < len(segment_times) + else segment_times[i] + ) current_direction = waypoints[i + 1] - waypoints[i] - avg_direction = (current_direction / segment_times[i] + next_direction / next_segment_time) * 0.5 + avg_direction = ( + current_direction / segment_times[i] + + next_direction / next_segment_time + ) * 0.5 vf = list(avg_direction[:6] * 0.7) # Scale factor for stability prev_vf = vf # Create multi-axis quintic trajectory - segment_traj = MultiAxisQuinticTrajectory(list(start_pose), list(end_pose), v0, vf, T=T) + segment_traj = MultiAxisQuinticTrajectory( + list(start_pose), list(end_pose), v0, vf, T=T + ) # Sample the segment segment_points = segment_traj.get_trajectory_points(self.dt) # Add to full trajectory (avoid duplicating waypoints) if i == 0: - full_trajectory.extend(segment_points["position"]) + full_trajectory.extend(segment_points["position"].tolist()) else: - full_trajectory.extend(segment_points["position"][1:]) + full_trajectory.extend(segment_points["position"][1:].tolist()) return np.array(full_trajectory) - def _generate_scurve_waypoints(self, waypoints, behavior, optimization, jerk_limit=None): + def _generate_scurve_waypoints( + self, waypoints, behavior, optimization, jerk_limit=None + ): """Generate S-curve trajectories between waypoints.""" waypoints = np.array(waypoints) num_waypoints = len(waypoints) @@ -115,7 +127,7 @@ def _generate_scurve_waypoints(self, waypoints, behavior, optimization, jerk_lim if num_waypoints < 2: return waypoints - full_trajectory: List[List[float]] = [] + full_trajectory: list[list[float]] = [] for i in range(num_waypoints - 1): start_pose = waypoints[i] @@ -129,13 +141,21 @@ def _generate_scurve_waypoints(self, waypoints, behavior, optimization, jerk_lim distance = end_pose[j] - start_pose[j] # Get joint constraints - constraints = self.constraints.get_joint_constraints(j) # type: ignore[attr-defined] + constraints = self.constraints.get_joint_constraints(j) + if constraints is None: + constraints = { + "v_max": 10000.0, + "a_max": 20000.0, + "j_max": (jerk_limit if jerk_limit is not None else 50000.0), + } # Use provided jerk limit if available, otherwise use constraints j_max = jerk_limit if jerk_limit is not None else constraints["j_max"] # Create S-curve profile - scurve = SCurveProfile(distance, constraints["v_max"], constraints["a_max"], j_max) + scurve = SCurveProfile( + distance, constraints["v_max"], constraints["a_max"], j_max + ) max_time = max(max_time, scurve.get_total_time()) segment_trajectories.append(scurve) @@ -164,10 +184,10 @@ def _generate_scurve_waypoints(self, waypoints, behavior, optimization, jerk_lim def generate_cubic_spline( self, - waypoints: List[List[float]], - timestamps: Optional[List[float]] = None, - velocity_start: Optional[List[float]] = None, - velocity_end: Optional[List[float]] = None, + waypoints: list[list[float]], + timestamps: list[float] | None = None, + velocity_start: list[float] | None = None, + velocity_end: list[float] | None = None, ) -> np.ndarray: """ Generate cubic spline trajectory through waypoints @@ -181,52 +201,65 @@ def generate_cubic_spline( Returns: Array of interpolated poses """ - waypoints = np.array(waypoints) - num_waypoints = len(waypoints) + waypoints_arr = np.asarray(waypoints, dtype=float) + num_waypoints = len(waypoints_arr) # Auto-generate timestamps if not provided if timestamps is None: total_dist = 0.0 for i in range(1, num_waypoints): - dist = np.linalg.norm(waypoints[i, :3] - waypoints[i - 1, :3]) - total_dist += dist + dist = np.linalg.norm(waypoints_arr[i, :3] - waypoints_arr[i - 1, :3]) + total_dist += float(dist) # Assume average speed of 50 mm/s total_time = total_dist / 50.0 if total_dist > 0 else 0.0 - timestamps = np.linspace(0, total_time, num_waypoints) + timestamps_arr = np.linspace(0, total_time, num_waypoints) + else: + timestamps_arr = np.asarray(timestamps, dtype=float) # Validate array dimensions before creating splines - if len(timestamps) != len(waypoints): - raise ValueError(f"Timestamps length ({len(timestamps)}) must match waypoints length ({len(waypoints)})") + if len(timestamps_arr) != len(waypoints_arr): + raise ValueError( + f"Timestamps length ({len(timestamps_arr)}) must match waypoints length ({len(waypoints_arr)})" + ) # Position trajectory splines pos_splines = [] for i in range(3): - bc_type = "not-a-knot" # Default boundary condition + # Provide boundary conditions per component + bc: Any if velocity_start is not None and velocity_end is not None: - bc_type = ((1, velocity_start[i]), (1, velocity_end[i])) # type: ignore[assignment] - spline = CubicSpline(timestamps, waypoints[:, i], bc_type=bc_type) + bc = ((1, float(velocity_start[i])), (1, float(velocity_end[i]))) + else: + bc = "not-a-knot" + spline = CubicSpline(timestamps_arr, waypoints_arr[:, i], bc_type=bc) pos_splines.append(spline) # Orientation trajectory splines - rotations = [Rotation.from_euler("xyz", wp[3:], degrees=True) for wp in waypoints] + rotations = [ + Rotation.from_euler("xyz", wp[3:], degrees=True) for wp in waypoints + ] quats = np.array([r.as_quat() for r in rotations]) key_rots = Rotation.from_quat(quats) - slerp = Slerp(timestamps, key_rots) + slerp = Slerp(timestamps_arr, key_rots) # Generate dense trajectory - t_eval = self.generate_timestamps(float(timestamps[-1] if len(timestamps) else 0.0)) - trajectory: List[NDArray[np.floating]] = [] + t_eval = self.generate_timestamps( + float(timestamps_arr[-1] if len(timestamps_arr) else 0.0) + ) + trajectory: list[list[float]] = [] for t in t_eval: - pos = [spline(float(t)) for spline in pos_splines] - rot = slerp(float(t)) - orient = rot.as_euler("xyz", degrees=True) - trajectory.append(np.concatenate([pos, orient])) + pos = [float(spline(float(t))) for spline in pos_splines] + rot_single = slerp(np.array([float(t)])) + orient = rot_single.as_euler("xyz", degrees=True)[0] + trajectory.append(np.concatenate([pos, orient]).tolist()) return np.array(trajectory) - def generate_quintic_spline(self, waypoints: List[List[float]], timestamps: Optional[List[float]] = None) -> np.ndarray: + def generate_quintic_spline( + self, waypoints: list[list[float]], timestamps: list[float] | None = None + ) -> np.ndarray: """ Generate quintic (5th order) spline with zero velocity and acceleration at endpoints @@ -235,14 +268,16 @@ def generate_quintic_spline(self, waypoints: List[List[float]], timestamps: Opti timestamps: Time for each waypoint """ # Quintic spline boundary conditions at the endpoints - return self.generate_cubic_spline(waypoints, timestamps, velocity_start=[0, 0, 0], velocity_end=[0, 0, 0]) + return self.generate_cubic_spline( + waypoints, timestamps, velocity_start=[0, 0, 0], velocity_end=[0, 0, 0] + ) def generate_scurve_spline( self, - waypoints: List[List[float]], - duration: Optional[float] = None, + waypoints: list[list[float]], + duration: float | None = None, jerk_limit: float = 1000.0, - timestamps: Optional[List[float]] = None, + timestamps: list[float] | None = None, ) -> np.ndarray: """ Generate S-curve spline with jerk-limited motion profile @@ -256,7 +291,9 @@ def generate_scurve_spline( Returns: Array of interpolated poses with S-curve velocity profile """ - basic_trajectory = self.generate_cubic_spline(waypoints, timestamps, velocity_start=[0, 0, 0], velocity_end=[0, 0, 0]) + basic_trajectory = self.generate_cubic_spline( + waypoints, timestamps, velocity_start=[0, 0, 0], velocity_end=[0, 0, 0] + ) if len(basic_trajectory) < 2: return basic_trajectory @@ -264,7 +301,10 @@ def generate_scurve_spline( # Calculate total path length path_length = 0.0 for i in range(1, len(basic_trajectory)): - segment_length = np.linalg.norm(np.array(basic_trajectory[i][:3]) - np.array(basic_trajectory[i - 1][:3])) + segment_length = np.linalg.norm( + np.array(basic_trajectory[i][:3]) + - np.array(basic_trajectory[i - 1][:3]) + ) path_length += float(segment_length) if path_length < 0.001: @@ -282,7 +322,7 @@ def generate_scurve_spline( # Create S-curve time parameterization time_points = np.linspace(0, duration, num_points) - s_curve_params: List[float] = [] + s_curve_params: list[float] = [] for t in time_points: tau = t / duration @@ -297,7 +337,7 @@ def generate_scurve_spline( # Re-sample the trajectory according to S-curve profile new_indices = np.array(s_curve_params) * (len(basic_trajectory) - 1) - resampled_trajectory: List[List[float]] = [] + resampled_trajectory: list[list[float]] = [] for new_idx in new_indices: if new_idx <= 0: resampled_trajectory.append(basic_trajectory[0].tolist()) diff --git a/parol6/smooth_motion/waypoints.py b/parol6/smooth_motion/waypoints.py index 5ff91e5..0062003 100644 --- a/parol6/smooth_motion/waypoints.py +++ b/parol6/smooth_motion/waypoints.py @@ -2,11 +2,20 @@ Waypoint trajectory planner. """ -from typing import Dict, List, Optional, Tuple +from typing import TypedDict import numpy as np +class BlendRegion(TypedDict): + waypoint_idx: int + entry: np.ndarray + exit: np.ndarray + radius: float + v_entry: np.ndarray + v_exit: np.ndarray + + class WaypointTrajectoryPlanner: """ Trajectory planner for smooth motion through waypoints with corner cutting. @@ -15,7 +24,12 @@ class WaypointTrajectoryPlanner: spikes and ensure smooth motion through complex paths. """ - def __init__(self, waypoints: List[List[float]], constraints: Optional[Dict] = None, sample_rate: float = 100.0): + def __init__( + self, + waypoints: list[list[float]], + constraints: dict | None = None, + sample_rate: float = 100.0, + ): """ Initialize waypoint trajectory planner. @@ -38,9 +52,9 @@ def __init__(self, waypoints: List[List[float]], constraints: Optional[Dict] = N self.constraints = constraints if constraints else default_constraints # Blend planning data - self.blend_radii: List[float] = [] - self.blend_regions: List[Optional[Dict[str, np.ndarray]]] = [] - self.segment_velocities: List[float] = [] + self.blend_radii: list[float] = [] + self.blend_regions: list[BlendRegion | None] = [] + self.segment_velocities: list[float] = [] self.via_modes = ["via"] * self.num_waypoints # Default: pass through all def calculate_corner_angle(self, idx: int) -> float: @@ -68,7 +82,7 @@ def calculate_corner_angle(self, idx: int) -> float: cos_angle = np.clip(np.dot(v_in_norm, v_out_norm), -1, 1) angle = np.arccos(cos_angle) - return angle + return float(angle) def calculate_safe_blend_radius(self, idx: int, approach_velocity: float) -> float: """ @@ -87,24 +101,24 @@ def calculate_safe_blend_radius(self, idx: int, approach_velocity: float) -> flo return 0.0 # Dynamic blend radius based on velocity and angle - a_max = self.constraints["max_acceleration"] + a_max = float(self.constraints["max_acceleration"]) # Centripetal acceleration constraint # r = v² / (a_max * sin(θ/2)) - sin_half_angle = np.sin(angle / 2) + sin_half_angle = float(np.sin(angle / 2)) if sin_half_angle > 0: - r_dynamic = (approach_velocity**2) / (a_max * sin_half_angle) + r_dynamic = float((approach_velocity**2) / (a_max * sin_half_angle)) else: r_dynamic = 0.0 # Geometric constraint - don't exceed segment lengths - r_geometric = self.get_max_geometric_radius(idx) + r_geometric = float(self.get_max_geometric_radius(idx)) # Apply safety factor and limits r_safe = min(r_dynamic, r_geometric) * 0.7 # 70% safety factor - r_safe = max(0, min(r_safe, 100)) # Cap at 100mm + r_safe = max(0.0, min(r_safe, 100.0)) # Cap at 100mm - return r_safe + return float(r_safe) def get_max_geometric_radius(self, idx: int) -> float: """ @@ -120,15 +134,19 @@ def get_max_geometric_radius(self, idx: int) -> float: return 0.0 # Distance to previous waypoint - dist_prev = np.linalg.norm(self.waypoints[idx][:3] - self.waypoints[idx - 1][:3]) + dist_prev = np.linalg.norm( + self.waypoints[idx][:3] - self.waypoints[idx - 1][:3] + ) # Distance to next waypoint - dist_next = np.linalg.norm(self.waypoints[idx + 1][:3] - self.waypoints[idx][:3]) + dist_next = np.linalg.norm( + self.waypoints[idx + 1][:3] - self.waypoints[idx][:3] + ) # Maximum radius is 40% of shortest segment max_radius = 0.4 * min(dist_prev, dist_next) - return max_radius + return float(max_radius) def calculate_auto_blend_radii(self): """ @@ -142,7 +160,9 @@ def calculate_auto_blend_radii(self): self.blend_radii.append(0.0) else: # Estimate approach velocity - segment_length = np.linalg.norm(self.waypoints[i][:3] - self.waypoints[i - 1][:3]) + segment_length = np.linalg.norm( + self.waypoints[i][:3] - self.waypoints[i - 1][:3] + ) # Simple velocity estimation if segment_length > 0: @@ -157,7 +177,9 @@ def calculate_auto_blend_radii(self): radius = self.calculate_safe_blend_radius(i, approach_velocity) self.blend_radii.append(radius) - def compute_blend_points(self, idx: int, blend_radius: float) -> Tuple[np.ndarray, np.ndarray]: + def compute_blend_points( + self, idx: int, blend_radius: float + ) -> tuple[np.ndarray, np.ndarray]: """ Calculate blend entry and exit points for a waypoint. @@ -197,8 +219,14 @@ def compute_blend_points(self, idx: int, blend_radius: float) -> Tuple[np.ndarra alpha_entry = 1.0 - (blend_radius / (np.linalg.norm(v_in[:3]) + 1e-9)) alpha_exit = blend_radius / (np.linalg.norm(v_out[:3]) + 1e-9) - blend_entry[3:] = (self.waypoints[idx - 1][3:] * (1 - alpha_entry) + self.waypoints[idx][3:] * alpha_entry) - blend_exit[3:] = (self.waypoints[idx][3:] * (1 - alpha_exit) + self.waypoints[idx + 1][3:] * alpha_exit) + blend_entry[3:] = ( + self.waypoints[idx - 1][3:] * (1 - alpha_entry) + + self.waypoints[idx][3:] * alpha_entry + ) + blend_exit[3:] = ( + self.waypoints[idx][3:] * (1 - alpha_exit) + + self.waypoints[idx + 1][3:] * alpha_exit + ) return blend_entry, blend_exit @@ -208,8 +236,8 @@ def generate_parabolic_blend( exit_point: np.ndarray, v_entry: np.ndarray, v_exit: np.ndarray, - blend_time: Optional[float] = None, - ) -> List[np.ndarray]: + blend_time: float | None = None, + ) -> list[np.ndarray]: """ Generate parabolic trajectory for corner blend with acceleration limits. @@ -237,31 +265,42 @@ def generate_parabolic_blend( delta_v_mag = np.linalg.norm(delta_v[:3]) # Minimum blend time from acceleration constraint - min_blend_time = delta_v_mag / (self.constraints["max_acceleration"] + 1e-9) + min_blend_time = float( + delta_v_mag / (self.constraints["max_acceleration"] + 1e-9) + ) # Calculate blend time if not specified if blend_time is None: - v_avg = (np.linalg.norm(v_entry[:3]) + np.linalg.norm(v_exit[:3])) / 2 - if v_avg > 0: - time_from_velocity = distance / v_avg + v_avg = ( + float(np.linalg.norm(v_entry[:3])) + float(np.linalg.norm(v_exit[:3])) + ) / 2.0 + if v_avg > 0.0: + time_from_velocity = float(distance) / v_avg else: - time_from_velocity = np.sqrt(2 * distance / (self.constraints["max_acceleration"] + 1e-9)) + time_from_velocity = float( + np.sqrt( + 2.0 + * float(distance) + / (self.constraints["max_acceleration"] + 1e-9) + ) + ) blend_time = max(min_blend_time, time_from_velocity) else: blend_time = max(blend_time, min_blend_time) - blend_time = max(blend_time, 0.01) # Numerical stability + bt = float(max(blend_time, 0.01)) # Numerical stability # Acceleration (bounded) - a_blend = delta_v / blend_time - a_mag = np.linalg.norm(a_blend[:3]) - if a_mag > self.constraints["max_acceleration"] * 1.1: # 10% tolerance - scale = self.constraints["max_acceleration"] / (a_mag + 1e-9) + a_blend = delta_v / bt + a_mag = float(np.linalg.norm(a_blend[:3])) + amax = float(self.constraints["max_acceleration"]) + if a_mag > amax * 1.1: # 10% tolerance + scale = amax / (a_mag + 1e-9) a_blend = a_blend * scale - blend_time = blend_time / (scale + 1e-9) + bt = float(bt / (scale + 1e-9)) # Generate trajectory using cubic Hermite interpolation (C1 continuity) - num_points = max(5, int(blend_time * self.sample_rate)) # At least 5 points + num_points = max(5, int(bt * self.sample_rate)) # At least 5 points blend_traj = [] for i in range(num_points): @@ -276,13 +315,20 @@ def generate_parabolic_blend( # Interpolate position using hermite spline # Scale velocities by blend_time to get tangents - pos = h00 * entry_point + h10 * (v_entry * blend_time) + h01 * exit_point + h11 * (v_exit * blend_time) + pos = ( + h00 * entry_point + + h10 * (v_entry * bt) + + h01 * exit_point + + h11 * (v_exit * bt) + ) blend_traj.append(pos) return blend_traj - def generate_linear_segment(self, start: np.ndarray, end: np.ndarray, velocity: Optional[float] = None) -> List[np.ndarray]: + def generate_linear_segment( + self, start: np.ndarray, end: np.ndarray, velocity: float | None = None + ) -> list[np.ndarray]: """ Generate linear trajectory segment between two points. @@ -301,10 +347,10 @@ def generate_linear_segment(self, start: np.ndarray, end: np.ndarray, velocity: # Determine velocity if velocity is None: - velocity = self.constraints["max_velocity"] + velocity = float(self.constraints["max_velocity"]) # Calculate duration and number of points - duration = distance / velocity + duration = float(distance) / float(velocity) num_points = max(2, int(duration * self.sample_rate)) # Generate trajectory @@ -328,25 +374,32 @@ def compute_blend_regions(self): # Calculate velocities at blend points v_entry_dir = self.waypoints[i] - self.waypoints[i - 1] - v_entry = v_entry_dir[:3] / (np.linalg.norm(v_entry_dir[:3]) + 1e-9) * self.segment_velocities[i - 1] + v_entry = ( + v_entry_dir[:3] + / (np.linalg.norm(v_entry_dir[:3]) + 1e-9) + * self.segment_velocities[i - 1] + ) v_entry_full = np.zeros(len(entry)) v_entry_full[:3] = v_entry v_exit_dir = self.waypoints[i + 1] - self.waypoints[i] - v_exit = v_exit_dir[:3] / (np.linalg.norm(v_exit_dir[:3]) + 1e-9) * self.segment_velocities[i] + v_exit = ( + v_exit_dir[:3] + / (np.linalg.norm(v_exit_dir[:3]) + 1e-9) + * self.segment_velocities[i] + ) v_exit_full = np.zeros(len(exit)) v_exit_full[:3] = v_exit - self.blend_regions.append( - { - "waypoint_idx": i, - "entry": entry, - "exit": exit, - "radius": self.blend_radii[i], - "v_entry": v_entry_full, - "v_exit": v_exit_full, - } - ) + region: BlendRegion = { + "waypoint_idx": i, + "entry": entry, + "exit": exit, + "radius": float(self.blend_radii[i]), + "v_entry": v_entry_full, + "v_exit": v_exit_full, + } + self.blend_regions.append(region) else: # No blend or stop point self.blend_regions.append(None) @@ -354,10 +407,10 @@ def compute_blend_regions(self): def plan_trajectory( self, blend_mode: str = "auto", - blend_radii: Optional[List[float]] = None, - via_modes: Optional[List[str]] = None, + blend_radii: list[float] | None = None, + via_modes: list[str] | None = None, trajectory_type: str = "cubic", - jerk_limit: Optional[float] = None, + jerk_limit: float | None = None, ) -> np.ndarray: """ Plan complete trajectory through waypoints with blending. @@ -393,13 +446,23 @@ def plan_trajectory( # Calculate segment velocities self.segment_velocities = [] for i in range(self.num_waypoints - 1): - segment_length = np.linalg.norm(self.waypoints[i + 1][:3] - self.waypoints[i][:3]) + segment_length = np.linalg.norm( + self.waypoints[i + 1][:3] - self.waypoints[i][:3] + ) # Simple velocity planning if self.via_modes[i] == "stop" or self.via_modes[i + 1] == "stop": # Trapezoid profile with acceleration - v_max = min(self.constraints["max_velocity"], np.sqrt(self.constraints["max_acceleration"] * segment_length)) + v_max = min( + float(self.constraints["max_velocity"]), + float( + np.sqrt( + float(self.constraints["max_acceleration"]) + * float(segment_length) + ) + ), + ) else: - v_max = self.constraints["max_velocity"] + v_max = float(self.constraints["max_velocity"]) self.segment_velocities.append(v_max) @@ -407,7 +470,7 @@ def plan_trajectory( self.compute_blend_regions() # Generate full trajectory - full_trajectory: List[np.ndarray] = [] + full_trajectory: list[np.ndarray] = [] for i in range(self.num_waypoints - 1): # Determine segment start and end @@ -415,16 +478,22 @@ def plan_trajectory( segment_start = self.waypoints[0] else: # Check for blend at current waypoint - blend_region_prev = self.blend_regions[i - 1] if i - 1 < len(self.blend_regions) else None - if blend_region_prev: + blend_region_prev = ( + self.blend_regions[i - 1] + if i - 1 < len(self.blend_regions) + else None + ) + if blend_region_prev is not None: segment_start = blend_region_prev["exit"] else: segment_start = self.waypoints[i] if i < self.num_waypoints - 2: # Check for blend at next waypoint - blend_region_next = self.blend_regions[i] if i < len(self.blend_regions) else None - if blend_region_next: + blend_region_next = ( + self.blend_regions[i] if i < len(self.blend_regions) else None + ) + if blend_region_next is not None: segment_end = blend_region_next["entry"] else: segment_end = self.waypoints[i + 1] @@ -432,7 +501,9 @@ def plan_trajectory( segment_end = self.waypoints[i + 1] # Generate linear segment - segment = self.generate_linear_segment(segment_start, segment_end, self.segment_velocities[i]) + segment = self.generate_linear_segment( + segment_start, segment_end, self.segment_velocities[i] + ) # Add segment to trajectory if i == 0: @@ -442,13 +513,14 @@ def plan_trajectory( full_trajectory.extend(segment[1:]) # Add blend if needed - if i < len(self.blend_regions) and self.blend_regions[i]: - blend_region = self.blend_regions[i] + if i < len(self.blend_regions) and self.blend_regions[i] is not None: + br = self.blend_regions[i] + assert br is not None blend_traj = self.generate_parabolic_blend( - blend_region["entry"], - blend_region["exit"], - blend_region["v_entry"], - blend_region["v_exit"], + br["entry"], + br["exit"], + br["v_entry"], + br["v_exit"], ) # Skip first point to avoid duplication full_trajectory.extend(blend_traj[1:]) @@ -457,12 +529,17 @@ def plan_trajectory( # Apply profile to the generated trajectory if not cubic if trajectory_type != "cubic": - trajectory_array = self.apply_velocity_profile(trajectory_array, trajectory_type, jerk_limit) + trajectory_array = self.apply_velocity_profile( + trajectory_array, trajectory_type, jerk_limit + ) return trajectory_array def apply_velocity_profile( - self, trajectory: np.ndarray, profile_type: str = "quintic", jerk_limit: Optional[float] = None + self, + trajectory: np.ndarray, + profile_type: str = "quintic", + jerk_limit: float | None = None, ) -> np.ndarray: """ Apply velocity profile to existing trajectory points. @@ -485,7 +562,7 @@ def apply_velocity_profile( # Calculate cumulative arc length arc_lengths = [0.0] for i in range(1, len(trajectory)): - dist = np.linalg.norm(trajectory[i][:3] - trajectory[i - 1][:3]) + dist = float(np.linalg.norm(trajectory[i][:3] - trajectory[i - 1][:3])) arc_lengths.append(arc_lengths[-1] + dist) total_length = arc_lengths[-1] @@ -532,7 +609,9 @@ def apply_velocity_profile( alpha = 0.0 # Linear interpolation between points - new_trajectory[i] = (1 - alpha) * trajectory[j] + alpha * trajectory[j + 1] + new_trajectory[i] = (1 - alpha) * trajectory[ + j + ] + alpha * trajectory[j + 1] break else: # If we didn't find it (shouldn't happen), use the last point @@ -540,7 +619,7 @@ def apply_velocity_profile( return new_trajectory - def validate_trajectory(self, trajectory: np.ndarray) -> Dict[str, bool]: + def validate_trajectory(self, trajectory: np.ndarray) -> dict[str, float | bool]: """ Validate that trajectory respects all constraints. @@ -550,7 +629,7 @@ def validate_trajectory(self, trajectory: np.ndarray) -> Dict[str, bool]: Returns: Dictionary of validation results with detailed information """ - results: Dict[str, float | bool] = { + results: dict[str, float | bool] = { "velocity_ok": True, "acceleration_ok": True, "jerk_ok": True, @@ -578,24 +657,52 @@ def validate_trajectory(self, trajectory: np.ndarray) -> Dict[str, bool]: # Velocity - use all relevant dimensions velocities = np.diff(trajectory[:, :n_dims], axis=0) / dt - velocity_magnitudes = np.linalg.norm(velocities[:, :3], axis=1) if velocities.shape[0] else np.array([0.0]) - max_vel = float(np.max(velocity_magnitudes)) if velocity_magnitudes.size else 0.0 + velocity_magnitudes = ( + np.linalg.norm(velocities[:, :3], axis=1) + if velocities.shape[0] + else np.array([0.0]) + ) + max_vel = ( + float(np.max(velocity_magnitudes)) if velocity_magnitudes.size else 0.0 + ) results["max_velocity"] = max_vel results["velocity_ok"] = max_vel <= self.constraints["max_velocity"] * 1.1 # Acceleration if len(trajectory) > 2: - accelerations = np.diff(velocities[:, :3], axis=0) / dt if velocities.shape[0] > 1 else np.zeros((0, 3)) - acceleration_magnitudes = np.linalg.norm(accelerations, axis=1) if accelerations.shape[0] else np.array([0.0]) - max_acc = float(np.max(acceleration_magnitudes)) if acceleration_magnitudes.size else 0.0 + accelerations = ( + np.diff(velocities[:, :3], axis=0) / dt + if velocities.shape[0] > 1 + else np.zeros((0, 3)) + ) + acceleration_magnitudes = ( + np.linalg.norm(accelerations, axis=1) + if accelerations.shape[0] + else np.array([0.0]) + ) + max_acc = ( + float(np.max(acceleration_magnitudes)) + if acceleration_magnitudes.size + else 0.0 + ) results["max_acceleration"] = max_acc - results["acceleration_ok"] = max_acc <= self.constraints["max_acceleration"] * 1.2 + results["acceleration_ok"] = ( + max_acc <= self.constraints["max_acceleration"] * 1.2 + ) # Jerk if len(trajectory) > 3: - jerks = np.diff(accelerations, axis=0) / dt if accelerations.shape[0] > 1 else np.zeros((0, 3)) - jerk_magnitudes = np.linalg.norm(jerks, axis=1) if jerks.shape[0] else np.array([0.0]) - max_jerk = float(np.max(jerk_magnitudes)) if jerk_magnitudes.size else 0.0 + jerks = ( + np.diff(accelerations, axis=0) / dt + if accelerations.shape[0] > 1 + else np.zeros((0, 3)) + ) + jerk_magnitudes = ( + np.linalg.norm(jerks, axis=1) if jerks.shape[0] else np.array([0.0]) + ) + max_jerk = ( + float(np.max(jerk_magnitudes)) if jerk_magnitudes.size else 0.0 + ) results["max_jerk"] = max_jerk results["jerk_ok"] = max_jerk <= self.constraints["max_jerk"] * 1.5 diff --git a/parol6/tools.py b/parol6/tools.py new file mode 100644 index 0000000..20032b2 --- /dev/null +++ b/parol6/tools.py @@ -0,0 +1,94 @@ +""" +Tool Configuration Module + +Defines available end-effector tools for the PAROL6 robot with their transforms and visualization data. +Tools are swappable at runtime and affect both kinematics and visualization. +""" + +from typing import Any + +import numpy as np +from spatialmath import SE3 + +TOOL_CONFIGS: dict[str, dict[str, Any]] = { + "NONE": { + "name": "No Tool", + "description": "Bare flange - no tool attached", + "transform": np.eye(4), + "stl_files": [], + }, + "PNEUMATIC": { + "name": "Pneumatic Gripper", + "description": "Pneumatic gripper assembly", + "transform": (SE3(-0.04525, 0, 0) @ SE3.Rx(np.pi)).A, + "stl_files": [ + { + "file": "pneumatic_gripper_assembly.STL", + "origin": [0, 0, 0], + "rpy": [0, 0, 0], + } + ], + }, +} + + +def get_tool_transform(tool_name: str) -> np.ndarray: + """ + Get the 4x4 transformation matrix for a tool. + + Parameters + ---------- + tool_name : str + Name of the tool (must be in TOOL_CONFIGS) + + Returns + ------- + np.ndarray + 4x4 homogeneous transformation matrix from flange to tool TCP + + Raises + ------ + ValueError + If tool_name is not recognized + """ + if tool_name not in TOOL_CONFIGS: + raise ValueError(f"Unknown tool '{tool_name}'. Available tools: {list_tools()}") + + return TOOL_CONFIGS[tool_name]["transform"] + + +def list_tools() -> list[str]: + """ + Get list of available tool names. + + Returns + ------- + List[str] + List of available tool configuration names + """ + return list(TOOL_CONFIGS.keys()) + + +def get_tool_info(tool_name: str) -> dict[str, Any]: + """ + Get complete configuration for a tool. + + Parameters + ---------- + tool_name : str + Name of the tool + + Returns + ------- + Dict[str, Any] + Complete tool configuration dictionary + + Raises + ------ + ValueError + If tool_name is not recognized + """ + if tool_name not in TOOL_CONFIGS: + raise ValueError(f"Unknown tool '{tool_name}'. Available tools: {list_tools()}") + + return TOOL_CONFIGS[tool_name] diff --git a/parol6/urdf_model/CMakeLists.txt b/parol6/urdf_model/CMakeLists.txt new file mode 100644 index 0000000..f104b11 --- /dev/null +++ b/parol6/urdf_model/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.5) +project(parol6) + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY urdf launch meshes + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/parol6/urdf_model/config/joint_names_PAROL6.yaml b/parol6/urdf_model/config/joint_names_PAROL6.yaml new file mode 100644 index 0000000..24c6a0e --- /dev/null +++ b/parol6/urdf_model/config/joint_names_PAROL6.yaml @@ -0,0 +1 @@ +controller_joint_names: ['', 'L1', 'L2', 'L3', 'L4', 'L5', 'L6', ] diff --git a/parol6/urdf_model/launch/display.launch b/parol6/urdf_model/launch/display.launch new file mode 100644 index 0000000..f2ee9b1 --- /dev/null +++ b/parol6/urdf_model/launch/display.launch @@ -0,0 +1,20 @@ + + + + + + + diff --git a/parol6/urdf_model/launch/gazebo.launch b/parol6/urdf_model/launch/gazebo.launch new file mode 100644 index 0000000..c6e6819 --- /dev/null +++ b/parol6/urdf_model/launch/gazebo.launch @@ -0,0 +1,20 @@ + + + + + + diff --git a/parol6/urdf_model/meshes/L1.STL b/parol6/urdf_model/meshes/L1.STL new file mode 100644 index 0000000..5e79a76 Binary files /dev/null and b/parol6/urdf_model/meshes/L1.STL differ diff --git a/parol6/urdf_model/meshes/L2.STL b/parol6/urdf_model/meshes/L2.STL new file mode 100644 index 0000000..0cbe487 Binary files /dev/null and b/parol6/urdf_model/meshes/L2.STL differ diff --git a/parol6/urdf_model/meshes/L3.STL b/parol6/urdf_model/meshes/L3.STL new file mode 100644 index 0000000..9eeb293 Binary files /dev/null and b/parol6/urdf_model/meshes/L3.STL differ diff --git a/parol6/urdf_model/meshes/L4.STL b/parol6/urdf_model/meshes/L4.STL new file mode 100644 index 0000000..e5b1d3e Binary files /dev/null and b/parol6/urdf_model/meshes/L4.STL differ diff --git a/parol6/urdf_model/meshes/L5.STL b/parol6/urdf_model/meshes/L5.STL new file mode 100644 index 0000000..1aa19a8 Binary files /dev/null and b/parol6/urdf_model/meshes/L5.STL differ diff --git a/parol6/urdf_model/meshes/L6.STL b/parol6/urdf_model/meshes/L6.STL new file mode 100644 index 0000000..8b89dff Binary files /dev/null and b/parol6/urdf_model/meshes/L6.STL differ diff --git a/parol6/urdf_model/meshes/base_link.STL b/parol6/urdf_model/meshes/base_link.STL new file mode 100644 index 0000000..b91c403 Binary files /dev/null and b/parol6/urdf_model/meshes/base_link.STL differ diff --git a/parol6/urdf_model/meshes/pneumatic_gripper_assembly.STL b/parol6/urdf_model/meshes/pneumatic_gripper_assembly.STL new file mode 100644 index 0000000..84e4b99 Binary files /dev/null and b/parol6/urdf_model/meshes/pneumatic_gripper_assembly.STL differ diff --git a/parol6/urdf_model/package.xml b/parol6/urdf_model/package.xml new file mode 100644 index 0000000..2156818 --- /dev/null +++ b/parol6/urdf_model/package.xml @@ -0,0 +1,23 @@ + + parol6 + 1.0.0 + + URDF Description package for PAROL6. This package contains configuration data, + 3D models, and launch files for the PAROL6 robot. + + + TODO + BSD + + ament_cmake + + robot_state_publisher + rviz2 + joint_state_publisher + joint_state_publisher_gui + gazebo_ros + + + ament_cmake + + diff --git a/parol6/urdf_model/urdf/PAROL6.csv b/parol6/urdf_model/urdf/PAROL6.csv new file mode 100644 index 0000000..74ba98e --- /dev/null +++ b/parol6/urdf_model/urdf/PAROL6.csv @@ -0,0 +1,8 @@ +Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity +base_link,-0.0274562034602567,-0.00137265400595656,0.031237920982338,0,0,0,0.812661291810144,0.00110740279540382,4.67463481205112E-05,8.47873933528831E-06,0.00106582362830227,-2.6194130573118E-06,0.00147883396699374,0,0,0,0,0,0,package://PAROL6/meshes/base_link.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://PAROL6/meshes/base_link.STL,,Assem9^PACKAssem1-1,Coordinate System1,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,, +L1,0.00524529891406311,0.0289722820784871,0.0992791429452917,0,0,0,0.644800830530233,0.00109429409029172,-6.27156874097953E-05,-5.70466862844753E-05,0.00056996197700816,-0.000121686402940313,0.00129487722474286,0,0,0,0,0,0,package://PAROL6/meshes/L1.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://PAROL6/meshes/L1.STL,,Assem5^PACKAssem1-1,Coordinate System1,Axis1,L1,revolute,0,0,0,0,0,0,base_link,0,0,1,0,0,0,0,,,,,,,, +L2,-0.0094181617193695,-0.0783879603613605,0.0369429741417731,0,0,0,0.512334779399788,0.00123357482646866,3.63236238096924E-06,-3.30834557934783E-07,0.000205462733796053,4.86441772103282E-07,0.00132225033266162,0,0,0,0,0,0,package://PAROL6/meshes/L2.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://PAROL6/meshes/L2.STL,,Assem8^PACKAssem1-1,Coordinate System2,Axis2,L2,revolute,0.0234207210610375,0,0.1105,-1.5707963267949,0,0,L1,0,0,1,0,0,0,0,,,,,,,, +L3,0.0155509885239593,-0.0191127686426613,-0.00153322577917236,0,0,0,0.550970997806044,0.00057827573271994,0.000116505232322314,-1.19955293256513E-05,0.000567011676944701,1.55814255036021E-05,0.000815862001506843,0,0,0,0,0,0,package://PAROL6/meshes/L3.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://PAROL6/meshes/L3.STL,,Assem1^PACKAssem1-1,Coordinate System3,Axis3,L3,revolute,0,-0.18,0,3.1416,0,-1.5708,L2,0,0,-1,0,0,0,0,,,,,,,, +L4,0.000947028430634433,-0.008978947112462,-0.0924798659827059,0,0,0,0.39346362633915,0.000371672516830215,-1.05560000666389E-05,-1.56899445418326E-07,0.000332364226344361,-6.10397110934672E-05,0.000250606789571909,0,0,0,0,0,0,package://PAROL6/meshes/L4.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://PAROL6/meshes/L4.STL,,Assem2^PACKAssem1-1,Coordinate System4,Axis4,L4,revolute,0.0435,0,0,1.5707963267949,0,3.14159265358979,L3,0,0,-1,0,0,0,0,,,,,,,, +L5,6.7162171684676E-06,-0.00621420980825221,-0.000960871025916146,0,0,0,0.185171777001718,0.000109176748460021,-1.27195013439061E-07,8.4436049148633E-08,8.41307588934268E-05,-4.41596193640049E-07,0.000110629136236229,0,0,0,0,0,0,package://PAROL6/meshes/L5.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://PAROL6/meshes/L5.STL,,Assem3^PACKAssem1-1,Coordinate System5,Axis5,L5,revolute,0,0,-0.17635,-1.5708,0,0,L4,0,0,-1,0,0,0,0,,,,,,,, +L6,0.00632955087228015,-0.000453856352095022,-0.0554441567386594,0,0,0,0.0703261410665101,1.40276237518287E-05,3.15503246005279E-07,-9.28469598788221E-07,1.81406721166117E-05,-9.43630009030796E-07,2.34608103291027E-05,0,0,0,0,0,0,package://PAROL6/meshes/L6.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://PAROL6/meshes/L6.STL,,Assem7^PACKAssem1-1,Coordinate System6,Axis6,L6,continuous,0,0,0,1.5708,0,0,L5,0,0,-1,0,0,0,0,,,,,,,, diff --git a/parol6/urdf_model/urdf/PAROL6.urdf b/parol6/urdf_model/urdf/PAROL6.urdf new file mode 100644 index 0000000..8dbab00 --- /dev/null +++ b/parol6/urdf_model/urdf/PAROL6.urdf @@ -0,0 +1,514 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + + hardware_interface/PositionJointInterface + + + + hardware_interface/PositionJointInterface + 1 + + + + + transmission_interface/SimpleTransmission + + + hardware_interface/PositionJointInterface + + + + hardware_interface/PositionJointInterface + 1 + + + + + transmission_interface/SimpleTransmission + + + hardware_interface/PositionJointInterface + + + + hardware_interface/PositionJointInterface + 1 + + + + + transmission_interface/SimpleTransmission + + + hardware_interface/PositionJointInterface + + + + hardware_interface/PositionJointInterface + 1 + + + + + transmission_interface/SimpleTransmission + + + hardware_interface/PositionJointInterface + + + + hardware_interface/PositionJointInterface + 1 + + + + + transmission_interface/SimpleTransmission + + + hardware_interface/PositionJointInterface + + + + hardware_interface/PositionJointInterface + 1 + + + + + + / + + + + + true + + + + true + + + + true + + + + true + + + + true + + + + true + + + diff --git a/parol6/utils/errors.py b/parol6/utils/errors.py index 79a18ba..d5b297d 100644 --- a/parol6/utils/errors.py +++ b/parol6/utils/errors.py @@ -3,11 +3,24 @@ Keep this focused and non-redundant; prefer built-ins where appropriate. """ + class IKError(RuntimeError): """Inverse kinematics failure (no solution, constraints violated, etc.).""" - pass + + def __init__(self, message: str): + self.original_message = message + super().__init__(f"IK ERROR: {message}") + + def __str__(self): + return f"IK ERROR: {self.original_message}" class TrajectoryPlanningError(RuntimeError): """Trajectory generation/planning failure.""" - pass + + def __init__(self, message: str): + self.original_message = message + super().__init__(f"Trajectory Planning Error: {message}") + + def __str__(self): + return f"Trajectory Planning Error: {self.original_message}" diff --git a/parol6/utils/frames.py b/parol6/utils/frames.py index c84a0db..217f4f7 100644 --- a/parol6/utils/frames.py +++ b/parol6/utils/frames.py @@ -8,66 +8,79 @@ from __future__ import annotations import logging -from typing import List, Optional, Sequence, Dict, Any, cast +from collections.abc import Sequence +from typing import Any, cast import numpy as np from numpy.typing import NDArray from spatialmath import SE3 -import parol6.PAROL6_ROBOT as PAROL6_ROBOT +from parol6.server.state import get_fkine_se3 logger = logging.getLogger(__name__) # Constants for TRF plane normal vectors -PLANE_NORMALS_TRF: Dict[str, NDArray] = { +PLANE_NORMALS_TRF: dict[str, NDArray] = { "XY": np.array([0.0, 0.0, 1.0]), # Tool's Z-axis "XZ": np.array([0.0, 1.0, 0.0]), # Tool's Y-axis "YZ": np.array([1.0, 0.0, 0.0]), # Tool's X-axis } -def point_trf_to_wrf_mm(point_mm: Sequence[float], tool_pose: SE3) -> NDArray: +def point_trf_to_wrf_mm(point_mm: Sequence[float], tool_pose: SE3) -> list[float]: """Convert 3D point from TRF to WRF coordinates (both in mm).""" point_trf = SE3(point_mm[0] / 1000.0, point_mm[1] / 1000.0, point_mm[2] / 1000.0) point_wrf = cast(SE3, tool_pose * point_trf) - return point_wrf.t * 1000.0 + return (point_wrf.t * 1000.0).tolist() -def pose6_trf_to_wrf(pose6_mm_deg: Sequence[float], tool_pose: SE3) -> NDArray: +def pose6_trf_to_wrf(pose6_mm_deg: Sequence[float], tool_pose: SE3) -> list[float]: """Convert 6D pose [x,y,z,rx,ry,rz] from TRF to WRF (mm, degrees).""" - pose_trf = SE3(pose6_mm_deg[0] / 1000.0, pose6_mm_deg[1] / 1000.0, pose6_mm_deg[2] / 1000.0) * SE3.RPY( - pose6_mm_deg[3:], unit="deg", order="xyz" - ) + pose_trf = SE3( + pose6_mm_deg[0] / 1000.0, pose6_mm_deg[1] / 1000.0, pose6_mm_deg[2] / 1000.0 + ) * SE3.RPY(pose6_mm_deg[3:], unit="deg", order="xyz") pose_wrf = cast(SE3, tool_pose * pose_trf) - return np.concatenate([pose_wrf.t * 1000.0, pose_wrf.rpy(unit="deg", order="xyz")]) + return np.concatenate( + [pose_wrf.t * 1000.0, pose_wrf.rpy(unit="deg", order="xyz")] + ).tolist() -def se3_to_pose6_mm_deg(T: SE3) -> NDArray: +def se3_to_pose6_mm_deg(T: SE3) -> list[float]: """Convert SE3 transform to 6D pose [x,y,z,rx,ry,rz] (mm, degrees).""" - return np.concatenate([T.t * 1000.0, T.rpy(unit="deg", order="xyz")]) + return np.concatenate([T.t * 1000.0, T.rpy(unit="deg", order="xyz")]).tolist() -def transform_center_trf_to_wrf(params: Dict[str, Any], tool_pose: SE3, transformed: Dict[str, Any]) -> None: +def transform_center_trf_to_wrf( + params: dict[str, Any], tool_pose: SE3, transformed: dict[str, Any] +) -> None: """Transform 'center' parameter from TRF (mm) to WRF (mm) using tool_pose.""" - center_trf = SE3(params["center"][0] / 1000.0, params["center"][1] / 1000.0, params["center"][2] / 1000.0) + center_trf = SE3( + params["center"][0] / 1000.0, + params["center"][1] / 1000.0, + params["center"][2] / 1000.0, + ) center_wrf = cast(SE3, tool_pose * center_trf) - transformed["center"] = center_wrf.t * 1000.0 + transformed["center"] = (center_wrf.t * 1000.0).tolist() def transform_start_pose_if_needed( - start_pose: Optional[Sequence[float]], frame: str, current_position_in: np.ndarray -) -> Optional[NDArray]: + start_pose: Sequence[float] | None, frame: str, current_position_in: np.ndarray +) -> list[float] | None: """Transform start_pose from TRF to WRF if needed.""" if frame == "TRF" and start_pose: - current_q = PAROL6_ROBOT.ops.steps_to_rad(current_position_in) - tool_pose = PAROL6_ROBOT.robot.fkine(current_q) + tool_pose = get_fkine_se3() return pose6_trf_to_wrf(start_pose, tool_pose) - return np.asarray(start_pose, dtype=float) if start_pose is not None else None + return ( + np.asarray(start_pose, dtype=float).tolist() if start_pose is not None else None + ) def transform_command_params_to_wrf( - command_type: str, params: Dict[str, Any], frame: str, current_position_in: np.ndarray -) -> Dict[str, Any]: + command_type: str, + params: dict[str, Any], + frame: str, + current_position_in: np.ndarray, +) -> dict[str, Any]: """ Transform command parameters from TRF to WRF. Handles position, orientation, and directional vectors correctly. @@ -76,8 +89,7 @@ def transform_command_params_to_wrf( return params # Get current tool pose - current_q = PAROL6_ROBOT.ops.steps_to_rad(current_position_in) - tool_pose = PAROL6_ROBOT.robot.fkine(current_q) + tool_pose = get_fkine_se3() transformed = params.copy() @@ -89,7 +101,7 @@ def transform_command_params_to_wrf( if "plane" in params: normal_trf = PLANE_NORMALS_TRF[params["plane"]] normal_wrf = tool_pose.R @ normal_trf - transformed["normal_vector"] = normal_wrf + transformed["normal_vector"] = normal_wrf.tolist() logger.info(f" -> TRF circle plane {params['plane']} transformed to WRF") # SMOOTH_ARC_CENTER - Transform center, end_pose, and implied plane @@ -103,7 +115,7 @@ def transform_command_params_to_wrf( if "plane" in params: normal_trf = PLANE_NORMALS_TRF[params["plane"]] normal_wrf = tool_pose.R @ normal_trf - transformed["normal_vector"] = normal_wrf + transformed["normal_vector"] = normal_wrf.tolist() # SMOOTH_ARC_PARAM - Transform end_pose and arc plane elif command_type == "SMOOTH_ARC_PARAM": @@ -116,7 +128,7 @@ def transform_command_params_to_wrf( normal_trf = PLANE_NORMALS_TRF[params.get("plane", "XY")] normal_wrf = tool_pose.R @ normal_trf - transformed["normal_vector"] = normal_wrf + transformed["normal_vector"] = normal_wrf.tolist() # SMOOTH_HELIX - Transform center and helix axis elif command_type == "SMOOTH_HELIX": @@ -126,12 +138,12 @@ def transform_command_params_to_wrf( # Transform helix axis (default is Z-axis of tool) axis_trf = np.array([0.0, 0.0, 1.0]) # Tool's Z-axis axis_wrf = tool_pose.R @ axis_trf - transformed["helix_axis"] = axis_wrf + transformed["helix_axis"] = axis_wrf.tolist() # Transform up vector (default is Y-axis of tool) up_trf = np.array([0.0, 1.0, 0.0]) # Tool's Y-axis up_wrf = tool_pose.R @ up_trf - transformed["up_vector"] = up_wrf + transformed["up_vector"] = up_wrf.tolist() # SMOOTH_SPLINE - Transform waypoints elif command_type == "SMOOTH_SPLINE": @@ -160,24 +172,28 @@ def transform_command_params_to_wrf( if "center" in seg: # Create a temporary params dict to use the helper seg_params = {"center": seg["center"]} - transform_center_trf_to_wrf(seg_params, tool_pose, seg_transformed) + transform_center_trf_to_wrf( + seg_params, tool_pose, seg_transformed + ) # Transform plane normal if specified if "plane" in seg: normal_trf = PLANE_NORMALS_TRF[seg["plane"]] normal_wrf = tool_pose.R @ normal_trf - seg_transformed["normal_vector"] = normal_wrf + seg_transformed["normal_vector"] = normal_wrf.tolist() elif seg["type"] == "CIRCLE": if "center" in seg: # Create a temporary params dict to use the helper seg_params = {"center": seg["center"]} - transform_center_trf_to_wrf(seg_params, tool_pose, seg_transformed) + transform_center_trf_to_wrf( + seg_params, tool_pose, seg_transformed + ) if "plane" in seg: normal_trf = PLANE_NORMALS_TRF[seg["plane"]] normal_wrf = tool_pose.R @ normal_trf - seg_transformed["normal_vector"] = normal_wrf + seg_transformed["normal_vector"] = normal_wrf.tolist() elif seg["type"] == "SPLINE": if "waypoints" in seg: diff --git a/parol6/utils/ik.py b/parol6/utils/ik.py index fde98f3..ca45e28 100644 --- a/parol6/utils/ik.py +++ b/parol6/utils/ik.py @@ -3,44 +3,44 @@ Shared functions used by multiple command classes for inverse kinematics calculations. """ -import numpy as np import logging -from collections import namedtuple +from collections.abc import Sequence +from typing import NamedTuple + +import numpy as np +from numpy.typing import NDArray from roboticstoolbox import DHRobot from spatialmath import SE3 -from spatialmath.base import trinterp + import parol6.PAROL6_ROBOT as PAROL6_ROBOT logger = logging.getLogger(__name__) -# Global variable to track previous tolerance for logging changes -_prev_tolerance = None - -# --- Wrapper class to make integers mutable when passed to functions --- -class CommandValue: - def __init__(self, value): - self.value = value - # This dictionary maps descriptive axis names to movement vectors # Format: ([x, y, z], [rx, ry, rz]) AXIS_MAP = { - 'X+': ([1, 0, 0], [0, 0, 0]), 'X-': ([-1, 0, 0], [0, 0, 0]), - 'Y+': ([0, 1, 0], [0, 0, 0]), 'Y-': ([0, -1, 0], [0, 0, 0]), - 'Z+': ([0, 0, 1], [0, 0, 0]), 'Z-': ([0, 0, -1], [0, 0, 0]), - 'RX+': ([0, 0, 0], [1, 0, 0]), 'RX-': ([0, 0, 0], [-1, 0, 0]), - 'RY+': ([0, 0, 0], [0, 1, 0]), 'RY-': ([0, 0, 0], [0, -1, 0]), - 'RZ+': ([0, 0, 0], [0, 0, 1]), 'RZ-': ([0, 0, 0], [0, 0, -1]), + "X+": ([1, 0, 0], [0, 0, 0]), + "X-": ([-1, 0, 0], [0, 0, 0]), + "Y+": ([0, 1, 0], [0, 0, 0]), + "Y-": ([0, -1, 0], [0, 0, 0]), + "Z+": ([0, 0, 1], [0, 0, 0]), + "Z-": ([0, 0, -1], [0, 0, 0]), + "RX+": ([0, 0, 0], [1, 0, 0]), + "RX-": ([0, 0, 0], [-1, 0, 0]), + "RY+": ([0, 0, 0], [0, 1, 0]), + "RY-": ([0, 0, 0], [0, -1, 0]), + "RZ+": ([0, 0, 0], [0, 0, 1]), + "RZ-": ([0, 0, 0], [0, 0, -1]), } -def normalize_angle(angle): - """Normalize angle(s) to [-pi, pi] range (supports scalar or ndarray).""" - a = np.asarray(angle, dtype=float) - a = (a + np.pi) % (2 * np.pi) - np.pi - return a.item() if np.isscalar(angle) else a -def unwrap_angles(q_solution, q_current): +def unwrap_angles( + q_solution: Sequence[float] | NDArray[np.float64], + q_current: Sequence[float] | NDArray[np.float64], +) -> NDArray[np.float64]: """ Vectorized unwrap: bring solution angles near current by adding/subtracting 2*pi. + This minimizes joint motion between consecutive configurations. """ qs = np.asarray(q_solution, dtype=float) qc = np.asarray(q_current, dtype=float) @@ -50,90 +50,25 @@ def unwrap_angles(q_solution, q_current): q_unwrapped[diff < -np.pi] += 2 * np.pi return q_unwrapped -IKResult = namedtuple('IKResult', 'success q iterations residual tolerance_used violations') -def calculate_adaptive_tolerance(robot, q, strict_tol=1e-10, loose_tol=1e-7): - """ - Calculate adaptive tolerance based on proximity to singularities. - Near singularities: looser tolerance for easier convergence. - Away from singularities: stricter tolerance for precise solutions. - """ - global _prev_tolerance - - q_array = np.asarray(q, dtype=float) - # Manipulability for singularity detection (scalar) - manip = float(robot.manipulability(q_array)) - singularity_threshold = 1e-3 - - ratio = manip / singularity_threshold if singularity_threshold > 0.0 else 1.0 - sing_normalized = float(np.clip(ratio, 0.0, 1.0)) - adaptive_tol = float(loose_tol + (strict_tol - loose_tol) * sing_normalized) - - # Log tolerance changes (only if DEBUG enabled and significant change) - prev = _prev_tolerance if _prev_tolerance is not None else adaptive_tol - if _prev_tolerance is None or abs(adaptive_tol - prev) > 0.5 * abs(prev): - if logger.isEnabledFor(logging.DEBUG): - tol_category = "LOOSE" if adaptive_tol > 1e-7 else "MODERATE" if adaptive_tol > 5e-10 else "STRICT" - logger.debug( - "Adaptive IK tolerance: %.2e (%s) - Manipulability: %.8f (threshold: %.3g)", - adaptive_tol, tol_category, manip, singularity_threshold - ) - _prev_tolerance = adaptive_tol +class IKResult(NamedTuple): + success: bool + q: NDArray[np.float64] | None + iterations: int + residual: float + violations: str | None - return adaptive_tol -def calculate_configuration_dependent_max_reach(q_seed): +def solve_ik( + robot: DHRobot, + target_pose: SE3, + current_q: Sequence[float] | NDArray[np.float64], + jogging: bool = False, + safety_margin_rad: float = 0.03, + quiet_logging: bool = False, +) -> IKResult: """ - Calculate maximum reach based on joint configuration, particularly joint 5. - When joint 5 is at 90 degrees, the effective reach is reduced by approximately 0.045. - - Parameters - ---------- - q_seed : array_like - Current joint configuration in radians - - Returns - ------- - float - Configuration-dependent maximum reach threshold - """ - base_max_reach = 0.44 # Base maximum reach from experimentation - - j5_angle = q_seed[4] if len(q_seed) > 4 else 0.0 - j5_normalized = normalize_angle(j5_angle) - angle_90_deg = np.pi / 2 - angle_neg_90_deg = -np.pi / 2 - dist_from_90 = abs(j5_normalized - angle_90_deg) - dist_from_neg_90 = abs(j5_normalized - angle_neg_90_deg) - dist_from_90_deg = min(dist_from_90, dist_from_neg_90) - reduction_range = np.pi / 4 # 45 degrees - if dist_from_90_deg <= reduction_range: - # Reach reduction near J5 90° positions - proximity_factor = 1.0 - (dist_from_90_deg / reduction_range) - reach_reduction = 0.045 * proximity_factor - effective_max_reach = base_max_reach - reach_reduction - - return effective_max_reach - else: - return base_max_reach - -def solve_ik_with_adaptive_tol_subdivision( - robot: DHRobot, - target_pose: SE3, - current_q, - current_pose: SE3 | None = None, - max_depth: int = 4, - ilimit: int = 100, - jogging: bool = False -): - """ - Uses adaptive tolerance based on proximity to singularities: - - Near singularities: looser tolerance for easier convergence - - Away from singularities: stricter tolerance for precise solutions - If necessary, recursively subdivide the motion until ikine_LMS converges - on every segment. Finally check that solution respects joint limits. From experimentation, - jogging with lower tolerances often produces q_paths that do not differ from current_q, - essentially freezing the robot. + IK solver Parameters ---------- @@ -142,89 +77,96 @@ def solve_ik_with_adaptive_tol_subdivision( target_pose : SE3 Target pose to reach current_q : array_like - Current joint configuration - current_pose : SE3, optional - Current pose (computed if None) - max_depth : int, optional - Maximum subdivision depth (default: 4) - ilimit : int, optional - Maximum iterations for IK solver (default: 100) + Current joint configuration in radians + jogging : bool, optional + If True, use very strict tolerance for jogging (default: False) + safety_margin_rad : float, optional + Buffer distance (radians) from joint limits (default: 0.03) Returns ------- IKResult - success - True/False - q_path - (mxn) array of the final joint configuration - iterations, residual - aggregated diagnostics - tolerance_used - which tolerance was used - violations - joint limit violations (if any) + success - True if solution found + q - Joint configuration in radians (or None if failed) + iterations - Number of iterations used + residual - Final error value + tolerance_used - Tolerance used for convergence + violations - Error message if failed, None if successful """ - if current_pose is None: - current_pose = robot.fkine(current_q) - - # ── inner recursive solver─────────────────── - def _solve(Ta: SE3, Tb: SE3, q_seed, depth, tol): - """Return (path_list, success_flag, iterations, residual).""" - # Workspace reach analysis - current_reach = np.linalg.norm(Ta.t) - target_reach = np.linalg.norm(Tb.t) - - # Inward motion detection for recovery mode - is_recovery = target_reach < current_reach - - # J5-dependent maximum reach threshold - max_reach_threshold = calculate_configuration_dependent_max_reach(q_seed) - - # Adaptive damping for IK convergence - if is_recovery: - # Recovery mode - always use low damping - damping = 0.0000001 - else: - # Workspace limit validation - if not is_recovery and target_reach > max_reach_threshold: - logger.warning(f"Target reach limit exceeded: {target_reach:.3f} > {max_reach_threshold:.3f}") - return [], False, depth, 0 - else: - damping = 0.0000001 # Normal low damping - - res = robot.ikine_LMS(Tb, q0=q_seed, ilimit=ilimit, tol=tol, wN=damping) - if res.success: - q_good = unwrap_angles(res.q, q_seed) # << unwrap vs *previous* - return [q_good], True, res.iterations, res.residual - - if depth >= max_depth: - return [], False, res.iterations, res.residual - # split the segment and recurse - Tc = SE3(trinterp(Ta.A, Tb.A, 0.5)) # mid-pose (screw interp) - - left_path, ok_L, it_L, r_L = _solve(Ta, Tc, q_seed, depth+1, tol) - if not ok_L: - return [], False, it_L, r_L - - q_mid = left_path[-1] # last solved joint set - right_path, ok_R, it_R, r_R = _solve(Tc, Tb, q_mid, depth+1, tol) - - return ( - left_path + right_path, - ok_R, - it_L + it_R, - r_R - ) + cq: NDArray[np.float64] = np.asarray(current_q, dtype=np.float64) + result = robot.ets().ik_LM( + target_pose, q0=cq, tol=1e-10, joint_limits=True, k=0.0, method="sugihara" + ) + q = result[0] + success = result[1] > 0 + iterations = result[2] + remaining = result[3] - # ── kick-off with adaptive tolerance ────────────────────────────────── - if jogging: - adaptive_tol = 1e-10 - else: - adaptive_tol = calculate_adaptive_tolerance(robot, current_q) - path, ok, its, resid = _solve(current_pose, target_pose, current_q, 0, adaptive_tol) - # Joint limit constraint validation - target_q = path[-1] if len(path) != 0 else None - solution_valid = PAROL6_ROBOT.check_limits(current_q, target_q, allow_recovery=True, log=True) violations = None - if ok and solution_valid: - return IKResult(True, path[-1], its, resid, adaptive_tol, violations) + + if success and jogging: + # Vectorized safety validation with recovery support + qlim = robot.qlim + buffered_min = qlim[0, :] + safety_margin_rad + buffered_max = qlim[1, :] - safety_margin_rad + + # Check which joints were in danger zone (beyond buffer) + in_danger_old = (current_q < buffered_min) | (current_q > buffered_max) + + # Compute distance from nearest limit for all joints + dist_old_lower = np.abs(current_q - qlim[0, :]) + dist_old_upper = np.abs(current_q - qlim[1, :]) + dist_old = np.minimum(dist_old_lower, dist_old_upper) + + dist_new_lower = np.abs(q - qlim[0, :]) + dist_new_upper = np.abs(q - qlim[1, :]) + dist_new = np.minimum(dist_new_lower, dist_new_upper) + + # Check for recovery violations (was in danger, moved closer to limit) + recovery_violations = in_danger_old & (dist_new < dist_old) + + # Check for safety violations (was safe, left buffer zone) + in_danger_new = (q < buffered_min) | (q > buffered_max) + safety_violations = (~in_danger_old) & in_danger_new + + # Report first violation found + if np.any(recovery_violations): + idx = np.argmax(recovery_violations) + success = False + violations = ( + f"J{idx + 1} moving further into danger zone (recovery blocked)" + ) + if not quiet_logging: + logger.warning(violations) + elif np.any(safety_violations): + idx = np.argmax(safety_violations) + success = False + violations = f"J{idx + 1} would leave safe zone (buffer violated)" + if not quiet_logging: + logger.warning(violations) + + if success: + # Valid solution - apply unwrapping to minimize joint motion + q_unwrapped = unwrap_angles(q, current_q) + + # Verify unwrapped solution still within actual limits + within_limits = PAROL6_ROBOT.check_limits( + current_q, q_unwrapped, allow_recovery=True, log=not quiet_logging + ) + + if within_limits: + q = q_unwrapped + # else: use original result.q which already passed library's limit check else: - return IKResult(False, None, its, resid, adaptive_tol, violations) + violations = "IK failed to solve." + return IKResult( + success=success, + q=q if success else None, + iterations=iterations, + residual=remaining, + violations=violations, + ) + def quintic_scaling(s: float) -> float: """ diff --git a/parol6/utils/trajectory.py b/parol6/utils/trajectory.py index 5268d2c..ef75193 100644 --- a/parol6/utils/trajectory.py +++ b/parol6/utils/trajectory.py @@ -2,7 +2,8 @@ Shared trajectory planning utilities. """ -from typing import Sequence, Tuple +from collections.abc import Sequence + import numpy as np from parol6.config import CONTROL_RATE_HZ @@ -75,7 +76,9 @@ def plan_linear_cubic( return traj -def _trapezoid_timings(distance: float, v_max: float, a_max: float) -> Tuple[float, float, float, float, bool]: +def _trapezoid_timings( + distance: float, v_max: float, a_max: float +) -> tuple[float, float, float, float, bool]: """ Compute trapezoid or triangular profile timing. @@ -155,7 +158,12 @@ def plan_trapezoid_position_1d( pos[i] = (0.5 * a_max * t_a**2) + v_max * (ti - t_a) else: td = ti - (t_a + t_c) - pos[i] = (0.5 * a_max * t_a**2) + v_max * t_c + v_peak * td - 0.5 * a_max * td**2 + pos[i] = ( + (0.5 * a_max * t_a**2) + + v_max * t_c + + v_peak * td + - 0.5 * a_max * td**2 + ) # Clamp last sample to exact L to avoid drift pos[-1] = L diff --git a/pyproject.toml b/pyproject.toml index 98a38ba..372af52 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -7,12 +7,34 @@ name = "parol6" version = "0.1.0" description = "Python library for controlling PAROL6 robot arms" -requires-python = ">=3.11" +requires-python = ">=3.10,<3.12" dependencies = [ - "numpy==1.26.4", + # TEMPORARY SOLUTION: Using custom-built robotics-toolbox-python wheels from forked repository + # The proper solution requires the upstream maintainers to complete the merge of their "future" branch. + # Until then, we use platform-specific wheels from: https://github.com/Jepson2k/robotics-toolbox-python/releases/tag/v1.2.2 + + # macOS ARM64 (Apple Silicon) + "roboticstoolbox-python @ https://github.com/Jepson2k/robotics-toolbox-python/releases/download/v1.2.2/roboticstoolbox_python-1.2.2-cp310-cp310-macosx_11_0_arm64.whl ; python_version == '3.10' and platform_system == 'Darwin' and platform_machine == 'arm64'", + "roboticstoolbox-python @ https://github.com/Jepson2k/robotics-toolbox-python/releases/download/v1.2.2/roboticstoolbox_python-1.2.2-cp311-cp311-macosx_11_0_arm64.whl ; python_version == '3.11' and platform_system == 'Darwin' and platform_machine == 'arm64'", + + # macOS x86_64 (Intel) + "roboticstoolbox-python @ https://github.com/Jepson2k/robotics-toolbox-python/releases/download/v1.2.2/roboticstoolbox_python-1.2.2-cp310-cp310-macosx_11_0_x86_64.whl ; python_version == '3.10' and platform_system == 'Darwin' and platform_machine == 'x86_64'", + "roboticstoolbox-python @ https://github.com/Jepson2k/robotics-toolbox-python/releases/download/v1.2.2/roboticstoolbox_python-1.2.2-cp311-cp311-macosx_11_0_x86_64.whl ; python_version == '3.11' and platform_system == 'Darwin' and platform_machine == 'x86_64'", + + # Windows AMD64 + "roboticstoolbox-python @ https://github.com/Jepson2k/robotics-toolbox-python/releases/download/v1.2.2/roboticstoolbox_python-1.2.2-cp310-cp310-win_amd64.whl ; python_version == '3.10' and platform_system == 'Windows' and platform_machine == 'AMD64'", + "roboticstoolbox-python @ https://github.com/Jepson2k/robotics-toolbox-python/releases/download/v1.2.2/roboticstoolbox_python-1.2.2-cp311-cp311-win_amd64.whl ; python_version == '3.11' and platform_system == 'Windows' and platform_machine == 'AMD64'", + + # Linux x86_64 + "roboticstoolbox-python @ https://github.com/Jepson2k/robotics-toolbox-python/releases/download/v1.2.2/roboticstoolbox_python-1.2.2-cp310-cp310-manylinux2014_x86_64.manylinux_2_17_x86_64.whl ; python_version == '3.10' and platform_system == 'Linux' and platform_machine == 'x86_64'", + "roboticstoolbox-python @ https://github.com/Jepson2k/robotics-toolbox-python/releases/download/v1.2.2/roboticstoolbox_python-1.2.2-cp311-cp311-manylinux2014_x86_64.manylinux_2_17_x86_64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'x86_64'", + + # Linux aarch64 (ARM64) + "roboticstoolbox-python @ https://github.com/Jepson2k/robotics-toolbox-python/releases/download/v1.2.2/roboticstoolbox_python-1.2.2-cp310-cp310-manylinux2014_aarch64.manylinux_2_17_aarch64.whl ; python_version == '3.10' and platform_system == 'Linux' and platform_machine == 'aarch64'", + "roboticstoolbox-python @ https://github.com/Jepson2k/robotics-toolbox-python/releases/download/v1.2.2/roboticstoolbox_python-1.2.2-cp311-cp311-manylinux2014_aarch64.manylinux_2_17_aarch64.whl ; python_version == '3.11' and platform_system == 'Linux' and platform_machine == 'aarch64'", + "pyserial", "spatialmath-python", - "roboticstoolbox-python==1.0.3", "scipy==1.11.4" ] @@ -20,7 +42,13 @@ dependencies = [ include = ["parol6*"] [project.optional-dependencies] -dev = ["ruff", "mypy", "pytest", "pytest-asyncio", "pre-commit"] +dev = [ + "ruff", + "mypy", + "pytest", + "pytest-asyncio", + "pre-commit" +] [project.scripts] parol6-server = "parol6.cli.server:main_entry" @@ -74,3 +102,19 @@ filterwarnings = [ [[tool.mypy.overrides]] module = ["roboticstoolbox", "roboticstoolbox.*", "spatialmath", "spatialmath.*"] ignore_missing_imports = true + +[[tool.mypy.overrides]] +module = ["scipy", "scipy.*", "serial"] +ignore_missing_imports = true + +[tool.setuptools] +include-package-data = true + +[tool.setuptools.package-data] +parol6 = ["urdf_model/**"] + +# ---------------- Mypy configuration ---------------- +[tool.mypy] +python_version = "3.11" +files = ["parol6"] +exclude = "(?x)(^build/|^dist/|^\\.venv/)" diff --git a/requirements-test.txt b/requirements-test.txt deleted file mode 100644 index b7592f1..0000000 --- a/requirements-test.txt +++ /dev/null @@ -1,15 +0,0 @@ -# Core testing framework -pytest>=7.0.0 -pytest-timeout>=2.1.0 -pytest-xdist>=3.0.0 -pytest-rerunfailures>=11.0.0 - -# Type checking support -typing-extensions>=4.0.0 - -# Hardware communication -pyserial>=3.5 - -# Additional test utilities -mock>=4.0.0 -pytest-mock>=3.10.0 diff --git a/tests/conftest.py b/tests/conftest.py index 799cd65..64a3fc0 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -5,14 +5,15 @@ environment configuration, and test utilities used across the test suite. """ +import logging import os +import signal import sys -import pytest import time -from typing import Generator, Dict, Optional +from collections.abc import Generator from dataclasses import dataclass -import logging -import signal + +import pytest # Add the parent directory to Python path so we can import the API modules sys.path.insert(0, os.path.dirname(os.path.dirname(__file__))) @@ -20,38 +21,42 @@ # Import parol6 for server management from parol6.client.manager import ServerManager + # Import utilities for port detection def find_available_ports(start_port: int = 5001, count: int = 2) -> list[int]: """Simple fallback port finder if utils import fails.""" import socket + available_ports: list[int] = [] current_port = start_port - + while len(available_ports) < count: try: with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: - sock.bind(('127.0.0.1', current_port)) + sock.bind(("127.0.0.1", current_port)) available_ports.append(current_port) except OSError: # Port in use, reset search if we were building a consecutive sequence available_ports.clear() - + current_port += 1 - + # Prevent infinite loop if current_port > start_port + 1000: break - + return available_ports + logger = logging.getLogger(__name__) @dataclass class TestPorts: """Configuration for test server ports.""" + server_ip: str = "127.0.0.1" - server_port: int = 5001 + server_port: int = 5001 ack_port: int = 5002 @@ -59,39 +64,40 @@ class TestPorts: # PYTEST COMMAND LINE OPTIONS # ============================================================================ + def pytest_addoption(parser): """Add custom command line options for the test suite.""" parser.addoption( "--run-hardware", action="store_true", default=False, - help="Enable hardware tests that require actual robot hardware and human confirmation" + help="Enable hardware tests that require actual robot hardware and human confirmation", ) parser.addoption( "--server-ip", action="store", default="127.0.0.1", - help="IP address for test server communication" + help="IP address for test server communication", ) parser.addoption( - "--server-port", + "--server-port", action="store", type=int, default=None, - help="Port for robot server communication (auto-detected if not specified)" + help="Port for robot server communication (auto-detected if not specified)", ) parser.addoption( "--ack-port", - action="store", + action="store", type=int, default=None, - help="Port for acknowledgment communication (auto-detected if not specified)" + help="Port for acknowledgment communication (auto-detected if not specified)", ) parser.addoption( "--keep-server-running", action="store_true", default=False, - help="Keep the test server running between test sessions for debugging" + help="Keep the test server running between test sessions for debugging", ) @@ -99,49 +105,47 @@ def pytest_addoption(parser): # PORT MANAGEMENT FIXTURE # ============================================================================ + @pytest.fixture(scope="session") def ports(request) -> TestPorts: """ Provide test port configuration. - + Automatically finds available ports if not specified via command line. Ensures ports don't conflict with existing services. """ server_ip = request.config.getoption("--server-ip") server_port = request.config.getoption("--server-port") ack_port = request.config.getoption("--ack-port") - + # Auto-detect available ports if not specified if server_port is None or ack_port is None: logger.info("Auto-detecting available ports...") available_ports = find_available_ports(start_port=5001, count=2) - + if len(available_ports) < 2: pytest.fail("Could not find 2 consecutive available ports for testing") - + if server_port is None: server_port = available_ports[0] if ack_port is None: ack_port = available_ports[1] - + logger.info(f"Using auto-detected ports: server={server_port}, ack={ack_port}") - - return TestPorts( - server_ip=server_ip, - server_port=server_port, - ack_port=ack_port - ) + + return TestPorts(server_ip=server_ip, server_port=server_port, ack_port=ack_port) # ============================================================================ -# ENVIRONMENT CONFIGURATION FIXTURE +# ENVIRONMENT CONFIGURATION FIXTURE # ============================================================================ + @pytest.fixture(scope="session") -def robot_api_env(ports: TestPorts) -> Generator[Dict[str, str], None, None]: +def robot_api_env(ports: TestPorts) -> Generator[dict[str, str], None, None]: """ Configure environment variables for robot_api client to use test ports. - + Sets environment variables so that robot_api.py will connect to the test server instead of the default production server. """ @@ -151,13 +155,13 @@ def robot_api_env(ports: TestPorts) -> Generator[Dict[str, str], None, None]: "PAROL6_CONTROLLER_IP": ports.server_ip, "PAROL6_CONTROLLER_PORT": str(ports.server_port), } - + for key, value in env_vars.items(): original_env[key] = os.environ.get(key) os.environ[key] = value - + logger.debug(f"Set test environment: {env_vars}") - + try: yield env_vars finally: @@ -174,67 +178,66 @@ def robot_api_env(ports: TestPorts) -> Generator[Dict[str, str], None, None]: # SERVER PROCESS FIXTURE # ============================================================================ + @pytest.fixture(scope="session") def server_proc(request, ports: TestPorts, robot_api_env): """ Launch parol6 server for integration tests using ServerManager. - + Starts the server with FAKE_SERIAL mode and waits for readiness. Automatically cleans up the server when tests complete. """ import asyncio import socket - + keep_running = request.config.getoption("--keep-server-running") - + # Create server manager manager = ServerManager() - + async def start_and_wait(): # Start the controller process - await manager.start_controller( + manager.start_controller( no_autohome=True, extra_env={ - "PAROL6_FAKE_SERIAL": "1", + "PAROL6_FAKE_SERIAL": "1", "PAROL6_NOAUTOHOME": "1", "PAROL6_CONTROLLER_IP": ports.server_ip, "PAROL6_CONTROLLER_PORT": str(ports.server_port), - } + }, ) - + # Wait for server to be ready with custom ping logic timeout = 10.0 start_time = time.time() - + while time.time() - start_time < timeout: try: with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: sock.settimeout(1.0) sock.sendto(b"PING", (ports.server_ip, ports.server_port)) data, _ = sock.recvfrom(256) - if data.decode('utf-8').strip().startswith("PONG"): + if data.decode("utf-8").strip().startswith("PONG"): return True - except (socket.timeout, Exception): + except (TimeoutError, Exception): pass await asyncio.sleep(0.5) return False - + # Start server using parol6's ServerManager logger.info(f"Starting test server on {ports.server_ip}:{ports.server_port}") - + ready = asyncio.run(start_and_wait()) if not ready: pytest.fail("Failed to start headless commander server for testing") - + try: yield manager - + finally: if not keep_running: logger.info("Stopping test server") - async def stop_server(): - await manager.stop_controller() - asyncio.run(stop_server()) + manager.stop_controller() else: logger.info("Leaving test server running (--keep-server-running)") @@ -243,63 +246,66 @@ async def stop_server(): # HARDWARE TEST SUPPORT # ============================================================================ + @pytest.fixture def human_prompt(request): """ Provide human confirmation prompts for hardware tests. - + Automatically skips tests marked with @pytest.mark.hardware unless --run-hardware is specified. For enabled hardware tests, provides a utility function to prompt for human confirmation. """ # Check if hardware tests are enabled run_hardware = request.config.getoption("--run-hardware") - + # Skip hardware tests if not enabled if request.node.get_closest_marker("hardware") and not run_hardware: pytest.skip("Hardware tests disabled. Use --run-hardware to enable.") - - def prompt_user(message: str, timeout: Optional[float] = None) -> bool: + + def prompt_user(message: str, timeout: float | None = None) -> bool: """ Prompt user for confirmation during hardware tests. - + Args: message: Message to display to user timeout: Optional timeout in seconds - + Returns: True if user confirms, False otherwise """ if not run_hardware: return False - - print(f"\n{'='*60}") + + print(f"\n{'=' * 60}") print("HARDWARE TEST CONFIRMATION REQUIRED") - print(f"{'='*60}") + print(f"{'=' * 60}") print(f"{message}") - print(f"{'='*60}") - + print(f"{'=' * 60}") + try: if timeout: + def timeout_handler(signum, frame): raise TimeoutError("User confirmation timeout") + signal.signal(signal.SIGALRM, timeout_handler) signal.alarm(int(timeout)) - + response = input("Continue? [y/N]: ").strip().lower() - + if timeout: signal.alarm(0) # Cancel timeout - - return response in ['y', 'yes'] - + + return response in ["y", "yes"] + except (KeyboardInterrupt, TimeoutError): print("\nUser confirmation cancelled or timed out") return False except Exception as e: print(f"\nError getting user confirmation: {e}") return False - + return prompt_user @@ -307,23 +313,25 @@ def timeout_handler(signum, frame): # COMMON TEST UTILITIES # ============================================================================ + @pytest.fixture def temp_env(): """ Provide temporary environment variable context manager. - + Useful for tests that need to modify environment variables temporarily. """ + class TempEnv: def __init__(self): self.original = {} - + def set(self, key: str, value: str): """Set an environment variable temporarily.""" if key not in self.original: self.original[key] = os.environ.get(key) os.environ[key] = value - + def restore(self): """Restore all modified environment variables.""" for key, original_value in self.original.items(): @@ -332,7 +340,7 @@ def restore(self): else: os.environ[key] = original_value self.original.clear() - + temp = TempEnv() try: yield temp @@ -344,29 +352,29 @@ def restore(self): def mock_time(monkeypatch): """ Provide controllable time mocking for tests that depend on timing. - + Useful for testing timeout behavior without actually waiting. """ import time - + class MockTime: def __init__(self): self.current_time = time.time() - + def time(self): return self.current_time - + def advance(self, seconds: float): """Advance the mock time by the specified number of seconds.""" self.current_time += seconds - + def sleep(self, seconds: float): """Mock sleep that just advances time.""" self.advance(seconds) - + mock = MockTime() - monkeypatch.setattr(time, 'time', mock.time) - monkeypatch.setattr(time, 'sleep', mock.sleep) + monkeypatch.setattr(time, "time", mock.time) + monkeypatch.setattr(time, "sleep", mock.sleep) return mock @@ -374,25 +382,30 @@ def sleep(self, seconds: float): # PYTEST CONFIGURATION HOOKS # ============================================================================ + def pytest_configure(config): """Configure pytest with custom markers.""" config.addinivalue_line( "markers", "unit: Unit tests that test individual components in isolation" ) config.addinivalue_line( - "markers", "integration: Integration tests that test component interactions with FAKE_SERIAL" + "markers", + "integration: Integration tests that test component interactions with FAKE_SERIAL", ) config.addinivalue_line( - "markers", "hardware: Hardware tests that require actual robot hardware and human confirmation" + "markers", + "hardware: Hardware tests that require actual robot hardware and human confirmation", ) config.addinivalue_line( - "markers", "slow: Slow-running tests (typically hardware or complex integration tests)" + "markers", + "slow: Slow-running tests (typically hardware or complex integration tests)", ) config.addinivalue_line( "markers", "e2e: End-to-end tests that exercise complete workflows" ) config.addinivalue_line( - "markers", "gcode: Tests specifically for GCODE parsing and interpretation functionality" + "markers", + "gcode: Tests specifically for GCODE parsing and interpretation functionality", ) @@ -400,7 +413,9 @@ def pytest_collection_modifyitems(config, items): """Modify test collection to add markers and skip conditions.""" # Skip hardware tests by default unless --run-hardware is specified if not config.getoption("--run-hardware"): - skip_hardware = pytest.mark.skip(reason="Hardware tests disabled (use --run-hardware to enable)") + skip_hardware = pytest.mark.skip( + reason="Hardware tests disabled (use --run-hardware to enable)" + ) for item in items: if item.get_closest_marker("hardware"): item.add_marker(skip_hardware) @@ -409,14 +424,16 @@ def pytest_collection_modifyitems(config, items): def pytest_sessionstart(session): """Called after the Session object has been created.""" logger.info("Starting PAROL6 Python API test session") - + # Print test configuration info config = session.config - logger.info(f"Hardware tests: {'enabled' if config.getoption('--run-hardware') else 'disabled'}") + logger.info( + f"Hardware tests: {'enabled' if config.getoption('--run-hardware') else 'disabled'}" + ) logger.info(f"Server IP: {config.getoption('--server-ip')}") - - server_port = config.getoption('--server-port') - ack_port = config.getoption('--ack-port') + + server_port = config.getoption("--server-port") + ack_port = config.getoption("--ack-port") if server_port and ack_port: logger.info(f"Server ports: {server_port}/{ack_port}") else: @@ -427,15 +444,17 @@ def pytest_sessionstart(session): # CLIENT FIXTURE # ============================================================================ + @pytest.fixture def client(ports: TestPorts): """ Provide a RobotClient configured for the test ports. - + This ensures all tests use the same connection configuration and connect to the auto-detected test server ports. """ from parol6 import RobotClient + return RobotClient( host=ports.server_ip, port=ports.server_port, @@ -444,4 +463,6 @@ def client(ports: TestPorts): def pytest_sessionfinish(session, exitstatus): """Called after whole test run finished.""" - logger.info(f"PAROL6 Python API test session finished with exit status: {exitstatus}") + logger.info( + f"PAROL6 Python API test session finished with exit status: {exitstatus}" + ) diff --git a/tests/hardware/test_manual_moves.py b/tests/hardware/test_manual_moves.py index 6517c45..d4ff6fe 100644 --- a/tests/hardware/test_manual_moves.py +++ b/tests/hardware/test_manual_moves.py @@ -4,18 +4,18 @@ These tests require actual robot hardware and human confirmation. They are marked with @pytest.mark.hardware and require the --run-hardware flag. -SAFETY NOTICE: These tests will move the physical robot. Ensure the robot +SAFETY NOTICE: These tests will move the physical robot. Ensure the robot workspace is clear and E-stop is within reach before running. """ -import pytest -import sys import os +import sys import time -from typing import List, Optional + +import pytest # Add the parent directory to Python path -sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..')) +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "..")) # Removed direct robot_api import - using client fixture from conftest.py @@ -25,31 +25,28 @@ SAFE_SMOOTH_START_JOINTS = [42.697, -89.381, 144.831, -0.436, 31.528, 180.0] -def initialize_hardware_position(client, human_prompt) -> Optional[List[float]]: +def initialize_hardware_position(client, human_prompt) -> list[float] | None: """ Moves the robot to the predefined safe starting joint angles. - + Args: client: RobotClient fixture from conftest.py human_prompt: Fixture for human confirmation - + Returns: Robot's Cartesian pose after moving, or None if failed """ - + print(f"Moving to safe starting position: {SAFE_SMOOTH_START_JOINTS}") - + # Move to the joint position result = client.move_joints( - SAFE_SMOOTH_START_JOINTS, - duration=4, - wait_for_ack=True, - timeout=15 + SAFE_SMOOTH_START_JOINTS, duration=4, wait_for_ack=True, timeout=15 ) print(f"Move command result: {result}") # Wait until robot stops - if client.wait_until_stopped(timeout=15): + if client.wait_until_stopped(timeout=20): print("Robot has reached the starting position.") time.sleep(1) start_pose = client.get_pose_rpy() # Get [x,y,z,rx,ry,rz] format @@ -68,7 +65,7 @@ def initialize_hardware_position(client, human_prompt) -> Optional[List[float]]: @pytest.mark.slow class TestHardwareBasicMoves: """Test basic robot movements with hardware.""" - + def test_hardware_homing(self, client, human_prompt): """Test robot homing sequence.""" if not human_prompt( @@ -77,98 +74,95 @@ def test_hardware_homing(self, client, human_prompt): "Ensure robot workspace is completely clear." ): pytest.skip("User declined homing test") - + # Check E-stop status first if client.is_estop_pressed(): pytest.fail("E-stop is pressed. Release E-stop before testing.") - + print("Starting homing sequence...") result = client.home(wait_for_ack=True, timeout=60) - + assert isinstance(result, dict) - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] - + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] + # Wait for homing to complete - use client's built-in wait - if client.wait_until_stopped(timeout=90, show_progress=True): + if client.wait_until_stopped(timeout=95, show_progress=True): print("Homing completed successfully") else: pytest.fail("Robot homing did not complete within timeout") - + def test_small_joint_movements(self, client, human_prompt): """Test small joint movements for safety verification.""" start_pose = initialize_hardware_position(client, human_prompt) if not start_pose: pytest.skip("Failed to reach starting position") - + if not human_prompt( "Ready to test small joint movements?\n" "Robot will move each joint individually by small amounts.\n" "Watch for smooth, controlled motion." ): pytest.skip("User declined joint movement test") - + # Test small movements on each joint for joint_idx in range(6): print(f"Testing joint {joint_idx + 1} movement...") - + # Small positive movement result = client.jog_joint( - joint_idx, - speed_percentage=20, - duration=1.0, - wait_for_ack=True + joint_idx, speed_percentage=20, duration=1.0, wait_for_ack=True ) - + assert isinstance(result, dict) - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] - - client.wait_until_stopped(timeout=5) - + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] + + client.wait_until_stopped(timeout=10) + # Small negative movement (return) - use joint_idx+6 for reverse direction result = client.jog_joint( joint_idx + 6, # +6 indicates reverse direction - speed_percentage=20, + speed_percentage=20, duration=1.0, - wait_for_ack=True + wait_for_ack=True, ) - - client.wait_until_stopped(timeout=5) - + + client.wait_until_stopped(timeout=10) + print("All joint movements completed successfully") - + def test_small_cartesian_movements(self, client, human_prompt): """Test small Cartesian movements in different axes.""" start_pose = initialize_hardware_position(client, human_prompt) if not start_pose: pytest.skip("Failed to reach starting position") - + if not human_prompt( "Ready to test small Cartesian movements?\n" "Robot will move end-effector in X, Y, Z directions.\n" "Movements will be small (10mm) and slow (20% speed)." ): pytest.skip("User declined Cartesian movement test") - - # Test movements in each axis - axes = ['X+', 'X-', 'Y+', 'Y-', 'Z+', 'Z-'] - + + # Test movements in each axis + axes = ["X+", "X-", "Y+", "Y-", "Z+", "Z-"] + for axis in axes: print(f"Testing Cartesian jog in {axis} direction...") - + result = client.jog_cartesian( - frame='WRF', + frame="WRF", axis=axis, speed_percentage=20, duration=1.0, - wait_for_ack=True + wait_for_ack=True, ) - + assert isinstance(result, dict) - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] - + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] + time.sleep(2.0) - client.wait_until_stopped(timeout=5) - + client.wait_until_stopped(timeout=10) + print("All Cartesian movements completed successfully") @@ -176,13 +170,13 @@ def test_small_cartesian_movements(self, client, human_prompt): @pytest.mark.slow class TestHardwareSmoothMotion: """Test smooth motion commands with actual hardware.""" - + def test_hardware_smooth_circle(self, client, human_prompt): """Test smooth circular motion on hardware.""" start_pose = initialize_hardware_position(client, human_prompt) if not start_pose: pytest.skip("Failed to reach starting position") - + if not human_prompt( "Ready to test smooth circular motion?\n" "Robot will execute a 30mm radius circle in XY plane.\n" @@ -190,28 +184,28 @@ def test_hardware_smooth_circle(self, client, human_prompt): "Watch for smooth, continuous motion without stops." ): pytest.skip("User declined circle test") - + # Execute smooth circle center_point = [start_pose[0], start_pose[1] + 30, start_pose[2]] - + print(f"Executing circle: center={center_point}, radius=30mm") result = client.smooth_circle( center=center_point, radius=30.0, - plane='XY', + plane="XY", duration=5.0, clockwise=False, wait_for_ack=True, - timeout=15 + timeout=15, ) - + assert isinstance(result, dict) - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] - + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] + # Wait for completion - if client.wait_until_stopped(timeout=15): + if client.wait_until_stopped(timeout=20): print("Circle motion completed successfully") - + if not human_prompt( "Did the robot execute a smooth circular motion?\n" "Motion should have been continuous without stops or jerks." @@ -219,25 +213,31 @@ def test_hardware_smooth_circle(self, client, human_prompt): pytest.fail("User reported motion was not smooth") else: pytest.fail("Robot did not stop after circle motion timeout") - + def test_hardware_smooth_arc(self, client, human_prompt): """Test smooth arc motion on hardware.""" start_pose = initialize_hardware_position(client, human_prompt) if not start_pose: pytest.skip("Failed to reach starting position") - + if not human_prompt( "Ready to test smooth arc motion?\n" "Robot will execute an arc from current position to a new pose.\n" "Motion will be controlled and smooth." ): pytest.skip("User declined arc test") - + # Define arc motion - end_pose = [start_pose[0] + 40, start_pose[1] + 20, start_pose[2], - start_pose[3], start_pose[4], start_pose[5] + 45] + end_pose = [ + start_pose[0] + 40, + start_pose[1] + 20, + start_pose[2], + start_pose[3], + start_pose[4], + start_pose[5] + 45, + ] center = [start_pose[0] + 20, start_pose[1], start_pose[2]] - + print(f"Executing arc: end={end_pose[:3]}, center={center}") result = client.smooth_arc_center( end_pose=end_pose, @@ -245,15 +245,15 @@ def test_hardware_smooth_arc(self, client, human_prompt): duration=4.0, clockwise=True, wait_for_ack=True, - timeout=12 + timeout=12, ) - + assert isinstance(result, dict) - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] - - if client.wait_until_stopped(timeout=12): + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] + + if client.wait_until_stopped(timeout=17): print("Arc motion completed successfully") - + if not human_prompt( "Did the robot execute a smooth arc motion?\n" "Path should have been curved, not straight lines." @@ -261,47 +261,71 @@ def test_hardware_smooth_arc(self, client, human_prompt): pytest.fail("User reported arc motion was not smooth") else: pytest.fail("Robot did not stop after arc motion timeout") - + def test_hardware_smooth_spline(self, client, human_prompt): """Test smooth spline motion through multiple waypoints.""" start_pose = initialize_hardware_position(client, human_prompt) if not start_pose: pytest.skip("Failed to reach starting position") - + if not human_prompt( "Ready to test smooth spline motion?\n" "Robot will move through multiple waypoints with smooth transitions.\n" "Motion should be continuous without stops at waypoints." ): pytest.skip("User declined spline test") - + # Define spline waypoints waypoints = [ - [start_pose[0] + 20, start_pose[1] + 10, start_pose[2], - start_pose[3], start_pose[4], start_pose[5]], - [start_pose[0] + 35, start_pose[1] + 25, start_pose[2] + 10, - start_pose[3], start_pose[4], start_pose[5] + 20], - [start_pose[0] + 20, start_pose[1] + 35, start_pose[2], - start_pose[3], start_pose[4], start_pose[5] + 40], - [start_pose[0] + 5, start_pose[1] + 20, start_pose[2], - start_pose[3], start_pose[4], start_pose[5]] + [ + start_pose[0] + 20, + start_pose[1] + 10, + start_pose[2], + start_pose[3], + start_pose[4], + start_pose[5], + ], + [ + start_pose[0] + 35, + start_pose[1] + 25, + start_pose[2] + 10, + start_pose[3], + start_pose[4], + start_pose[5] + 20, + ], + [ + start_pose[0] + 20, + start_pose[1] + 35, + start_pose[2], + start_pose[3], + start_pose[4], + start_pose[5] + 40, + ], + [ + start_pose[0] + 5, + start_pose[1] + 20, + start_pose[2], + start_pose[3], + start_pose[4], + start_pose[5], + ], ] - + print(f"Executing spline through {len(waypoints)} waypoints") result = client.smooth_spline( waypoints=waypoints, duration=6.0, - frame='WRF', + frame="WRF", wait_for_ack=True, - timeout=20 + timeout=20, ) - + assert isinstance(result, dict) - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] - - if client.wait_until_stopped(timeout=20): + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] + + if client.wait_until_stopped(timeout=25): print("Spline motion completed successfully") - + if not human_prompt( "Did the robot move smoothly through all waypoints?\n" "Motion should have been continuous without stops at intermediate points." @@ -312,16 +336,16 @@ def test_hardware_smooth_spline(self, client, human_prompt): @pytest.mark.hardware -@pytest.mark.slow +@pytest.mark.slow class TestHardwareAdvancedSmooth: """Test advanced smooth motion features with hardware.""" - + def test_hardware_helix_motion(self, client, human_prompt): """Test helical motion on hardware.""" start_pose = initialize_hardware_position(client, human_prompt) if not start_pose: pytest.skip("Failed to reach starting position") - + if not human_prompt( "Ready to test helical motion?\n" "Robot will execute a helical (screw-like) motion.\n" @@ -329,10 +353,10 @@ def test_hardware_helix_motion(self, client, human_prompt): "Radius: 25mm, Height: 40mm, 3 revolutions." ): pytest.skip("User declined helix test") - + # Execute helix motion center = [start_pose[0], start_pose[1] + 25, start_pose[2] - 20] - + print(f"Executing helix: center={center}, radius=25mm, height=40mm") result = client.smooth_helix( center=center, @@ -342,70 +366,68 @@ def test_hardware_helix_motion(self, client, human_prompt): duration=8.0, clockwise=False, wait_for_ack=True, - timeout=20 + timeout=20, ) - + assert isinstance(result, dict) - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] - - if client.wait_until_stopped(timeout=20): + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] + + if client.wait_until_stopped(timeout=25): print("Helix motion completed successfully") - - if not human_prompt( - "Did the robot execute a smooth helical motion?\n" - ): + + if not human_prompt("Did the robot execute a smooth helical motion?\n"): pytest.fail("User reported helix motion was incorrect") else: pytest.fail("Robot did not stop after helix motion timeout") - + def test_hardware_reference_frame_comparison(self, client, human_prompt): """Test motion in different reference frames (WRF vs TRF).""" start_pose = initialize_hardware_position(client, human_prompt) if not start_pose: pytest.skip("Failed to reach starting position") - + if not human_prompt( "Ready to test reference frame comparison?\n" "Robot will execute similar motions in World frame (WRF) and Tool frame (TRF).\n" "You should observe different motion patterns." ): pytest.skip("User declined reference frame test") - + # Test 1: Circle in World Reference Frame print("Executing circle in World Reference Frame (WRF)...") result_wrf = client.smooth_circle( center=[start_pose[0], start_pose[1] + 30, start_pose[2]], radius=20, duration=4.0, - frame='WRF', - plane='XY', + frame="WRF", + plane="XY", wait_for_ack=True, - timeout=12 + timeout=12, ) - + assert isinstance(result_wrf, dict) - assert result_wrf.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] - - client.wait_until_stopped(timeout=12) + assert result_wrf.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] + + client.wait_until_stopped(timeout=17) time.sleep(2) - + # Test 2: Circle in Tool Reference Frame print("Executing circle in Tool Reference Frame (TRF)...") result_trf = client.smooth_circle( center=[0, 30, 0], # Relative to tool position radius=20, duration=4.0, - frame='TRF', - plane='XY', # Tool's XY plane + frame="TRF", + plane="XY", # Tool's XY plane wait_for_ack=True, - timeout=12 + timeout=12, ) - + assert isinstance(result_trf, dict) - assert result_trf.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] - - client.wait_until_stopped(timeout=12) - + assert result_trf.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] + + client.wait_until_stopped(timeout=17) + if not human_prompt( "Did you observe different motion patterns?\n" "WRF motion should follow world coordinates.\n" @@ -418,7 +440,7 @@ def test_hardware_reference_frame_comparison(self, client, human_prompt): @pytest.mark.slow class TestHardwareSafety: """Test hardware safety features and error conditions.""" - + def test_joint_limit_safety(self, client, human_prompt): """Test joint limit safety (if supported by controller).""" if not human_prompt( @@ -428,23 +450,30 @@ def test_joint_limit_safety(self, client, human_prompt): "This test is informational only." ): pytest.skip("User declined joint limit test") - + # Try to move to a potentially extreme position (should be rejected or limited) - extreme_joints = [180.0, -180.0, 180.0, -180.0, 180.0, -180.0] # Extreme angles as floats - + extreme_joints = [ + 180.0, + -180.0, + 180.0, + -180.0, + 180.0, + -180.0, + ] # Extreme angles as floats + print("Testing extreme joint angles (should be rejected or limited)...") result = client.move_joints( extreme_joints, speed_percentage=5, # Very slow for safety wait_for_ack=True, - timeout=10 + timeout=10, ) - + print(f"Result of extreme joint command: {result}") - + # This test is informational - we just want to see how the system responds time.sleep(5.0) - + # Return to safe position initialize_hardware_position(client, human_prompt) @@ -453,11 +482,11 @@ def test_joint_limit_safety(self, client, human_prompt): @pytest.mark.slow class TestHardwareLegacySequence: """Test the exact sequence from the legacy test_script.py for verified safe operation.""" - + def test_legacy_script_safe_sequence(self, client, human_prompt): """ Reproduce the exact sequence from test_script.py with verified safe waypoints. - + This test uses the exact same joint angles, poses, and parameters that were manually verified to be safe in the original test script. """ @@ -472,50 +501,48 @@ def test_legacy_script_safe_sequence(self, client, human_prompt): "These waypoints were verified safe in the original script." ): pytest.skip("User declined legacy sequence test") - + # Check E-stop status first if client.is_estop_pressed(): pytest.fail("E-stop is pressed. Release E-stop before testing.") - + print("Starting legacy test sequence with verified safe waypoints...") - + # Electric gripper calibration and moves print("Calibrating electric gripper...") result = client.control_electric_gripper( - action="calibrate", - wait_for_ack=True, - timeout=10 + action="calibrate", wait_for_ack=True, timeout=10 ) if isinstance(result, dict): - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] time.sleep(2) - + print("Moving electric gripper to position 100...") result = client.control_electric_gripper( - action='move', - position=100, - speed=150, + action="move", + position=100, + speed=150, current=200, wait_for_ack=True, - timeout=10 + timeout=10, ) if isinstance(result, dict): - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] time.sleep(2) - + print("Moving electric gripper to position 200...") result = client.control_electric_gripper( - action='move', - position=200, - speed=150, + action="move", + position=200, + speed=150, current=200, wait_for_ack=True, - timeout=10 + timeout=10, ) if isinstance(result, dict): - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] time.sleep(2) - + # Get and verify initial status print("Getting robot joint angles and pose...") angles = client.get_angles() @@ -524,7 +551,7 @@ def test_legacy_script_safe_sequence(self, client, human_prompt): assert pose is not None print(f"Initial angles: {angles}") print(f"Initial pose: {pose}") - + # Pneumatic gripper sequence (exact timing from test_script.py) print("Testing pneumatic gripper sequence...") client.control_pneumatic_gripper("open", 1) @@ -535,82 +562,70 @@ def test_legacy_script_safe_sequence(self, client, human_prompt): time.sleep(0.3) client.control_pneumatic_gripper("close", 1) time.sleep(0.3) - + # Joint movement sequence (exact waypoints and timing from test_script.py) print("Moving to first joint position: [90, -90, 160, 12, 12, 180]...") result = client.move_joints( - [90, -90, 160, 12, 12, 180], - duration=5.5, - wait_for_ack=True, - timeout=15 + [90, -90, 160, 12, 12, 180], duration=5.5, wait_for_ack=True, timeout=15 ) if isinstance(result, dict): - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] time.sleep(6) - + print("Moving to second joint position: [50, -60, 180, -12, 32, 0]...") result = client.move_joints( - [50, -60, 180, -12, 32, 0], - duration=5.5, - wait_for_ack=True, - timeout=15 + [50, -60, 180, -12, 32, 0], duration=5.5, wait_for_ack=True, timeout=15 ) if isinstance(result, dict): - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] time.sleep(6) - + print("Moving back to first joint position: [90, -90, 160, 12, 12, 180]...") result = client.move_joints( - [90, -90, 160, 12, 12, 180], - duration=5.5, - wait_for_ack=True, - timeout=15 + [90, -90, 160, 12, 12, 180], duration=5.5, wait_for_ack=True, timeout=15 ) if isinstance(result, dict): - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] time.sleep(6) - + # Pose movement (exact waypoint from test_script.py) print("Moving to pose: [7, 250, 200, -100, 0, -90]...") result = client.move_pose( - [7, 250, 200, -100, 0, -90], - duration=5.5, - wait_for_ack=True, - timeout=15 + [7, 250, 200, -100, 0, -90], duration=5.5, wait_for_ack=True, timeout=15 ) if isinstance(result, dict): - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] time.sleep(6) - + # Cartesian movement (exact waypoint from test_script.py) print("Moving cartesian to: [7, 250, 150, -100, 0, -90]...") result = client.move_cartesian( - [7, 250, 150, -100, 0, -90], + [7, 250, 150, -100, 0, -90], speed_percentage=50, wait_for_ack=True, - timeout=15 + timeout=15, ) if isinstance(result, dict): - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] - + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] + # Final status checks (exact from test_script.py) print("Getting final gripper and IO status...") gripper_status = client.get_gripper_status() io_status = client.get_io() - + assert gripper_status is not None assert io_status is not None - + print(f"Final gripper status: {gripper_status}") print(f"Final IO status: {io_status}") - + if not human_prompt( "Legacy test sequence completed successfully.\n" "Did all movements execute safely and as expected?\n" "This sequence replicates the verified safe waypoints from the original test_script.py." ): pytest.fail("User reported legacy sequence did not execute correctly") - + print("Legacy safe sequence test completed successfully") @@ -618,7 +633,7 @@ def test_legacy_script_safe_sequence(self, client, human_prompt): @pytest.mark.slow class TestHardwareGripper: """Test gripper functionality with hardware.""" - + def test_pneumatic_gripper(self, client, human_prompt): """Test pneumatic gripper operation.""" if not human_prompt( @@ -627,41 +642,37 @@ def test_pneumatic_gripper(self, client, human_prompt): "Gripper will open and close on port 1." ): pytest.skip("User declined gripper test") - + # Test gripper open print("Opening pneumatic gripper...") result = client.control_pneumatic_gripper( - 'open', 1, - wait_for_ack=True, - timeout=5 + "open", 1, wait_for_ack=True, timeout=5 ) - + assert isinstance(result, dict) - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] - + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] + time.sleep(2.0) - + if not human_prompt("Did the gripper open? Confirm before continuing."): pytest.fail("User reported gripper did not open") - + # Test gripper close print("Closing pneumatic gripper...") result = client.control_pneumatic_gripper( - 'close', 1, - wait_for_ack=True, - timeout=5 + "close", 1, wait_for_ack=True, timeout=5 ) - + assert isinstance(result, dict) - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] - + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] + time.sleep(2.0) - + if not human_prompt("Did the gripper close? Confirm operation."): pytest.fail("User reported gripper did not close") - + print("Pneumatic gripper test completed successfully") - + def test_electric_gripper(self, client, human_prompt): """Test electric gripper operation including calibration.""" if not human_prompt( @@ -670,52 +681,45 @@ def test_electric_gripper(self, client, human_prompt): "Gripper will calibrate, then move to different positions." ): pytest.skip("User declined electric gripper test") - + # Get current gripper status gripper_status = client.get_gripper_status() if gripper_status: print(f"Initial gripper status: {gripper_status}") - + # Test gripper calibration (from legacy test_script.py) print("Calibrating electric gripper...") result = client.control_electric_gripper( - action="calibrate", - wait_for_ack=True, - timeout=10 + action="calibrate", wait_for_ack=True, timeout=10 ) - + assert isinstance(result, dict) - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] - + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] + time.sleep(2.0) - + # Test gripper movement print("Moving electric gripper to position 100...") result = client.control_electric_gripper( - 'move', - position=100, - speed=100, - current=400, - wait_for_ack=True, - timeout=10 + "move", position=100, speed=100, current=400, wait_for_ack=True, timeout=10 ) - + assert isinstance(result, dict) - assert result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING'] - + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] + time.sleep(3.0) - + # Check new position new_status = client.get_gripper_status() if new_status: print(f"Gripper status after move: {new_status}") - + if not human_prompt( "Did the electric gripper move to the commanded position?\n" "Check gripper position and movement quality." ): pytest.fail("User reported electric gripper did not move correctly") - + print("Electric gripper test completed successfully") diff --git a/tests/integration/test_ack_and_nonblocking.py b/tests/integration/test_ack_and_nonblocking.py index d5d31a9..711477d 100644 --- a/tests/integration/test_ack_and_nonblocking.py +++ b/tests/integration/test_ack_and_nonblocking.py @@ -5,28 +5,26 @@ timeout handling, and command tracking with live server communication. """ -import pytest -import sys import os -import time +import sys -# Add the parent directory to Python path -sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..')) +import pytest -from parol6 import RobotClient +# Add the parent directory to Python path +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "..")) @pytest.mark.integration class TestAcknowledmentTracking: """Test command acknowledgment tracking functionality.""" - + def test_tracked_command_basic_flow(self, server_proc, client): """Test basic acknowledgment flow with tracked commands.""" # Send a home command (fire-and-forget) result = client.home() # Should return True on successful send (or OK if FORCE_ACK/system) assert result is True - + def test_non_blocking_command_returns_id(self, server_proc, client): """Test that non-blocking commands return command ID immediately.""" # Send command @@ -36,7 +34,7 @@ def test_non_blocking_command_returns_id(self, server_proc, client): ) # Should return True on successful send assert result is True - + def test_multiple_tracked_commands(self, server_proc, client): """Test tracking multiple commands simultaneously.""" # Send several commands @@ -50,8 +48,8 @@ def test_multiple_tracked_commands(self, server_proc, client): # Each should be True assert all(r is True for r in results) # Wait for motion to complete instead of sleeping - assert client.wait_until_stopped(timeout=3.0) - + assert client.wait_until_stopped(timeout=8.0) + def test_command_status_polling(self, server_proc, client): """Test polling command status during execution.""" # Send a longer running command with valid joint targets (fire-and-forget) @@ -67,13 +65,13 @@ def test_command_status_polling(self, server_proc, client): @pytest.mark.integration class TestErrorConditions: """Test error conditions with acknowledgment tracking.""" - + def test_invalid_command_with_tracking(self, server_proc, client): """Test that invalid commands return proper error acknowledgments.""" # Try to send invalid command; client enforces timing requirement with pytest.raises(RuntimeError): _ = client.move_joints([0, 0, 0, 0, 0, 0]) # Missing timing parameters - + def test_malformed_parameters_with_tracking(self, server_proc, client): """Test acknowledgment for commands with malformed parameters.""" # Test with out-of-range speed percentage; client sends fire-and-forget @@ -87,27 +85,29 @@ def test_malformed_parameters_with_tracking(self, server_proc, client): @pytest.mark.integration class TestBasicCommands: """Test basic commands work with the server.""" - + def test_ping(self, server_proc, client): """Test ping functionality.""" assert client.ping() is not None - + def test_get_angles(self, server_proc, client): """Test angle retrieval.""" angles = client.get_angles() assert isinstance(angles, list) assert len(angles) == 6 - + def test_get_io(self, server_proc, client): """Test IO status retrieval.""" io_status = client.get_io() assert isinstance(io_status, list) assert len(io_status) >= 5 - + def test_stop_command(self, server_proc, client): """Test stop command.""" result = client.stop() assert result is True + # Re-enable controller to avoid leaking disabled state to subsequent tests + assert client.enable() is True if __name__ == "__main__": diff --git a/tests/integration/test_movecart_accuracy.py b/tests/integration/test_movecart_accuracy.py new file mode 100644 index 0000000..301a6ad --- /dev/null +++ b/tests/integration/test_movecart_accuracy.py @@ -0,0 +1,132 @@ +""" +Integration test for MoveCart pose accuracy. +Verifies that movecart commands reach the correct final pose. +""" + +import os +import sys + +import numpy as np +import pytest + +# Skip on macOS CI runners due to flakiness +pytestmark = pytest.mark.skipif( + sys.platform == "darwin" and os.getenv("CI") == "true", + reason="Flaky on the slow macOS GitHub Actions runners.; skip on CI", +) + +# Add the parent directory to Python path +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "..")) + + +@pytest.mark.integration +class TestMoveCartAccuracy: + """Test that MoveCart commands reach correct final poses.""" + + def test_movecart_from_home(self, client, server_proc): + """Test MoveCart accuracy starting from home position.""" + # Ensure controller is enabled before motion + assert client.enable() is True + # Home the robot first + assert client.home() is True + assert client.wait_until_stopped(timeout=15.0) + + # Get home pose for reference + home_pose = client.get_pose_rpy() + print(f"\nHome pose (mm, deg): {home_pose}") + + # This is in mm for position, degrees for orientation + target = [0.000, 263, 242, 90, 0, 90] + + # Execute movecart + result = client.move_cartesian(target, speed_percentage=50) + assert result is True + + # Wait for completion + assert client.wait_until_stopped(timeout=15.0) + + # Get final pose + final_pose = client.get_pose_rpy() + print(f"Target pose (mm, deg): {target}") + print(f"Final pose (mm, deg): {final_pose}") + + # Verify pose accuracy + # Position tolerance: 1mm + pos_error = np.linalg.norm(np.array(final_pose[:3]) - np.array(target[:3])) + print(f"Position error: {pos_error:.3f} mm") + assert pos_error < 1.0, ( + f"Position error {pos_error:.3f}mm exceeds 1mm tolerance" + ) + + # Orientation tolerance: 1 degree per axis + # Note: Need to handle angle wrapping for comparison + def angle_diff(a, b): + """Compute smallest angle difference considering wrapping.""" + diff = (a - b + 180) % 360 - 180 + return abs(diff) + + for i, axis in enumerate(["RX", "RY", "RZ"]): + ori_error = angle_diff(final_pose[3 + i], target[3 + i]) + print(f"{axis} error: {ori_error:.3f} deg") + assert ori_error < 1.0, ( + f"{axis} error {ori_error:.3f}° exceeds 1 degree tolerance" + ) + + print("✓ MoveCart pose accuracy test passed!") + + def test_movecart_multiple_targets(self, client, server_proc): + """Test MoveCart accuracy with multiple sequential targets.""" + # Ensure controller is enabled before motion + assert client.enable() is True + # Home first + assert client.home() is True + assert client.wait_until_stopped(timeout=15.0) + + # Define multiple targets to test + targets = [ + [0.0, 200.0, 250.0, 90.0, 0, 90.0], + [50.0, 250.0, 200.0, 90, 0, 90.0], + [0.0, 263.0, 242.0, 90, 0, 90.0], + ] + + for idx, target in enumerate(targets): + print(f"\n--- Target {idx + 1}/{len(targets)} ---") + print(f"Moving to: {target}") + + # Execute movecart + result = client.move_cartesian(target, speed_percentage=30) + assert result is True + + # Wait for completion + assert client.wait_until_stopped(timeout=15.0) + + # Get final pose + final_pose = client.get_pose_rpy() + print(f"Achieved: {final_pose}") + + # Verify position accuracy (1mm tolerance) + pos_error = np.linalg.norm(np.array(final_pose[:3]) - np.array(target[:3])) + print(f"Position error: {pos_error:.3f} mm") + assert pos_error < 1.0, ( + f"Target {idx + 1}: Position error {pos_error:.3f}mm exceeds 1mm" + ) + + # Verify orientation accuracy (1° tolerance per axis) + def angle_diff(a, b): + diff = (a - b + 180) % 360 - 180 + return abs(diff) + + for i, axis in enumerate(["RX", "RY", "RZ"]): + ori_error = angle_diff(final_pose[3 + i], target[3 + i]) + print(f"{axis} error: {ori_error:.3f} deg") + assert ori_error < 1.0, ( + f"Target {idx + 1}: {axis} error {ori_error:.3f}° exceeds 1°" + ) + + print(f"✓ Target {idx + 1} reached successfully") + + print("\n✓ All targets reached with required accuracy!") + + +if __name__ == "__main__": + pytest.main([__file__, "-v", "-s"]) diff --git a/tests/integration/test_movecart_idempotence.py b/tests/integration/test_movecart_idempotence.py new file mode 100644 index 0000000..dd608c3 --- /dev/null +++ b/tests/integration/test_movecart_idempotence.py @@ -0,0 +1,69 @@ +""" +Integration test for MoveCart idempotence. +Verifies that moving to the current pose results in no motion (angular distance ≈ 0). +""" + +import os +import sys + +import numpy as np +import pytest + +# Add the parent directory to Python path +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "..")) + + +@pytest.mark.integration +class TestMoveCartIdempotence: + """Test that MoveCart to current pose results in zero movement.""" + + def test_movecart_to_current_pose_no_rotation(self, client, server_proc): + """Test that moving to the current pose results in no rotation.""" + # Home the robot first + assert client.home() is True + assert client.wait_until_stopped(timeout=15.0) + + # Get current pose (should be home position) + current_pose = client.get_pose_rpy() + print(f"\nCurrent pose at home (mm, deg): {current_pose}") + + # Move to the exact same pose - should result in zero angular distance + # and effectively be a no-op + result = client.move_cartesian(current_pose, speed_percentage=50) + assert result is True + + # Wait for completion (should be instant if duration is ~0) + assert client.wait_until_stopped(timeout=10.0) + + # Get final pose + final_pose = client.get_pose_rpy() + print(f"Final pose after 'move to same' (mm, deg): {final_pose}") + + # Verify pose hasn't changed significantly + # Position tolerance: 0.1mm (very strict since we didn't move) + pos_error = np.linalg.norm( + np.array(final_pose[:3]) - np.array(current_pose[:3]) + ) + print(f"Position error: {pos_error:.4f} mm") + assert pos_error < 0.1, ( + f"Position changed by {pos_error:.4f}mm when moving to same pose" + ) + + # Orientation tolerance: 0.5 degrees per axis (accounting for numerical precision) + def angle_diff(a, b): + """Compute smallest angle difference considering wrapping.""" + diff = (a - b + 180) % 360 - 180 + return abs(diff) + + for i, axis in enumerate(["RX", "RY", "RZ"]): + ori_error = angle_diff(final_pose[3 + i], current_pose[3 + i]) + print(f"{axis} error: {ori_error:.4f} deg") + assert ori_error < 0.5, ( + f"{axis} changed by {ori_error:.4f}° when moving to same pose" + ) + + print("✓ MoveCart idempotence test passed - no unwanted rotation!") + + +if __name__ == "__main__": + pytest.main([__file__, "-v", "-s"]) diff --git a/tests/integration/test_smooth_commands_e2e.py b/tests/integration/test_smooth_commands_e2e.py index d639da5..1516cd8 100644 --- a/tests/integration/test_smooth_commands_e2e.py +++ b/tests/integration/test_smooth_commands_e2e.py @@ -5,100 +5,103 @@ Verifies command acceptance, completion status transitions, and basic functionality. """ -import pytest -import sys import os -import time +import sys -# Add the parent directory to Python path -sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..')) +import pytest +# Add the parent directory to Python path +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "..")) def _check_if_fake_serial_xfail(result): """Helper to mark test as xfail if smooth motion fails due to IK in FAKE_SERIAL.""" - if (isinstance(result, dict) and - result.get('status') == 'FAILED' and - 'IK failed' in result.get('details', '')): + if ( + isinstance(result, dict) + and result.get("status") == "FAILED" + and "IK failed" in result.get("details", "") + ): pytest.xfail("Smooth motion commands fail IK in FAKE_SERIAL mode (expected)") @pytest.mark.integration class TestSmoothMotionMinimal: """Minimal set of smooth motion tests - one per command family.""" - + @pytest.fixture def homed_robot(self, client, server_proc, robot_api_env): """Ensure robot is homed before smooth motion tests.""" print("Homing robot for smooth motion tests...") - + # Home the robot first result = client.home() assert result is True - + # Wait for robot to be stopped - assert client.wait_until_stopped(timeout=10.0) + assert client.wait_until_stopped(timeout=15.0) print("Robot homed and ready for smooth motion tests") - + return True - + def test_smooth_circle_basic(self, client, server_proc, robot_api_env, homed_robot): """Test basic circular motion in FAKE_SERIAL mode.""" result = client.smooth_circle( center=[0, 0, 100], radius=30, duration=2.0, - plane='XY', - frame='WRF', + plane="XY", + frame="WRF", ) - + # Check if we should xfail due to FAKE_SERIAL limitations _check_if_fake_serial_xfail(result) - + # Should complete successfully in FAKE_SERIAL mode assert result is True - + # Wait for completion and verify robot stops - assert client.wait_until_stopped(timeout=4.0) + assert client.wait_until_stopped(timeout=9.0) assert client.is_robot_stopped(threshold_speed=5.0) - - def test_smooth_arc_center_basic(self, client, server_proc, robot_api_env, homed_robot): + + def test_smooth_arc_center_basic( + self, client, server_proc, robot_api_env, homed_robot + ): """Test basic arc motion defined by center point.""" result = client.smooth_arc_center( end_pose=[100, 100, 150, 0, 0, 90], center=[50, 50, 150], duration=2.0, - frame='WRF', + frame="WRF", ) - + _check_if_fake_serial_xfail(result) - + assert result is True - - assert client.wait_until_stopped(timeout=4.0) + + assert client.wait_until_stopped(timeout=9.0) assert client.is_robot_stopped(threshold_speed=5.0) - + def test_smooth_spline_basic(self, client, server_proc, robot_api_env, homed_robot): """Test basic spline motion through waypoints.""" waypoints = [ [100.0, 100.0, 120.0, 0.0, 0.0, 0.0], [150.0, 150.0, 130.0, 0.0, 0.0, 30.0], - [200.0, 100.0, 120.0, 0.0, 0.0, 60.0] + [200.0, 100.0, 120.0, 0.0, 0.0, 60.0], ] - + result = client.smooth_spline( waypoints=waypoints, duration=3.0, - frame='WRF', + frame="WRF", ) - + _check_if_fake_serial_xfail(result) - + assert result is True - - assert client.wait_until_stopped(timeout=5.0) + + assert client.wait_until_stopped(timeout=10.0) assert client.is_robot_stopped(threshold_speed=5.0) - + def test_smooth_helix_basic(self, client, server_proc, robot_api_env, homed_robot): """Test basic helical motion.""" result = client.smooth_helix( @@ -107,45 +110,45 @@ def test_smooth_helix_basic(self, client, server_proc, robot_api_env, homed_robo pitch=20, height=60, duration=3.0, - frame='WRF', + frame="WRF", ) - + _check_if_fake_serial_xfail(result) - + assert result is True - - assert client.wait_until_stopped(timeout=5.0) + + assert client.wait_until_stopped(timeout=10.0) assert client.is_robot_stopped(threshold_speed=5.0) - + def test_smooth_blend_basic(self, client, server_proc, robot_api_env, homed_robot): """Test basic blended motion through segments.""" segments = [ { - 'type': 'LINE', - 'end': [120.0, 80.0, 140.0, 0.0, 0.0, 30.0], - 'duration': 1.0 + "type": "LINE", + "end": [120.0, 80.0, 140.0, 0.0, 0.0, 30.0], + "duration": 1.0, }, { - 'type': 'CIRCLE', - 'center': [120, 120, 140], - 'radius': 25, - 'plane': 'XY', - 'duration': 2.0, - 'clockwise': False - } + "type": "CIRCLE", + "center": [120, 120, 140], + "radius": 25, + "plane": "XY", + "duration": 2.0, + "clockwise": False, + }, ] - + result = client.smooth_blend( segments=segments, blend_time=0.3, - frame='WRF', + frame="WRF", ) - + _check_if_fake_serial_xfail(result) - + assert result is True - - assert client.wait_until_stopped(timeout=5.0) + + assert client.wait_until_stopped(timeout=10.0) assert client.is_robot_stopped(threshold_speed=5.0) diff --git a/tests/integration/test_status_broadcast_autofailover.py b/tests/integration/test_status_broadcast_autofailover.py new file mode 100644 index 0000000..62fd05e --- /dev/null +++ b/tests/integration/test_status_broadcast_autofailover.py @@ -0,0 +1,182 @@ +import asyncio +import contextlib +import socket +import time + +import pytest + +from parol6 import config as cfg +from parol6.server.state import StateManager +from parol6.server.status_broadcast import StatusBroadcaster +from parol6.server.status_cache import get_cache +from parol6.client.status_subscriber import subscribe_status + + +def _free_udp_port() -> int: + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + try: + s.bind(("", 0)) + return s.getsockname()[1] + finally: + s.close() + + +@pytest.mark.asyncio +async def test_status_broadcast_auto_failover_receives_frame(monkeypatch): + """ + Deterministically force multicast setup to fail so the broadcaster falls back + to UNICAST and verify that a multicast-configured subscriber still receives + frames on the same port. + """ + port = _free_udp_port() + group = cfg.MCAST_GROUP + iface = "127.0.0.1" + + # Ensure subscriber uses multicast socket (which also accepts unicast to port) + monkeypatch.setattr(cfg, "STATUS_TRANSPORT", "MULTICAST", raising=False) + monkeypatch.setattr(cfg, "STATUS_UNICAST_HOST", "127.0.0.1", raising=False) + + # Prepare state/cache so broadcaster is allowed to send (cache not stale) + cache = get_cache() + cache.mark_serial_observed() + + # Start broadcaster with our chosen port and force unicast fallback + state_mgr = StateManager() + + def _force_unicast_setup(self: StatusBroadcaster) -> None: # type: ignore[no-redef] + self._use_unicast = True + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF, 1 << 20) + self._sock = sock + + monkeypatch.setattr(StatusBroadcaster, "_setup_socket", _force_unicast_setup) + + broadcaster = StatusBroadcaster( + state_mgr=state_mgr, + group=group, + port=port, + iface_ip=iface, + rate_hz=20.0, + stale_s=1.0, + ) + + broadcaster.start() + + try: + # Give broadcaster a tiny moment to initialize + await asyncio.sleep(0.05) + assert broadcaster._use_unicast is True, ( + "Broadcaster did not fall back to unicast" + ) + + async def _consume_one(timeout: float = 3.0) -> bool: + deadline = time.time() + timeout + async for status in subscribe_status( + group=group, port=port, iface_ip=iface + ): + assert isinstance(status, dict) + assert "angles" in status + return True + if time.time() > deadline: + break + return False + + ok = await asyncio.wait_for(_consume_one(), timeout=4.0) + assert ok, "Did not receive a status frame within timeout" + + finally: + broadcaster.stop() + with contextlib.suppress(Exception): + broadcaster.join(timeout=1.0) + + +@pytest.mark.asyncio +async def test_subscriber_multicast_socket_receives_unicast(monkeypatch): + """ + Verify that when the subscriber is configured for multicast, it still receives + a unicast datagram sent to the same port (since it binds to ("", port)). + """ + port = _free_udp_port() + group = cfg.MCAST_GROUP + iface = "127.0.0.1" + + # Ensure subscriber chooses multicast socket + monkeypatch.setattr(cfg, "STATUS_TRANSPORT", "MULTICAST", raising=False) + + # Craft a valid STATUS payload (defaults are acceptable for parsing) + payload = get_cache().to_ascii().encode("ascii", errors="ignore") + + async def _send_once(): + await asyncio.sleep(0.05) + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + try: + s.sendto(payload, ("127.0.0.1", port)) + finally: + s.close() + + async def _consume_one(timeout: float = 3.0) -> bool: + # Start sender in background + sender = asyncio.create_task(_send_once()) + try: + async for status in subscribe_status( + group=group, port=port, iface_ip=iface + ): + assert isinstance(status, dict) + assert "io" in status + return True + finally: + with contextlib.suppress(Exception): + sender.cancel() + await sender + # If we exit the loop without receiving, signal failure + return False + + ok = await asyncio.wait_for(_consume_one(), timeout=4.0) + assert ok, "Subscriber did not receive unicast datagram on multicast socket" + + +def _raise_sendto(*args, **kwargs): # helper for monkeypatching socket.sendto + raise OSError("simulated send failure") + + +@pytest.mark.timeout(5) +@pytest.mark.asyncio +async def test_multicast_send_errors_should_trigger_fallback_but_currently_do_not( + monkeypatch, +): + """ + Demonstrate the bug: if multicast setup succeeds but subsequent send() calls fail, + the broadcaster should fall back to UNICAST. Current implementation does not, + so this test is expected to FAIL until the logic is improved. + """ + port = _free_udp_port() + # Ensure we attempt multicast path + monkeypatch.setattr(cfg, "STATUS_TRANSPORT", "MULTICAST", raising=False) + + cache = get_cache() + cache.mark_serial_observed() + + state_mgr = StateManager() + broadcaster = StatusBroadcaster( + state_mgr=state_mgr, port=port, iface_ip="127.0.0.1", rate_hz=20.0, stale_s=2.0 + ) + broadcaster.start() + try: + # Allow setup to complete and at least one send to work + await asyncio.sleep(0.1) + + # From now on, every sendto should fail + monkeypatch.setattr(socket.socket, "sendto", _raise_sendto) + + # Give it a few cycles to "detect" and hypothetically fall back + await asyncio.sleep(0.3) + + # The desired behavior would be to switch to unicast after persistent errors. + # Current code does not, so this assertion should FAIL, making the problem visible. + assert broadcaster._use_unicast is True, ( + "Broadcaster did not fall back to unicast on repeated send errors" + ) + finally: + broadcaster.stop() + with contextlib.suppress(Exception): + broadcaster.join(timeout=1.0) diff --git a/tests/integration/test_udp_smoke.py b/tests/integration/test_udp_smoke.py index 23477d7..b0a1881 100644 --- a/tests/integration/test_udp_smoke.py +++ b/tests/integration/test_udp_smoke.py @@ -3,12 +3,13 @@ Covers PING/PONG, GET_* endpoints, STOP semantics, and basic functionality. """ -import pytest -import sys import os +import sys + +import pytest # Add the parent directory to Python path -sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..')) +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "..")) from parol6 import RobotClient @@ -16,98 +17,97 @@ @pytest.mark.integration class TestBasicCommunication: """Test basic UDP communication with the server.""" - + def test_ping_pong(self, client, server_proc): """Test PING/PONG communication.""" assert client.ping() -@pytest.mark.integration +@pytest.mark.integration class TestGetEndpoints: """Test GET_* command endpoints that return immediate data.""" - + def test_get_pose(self, client, server_proc): """Test GET_POSE command.""" pose = client.get_pose() assert pose is not None assert isinstance(pose, list) assert len(pose) == 16 # 4x4 transformation matrix flattened - + # Test helper methods too pose_rpy = client.get_pose_rpy() assert pose_rpy is not None assert isinstance(pose_rpy, list) assert len(pose_rpy) == 6 # [x, y, z, rx, ry, rz] - + pose_xyz = client.get_pose_xyz() assert pose_xyz is not None assert isinstance(pose_xyz, list) assert len(pose_xyz) == 3 # [x, y, z] - + def test_get_angles(self, client, server_proc): """Test GET_ANGLES command.""" angles = client.get_angles() assert angles is not None assert isinstance(angles, list) assert len(angles) == 6 # 6 joint angles - + def test_get_io(self, client, server_proc): """Test GET_IO command.""" io_status = client.get_io() assert io_status is not None assert isinstance(io_status, list) assert len(io_status) == 5 # IN1, IN2, OUT1, OUT2, ESTOP - + # In FAKE_SERIAL mode, ESTOP should be released (1) assert io_status[4] == 1 - + # Test helper method too assert not client.is_estop_pressed() # Should be False in FAKE_SERIAL - + def test_get_gripper(self, client, server_proc): """Test GET_GRIPPER command.""" gripper = client.get_gripper_status() assert gripper is not None assert isinstance(gripper, list) assert len(gripper) == 6 # ID, Position, Speed, Current, Status, ObjDetection - + def test_get_speeds(self, client, server_proc): """Test GET_SPEEDS command.""" speeds = client.get_speeds() assert speeds is not None assert isinstance(speeds, list) assert len(speeds) == 6 # 6 joint speeds - + # Test helper method too stopped = client.is_robot_stopped() assert isinstance(stopped, bool) - + def test_get_status_aggregate(self, client, server_proc): """Test GET_STATUS aggregate command.""" status = client.get_status() assert status is not None assert isinstance(status, dict) - - # Should contain all status components - assert 'pose' in status - assert 'angles' in status - assert 'io' in status - assert 'gripper' in status + # Should contain all status components + assert "pose" in status + assert "angles" in status + assert "io" in status + assert "gripper" in status @pytest.mark.integration class TestStreamMode: """Test streaming mode functionality.""" - + def test_stream_mode_toggle(self, server_proc, ports): """Test STREAM ON/OFF commands.""" client = RobotClient(ports.server_ip, ports.server_port) - + # Enable stream mode and verify responsiveness assert client.stream_on() is True assert client.ping() is not None - + # Disable stream mode and verify responsiveness assert client.stream_off() is True assert client.ping() is not None @@ -116,23 +116,23 @@ def test_stream_mode_toggle(self, server_proc, ports): @pytest.mark.integration class TestBasicMotionCommands: """Test basic motion commands with improved assertions.""" - + def test_home_command(self, client, server_proc): """Test HOME command (fire-and-forget).""" result = client.home() assert result is True - + # Wait for completion and verify robot stops - assert client.wait_until_stopped(timeout=10.0) - + assert client.wait_until_stopped(timeout=15.0) + # Check that robot is responsive after homing assert client.ping() is not None - + # Check that angles are available after homing angles = client.get_angles() assert angles is not None assert len(angles) == 6 - + def test_basic_joint_move(self, client, server_proc): """Test basic joint movement command (fire-and-forget).""" # Use joint angles that are within the robot's limits @@ -143,15 +143,15 @@ def test_basic_joint_move(self, client, server_proc): duration=2.0, ) assert result is True - + # Wait for completion and verify robot stops - assert client.wait_until_stopped(timeout=5.0) - + assert client.wait_until_stopped(timeout=15.0) + # Verify robot state after move attempt angles = client.get_angles() assert angles is not None assert client.ping() is not None - + def test_basic_pose_move(self, client, server_proc): """Test basic pose movement command with validation.""" result = client.move_pose( @@ -159,21 +159,21 @@ def test_basic_pose_move(self, client, server_proc): speed_percentage=50, ) assert result is True - + # Wait for completion and verify robot stops - assert client.wait_until_stopped(timeout=5.0) - + assert client.wait_until_stopped(timeout=15.0) + # Verify robot state pose = client.get_pose_rpy() assert pose is not None assert len(pose) == 6 - + def test_cartesian_move_validation(self, client, server_proc): """Test cartesian movement with proper validation.""" # Test that move requires either duration or speed (client raises) with pytest.raises(RuntimeError): client.move_cartesian([50, 50, 50, 0, 0, 0]) # No duration or speed - + # Valid cartesian move (may still fail IK in FAKE_SERIAL) result = client.move_cartesian( [50, 50, 50, 0, 0, 0], @@ -185,37 +185,39 @@ def test_cartesian_move_validation(self, client, server_proc): @pytest.mark.integration class TestErrorHandling: """Test error handling and edge cases.""" - + def test_invalid_command_format(self, server_proc, ports): """Test server response to invalid commands.""" client = RobotClient(ports.server_ip, ports.server_port) - + # Send malformed command and consume server error response - reply = client.send_raw("INVALID_COMMAND|BAD|PARAMS", await_reply=True, timeout=1.0) + reply = client.send_raw( + "INVALID_COMMAND|BAD|PARAMS", await_reply=True, timeout=1.0 + ) assert isinstance(reply, str) and reply.startswith("ERROR|") - + # Server should remain responsive after handling the error assert client.ping() is not None - + def test_empty_command(self, server_proc, ports): """Test server response to empty commands.""" client = RobotClient(ports.server_ip, ports.server_port) - + # Send empty command (fire-and-forget) sent = client.send_raw("") assert sent is True - + # Server should remain responsive assert client.ping() is not None - + def test_rapid_command_sequence(self, server_proc, ports): """Test server stability under rapid command sequence.""" client = RobotClient(ports.server_ip, ports.server_port) - + # Send multiple commands rapidly (ping) for _ in range(10): assert client.ping() is not None - + # Server should still be responsive assert client.ping() is not None @@ -223,25 +225,25 @@ def test_rapid_command_sequence(self, server_proc, ports): @pytest.mark.integration class TestCommandQueuing: """Test basic command queuing behavior.""" - + def test_command_sequence_execution(self, server_proc, ports): """Test that commands execute in sequence.""" client = RobotClient(ports.server_ip, ports.server_port) - + start_time = __import__("time").time() - + # Execute sequence using public API assert client.home() is True assert client.delay(0.2) is True assert client.delay(0.2) is True assert client.delay(0.2) is True - + # Wait for all commands to complete via speeds - assert client.wait_until_stopped(timeout=5.0) - + assert client.wait_until_stopped(timeout=10.0) + # Server should be responsive after sequence assert client.ping() is not None - + # Total time should be reasonable (commands + processing overhead) total_time = __import__("time").time() - start_time assert total_time < 5.0 # Should complete within reasonable time diff --git a/tests/unit/test_async_client_lifecycle.py b/tests/unit/test_async_client_lifecycle.py new file mode 100644 index 0000000..52a2f80 --- /dev/null +++ b/tests/unit/test_async_client_lifecycle.py @@ -0,0 +1,93 @@ +import asyncio + +import pytest + +from parol6.client.async_client import AsyncRobotClient + + +@pytest.mark.asyncio +@pytest.mark.integration +async def test_multicast_listener_shuts_down_on_close(ports, server_proc): + """AsyncRobotClient.close() should cancel and clean up the real multicast listener. + + This test uses the real server process (server_proc), the real AsyncRobotClient, + and the real multicast subscriber stack. It only relies on the existing + test fixtures to start the FAKE_SERIAL controller on the auto-detected + test ports. + """ + + client = AsyncRobotClient( + host=ports.server_ip, port=ports.server_port, timeout=1.0, retries=0 + ) + + try: + # Ensure the controller is responsive before starting the multicast listener + await client.wait_for_server_ready(timeout=5.0) + + # Force endpoint and multicast listener creation; this will invoke the + # real _start_multicast_listener, which uses subscribe_status and + # the underlying multicast socket implementation. + await client._ensure_endpoint() + task = client._multicast_task + + assert task is not None, "Multicast listener task should be created" + assert not task.done(), ( + "Multicast listener task should be running before close()" + ) + + # Invoke graceful shutdown + await client.close() + + # After close(), the task should be finished and cleared + assert client._multicast_task is None, ( + "Multicast listener reference should be cleared after close()" + ) + assert task.done(), "Multicast listener task should be completed after close()" + finally: + # close() is idempotent; ensure cleanup even if assertions fail earlier + await client.close() + + +@pytest.mark.asyncio +@pytest.mark.integration +async def test_status_stream_terminates_on_close(ports, server_proc): + """status_stream consumers should terminate when AsyncRobotClient.close() is called. + + This test exercises the real server process and the real multicast subscriber + stack to ensure that any background tasks consuming status_stream() are + unblocked and finished by the time close() completes. + """ + + client = AsyncRobotClient( + host=ports.server_ip, port=ports.server_port, timeout=1.0, retries=0 + ) + + async def consumer() -> None: + # Consume a few status messages until the client is closed. + # The loop should terminate automatically when close() is invoked. + async for _ in client.status_stream(): + # Yield control so we don't spin too fast in tests + await asyncio.sleep(0) + + try: + # Ensure the controller is responsive before starting the multicast listener + await client.wait_for_server_ready(timeout=5.0) + + # Start the consumer task; this will internally trigger _ensure_endpoint() + consumer_task = asyncio.create_task(consumer()) + + # Wait briefly to allow the multicast listener and status stream to start + await asyncio.sleep(0.5) + + # Closing the client should cause the consumer to exit its async-for loop + await client.close() + + # The consumer task should complete promptly after close() + await asyncio.wait_for(consumer_task, timeout=5.0) + + assert consumer_task.done(), ( + "status_stream consumer should be finished after close()" + ) + finally: + # Ensure cleanup even if assertions fail earlier + await client.close() diff --git a/tests/unit/test_conversions.py b/tests/unit/test_conversions.py index b566bd9..dcf2712 100644 --- a/tests/unit/test_conversions.py +++ b/tests/unit/test_conversions.py @@ -1,4 +1,3 @@ -import pytest from unittest.mock import AsyncMock from parol6 import RobotClient @@ -43,37 +42,6 @@ def test_get_pose_rpy_identity_translation(monkeypatch): assert abs(rz) < 1e-6 -def test_get_pose_rpy_rz_90(monkeypatch): - """ - Validate simple +90deg rotation around Z axis. - Rotation matrix: - [ 0 -1 0 ] - [ 1 0 0 ] - [ 0 0 1 ] - """ - client = RobotClient() - - mat = [ - [0, -1, 0, 0], - [1, 0, 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1], - ] - payload = _pose_payload_from_matrix(mat) - - mock_request = AsyncMock(return_value=payload) - monkeypatch.setattr(client.async_client, "_request", mock_request) - - pose_rpy = client.get_pose_rpy() - assert pose_rpy is not None - x, y, z, rx, ry, rz = pose_rpy - # No translation, 90deg about Z - assert x == 0 and y == 0 and z == 0 - assert abs(rx) < 1e-6 - assert abs(ry) < 1e-6 - assert abs(rz - 90.0) < 1e-6 - - def test_get_pose_rpy_malformed_payload(monkeypatch): """ Malformed POSE payload (wrong length) should return None. diff --git a/tests/unit/test_mock_transport.py b/tests/unit/test_mock_transport.py index 9faf67b..10fd837 100644 --- a/tests/unit/test_mock_transport.py +++ b/tests/unit/test_mock_transport.py @@ -9,21 +9,24 @@ """ import os -import time import threading -import pytest -from unittest.mock import patch +import time import numpy as np - -from parol6.server.transports.mock_serial_transport import MockSerialTransport, MockRobotState -from parol6.server.transports import create_transport, is_simulation_mode -from parol6.protocol.wire import CommandCode, unpack_rx_frame_into import parol6.PAROL6_ROBOT as PAROL6_ROBOT +import pytest from parol6.config import HOME_ANGLES_DEG +from parol6.protocol.wire import CommandCode, unpack_rx_frame_into +from parol6.server.transports import create_transport, is_simulation_mode +from parol6.server.transports.mock_serial_transport import ( + MockRobotState, + MockSerialTransport, +) -def _wait_for_latest_frame_and_decode(transport: MockSerialTransport, timeout_s: float = 0.5): +def _wait_for_latest_frame_and_decode( + transport: MockSerialTransport, timeout_s: float = 0.5 +): """ Helper: wait for a latest frame publication and decode into numpy arrays. Returns dict-like with arrays or None on timeout. @@ -76,37 +79,37 @@ def _wait_for_latest_frame_and_decode(transport: MockSerialTransport, timeout_s: class TestMockSerialTransport: """Test MockSerialTransport functionality.""" - + def test_create_and_connect(self): """Test that MockSerialTransport can be created and connected.""" transport = MockSerialTransport() assert transport is not None assert not transport.is_connected() - + # Connect should always succeed for mock assert transport.connect() assert transport.is_connected() - + # Disconnect transport.disconnect() assert not transport.is_connected() - + def test_auto_reconnect(self): """Test auto-reconnect functionality.""" transport = MockSerialTransport() - + # Auto-reconnect should succeed when not connected assert transport.auto_reconnect() assert transport.is_connected() - + # Auto-reconnect should return False when already connected assert not transport.auto_reconnect() - + def test_write_frame(self): """Test writing command frames.""" transport = MockSerialTransport() transport.connect() - + # Prepare command data position_out = [0, 0, 0, 0, 0, 0] speed_out = [100, 100, 100, 100, 100, 100] @@ -115,37 +118,47 @@ def test_write_frame(self): inout = [0, 0, 0, 0, 0, 0, 0, 0] timeout = 0 gripper_data = [0, 0, 0, 0, 0, 0] - + # Write should succeed when connected success = transport.write_frame( - position_out, speed_out, command_out, - affected_joint, inout, timeout, gripper_data + position_out, + speed_out, + command_out, + affected_joint, + inout, + timeout, + gripper_data, ) assert success - + # Verify internal state updated assert transport._state.command_out == command_out - assert transport._state.position_out == position_out - assert transport._state.speed_out == speed_out - + assert np.array_equal(transport._state.position_out, position_out) + assert np.array_equal(transport._state.speed_out, speed_out) + # Disconnect and try again - should fail transport.disconnect() success = transport.write_frame( - position_out, speed_out, command_out, - affected_joint, inout, timeout, gripper_data + position_out, + speed_out, + command_out, + affected_joint, + inout, + timeout, + gripper_data, ) assert not success - + def test_read_frames(self): """ Test reading response frames using latest-frame API (no legacy queues). """ transport = MockSerialTransport() transport.connect() - + decoded = _wait_for_latest_frame_and_decode(transport, timeout_s=0.5) assert decoded is not None, "No frame published by mock transport" - + # Check data shapes assert decoded["pos"].shape == (6,) assert decoded["spd"].shape == (6,) @@ -155,27 +168,26 @@ def test_read_frames(self): assert decoded["poserr"].shape == (8,) assert decoded["timing"].shape == (1,) assert decoded["grip"].shape == (6,) - + # E-stop should be released in simulation (io bit 4) assert int(decoded["io"][4]) == 1 - + def test_motion_simulation_jog(self): """Test JOG command simulation via latest-frame API.""" transport = MockSerialTransport() transport.connect() - + # Baseline baseline = _wait_for_latest_frame_and_decode(transport, timeout_s=0.5) assert baseline is not None initial_pos = int(baseline["pos"][0]) - + # Send JOG command to move joint 1 speed_out = [1000, 0, 0, 0, 0, 0] assert transport.write_frame( - [0]*6, speed_out, CommandCode.JOG, - [1]*8, [0]*8, 0, [0]*6 + [0] * 6, speed_out, CommandCode.JOG, [1] * 8, [0] * 8, 0, [0] * 6 ) - + # Wait for movement moved = None t0 = time.time() @@ -187,18 +199,17 @@ def test_motion_simulation_jog(self): moved = decoded break assert moved is not None, "Joint didn't move during JOG" - + def test_motion_simulation_move(self): """Test MOVE command simulation via latest-frame API.""" transport = MockSerialTransport() transport.connect() - + target_pos = [5000, 0, 0, 0, 0, 0] assert transport.write_frame( - target_pos, [0]*6, CommandCode.MOVE, - [1]*8, [0]*8, 0, [0]*6 + target_pos, [0] * 6, CommandCode.MOVE, [1] * 8, [0] * 8, 0, [0] * 6 ) - + # Poll until position moves toward target or timeout final = None t0 = time.time() @@ -211,27 +222,29 @@ def test_motion_simulation_move(self): final = decoded break assert final is not None, "Didn't move toward target sufficiently" - + def test_homing_simulation(self): """Test HOME command simulation via latest-frame API.""" transport = MockSerialTransport() transport.connect() - + # Expected home positions (steps) derived from config HOME_ANGLES_DEG - expected_steps = [int(PAROL6_ROBOT.ops.deg_to_steps(float(deg), i)) for i, deg in enumerate(HOME_ANGLES_DEG)] + expected_steps = [ + int(PAROL6_ROBOT.ops.deg_to_steps(float(deg), i)) + for i, deg in enumerate(HOME_ANGLES_DEG) + ] tol_steps = 500 # tolerance in steps - + # Send HOME command assert transport.write_frame( - [0]*6, [0]*6, CommandCode.HOME, - [1]*8, [0]*8, 0, [0]*6 + [0] * 6, [0] * 6, CommandCode.HOME, [1] * 8, [0] * 8, 0, [0] * 6 ) - + homing_started = False homing_completed = False t0 = time.time() last_homed_bits = None - + while time.time() - t0 < 1.0: decoded = _wait_for_latest_frame_and_decode(transport, timeout_s=0.1) if decoded is None: @@ -242,141 +255,144 @@ def test_homing_simulation(self): if homing_started and all(h == 1 for h in homed_bits): # Verify positions near configured home posture pos_list = decoded["pos"].tolist() - if all(abs(int(pos_list[i]) - expected_steps[i]) < tol_steps for i in range(6)): + if all( + abs(int(pos_list[i]) - expected_steps[i]) < tol_steps + for i in range(6) + ): homing_completed = True break last_homed_bits = homed_bits - + # Either already homed or homed sequence executed if not homing_started: assert last_homed_bits is not None assert all(h == 1 for h in last_homed_bits), "Robot should be homed" else: assert homing_completed, "Homing sequence started but did not complete" - + def test_gripper_simulation(self): """Test gripper command simulation.""" transport = MockSerialTransport() transport.connect() - + # Test calibration mode gripper_data = [100, 150, 500, 0, 1, 42] # mode=1 for calibration, id=42 transport.write_frame( - [0]*6, [0]*6, CommandCode.IDLE, - [0]*8, [0]*8, 0, gripper_data + [0] * 6, [0] * 6, CommandCode.IDLE, [0] * 8, [0] * 8, 0, gripper_data ) - + # Check gripper state updated assert transport._state.gripper_data_in[0] == 42 # Device ID set assert transport._state.gripper_data_in[4] & 0x40 != 0 # Calibrated bit set - + # Test error clear mode gripper_data[4] = 2 # mode=2 for error clear transport.write_frame( - [0]*6, [0]*6, CommandCode.IDLE, - [0]*8, [0]*8, 0, gripper_data + [0] * 6, [0] * 6, CommandCode.IDLE, [0] * 8, [0] * 8, 0, gripper_data ) - + # Error bit should be cleared assert transport._state.gripper_data_in[4] & 0x20 == 0 - + def test_get_info(self): """Test get_info method.""" transport = MockSerialTransport(port="TEST_PORT", baudrate=115200) - + info = transport.get_info() - assert info['port'] == "TEST_PORT" - assert info['baudrate'] == 115200 - assert info['connected'] == False - assert info['mode'] == 'MOCK_SERIAL' - + assert info["port"] == "TEST_PORT" + assert info["baudrate"] == 115200 + assert not info["connected"] + assert info["mode"] == "MOCK_SERIAL" + transport.connect() info = transport.get_info() - assert info['connected'] == True - assert 'frames_sent' in info - assert 'frames_received' in info - assert 'simulation_rate_hz' in info - assert 'robot_state' in info + assert info["connected"] + assert "frames_sent" in info + assert "frames_received" in info + assert "simulation_rate_hz" in info + assert "robot_state" in info class TestTransportFactory: """Test transport factory with mock mode.""" - + def test_simulation_mode_detection(self): """Test is_simulation_mode function.""" # Should be False by default - if 'PAROL6_FAKE_SERIAL' in os.environ: - del os.environ['PAROL6_FAKE_SERIAL'] + if "PAROL6_FAKE_SERIAL" in os.environ: + del os.environ["PAROL6_FAKE_SERIAL"] assert not is_simulation_mode() - + # Test various true values - for value in ['1', 'true', 'TRUE', 'yes', 'YES', 'on', 'ON']: - os.environ['PAROL6_FAKE_SERIAL'] = value + for value in ["1", "true", "TRUE", "yes", "YES", "on", "ON"]: + os.environ["PAROL6_FAKE_SERIAL"] = value assert is_simulation_mode() - + # Test false values - for value in ['0', 'false', 'FALSE', 'no', 'NO', 'off', 'OFF', '']: - os.environ['PAROL6_FAKE_SERIAL'] = value + for value in ["0", "false", "FALSE", "no", "NO", "off", "OFF", ""]: + os.environ["PAROL6_FAKE_SERIAL"] = value assert not is_simulation_mode() - + # Clean up - del os.environ['PAROL6_FAKE_SERIAL'] - + del os.environ["PAROL6_FAKE_SERIAL"] + def test_create_transport_auto_detect(self): """Test transport factory auto-detection.""" # Without FAKE_SERIAL, should create SerialTransport - if 'PAROL6_FAKE_SERIAL' in os.environ: - del os.environ['PAROL6_FAKE_SERIAL'] - + if "PAROL6_FAKE_SERIAL" in os.environ: + del os.environ["PAROL6_FAKE_SERIAL"] + from parol6.server.transports.serial_transport import SerialTransport + transport = create_transport() assert isinstance(transport, SerialTransport) - + # With FAKE_SERIAL, should create MockSerialTransport - os.environ['PAROL6_FAKE_SERIAL'] = '1' + os.environ["PAROL6_FAKE_SERIAL"] = "1" transport = create_transport() assert isinstance(transport, MockSerialTransport) - + # Clean up - del os.environ['PAROL6_FAKE_SERIAL'] - + del os.environ["PAROL6_FAKE_SERIAL"] + def test_create_transport_explicit(self): """Test explicit transport type selection.""" # Explicit mock regardless of environment - transport = create_transport(transport_type='mock') + transport = create_transport(transport_type="mock") assert isinstance(transport, MockSerialTransport) - + # Explicit serial regardless of environment from parol6.server.transports.serial_transport import SerialTransport - os.environ['PAROL6_FAKE_SERIAL'] = '1' - transport = create_transport(transport_type='serial') + + os.environ["PAROL6_FAKE_SERIAL"] = "1" + transport = create_transport(transport_type="serial") assert isinstance(transport, SerialTransport) - + # Invalid type should raise with pytest.raises(ValueError): - create_transport(transport_type='invalid') - + create_transport(transport_type="invalid") + # Clean up - if 'PAROL6_FAKE_SERIAL' in os.environ: - del os.environ['PAROL6_FAKE_SERIAL'] + if "PAROL6_FAKE_SERIAL" in os.environ: + del os.environ["PAROL6_FAKE_SERIAL"] class TestMockRobotState: """Test MockRobotState initialization.""" - + def test_initial_state(self): """Test initial robot state.""" state = MockRobotState() - + # Should start at standby position for i in range(6): deg = float(PAROL6_ROBOT.joint.standby.deg[i]) steps = int(PAROL6_ROBOT.ops.deg_to_steps(deg, i)) assert state.position_in[i] == steps - + # E-stop should be released assert state.io_in[4] == 1 - + # No errors initially assert all(e == 0 for e in state.temperature_error_in) assert all(e == 0 for e in state.position_error_in) diff --git a/tests/unit/test_protocol_status_decode_enablement.py b/tests/unit/test_protocol_status_decode_enablement.py new file mode 100644 index 0000000..9395cac --- /dev/null +++ b/tests/unit/test_protocol_status_decode_enablement.py @@ -0,0 +1,34 @@ +import pytest + +from parol6.protocol import wire + + +@pytest.mark.unit +def test_decode_status_with_enablement_arrays(): + # Minimal valid STATUS with ANGLES present to satisfy parser + joint_en = ",".join(["1", "0"] * 6) + cart_wrf = ",".join(["1"] * 12) + cart_trf = ",".join(["0"] * 12) + status = ( + "STATUS|ANGLES=0,0,0,0,0,0" + f"|JOINT_EN={joint_en}" + f"|CART_EN_WRF={cart_wrf}" + f"|CART_EN_TRF={cart_trf}" + ) + + decoded = wire.decode_status(status) + assert decoded is not None + assert isinstance(decoded.get("joint_en"), list) + assert isinstance(decoded.get("cart_en_wrf"), list) + assert isinstance(decoded.get("cart_en_trf"), list) + je = decoded.get("joint_en") + wrf = decoded.get("cart_en_wrf") + trf = decoded.get("cart_en_trf") + assert isinstance(je, list) and len(je) == 12 + assert isinstance(wrf, list) and len(wrf) == 12 + assert isinstance(trf, list) and len(trf) == 12 + + # Spot-check values + assert je[0] == 1 and je[1] == 0 + assert all(v == 1 for v in wrf) # all ones + assert all(v == 0 for v in trf) # all zeros diff --git a/tests/unit/test_query_commands_actions.py b/tests/unit/test_query_commands_actions.py new file mode 100644 index 0000000..678703a --- /dev/null +++ b/tests/unit/test_query_commands_actions.py @@ -0,0 +1,217 @@ +""" +Unit tests for action-related query commands. + +Tests GET_CURRENT_ACTION and GET_QUEUE query commands without requiring a running server. +Uses stub UDP transport and minimal state objects to test command logic in isolation. +""" + +import json +from types import SimpleNamespace + +from parol6.commands.base import CommandContext +from parol6.commands.query_commands import GetCurrentActionCommand, GetQueueCommand + + +class StubUDPTransport: + """Stub UDP transport that captures sent responses.""" + + def __init__(self): + self.sent = [] + + def send_response(self, message: str, addr: tuple): + """Capture sent responses for verification.""" + self.sent.append((message, addr)) + + +def test_get_current_action_command_match(): + """Test that GET_CURRENT_ACTION command matches correctly.""" + cmd = GetCurrentActionCommand() + + # Should match + can_handle, error = cmd.do_match(["GET_CURRENT_ACTION"]) + assert can_handle + assert error is None + + # Should not match other commands + can_handle, error = cmd.do_match(["GET_QUEUE"]) + assert not can_handle + + can_handle, error = cmd.do_match(["UNKNOWN"]) + assert not can_handle + + +def test_get_current_action_replies_json(): + """Test that GET_CURRENT_ACTION returns correct JSON response.""" + # Setup + udp = StubUDPTransport() + ctx = CommandContext( + udp_transport=udp, addr=("127.0.0.1", 5001), gcode_interpreter=None, dt=0.01 + ) + + # Create minimal state with action tracking fields + state = SimpleNamespace( + action_current="MovePoseCommand", + action_state="EXECUTING", + action_next="HomeCommand", + ) + + # Execute command + cmd = GetCurrentActionCommand() + cmd.bind(ctx) + cmd.setup(state) + status = cmd.tick(state) + + # Verify response sent + assert len(udp.sent) == 1 + message, addr = udp.sent[0] + + # Verify message format + assert message.startswith("ACTION|") + + # Parse and verify JSON payload + json_str = message.split("|", 1)[1] + payload = json.loads(json_str) + + assert payload["current"] == "MovePoseCommand" + assert payload["state"] == "EXECUTING" + assert payload["next"] == "HomeCommand" + + # Verify command completed + assert status.code.value == "COMPLETED" + assert cmd.is_finished + + +def test_get_current_action_with_idle_state(): + """Test GET_CURRENT_ACTION when robot is idle.""" + udp = StubUDPTransport() + ctx = CommandContext( + udp_transport=udp, addr=("127.0.0.1", 5001), gcode_interpreter=None, dt=0.01 + ) + + # Idle state - no current action + state = SimpleNamespace(action_current="", action_state="IDLE", action_next="") + + cmd = GetCurrentActionCommand() + cmd.bind(ctx) + cmd.setup(state) + cmd.tick(state) + + # Verify response + assert len(udp.sent) == 1 + message, _ = udp.sent[0] + json_str = message.split("|", 1)[1] + payload = json.loads(json_str) + + assert payload["current"] == "" + assert payload["state"] == "IDLE" + assert payload["next"] == "" + + +def test_get_queue_command_match(): + """Test that GET_QUEUE command matches correctly.""" + cmd = GetQueueCommand() + + # Should match + can_handle, error = cmd.do_match(["GET_QUEUE"]) + assert can_handle + assert error is None + + # Should not match other commands + can_handle, error = cmd.do_match(["GET_CURRENT_ACTION"]) + assert not can_handle + + +def test_get_queue_replies_json(): + """Test that GET_QUEUE returns correct JSON response.""" + # Setup + udp = StubUDPTransport() + ctx = CommandContext( + udp_transport=udp, addr=("127.0.0.1", 5001), gcode_interpreter=None, dt=0.01 + ) + + # Create state with queued commands + state = SimpleNamespace( + queue_nonstreamable=["MovePoseCommand", "HomeCommand", "MoveJointCommand"] + ) + + # Execute command + cmd = GetQueueCommand() + cmd.bind(ctx) + cmd.setup(state) + status = cmd.tick(state) + + # Verify response sent + assert len(udp.sent) == 1 + message, addr = udp.sent[0] + + # Verify message format + assert message.startswith("QUEUE|") + + # Parse and verify JSON payload + json_str = message.split("|", 1)[1] + payload = json.loads(json_str) + + assert payload["non_streamable"] == [ + "MovePoseCommand", + "HomeCommand", + "MoveJointCommand", + ] + assert payload["size"] == 3 + + # Verify command completed + assert status.code.value == "COMPLETED" + assert cmd.is_finished + + +def test_get_queue_with_empty_queue(): + """Test GET_QUEUE when queue is empty.""" + udp = StubUDPTransport() + ctx = CommandContext( + udp_transport=udp, addr=("127.0.0.1", 5001), gcode_interpreter=None, dt=0.01 + ) + + # Empty queue + state = SimpleNamespace(queue_nonstreamable=[]) + + cmd = GetQueueCommand() + cmd.bind(ctx) + cmd.setup(state) + cmd.tick(state) + + # Verify response + assert len(udp.sent) == 1 + message, _ = udp.sent[0] + json_str = message.split("|", 1)[1] + payload = json.loads(json_str) + + assert payload["non_streamable"] == [] + assert payload["size"] == 0 + + +def test_get_queue_excludes_streamable(): + """Test that queue only contains non-streamable commands (by design).""" + # This test verifies the API contract - the queue_nonstreamable field + # should already have streamable commands filtered out by the controller + + udp = StubUDPTransport() + ctx = CommandContext( + udp_transport=udp, addr=("127.0.0.1", 5001), gcode_interpreter=None, dt=0.01 + ) + + # State should only contain non-streamable commands + # (streamable commands like JogJointCommand are filtered by controller) + state = SimpleNamespace(queue_nonstreamable=["MovePoseCommand", "HomeCommand"]) + + cmd = GetQueueCommand() + cmd.bind(ctx) + cmd.setup(state) + cmd.tick(state) + + message, _ = udp.sent[0] + json_str = message.split("|", 1)[1] + payload = json.loads(json_str) + + # Verify only non-streamable commands in response + assert "MovePoseCommand" in payload["non_streamable"] + assert "HomeCommand" in payload["non_streamable"] + assert payload["size"] == 2 diff --git a/tests/unit/test_server_manager.py b/tests/unit/test_server_manager.py new file mode 100644 index 0000000..341a1a5 --- /dev/null +++ b/tests/unit/test_server_manager.py @@ -0,0 +1,62 @@ +import pytest + +from parol6.client.manager import ServerManager, is_server_running, manage_server + + +@pytest.mark.asyncio +async def test_is_server_running_false_when_no_server(monkeypatch): + # Use an unlikely high port to avoid collisions + assert not is_server_running(host="127.0.0.1", port=59999, timeout=0.2) + + +@pytest.mark.asyncio +async def test_manage_server_starts_and_reports_running(monkeypatch): + # Choose a high-numbered UDP port to minimize collision risk + host = "127.0.0.1" + port = 59998 + + # Ensure no server is running before we start + assert not is_server_running(host=host, port=port, timeout=0.2) + + manager: ServerManager | None = None + try: + manager = manage_server( + host=host, port=port, com_port=None, extra_env=None, normalize_logs=False + ) + assert isinstance(manager, ServerManager) + + # After manage_server, the UDP endpoint should respond to PING + assert is_server_running(host=host, port=port, timeout=1.0) + finally: + if manager is not None: + manager.stop_controller() + + # After stop_controller returns, the server should no longer respond to PING + assert not is_server_running(host=host, port=port, timeout=0.5) + + +@pytest.mark.asyncio +async def test_manage_server_fast_fails_when_already_running(monkeypatch): + host = "127.0.0.1" + port = 59997 + + manager: ServerManager | None = None + try: + # First start a server + manager = manage_server( + host=host, port=port, com_port=None, extra_env=None, normalize_logs=False + ) + assert is_server_running(host=host, port=port, timeout=1.0) + + # Second attempt should raise RuntimeError because the port is taken by an existing server + with pytest.raises(RuntimeError): + manage_server( + host=host, + port=port, + com_port=None, + extra_env=None, + normalize_logs=False, + ) + finally: + if manager is not None: + manager.stop_controller() diff --git a/tests/unit/test_smooth_motion_api.py b/tests/unit/test_smooth_motion_api.py index 2ea29a2..21f647c 100644 --- a/tests/unit/test_smooth_motion_api.py +++ b/tests/unit/test_smooth_motion_api.py @@ -1,5 +1,5 @@ -import inspect import importlib +import inspect def test_smooth_motion_reexports_exist(): diff --git a/tests/unit/test_status_cache_enablement_ascii.py b/tests/unit/test_status_cache_enablement_ascii.py new file mode 100644 index 0000000..dcb4036 --- /dev/null +++ b/tests/unit/test_status_cache_enablement_ascii.py @@ -0,0 +1,74 @@ +import numpy as np +import pytest + +from parol6.server.status_cache import StatusCache +from parol6.server.state import ControllerState + + +@pytest.mark.unit +def test_status_cache_includes_enablement_fields(monkeypatch): + cache = StatusCache() + + # Patch heavy dependencies to be deterministic and fast + monkeypatch.setattr( + "parol6.server.status_cache.PAROL6_ROBOT", + type( + "_Dummy", + (), + { + "ops": type( + "_Ops", + (), + { + "steps_to_deg": staticmethod(lambda steps: [0.0] * 6), + "steps_to_rad": staticmethod(lambda steps: [0.0] * 6), + }, + )(), + "robot": type( + "_Robot", (), {"qlim": np.vstack([[-3.14] * 6, [3.14] * 6])} + )(), + }, + ), + raising=True, + ) + + # Short-circuit fkine to avoid spatialmath calls + monkeypatch.setattr( + "parol6.server.status_cache.get_fkine_flat_mm", + lambda state: np.zeros((16,), dtype=float), + raising=True, + ) + + # Patch enablement calculators to fixed values + def _fake_joint(cache_self, q_rad): # type: ignore[no-redef] + bits = [1, 0] * 6 + cache_self.joint_en[:] = np.asarray(bits, dtype=np.uint8) + cache_self._joint_en_ascii = ",".join(str(int(v)) for v in bits) + + def _fake_cart(cache_self, T, frame, q_rad): # type: ignore[no-redef] + bits = [1] * 12 if frame == "WRF" else [0] * 12 + ascii_bits = ",".join(str(b) for b in bits) + if frame == "WRF": + cache_self.cart_en_wrf[:] = np.asarray(bits, dtype=np.uint8) + cache_self._cart_en_wrf_ascii = ascii_bits + else: + cache_self.cart_en_trf[:] = np.asarray(bits, dtype=np.uint8) + cache_self._cart_en_trf_ascii = ascii_bits + + monkeypatch.setattr(StatusCache, "_compute_joint_enable", _fake_joint, raising=True) + monkeypatch.setattr(StatusCache, "_compute_cart_enable", _fake_cart, raising=True) + + # Trigger an update with a fresh state + state = ControllerState() + # Change Position_in so StatusCache treats it as an update (pos_changed=True) + arr = np.zeros((6,), dtype=np.int32) + arr[0] = 1 + state.Position_in[:] = arr + cache.update_from_state(state) + + txt = cache.to_ascii() + assert "JOINT_EN=" in txt + assert "CART_EN_WRF=" in txt and "CART_EN_TRF=" in txt + assert "JOINT_EN=1,0,1,0,1,0,1,0,1,0,1,0" in txt + assert "CART_EN_WRF=" + ",".join(["1"] * 12) in txt + assert "CART_EN_TRF=" + ",".join(["0"] * 12) in txt diff --git a/tests/unit/test_trajectory.py b/tests/unit/test_trajectory.py index a73937b..2f9daf1 100644 --- a/tests/unit/test_trajectory.py +++ b/tests/unit/test_trajectory.py @@ -1,9 +1,9 @@ import math + import numpy as np import pytest - -from parol6.utils import trajectory as traj from parol6.config import CONTROL_RATE_HZ +from parol6.utils import trajectory as traj def approx_equal(a, b, tol=1e-6): diff --git a/tests/unit/test_wire.py b/tests/unit/test_wire.py index 70f12bc..e91dc98 100644 --- a/tests/unit/test_wire.py +++ b/tests/unit/test_wire.py @@ -1,6 +1,4 @@ -import json import pytest - from parol6.protocol import wire @@ -77,7 +75,11 @@ def test_encode_gcode_program_inline(): ("IO|1,0,1,0,1", "IO", [1, 0, 1, 0, 1]), ("GRIPPER|1,255,150,500,3,1", "GRIPPER", [1, 255, 150, 500, 3, 1]), ("SPEEDS|0,0.5,-1,2.5,3,4", "SPEEDS", [0.0, 0.5, -1.0, 2.5, 3.0, 4.0]), - ("POSE|1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16", "POSE", [float(i) for i in range(1, 17)]), + ( + "POSE|1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16", + "POSE", + [float(i) for i in range(1, 17)], + ), ], ) def test_decode_simple_success(resp, prefix, expected): diff --git a/tests/unit/test_wire_actions.py b/tests/unit/test_wire_actions.py new file mode 100644 index 0000000..bf43772 --- /dev/null +++ b/tests/unit/test_wire_actions.py @@ -0,0 +1,100 @@ +""" +Unit tests for wire protocol action field parsing. + +Tests that decode_status correctly parses ACTION_CURRENT, ACTION_STATE, and ACTION_NEXT fields. +""" + +from parol6.protocol import wire + + +def test_decode_status_with_action_fields(): + """Test that decode_status parses ACTION_* fields from status string.""" + # Build status string with action fields + resp = ( + "STATUS|" + "POSE=" + ",".join(str(i) for i in range(1, 17)) + "|" + "ANGLES=0,10,20,30,40,50|" + "IO=1,1,0,0,1|" + "GRIPPER=1,20,150,500,3,1|" + "ACTION_CURRENT=MovePoseCommand|" + "ACTION_STATE=EXECUTING|" + "ACTION_NEXT=HomeCommand" + ) + + result = wire.decode_status(resp) + + assert result is not None + assert isinstance(result, dict) + + # Verify traditional fields still work + assert len(result["pose"]) == 16 + assert result["angles"] == [0.0, 10.0, 20.0, 30.0, 40.0, 50.0] + assert result["io"] == [1, 1, 0, 0, 1] + assert result["gripper"] == [1, 20, 150, 500, 3, 1] + + # Verify new action fields + assert result["action_current"] == "MovePoseCommand" + assert result["action_state"] == "EXECUTING" + + +def test_decode_status_with_empty_action_fields(): + """Test parsing when action fields are present but empty.""" + resp = ( + "STATUS|" + "POSE=" + ",".join(str(i) for i in range(1, 17)) + "|" + "ANGLES=0,0,0,0,0,0|" + "IO=1,1,0,0,1|" + "GRIPPER=0,0,0,0,0,0|" + "ACTION_CURRENT=|" + "ACTION_STATE=IDLE|" + "ACTION_NEXT=" + ) + + result = wire.decode_status(resp) + + assert result is not None + assert result["action_current"] == "" + assert result["action_state"] == "IDLE" + + +def test_decode_status_backward_compatible_without_actions(): + """Test that status without ACTION_* fields still decodes (backward compat).""" + # Old-style status without action fields + resp = ( + "STATUS|" + "POSE=" + ",".join(str(i) for i in range(1, 17)) + "|" + "ANGLES=0,10,20,30,40,50|" + "IO=1,1,0,0,1|" + "GRIPPER=1,20,150,500,3,1" + ) + + result = wire.decode_status(resp) + + assert result is not None + # Traditional fields should work + assert result["angles"] == [0.0, 10.0, 20.0, 30.0, 40.0, 50.0] + + # Action fields should be None when not present + assert result.get("action_current") is None + assert result.get("action_state") is None + + +def test_decode_status_with_various_action_states(): + """Test parsing with different action state values.""" + states = ["IDLE", "EXECUTING", "COMPLETED", "FAILED"] + + for state_value in states: + resp = ( + "STATUS|" + "POSE=" + ",".join("0" for _ in range(16)) + "|" + "ANGLES=0,0,0,0,0,0|" + "IO=1,1,0,0,1|" + "GRIPPER=0,0,0,0,0,0|" + f"ACTION_CURRENT=TestCommand|" + f"ACTION_STATE={state_value}|" + f"ACTION_NEXT=NextCommand" + ) + + result = wire.decode_status(resp) + assert result is not None + assert result["action_state"] == state_value diff --git a/tests/unit/test_wire_pack.py b/tests/unit/test_wire_pack.py index 05ca8aa..4b125e6 100644 --- a/tests/unit/test_wire_pack.py +++ b/tests/unit/test_wire_pack.py @@ -1,6 +1,4 @@ -import pytest import numpy as np - from parol6.protocol import wire from parol6.protocol.wire import CommandCode @@ -9,7 +7,7 @@ def test_pack_tx_frame_structure_and_command_byte(): position_out = [1, 2, 3, 4, 5, 6] speed_out = [10, 20, 30, 40, 50, 60] affected_joint_out = [1, 0, 0, 0, 0, 0, 0, 1] # MSB..LSB - inout_out = [0, 1, 0, 1, 0, 1, 0, 1] # MSB..LSB + inout_out = [0, 1, 0, 1, 0, 1, 0, 1] # MSB..LSB timeout_out = 7 gripper_data_out = [123, 45, 67, 3, 0, 5] # pos, spd, cur, cmd, mode, id diff --git a/tests/utils/__init__.py b/tests/utils/__init__.py index 35ff7b8..7cc9d38 100644 --- a/tests/utils/__init__.py +++ b/tests/utils/__init__.py @@ -4,10 +4,10 @@ Provides helper functions and classes for testing the PAROL6 Python API. """ -from .process import HeadlessCommanderProc, wait_for_completion, find_available_ports +from .process import HeadlessCommanderProc, find_available_ports, wait_for_completion __all__ = [ - 'HeadlessCommanderProc', - 'wait_for_completion', - 'find_available_ports', + "HeadlessCommanderProc", + "wait_for_completion", + "find_available_ports", ] diff --git a/tests/utils/process.py b/tests/utils/process.py index 0110568..546636d 100644 --- a/tests/utils/process.py +++ b/tests/utils/process.py @@ -5,14 +5,14 @@ during integration tests, including startup, readiness checks, and cleanup. """ +import logging import os -import sys -import time import socket import subprocess +import sys import threading -from typing import Optional, Dict, Any, List -import logging +import time +from typing import Any logger = logging.getLogger(__name__) @@ -20,21 +20,21 @@ class HeadlessCommanderProc: """ Manages a controller.py subprocess for integration testing. - + Handles starting, stopping, and checking readiness of the commander process with configurable ports and environment variables. """ - + def __init__( - self, - ip: str = "127.0.0.1", - port: int = 5001, + self, + ip: str = "127.0.0.1", + port: int = 5001, ack_port: int = 5002, - env: Optional[Dict[str, str]] = None + env: dict[str, str] | None = None, ): """ Initialize the process manager. - + Args: ip: IP address for the server to bind to port: UDP port for command reception @@ -45,60 +45,59 @@ def __init__( self.port = port self.ack_port = ack_port self.env = env or {} - - self.process: Optional[subprocess.Popen] = None - self._output_lines: List[str] = [] - self._error_lines: List[str] = [] - self._output_thread: Optional[threading.Thread] = None - self._error_thread: Optional[threading.Thread] = None - + + self.process: subprocess.Popen | None = None + self._output_lines: list[str] = [] + self._error_lines: list[str] = [] + self._output_thread: threading.Thread | None = None + self._error_thread: threading.Thread | None = None + def start(self, log_level: str = "WARNING", timeout: float = 10.0) -> bool: """ Start the headless commander process. - + Args: log_level: Logging level for the subprocess timeout: Maximum time to wait for process startup - + Returns: True if started successfully, False otherwise """ if self.process and self.process.poll() is None: logger.warning("Process already running") return True - + # Prepare environment process_env = os.environ.copy() - process_env.update({ - "PAROL6_FAKE_SERIAL": "1", # Enable fake serial simulation - "PAROL6_NOAUTOHOME": "1", # Disable auto-homing for tests - "PAROL6_SERVER_IP": self.ip, - "PAROL6_SERVER_PORT": str(self.port), - "PAROL6_ACK_PORT": str(self.ack_port), - }) + process_env.update( + { + "PAROL6_FAKE_SERIAL": "1", # Enable fake serial simulation + "PAROL6_NOAUTOHOME": "1", # Disable auto-homing for tests + "PAROL6_SERVER_IP": self.ip, + "PAROL6_SERVER_PORT": str(self.port), + "PAROL6_ACK_PORT": str(self.ack_port), + } + ) process_env.update(self.env) - + # Find the controller.py script script_path = os.path.join( - os.path.dirname(os.path.dirname(os.path.dirname(__file__))), - "controller.py" + os.path.dirname(os.path.dirname(os.path.dirname(__file__))), "controller.py" ) - + if not os.path.exists(script_path): logger.error(f"controller.py not found at {script_path}") return False - + # Prepare command - cmd = [ - sys.executable, script_path, - "--log-level", log_level, - "--no-auto-home" - ] - + cmd = [sys.executable, script_path, "--log-level", log_level, "--no-auto-home"] + try: logger.info(f"Starting headless commander: {' '.join(cmd)}") - logger.debug(f"Environment: FAKE_SERIAL=1, NOAUTOHOME=1, IP={self.ip}, PORT={self.port}, ACK_PORT={self.ack_port}") - + logger.debug( + f"Environment: FAKE_SERIAL=1, NOAUTOHOME=1, IP={self.ip}, PORT={self.port}, ACK_PORT={self.ack_port}" + ) + self.process = subprocess.Popen( cmd, env=process_env, @@ -106,213 +105,219 @@ def start(self, log_level: str = "WARNING", timeout: float = 10.0) -> bool: stderr=subprocess.PIPE, text=True, bufsize=1, # Line buffered - universal_newlines=True + universal_newlines=True, ) - + # Start output capture threads self._start_output_capture() - + # Wait for process to be ready if self.wait_ready(timeout): - logger.info(f"Headless commander started successfully (PID: {self.process.pid})") + logger.info( + f"Headless commander started successfully (PID: {self.process.pid})" + ) return True else: logger.error("Process failed to become ready within timeout") self.stop() return False - + except Exception as e: logger.error(f"Failed to start process: {e}") if self.process: self.process.terminate() self.process = None return False - + def _start_output_capture(self): """Start threads to capture stdout and stderr.""" if not self.process: return - + def capture_output(stream, output_list): try: - for line in iter(stream.readline, ''): + for line in iter(stream.readline, ""): if line: - line = line.rstrip('\n\r') + line = line.rstrip("\n\r") output_list.append(line) # Also log to our test logger for debugging logger.debug(f"PROC: {line}") except Exception as e: logger.debug(f"Output capture error: {e}") - + self._output_thread = threading.Thread( - target=capture_output, + target=capture_output, args=(self.process.stdout, self._output_lines), - daemon=True + daemon=True, ) self._error_thread = threading.Thread( - target=capture_output, + target=capture_output, args=(self.process.stderr, self._error_lines), - daemon=True + daemon=True, ) - + self._output_thread.start() self._error_thread.start() - + def wait_ready(self, timeout: float = 10.0) -> bool: """ Wait for the process to be ready by sending PING commands. - + Args: timeout: Maximum time to wait in seconds - + Returns: True if process responds to PING, False otherwise """ start_time = time.time() - + while time.time() - start_time < timeout: if self.process and self.process.poll() is not None: # Process died logger.error("Process terminated during startup") return False - + # Try to ping the server try: with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: sock.settimeout(1.0) sock.sendto(b"PING", (self.ip, self.port)) - + try: data, _ = sock.recvfrom(1024) - if data.decode('utf-8').strip() == "PONG": + if data.decode("utf-8").strip() == "PONG": return True - except socket.timeout: + except TimeoutError: pass # No response yet - + except Exception as e: logger.debug(f"Ping attempt failed: {e}") - + time.sleep(0.5) # Wait before retry - + return False - + def stop(self) -> bool: """ Stop the headless commander process. - + Returns: True if stopped successfully, False otherwise """ if not self.process: return True - + try: # Try graceful shutdown first logger.debug("Attempting graceful shutdown...") self.process.terminate() - + try: self.process.wait(timeout=5.0) - logger.info(f"Process terminated gracefully (exit code: {self.process.returncode})") + logger.info( + f"Process terminated gracefully (exit code: {self.process.returncode})" + ) except subprocess.TimeoutExpired: - logger.warning("Process did not terminate gracefully, forcing shutdown...") + logger.warning( + "Process did not terminate gracefully, forcing shutdown..." + ) self.process.kill() self.process.wait() logger.info(f"Process killed (exit code: {self.process.returncode})") - + except Exception as e: logger.error(f"Error stopping process: {e}") return False finally: self.process = None - + return True - + def is_running(self) -> bool: """Check if the process is currently running.""" return self.process is not None and self.process.poll() is None - - def get_output_lines(self) -> List[str]: + + def get_output_lines(self) -> list[str]: """Get captured stdout lines.""" return self._output_lines.copy() - - def get_error_lines(self) -> List[str]: + + def get_error_lines(self) -> list[str]: """Get captured stderr lines.""" return self._error_lines.copy() - + def clear_output(self): """Clear captured output lines.""" self._output_lines.clear() self._error_lines.clear() -def wait_for_completion(result_or_id: Any, timeout: float = 10.0) -> Dict[str, Any]: +def wait_for_completion(result_or_id: Any, timeout: float = 10.0) -> dict[str, Any]: """ Unified waiting logic for acknowledgment-tracked results in tests. - + Handles both direct result dictionaries and command IDs that need polling. - + Args: result_or_id: Either a result dict with status info, or a command ID string timeout: Maximum time to wait for completion - + Returns: Dictionary with status information """ if isinstance(result_or_id, dict): # Already a result dictionary return result_or_id - + # If it's a string, it might be a command ID - for now just return a placeholder # In a real implementation, this would poll the robot_api for status return { - 'status': 'TIMEOUT', - 'details': 'wait_for_completion not fully implemented for command IDs', - 'completed': True, - 'command_id': result_or_id + "status": "TIMEOUT", + "details": "wait_for_completion not fully implemented for command IDs", + "completed": True, + "command_id": result_or_id, } -def find_available_ports(start_port: int = 5001, count: int = 2) -> List[int]: +def find_available_ports(start_port: int = 5001, count: int = 2) -> list[int]: """ Find available UDP ports starting from the given port. - + Args: start_port: Port to start searching from count: Number of consecutive ports needed - + Returns: List of available port numbers """ - available_ports = [] + available_ports: list[int] = [] current_port = start_port - + while len(available_ports) < count: try: with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: - sock.bind(('127.0.0.1', current_port)) + sock.bind(("127.0.0.1", current_port)) available_ports.append(current_port) except OSError: # Port in use, reset search if we were building a consecutive sequence available_ports.clear() - + current_port += 1 - + # Prevent infinite loop if current_port > start_port + 1000: break - + return available_ports -def check_port_available(port: int, host: str = '127.0.0.1') -> bool: +def check_port_available(port: int, host: str = "127.0.0.1") -> bool: """ Check if a UDP port is available. - + Args: port: Port number to check host: Host address to check - + Returns: True if port is available, False otherwise """ diff --git a/tests/utils/udp.py b/tests/utils/udp.py index 4526b72..a408a8b 100644 --- a/tests/utils/udp.py +++ b/tests/utils/udp.py @@ -5,12 +5,12 @@ including acknowledgment listening and command sending utilities. """ -import socket -import time -import threading -from typing import Optional, Dict, Any, List, Tuple import logging import queue +import socket +import threading +import time +from typing import Any logger = logging.getLogger(__name__) @@ -19,64 +19,64 @@ class UDPClient: """ Simple UDP client for sending commands to the robot server during tests. """ - + def __init__(self, server_ip: str = "127.0.0.1", server_port: int = 5001): """ Initialize UDP client. - + Args: server_ip: IP address of the robot server server_port: Port of the robot server """ self.server_ip = server_ip self.server_port = server_port - - def send_command(self, command: str, timeout: float = 2.0) -> Optional[str]: + + def send_command(self, command: str, timeout: float = 2.0) -> str | None: """ Send a command and wait for immediate response (for GET commands). - + Args: command: Command string to send timeout: Timeout for waiting for response - + Returns: Response string if received, None otherwise """ try: with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: sock.settimeout(timeout) - + # Send command - sock.sendto(command.encode('utf-8'), (self.server_ip, self.server_port)) + sock.sendto(command.encode("utf-8"), (self.server_ip, self.server_port)) logger.debug(f"Sent command: {command}") - + # Wait for response (for GET commands) try: data, _ = sock.recvfrom(2048) - response = data.decode('utf-8') + response = data.decode("utf-8") logger.debug(f"Received response: {response}") return response - except socket.timeout: + except TimeoutError: logger.debug(f"No response received for command: {command}") return None - + except Exception as e: logger.error(f"Error sending command '{command}': {e}") return None - + def send_command_no_response(self, command: str) -> bool: """ Send a command without waiting for response (for motion commands). - + Args: command: Command string to send - + Returns: True if sent successfully, False otherwise """ try: with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: - sock.sendto(command.encode('utf-8'), (self.server_ip, self.server_port)) + sock.sendto(command.encode("utf-8"), (self.server_ip, self.server_port)) logger.debug(f"Sent command (no response): {command}") return True except Exception as e: @@ -87,125 +87,122 @@ def send_command_no_response(self, command: str) -> bool: class AckListener: """ Listens for UDP acknowledgment messages during tests. - + Provides functionality to capture and wait for specific acknowledgments from the robot controller during command execution. """ - + def __init__(self, listen_port: int = 5002): """ Initialize acknowledgment listener. - + Args: listen_port: Port to listen on for acknowledgments """ self.listen_port = listen_port - self.socket: Optional[socket.socket] = None - self.thread: Optional[threading.Thread] = None + self.socket: socket.socket | None = None + self.thread: threading.Thread | None = None self.running = False - + # Storage for received acknowledgments - self.ack_queue = queue.Queue() - self.ack_history: List[Dict[str, Any]] = [] - + self.ack_queue: queue.Queue[dict[str, Any]] = queue.Queue() + self.ack_history: list[dict[str, Any]] = [] + def start(self) -> bool: """ Start listening for acknowledgments. - + Returns: True if started successfully, False otherwise """ if self.running: logger.warning("AckListener already running") return True - + try: self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - self.socket.bind(('127.0.0.1', self.listen_port)) + self.socket.bind(("127.0.0.1", self.listen_port)) self.socket.settimeout(0.1) # Short timeout for non-blocking operation - + self.running = True self.thread = threading.Thread(target=self._listen_loop, daemon=True) self.thread.start() - + logger.debug(f"AckListener started on port {self.listen_port}") return True - + except Exception as e: logger.error(f"Failed to start AckListener: {e}") self.stop() return False - + def stop(self): """Stop listening for acknowledgments.""" self.running = False - + if self.thread: self.thread.join(timeout=1.0) self.thread = None - + if self.socket: self.socket.close() self.socket = None - + logger.debug("AckListener stopped") - + def _listen_loop(self): """Main listening loop (runs in separate thread).""" while self.running and self.socket: try: data, addr = self.socket.recvfrom(1024) - message = data.decode('utf-8') - + message = data.decode("utf-8") + # Parse acknowledgment message: ACK|cmd_id|status|details - parts = message.split('|', 3) - if parts[0] == 'ACK' and len(parts) >= 3: + parts = message.split("|", 3) + if parts[0] == "ACK" and len(parts) >= 3: ack_info = { - 'cmd_id': parts[1], - 'status': parts[2], - 'details': parts[3] if len(parts) > 3 else '', - 'timestamp': time.time(), - 'sender': addr + "cmd_id": parts[1], + "status": parts[2], + "details": parts[3] if len(parts) > 3 else "", + "timestamp": time.time(), + "sender": addr, } - + # Add to both queue and history self.ack_queue.put(ack_info) self.ack_history.append(ack_info) - + logger.debug(f"Received ACK: {ack_info}") - - except socket.timeout: + + except TimeoutError: continue # Normal timeout, keep listening except Exception as e: if self.running: # Only log if we should still be running logger.debug(f"Listen loop error: {e}") - + def wait_for_ack( - self, - cmd_id: str, - timeout: float = 5.0, - expected_status: Optional[str] = None - ) -> Optional[Dict[str, Any]]: + self, cmd_id: str, timeout: float = 5.0, expected_status: str | None = None + ) -> dict[str, Any] | None: """ Wait for a specific acknowledgment. - + Args: cmd_id: Command ID to wait for timeout: Maximum time to wait expected_status: Specific status to wait for (optional) - + Returns: Acknowledgment info dict if received, None if timeout """ start_time = time.time() - + while time.time() - start_time < timeout: try: # Check queue with short timeout ack_info = self.ack_queue.get(timeout=0.1) - - if ack_info['cmd_id'] == cmd_id: - if expected_status is None or ack_info['status'] == expected_status: + + if ack_info["cmd_id"] == cmd_id: + if expected_status is None or ack_info["status"] == expected_status: return ack_info else: # Put back in queue if status doesn't match @@ -213,37 +210,39 @@ def wait_for_ack( else: # Put back in queue if cmd_id doesn't match self.ack_queue.put(ack_info) - + except queue.Empty: continue - - logger.debug(f"Timeout waiting for ACK: cmd_id={cmd_id}, expected_status={expected_status}") + + logger.debug( + f"Timeout waiting for ACK: cmd_id={cmd_id}, expected_status={expected_status}" + ) return None - - def get_all_acks_for_command(self, cmd_id: str) -> List[Dict[str, Any]]: + + def get_all_acks_for_command(self, cmd_id: str) -> list[dict[str, Any]]: """ Get all acknowledgments received for a specific command ID. - + Args: cmd_id: Command ID to search for - + Returns: List of acknowledgment info dicts """ - return [ack for ack in self.ack_history if ack['cmd_id'] == cmd_id] - - def get_recent_acks(self, count: int = 10) -> List[Dict[str, Any]]: + return [ack for ack in self.ack_history if ack["cmd_id"] == cmd_id] + + def get_recent_acks(self, count: int = 10) -> list[dict[str, Any]]: """ Get the most recent acknowledgments. - + Args: count: Number of recent acknowledgments to return - + Returns: List of recent acknowledgment info dicts """ return self.ack_history[-count:] if self.ack_history else [] - + def clear_history(self): """Clear acknowledgment history and queue.""" self.ack_history.clear() @@ -256,24 +255,24 @@ def clear_history(self): def send_command_with_ack( command: str, - server_ip: str = "127.0.0.1", + server_ip: str = "127.0.0.1", server_port: int = 5001, ack_port: int = 5002, - timeout: float = 5.0 -) -> Tuple[Optional[str], Optional[Dict[str, Any]]]: + timeout: float = 5.0, +) -> tuple[str | None, dict[str, Any] | None]: """ Send a command with acknowledgment tracking. - + This is a convenience function that sets up an acknowledgment listener, sends a command, and waits for the acknowledgment. - + Args: command: Command to send server_ip: Server IP address server_port: Server command port ack_port: Acknowledgment port timeout: Timeout for acknowledgment - + Returns: Tuple of (immediate_response, final_ack_info) """ @@ -281,98 +280,100 @@ def send_command_with_ack( ack_listener = AckListener(ack_port) if not ack_listener.start(): return None, None - + try: # Send command client = UDPClient(server_ip, server_port) response = client.send_command(command, timeout=1.0) - + # For tracked commands, the response might be empty and we need to wait for ACK # For immediate commands (GET_*), we get response right away - if response and not response.startswith('ACK'): + if response and not response.startswith("ACK"): # Got immediate response, no ACK expected return response, None - + # Wait for acknowledgment (extract command ID if present) # This is a simplified version - in practice, you'd extract the actual command ID # from the command string if it contains one - parts = command.split('|', 1) - if len(parts) > 1 and len(parts[0]) == 8 and parts[0].replace('-', '').isalnum(): + parts = command.split("|", 1) + if ( + len(parts) > 1 + and len(parts[0]) == 8 + and parts[0].replace("-", "").isalnum() + ): cmd_id = parts[0] ack_info = ack_listener.wait_for_ack(cmd_id, timeout) return response, ack_info else: # No command ID in command, can't track ACK return response, None - + finally: ack_listener.stop() -def create_tracked_command(base_command: str, cmd_id: Optional[str] = None) -> str: +def create_tracked_command(base_command: str, cmd_id: str | None = None) -> str: """ Create a command with tracking ID. - + Args: base_command: Base command string cmd_id: Command ID to use (generates one if None) - + Returns: Command string with tracking ID prepended """ import uuid + if cmd_id is None: cmd_id = str(uuid.uuid4())[:8] - + return f"{cmd_id}|{base_command}" -def parse_server_response(response: str) -> Dict[str, Any]: +def parse_server_response(response: str) -> dict[str, Any]: """ Parse a server response into a structured format. - + Args: response: Raw response string from server - + Returns: Dictionary with parsed response data """ if not response: - return {'type': 'empty', 'data': None} - - parts = response.split('|', 1) + return {"type": "empty", "data": None} + + parts = response.split("|", 1) response_type = parts[0] - - parsed = { - 'type': response_type, - 'raw': response - } - + + parsed: dict[str, Any] = {"type": response_type, "raw": response} + if len(parts) > 1: data = parts[1] - - if response_type in ['POSE', 'ANGLES', 'SPEEDS']: + + if response_type in ["POSE", "ANGLES", "SPEEDS"]: # Numeric array data try: - parsed['data'] = [float(x) for x in data.split(',')] + parsed["data"] = [float(x) for x in data.split(",")] except ValueError: - parsed['data'] = data - elif response_type in ['IO', 'GRIPPER']: + parsed["data"] = data + elif response_type in ["IO", "GRIPPER"]: # Integer array data try: - parsed['data'] = [int(x) for x in data.split(',')] + parsed["data"] = [int(x) for x in data.split(",")] except ValueError: - parsed['data'] = data - elif response_type == 'STATUS': + parsed["data"] = data + elif response_type == "STATUS": # Complex status data - parsed['data'] = {} - for item in data.split('|'): - if '=' in item: - key, value = item.split('=', 1) - parsed['data'][key] = value + parsed["data"] = {} + for item in data.split("|"): + if "=" in item: + key, value = item.split("=", 1) + parsed["data"][key] = value else: - parsed['data'] = data + parsed["data"] = data else: - parsed['data'] = None - + parsed["data"] = None + return parsed