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 new file mode 100644 index 0000000..26ef302 --- /dev/null +++ b/.gitignore @@ -0,0 +1,127 @@ +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# venv +venv/ +ENV/ +env/ +.env +.venv + +# virtualenv +# Use '.env' for consistency. +# .env + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ + +# VS Code +.vscode/ + +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/PAROL6_ROBOT.py b/PAROL6_ROBOT.py deleted file mode 100644 index 09234e9..0000000 --- a/PAROL6_ROBOT.py +++ /dev/null @@ -1,340 +0,0 @@ -# This file acts as configuration file for robot you are using -# It works in conjustion with configuration file from robotics toolbox - -from swift import Swift -import spatialmath.base.symbolic as sym -from roboticstoolbox import ETS as ET -from roboticstoolbox import * -import roboticstoolbox as rtb -from spatialmath import * -from spatialgeometry import * -from math import pi -import numpy as np -import time -import random - -Joint_num = 6 # Number of joints -Microstep = 32 -steps_per_revolution=200 -degree_per_step_constant = 360/(32*200) -radian_per_step_constant = (2*pi) / (32*200) -radian_per_sec_2_deg_per_sec_const = 360/ (2*np.pi) -deg_per_sec_2_radian_per_sec_const = (2*np.pi) / 360 - -# robot length values (metres) -a1 = 110.50 / 1000 -a2 = 23.42 / 1000 -a3 = 180 / 1000 -a4 = 43.5 / 1000 -a5 = 176.35 / 1000 -a6 = 62.8 / 1000 -a7 = 45.25 / 1000 - -alpha_DH = [-pi / 2,pi,pi/2,-pi/2,pi/2,pi] - -robot = DHRobot( - [ - RevoluteDH(d=a1, a=a2, alpha=alpha_DH[0]), - RevoluteDH(a=a3,d = 0,alpha=alpha_DH[1]), - RevoluteDH(alpha= alpha_DH[2], a= -a4), - RevoluteDH(d=-a5, a=0, alpha=alpha_DH[3]), - RevoluteDH(a=0,d=0,alpha=alpha_DH[4]), - RevoluteDH(alpha=alpha_DH[5], a = -a7,d = -a6), - ], - name="PAROL6", -) -#print(robot.isspherical()) -#pyplot = rtb.backends.PyPlot() - -# in degrees -Joints_standby_position_degree = np.array([0,-90,180,0,0,180]) -# in radians -Joints_standby_position_radian = [np.deg2rad(angle) for angle in Joints_standby_position_degree] - -# values you get after homing robot and moving it to its most left and right sides -# In degrees -Joint_limits_degree =[[-123.046875,123.046875], [-145.0088,-3.375], [107.866,287.8675], [-105.46975,105.46975], [-90,90], [0,360]] - -# in radians -Joint_limits_radian = [] -for limits in Joint_limits_degree: - radian_limits = [np.deg2rad(angle) for angle in limits] - Joint_limits_radian.append(radian_limits) - -# Reduction ratio we have on our joints -Joint_reduction_ratio = [6.4, 20, 20*(38/42) , 4, 4, 10] - -# min and max jog speeds. Usually slower from real maximal speeds -Joint_max_jog_speed = [1500, 3000, 3600, 7000, 7000, 18000] -Joint_min_jog_speed = [100,100,100,100,100,100] - -# LINEAR CARTESIAN JOG MAX MIN SPEED IN METERS PER SECOND -Cartesian_linear_velocity_min_JOG = 0.002 -Cartesian_linear_velocity_max_JOG = 0.06 - -# LINEAR CARTESIAN MAX MIN SPEED IN METERS PER SECOND -Cartesian_linear_velocity_min = 0.002 -Cartesian_linear_velocity_max = 0.06 - -# LINEAR CARTESIAN MAX MIN ACC IN METERS PER SECOND² -Cartesian_linear_acc_min = 0.002 -Cartesian_linear_acc_max = 0.06 - -# ANGULAR CARTESIAN JOG MAX MIN SPEED IN DEGREES PER SECOND -Cartesian_angular_velocity_min = 0.7 -Cartesian_angular_velocity_max = 25 - -Joint_max_speed = [6500,18000,20000,20000,22000,22000] # max speed in STEP/S used -Joint_min_speed = [100,100,100,100,100,100] # min speed in STEP/S used - -Joint_max_acc = 32000 # max acceleration in RAD/S² -Joint_min_acc = 100 # min acceleration in RAD/S² - -Cart_lin_velocity_limits = [[-100,100],[-100,100],[-100,100]] -Cart_ang_velocity_limits = [[-100,100],[-100,100],[-100,100]] - - -Commands_list = [ "Input","Output","Dummy","Begin","Home","Delay","End","Loop","MoveJoint","MovePose","SpeedJoint","MoveCart", - "MoveCart","MoveCartRelTRF","Gripper","Gripper_cal"] - -Commands_list_true = [item + "()" for item in Commands_list] - -# 360 / (200 * 32) = 0.05625 -def DEG2STEPS(Degrees, index): - Steps = Degrees / degree_per_step_constant * Joint_reduction_ratio[index] - return Steps - -Joint_limits_steps =[[DEG2STEPS(Joint_limits_degree[0][0],0),DEG2STEPS(Joint_limits_degree[0][1],0)], - [DEG2STEPS(Joint_limits_degree[1][0],1),DEG2STEPS(Joint_limits_degree[1][1],1)], - [DEG2STEPS(Joint_limits_degree[2][0],2),DEG2STEPS(Joint_limits_degree[2][1],2)], - [DEG2STEPS(Joint_limits_degree[3][0],3),DEG2STEPS(Joint_limits_degree[3][1],3)], - [DEG2STEPS(Joint_limits_degree[4][0],4),DEG2STEPS(Joint_limits_degree[4][1],4)], - [DEG2STEPS(Joint_limits_degree[5][0],5),DEG2STEPS(Joint_limits_degree[5][1],5)]] -Joint_limits_steps = [[int(i[0]),int(i[1])] for i in Joint_limits_steps] - - -def STEPS2DEG(Steps,index): - Degrees = Steps * degree_per_step_constant / Joint_reduction_ratio[index] - return Degrees - -def RAD2STEPS(Rads,index): - deg = np.rad2deg(Rads) - steps = DEG2STEPS(deg,index) - return steps - -def STEPS2RADS(Steps,index): - deg = STEPS2DEG(Steps,index) - rads = np.deg2rad(deg) - return rads - -def RAD2DEG(radian): - return np.rad2deg(radian) - -def DEG2RAD(degree): - return np.deg2rad(degree) - -def SPEED_STEPS2DEG(Steps_per_second,index): - - ''' Transform true RADS/S to true RPM. - Both these values are true values at witch MOTORS SPIN ''' - - degrees_per_step = degree_per_step_constant / Joint_reduction_ratio[index] - degrees_per_second = Steps_per_second * degrees_per_step - return degrees_per_second - -def SPEED_DEG2STEPS(Deg_per_second,index): - steps_per_second = Deg_per_second / degree_per_step_constant * Joint_reduction_ratio[index] - return steps_per_second - -def SPEED_STEP2RAD(Steps_per_second,index): - degrees_per_step = radian_per_step_constant / Joint_reduction_ratio[index] - rad_per_second = Steps_per_second * degrees_per_step - return rad_per_second - -def SPEED_RAD2STEP(Rad_per_second,index): - steps_per_second = Rad_per_second / radian_per_step_constant * Joint_reduction_ratio[index] - return steps_per_second - -def RAD_SEC_2_DEG_SEC(rad_per_sec): - return rad_per_sec * radian_per_sec_2_deg_per_sec_const - -def DEG_SEC_2_RAD_SEC(deg_per_sec): - return deg_per_sec * deg_per_sec_2_radian_per_sec_const - - -def check_joint_limits(q, target_q=None, allow_recovery=True): - """ - Check if joint angles are within their limits, with support for recovery movements. - - Parameters - ---------- - q : array_like - Current joint angles in radians - target_q : array_like, optional - Target joint angles in radians. If provided, recovery logic is applied. - allow_recovery : bool, optional - Whether to allow recovery movements when current position violates limits - - Returns - ------- - bool - True if movement is allowed (within limits or valid recovery), False otherwise - dict - Dictionary with joint limit violation details and recovery information - """ - q_array = np.array(q) - target_array = np.array(target_q) if target_q is not None else None - violations = {} - all_valid = True - - for i in range(min(len(q_array), len(Joint_limits_radian))): - min_limit = Joint_limits_radian[i][0] - max_limit = Joint_limits_radian[i][1] - current_pos = q_array[i] - - # Check if current position violates limits - current_violates = current_pos < min_limit or current_pos > max_limit - - if current_violates: - violation_type = 'below_min' if current_pos < min_limit else 'above_max' - - # If we have a target and recovery is enabled, check if it's a recovery movement - if target_array is not None and allow_recovery: - target_pos = target_array[i] - is_recovery = False - - if current_pos > max_limit: # Past upper limit - # Recovery means moving towards or below the upper limit - is_recovery = target_pos <= current_pos - recovery_direction = "move joint towards negative direction" - elif current_pos < min_limit: # Past lower limit - # Recovery means moving towards or above the lower limit - is_recovery = target_pos >= current_pos - recovery_direction = "move joint towards positive direction" - - violations[f'joint_{i+1}'] = { - 'current_value': current_pos, - 'target_value': target_pos if target_array is not None else None, - 'min_limit': min_limit, - 'max_limit': max_limit, - 'violation': violation_type, - 'is_recovery': is_recovery, - 'recovery_direction': recovery_direction if not is_recovery else None, - 'movement_allowed': is_recovery - } - - # Only flag as invalid if it's not a recovery movement - if not is_recovery: - all_valid = False - else: - # No target provided or recovery disabled - flag as violation - violations[f'joint_{i+1}'] = { - 'current_value': current_pos, - 'target_value': None, - 'min_limit': min_limit, - 'max_limit': max_limit, - 'violation': violation_type, - 'is_recovery': False, - 'recovery_direction': None, - 'movement_allowed': False - } - all_valid = False - elif target_array is not None: - # Current is within limits, check if target would violate - target_pos = target_array[i] - target_violates = target_pos < min_limit or target_pos > max_limit - - if target_violates: - target_violation_type = 'below_min' if target_pos < min_limit else 'above_max' - violations[f'joint_{i+1}'] = { - 'current_value': current_pos, - 'target_value': target_pos, - 'min_limit': min_limit, - 'max_limit': max_limit, - 'violation': f'target_{target_violation_type}', - 'is_recovery': False, - 'recovery_direction': None, - 'movement_allowed': False - } - all_valid = False - - return all_valid, violations - -def extract_from_can_id(can_id): - # Extracting ID2 (first 4 MSB) - id2 = (can_id >> 7) & 0xF - - # Extracting CAN Command (next 6 bits) - can_command = (can_id >> 1) & 0x3F - - # Extracting Error Bit (last bit) - error_bit = can_id & 0x1 - - return id2, can_command, error_bit - - -def combine_2_can_id(id2, can_command, error_bit): - # Combine components into an 11-bit CAN ID - can_id = 0 - - # Add ID2 (first 4 MSB) - can_id |= (id2 & 0xF) << 7 - - # Add CAN Command (next 6 bits) - can_id |= (can_command & 0x3F) << 1 - - # Add Error Bit (last bit) - can_id |= (error_bit & 0x1) - - return can_id - -# Fuse bitfield list to byte -def fuse_bitfield_2_bytearray(var_in): - number = 0 - for b in var_in: - number = (2 * number) + b - return bytes([number]) - -# Splits byte to bitfield list -def split_2_bitfield(var_in): - #return [var_in >> i & 1 for i in range(7,-1,-1)] - return [(var_in >> i) & 1 for i in range(7, -1, -1)] - - -if __name__ == "__main__": - """ - print(DEG2STEPS(180,2)) - print(STEPS2DEG(57905,2)) - print(RAD2STEPS(pi,5)) - print(STEPS2RADS(32000,5)) - print(SPEED_STEPS2DEG(1000,5)) - print(SPEED_STEP2RAD(1000,5)) - print(Joint_limits_radian) - print(Joints_standby_position_radian) - print(Joint_limits_steps) - print(Joint_limits_radian) - print(DEG2STEPS(-62.5,1)) - """ - - J0_var = STEPS2RADS(1,0) - J1_var = STEPS2RADS(1,1) - J2_var = STEPS2RADS(1,2) - J3_var = STEPS2RADS(1,3) - J4_var = STEPS2RADS(1,4) - J5_var = STEPS2RADS(1,5) - - - print("Joint 1 smallest step:",RAD2DEG(J0_var)) - print("Joint 2 smallest step:",RAD2DEG(J1_var)) - print("Joint 3 smallest step:",RAD2DEG(J2_var)) - print("Joint 4 smallest step:",RAD2DEG(J3_var)) - print("Joint 5 smallest step:",RAD2DEG(J4_var)) - print("Joint 6 smallest step:",RAD2DEG(J5_var)) - print("rad 2 step:",SPEED_RAD2STEP(-2.948504399390715 / 2,5)) - print("standby radian is",Joints_standby_position_radian) - - test = RAD2STEPS(0.0001,5) - print(test) - - #robot.ikine_LMS() - diff --git a/README.md b/README.md index 0786d66..90d17c5 100644 --- a/README.md +++ b/README.md @@ -1,820 +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 `headless_commander.py` script will immediately command the robot to home itself upon startup. This is done for convenience but can be disabled. To prevent automatic homing, comment out or delete the corresponding line in `headless_commander.py`. -* **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 (`headless_commander.py`)**: - - Runs on the computer physically connected to the robot via USB/Serial - - Maintains a high-frequency control loop (100Hz) 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 Remote Client (`robot_api.py`)**: - - 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**: - - `smooth_motion.py`: Advanced trajectory generation algorithms - - `PAROL6_ROBOT.py`: Robot-specific parameters and kinematic model - -### 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 commands support reference frame selection via the `frame` parameter: - -- **WRF (World Reference Frame)**: Default. All coordinates are interpreted relative to the robot's base coordinate system. -- **TRF (Tool Reference Frame)**: All coordinates are interpreted relative to the tool's current position and orientation. This means: - - Positions are relative to the tool's origin - - Planes (XY, XZ, YZ) are the tool's local planes, not world planes - - If the tool is rotated, the entire motion rotates with it - -#### `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' or 'TRF'). Default: 'WRF' - * `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 - * `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 in world XY plane - smooth_circle(center=[200, 0, 200], radius=50, plane='XY', duration=5.0) - - # Draw a circle in tool's XY plane (follows tool orientation) - smooth_circle(center=[0, 30, 0], radius=25, plane='XY', frame='TRF', duration=4.0) - ``` - -#### `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' or 'TRF'). 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 - * `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 - - # Arc in world coordinates - smooth_arc_center(end_pose=[250, 50, 200, 0, 0, 90], - center=[200, 0, 200], - duration=3.0) - - # Arc in tool coordinates (relative to tool position/orientation) - smooth_arc_center(end_pose=[30, 30, 0, 0, 0, 45], - center=[15, 15, 0], - frame='TRF', - 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' or 'TRF'). 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 - * `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 - - # Parametric arc in world frame - smooth_arc_parametric(end_pose=[250, 50, 200, 0, 0, 90], - radius=50, arc_angle=90, duration=3.0) - - # Parametric arc in tool frame - smooth_arc_parametric(end_pose=[40, 0, 0, 0, 0, 60], - radius=25, arc_angle=60, - frame='TRF', - speed_percentage=40) - ``` - -#### `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' or 'TRF'). 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) - * `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 - - # Spline through world coordinates - 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) - - # Spline through tool-relative coordinates - tool_waypoints = [ - [20, 0, 0, 0, 0, 0], - [20, 20, 10, 0, 0, 30], - [0, 20, 20, 0, 0, 60] - ] - smooth_spline(tool_waypoints, frame='TRF', duration=6.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' or 'TRF'). Default: 'WRF' - * `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.* -* > *Note: In TRF mode, the helix rises along the tool's Z-axis, not the world Z-axis.* -* **Python API Usage**: - ```python - from robot_api import smooth_helix - - # Vertical helix in world frame - smooth_helix(center=[200, 0, 150], radius=30, pitch=20, - height=100, duration=10.0) - - # Helix along tool's Z-axis (follows tool orientation) - smooth_helix(center=[0, 30, 0], radius=20, pitch=15, - height=60, frame='TRF', duration=8.0) - ``` - -#### `smooth_blend()` -* **Purpose**: Blend multiple motion segments smoothly. -* **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' or 'TRF') for all segments. 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 - - # Blend in world coordinates - 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) - - # Blend in tool coordinates (all segments relative to tool) - tool_segments = [ - {'type': 'LINE', 'end': [30, 0, 0, 0, 0, 0], 'duration': 2.0}, - {'type': 'CIRCLE', 'center': [30, 20, 0], 'radius': 20, - 'plane': 'XY', 'duration': 4.0, 'clockwise': False}, - {'type': 'LINE', 'end': [0, 20, 0, 0, 0, 0], 'duration': 2.0} - ] - smooth_blend(tool_segments, frame='TRF', blend_time=0.5, duration=10.0) - ``` -### 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 `headless_commander.py`) -Install Python 3 and the following packages on the computer connected to the robot: +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. + +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: + +## Development setup +For contributors working on this repository: ```bash -# Only needed for get_robot_pose() matrix conversion -pip3 install numpy==1.23.4 -pip3 install spatialmath +# From external/PAROL6-python-API/ +pip install -e .[dev] +pre-commit install +``` + +- 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 -# All other functionality uses only Python standard library: -# socket, threading, time, uuid, datetime, collections, typing +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 parol6 import RobotClient +with RobotClient() as c: + c.set_tool("PNEUMATIC") +``` + +Add a new tool by extending `TOOL_CONFIGS` with a name, description, and `transform` (SE3 → 4×4 matrix). + + +## Quickstart + +### 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()) ``` -**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: -* `headless_commander.py` - Main server/controller -* `PAROL6_ROBOT.py` - Robot configuration and kinematic model -* `smooth_motion.py` - Advanced trajectory generation -* `GUI/files/` folder structure - For imports to work correctly - -Optional: -* `com_port.txt` - Contains the USB COM port (e.g., COM5) - -#### 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 headless_commander.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: +### Sync client (convenience wrapper) +```python +from parol6 import RobotClient + +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 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 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() ``` -## 7. Troubleshooting -* **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 +## 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 + +Run an example from the repository root: + +```bash +python external/PAROL6-python-API/examples/manage_server_demo.py +``` -For additional support, refer to the [PAROL commander software repository](https://github.com/PCrnjak/PAROL-commander-software). -Or you can head over to the [PAROL6 Discord channel](https://discord.com/invite/prjUvjmGpZ) for extra support +## 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/com_port.txt b/com_port.txt deleted file mode 100644 index e78034d..0000000 --- a/com_port.txt +++ /dev/null @@ -1 +0,0 @@ -COM6 \ No newline at end of file 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/headless_commander.py b/headless_commander.py deleted file mode 100644 index ab3498b..0000000 --- a/headless_commander.py +++ /dev/null @@ -1,3611 +0,0 @@ -''' -A full fledged "API" for the PAROL6 robot. To use this, you should pair it with the "robot_api.py" where you can import commands -from said file and use them anywhere within your code. This Python script will handle sending and performing all the commands -to the PAROL6 robot, as well as E-Stop functionality and safety limitations. - -To run this program, you must use the "experimental-kinematics" branch of the "PAROL-commander-software" on GitHub -which can be found through this link: https://github.com/PCrnjak/PAROL-commander-software/tree/experimental_kinematics. -You must also save these files into the following folder: "Project Files\PAROL-commander-software\GUI\files". -''' - -# * If you press estop robot will stop and you need to enable it by pressing e - -from roboticstoolbox import DHRobot, RevoluteDH, ERobot, ELink, ETS, trapezoidal, quintic -import roboticstoolbox as rp -from math import pi, sin, cos -import numpy as np -from oclock import Timer, loop, interactiveloop -import time -import socket -from spatialmath import SE3 -import select -import serial -import platform -import os -import re -import logging -import struct -import keyboard -from typing import Optional, Tuple -from spatialmath.base import trinterp -from collections import namedtuple, deque -import GUI.files.PAROL6_ROBOT as PAROL6_ROBOT -from smooth_motion import CircularMotion, SplineMotion, MotionBlender - -# Set interval -INTERVAL_S = 0.01 -prev_time = 0 - -logging.basicConfig(level = logging.DEBUG, - format='%(asctime)s.%(msecs)03d %(levelname)s:\t%(message)s', - datefmt='%H:%M:%S' -) -logging.disable(logging.DEBUG) - - -my_os = platform.system() -if my_os == "Windows": - # Try to read the COM port from a file - try: - with open("com_port.txt", "r") as f: - com_port_str = f.read().strip() - ser = serial.Serial(port=com_port_str, baudrate=3000000, timeout=0) - print(f"Connected to saved COM port: {com_port_str}") - except (FileNotFoundError, serial.SerialException): - # If the file doesn't exist or the port is invalid, ask the user - while True: - try: - com_port = input("Enter the COM port (e.g., COM9): ") - ser = serial.Serial(port=com_port, baudrate=3000000, timeout=0) - print(f"Successfully connected to {com_port}") - # Save the successful port to the file - with open("com_port.txt", "w") as f: - f.write(com_port) - break - except serial.SerialException: - print(f"Could not open port {com_port}. Please try again.") - -# in big endian machines, first byte of binary representation of the multibyte data-type is stored first. -int_to_3_bytes = struct.Struct('>I').pack # BIG endian order - -# data for output string (data that is being sent to the robot) -####################################################################################### -####################################################################################### -start_bytes = [0xff,0xff,0xff] -start_bytes = bytes(start_bytes) - -end_bytes = [0x01,0x02] -end_bytes = bytes(end_bytes) - - -# data for input string (Data that is being sent by the robot) -####################################################################################### -####################################################################################### -input_byte = 0 # Here save incoming bytes from serial - -start_cond1_byte = bytes([0xff]) -start_cond2_byte = bytes([0xff]) -start_cond3_byte = bytes([0xff]) - -end_cond1_byte = bytes([0x01]) -end_cond2_byte = bytes([0x02]) - -start_cond1 = 0 #Flag if start_cond1_byte is received -start_cond2 = 0 #Flag if start_cond2_byte is received -start_cond3 = 0 #Flag if start_cond3_byte is received - -good_start = 0 #Flag if we got all 3 start condition bytes -data_len = 0 #Length of the data after -3 start condition bytes and length byte, so -4 bytes - -data_buffer = [None]*255 #Here save all data after data length byte -data_counter = 0 #Data counter for incoming bytes; compared to data length to see if we have correct length -####################################################################################### -####################################################################################### -prev_positions = [0,0,0,0,0,0] -prev_speed = [0,0,0,0,0,0] -robot_pose = [0,0,0,0,0,0] #np.array([0,0,0,0,0,0]) -####################################################################################### -####################################################################################### - -# --- Wrapper class to make integers mutable when passed to functions --- -class CommandValue: - def __init__(self, value): - self.value = value - -####################################################################################### -####################################################################################### -Position_out = [1,11,111,1111,11111,10] -Speed_out = [2,21,22,23,24,25] -Command_out = CommandValue(255) -Affected_joint_out = [1,1,1,1,1,1,1,1] -InOut_out = [0,0,0,0,0,0,0,0] -Timeout_out = 0 -#Positon,speed,current,command,mode,ID -Gripper_data_out = [1,1,1,1,0,0] -####################################################################################### -####################################################################################### -# Data sent from robot to PC -Position_in = [31,32,33,34,35,36] -Speed_in = [41,42,43,44,45,46] -Homed_in = [0,0,0,0,0,0,0,0] -InOut_in = [1,1,1,1,1,1,1,1] -Temperature_error_in = [1,1,1,1,1,1,1,1] -Position_error_in = [1,1,1,1,1,1,1,1] -Timeout_error = 0 -# how much time passed between 2 sent commands (2byte value, last 2 digits are decimal so max value is 655.35ms?) -Timing_data_in = [0] -XTR_data = 0 - -# --- State variables for program execution --- -Robot_mode = "Dummy" # Start in an idle state -Program_step = 0 # Which line of the program to run -Command_step = 0 # The current step within a single command -Command_len = 0 # The total steps for the current command -ik_error = 0 # Flag for inverse kinematics errors -error_state = 0 # General error flag -program_running = False # A flag to start and stop the program - -# This will be your "program" -command_list = [] - -#ID,Position,speed,current,status,obj_detection -Gripper_data_in = [1,1,1,1,1,1] - -# Global variable to track previous tolerance for logging changes -_prev_tolerance = None - -def normalize_angle(angle): - """Normalize angle to [-pi, pi] range to handle angle wrapping""" - while angle > np.pi: - angle -= 2 * np.pi - while angle < -np.pi: - angle += 2 * np.pi - return angle - -def unwrap_angles(q_solution, q_current): - """ - Unwrap angles in the solution to be closest to current position. - This handles the angle wrapping issue where -179° and 181° are close but appear far. - """ - q_unwrapped = q_solution.copy() - - for i in range(len(q_solution)): - # Calculate the difference - diff = q_solution[i] - q_current[i] - - # If the difference is more than pi, we need to unwrap - if diff > np.pi: - # Solution is too far in positive direction, subtract 2*pi - q_unwrapped[i] = q_solution[i] - 2 * np.pi - elif diff < -np.pi: - # Solution is too far in negative direction, add 2*pi - q_unwrapped[i] = q_solution[i] + 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. - - Parameters - ---------- - robot : DHRobot - Robot model - q : array_like - Joint configuration - strict_tol : float - Strict tolerance away from singularities (default: 1e-10) - loose_tol : float - Loose tolerance near singularities (1e-7) - - Returns - ------- - float - Adaptive tolerance value - """ - global _prev_tolerance - - q_array = np.array(q, dtype=float) - - # Calculate manipulability measure (closer to 0 = closer to singularity) - manip = robot.manipulability(q_array) - singularity_threshold = 0.001 - - sing_normalized = np.clip(manip / singularity_threshold, 0.0, 1.0) - adaptive_tol = loose_tol + (strict_tol - loose_tol) * sing_normalized - - # Log tolerance changes (only log significant changes to avoid spam) - if _prev_tolerance is None or abs(adaptive_tol - _prev_tolerance) / _prev_tolerance > 0.5: - tol_category = "LOOSE" if adaptive_tol > 1e-7 else "MODERATE" if adaptive_tol > 5e-10 else "STRICT" - print(f"Adaptive IK tolerance: {adaptive_tol:.2e} ({tol_category}) - Manipulability: {manip:.8f} (threshold: {singularity_threshold})") - _prev_tolerance = adaptive_tol - - return adaptive_tol - -def calculate_configuration_dependent_max_reach(q_seed): - """ - 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: - # Calculate reduction factor based on proximity to 90° - 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. - - Parameters - ---------- - robot : DHRobot - Robot model - 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: 8) - ilimit : int, optional - Maximum iterations for IK solver (default: 100) - - 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) - """ - 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).""" - # Calculate current and target reach - current_reach = np.linalg.norm(Ta.t) - target_reach = np.linalg.norm(Tb.t) - - # Check if this is an inward movement (recovery) - is_recovery = target_reach < current_reach - - # Calculate configuration-dependent maximum reach based on joint 5 position - max_reach_threshold = calculate_configuration_dependent_max_reach(q_seed) - - # Determine damping based on reach and movement direction - if is_recovery: - # Recovery mode - always use low damping - damping = 0.0000001 - else: - # Check if we're near configuration-dependent max reach - # print(f"current_reach:{current_reach:.3f}, max_reach_threshold:{max_reach_threshold:.3f}") - if not is_recovery and target_reach > max_reach_threshold: - print(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 - ) - - # ── 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) - # Check if solution respects joint limits - target_q = path[-1] if len(path) != 0 else None - solution_valid, violations = PAROL6_ROBOT.check_joint_limits(current_q, target_q) - if ok and solution_valid: - return IKResult(True, path[-1], its, resid, adaptive_tol, violations) - else: - return IKResult(False, None, its, resid, adaptive_tol, violations) - -#Setup IP address and Simulator port -ip = "127.0.0.1" #Loopback address -port = 5001 -# Create a UDP socket -sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) -sock.bind((ip, port)) -print(f'Start listening to {ip}:{port}') - -def Unpack_data(data_buffer_list, Position_in,Speed_in,Homed_in,InOut_in,Temperature_error_in,Position_error_in,Timeout_error,Timing_data_in, - XTR_data,Gripper_data_in): - - Joints = [] - Speed = [] - - for i in range(0,18, 3): - variable = data_buffer_list[i:i+3] - Joints.append(variable) - - for i in range(18,36, 3): - variable = data_buffer_list[i:i+3] - Speed.append(variable) - - - for i in range(6): - var = b'\x00' + b''.join(Joints[i]) - Position_in[i] = Fuse_3_bytes(var) - var = b'\x00' + b''.join(Speed[i]) - Speed_in[i] = Fuse_3_bytes(var) - - Homed = data_buffer_list[36] - IO_var = data_buffer_list[37] - temp_error = data_buffer_list[38] - position_error = data_buffer_list[39] - timing_data = data_buffer_list[40:42] - Timeout_error_var = data_buffer_list[42] - xtr2 = data_buffer_list[43] - device_ID = data_buffer_list[44] - Gripper_position = data_buffer_list[45:47] - Gripper_speed = data_buffer_list[47:49] - Gripper_current = data_buffer_list[49:51] - Status = data_buffer_list[51] - # The original object_detection byte at index 52 is ignored as it is not reliable. - CRC_byte = data_buffer_list[53] - endy_byte1 = data_buffer_list[54] - endy_byte2 = data_buffer_list[55] - - # ... (Code for Homed, IO_var, temp_error, etc. remains the same) ... - - temp = Split_2_bitfield(int.from_bytes(Homed,"big")) - for i in range(8): - Homed_in[i] = temp[i] - - temp = Split_2_bitfield(int.from_bytes(IO_var,"big")) - for i in range(8): - InOut_in[i] = temp[i] - - temp = Split_2_bitfield(int.from_bytes(temp_error,"big")) - for i in range(8): - Temperature_error_in[i] = temp[i] - - temp = Split_2_bitfield(int.from_bytes(position_error,"big")) - for i in range(8): - Position_error_in[i] = temp[i] - - var = b'\x00' + b'\x00' + b''.join(timing_data) - Timing_data_in[0] = Fuse_3_bytes(var) - Timeout_error = int.from_bytes(Timeout_error_var,"big") - XTR_data = int.from_bytes(xtr2,"big") - - # --- Gripper Data Unpacking --- - Gripper_data_in[0] = int.from_bytes(device_ID,"big") - - var = b'\x00'+ b'\x00' + b''.join(Gripper_position) - Gripper_data_in[1] = Fuse_2_bytes(var) - - var = b'\x00'+ b'\x00' + b''.join(Gripper_speed) - Gripper_data_in[2] = Fuse_2_bytes(var) - - var = b'\x00'+ b'\x00' + b''.join(Gripper_current) - Gripper_data_in[3] = Fuse_2_bytes(var) - - # --- Start of Corrected Logic --- - # This section now mirrors the working logic from GUI_PAROL_latest.py - - # 1. Store the raw status byte (from index 51) - status_byte = int.from_bytes(Status,"big") - Gripper_data_in[4] = status_byte - - # 2. Split the status byte into a list of 8 individual bits - status_bits = Split_2_bitfield(status_byte) - - # 3. Combine the 3rd and 4th bits (at indices 2 and 3) to get the true object detection status - # This creates a 2-bit number (0-3) which represents the full state. - object_detection_status = (status_bits[2] << 1) | status_bits[3] - Gripper_data_in[5] = object_detection_status - # --- End of Corrected Logic --- - -def Pack_data(Position_out,Speed_out,Command_out,Affected_joint_out,InOut_out,Timeout_out,Gripper_data_out): - - # Len is defined by all bytes EXCEPT start bytes and len - # Start bytes = 3 - len = 52 #1 - Position = [Position_out[0],Position_out[1],Position_out[2],Position_out[3],Position_out[4],Position_out[5]] #18 - Speed = [Speed_out[0], Speed_out[1], Speed_out[2], Speed_out[3], Speed_out[4], Speed_out[5],] #18 - Command = Command_out#1 - Affected_joint = Affected_joint_out - InOut = InOut_out #1 - Timeout = Timeout_out #1 - Gripper_data = Gripper_data_out #9 - CRC_byte = 228 #1 - # End bytes = 2 - - - test_list = [] - #print(test_list) - - #x = bytes(start_bytes) - test_list.append((start_bytes)) - - test_list.append(bytes([len])) - - - # Position data - for i in range(6): - position_split = Split_2_3_bytes(Position[i]) - test_list.append(position_split[1:4]) - - # Speed data - for i in range(6): - speed_split = Split_2_3_bytes(Speed[i]) - test_list.append(speed_split[1:4]) - - # Command data - test_list.append(bytes([Command])) - - # Affected joint data - Affected_list = Fuse_bitfield_2_bytearray(Affected_joint[:]) - test_list.append(Affected_list) - - # Inputs outputs data - InOut_list = Fuse_bitfield_2_bytearray(InOut[:]) - test_list.append(InOut_list) - - # Timeout data - test_list.append(bytes([Timeout])) - - # Gripper position - Gripper_position = Split_2_3_bytes(Gripper_data[0]) - test_list.append(Gripper_position[2:4]) - - # Gripper speed - Gripper_speed = Split_2_3_bytes(Gripper_data[1]) - test_list.append(Gripper_speed[2:4]) - - # Gripper current - Gripper_current = Split_2_3_bytes(Gripper_data[2]) - test_list.append(Gripper_current[2:4]) - - # Gripper command - test_list.append(bytes([Gripper_data[3]])) - # Gripper mode - test_list.append(bytes([Gripper_data[4]])) - - # ========================================================== - # === FIX: Make sure calibrate is a one-shot command ==== - # ========================================================== - # If the mode was set to calibrate (1) or clear_error (2), reset it - # back to normal (0) for the next cycle. This prevents an endless loop. - if Gripper_data_out[4] == 1 or Gripper_data_out[4] == 2: - Gripper_data_out[4] = 0 - # ========================================================== - - # Gripper ID - test_list.append(bytes([Gripper_data[5]])) - - # CRC byte - test_list.append(bytes([CRC_byte])) - - # END bytes - test_list.append((end_bytes)) - - #print(test_list) - return test_list - -def Get_data(Position_in,Speed_in,Homed_in,InOut_in,Temperature_error_in,Position_error_in,Timeout_error,Timing_data_in, - XTR_data,Gripper_data_in): - global input_byte - - global start_cond1_byte - global start_cond2_byte - global start_cond3_byte - - global end_cond1_byte - global end_cond2_byte - - global start_cond1 - global start_cond2 - global start_cond3 - - global good_start - global data_len - - global data_buffer - global data_counter - - while (ser.inWaiting() > 0): - input_byte = ser.read() - - #UNCOMMENT THIS TO GET ALL DATA FROM THE ROBOT PRINTED - #print(input_byte) - - # When data len is received start is good and after that put all data in receive buffer - # Data len is ALL data after it; that includes input buffer, end bytes and CRC - if (good_start != 1): - # All start bytes are good and next byte is data len - if (start_cond1 == 1 and start_cond2 == 1 and start_cond3 == 1): - good_start = 1 - data_len = input_byte - data_len = struct.unpack('B', data_len)[0] - logging.debug("data len we got from robot packet= ") - logging.debug(input_byte) - logging.debug("good start for DATA that we received at PC") - # Third start byte is good - if (input_byte == start_cond3_byte and start_cond2 == 1 and start_cond1 == 1): - start_cond3 = 1 - #print("good cond 3 PC") - #Third start byte is bad, reset all flags - elif (start_cond2 == 1 and start_cond1 == 1): - #print("bad cond 3 PC") - start_cond1 = 0 - start_cond2 = 0 - # Second start byte is good - if (input_byte == start_cond2_byte and start_cond1 == 1): - start_cond2 = 1 - #print("good cond 2 PC ") - #Second start byte is bad, reset all flags - elif (start_cond1 == 1): - #print("Bad cond 2 PC") - start_cond1 = 0 - # First start byte is good - if (input_byte == start_cond1_byte): - start_cond1 = 1 - #print("good cond 1 PC") - else: - # Here data goes after good start - data_buffer[data_counter] = input_byte - if (data_counter == data_len - 1): - - logging.debug("Data len PC") - logging.debug(data_len) - logging.debug("End bytes are:") - logging.debug(data_buffer[data_len -1]) - logging.debug(data_buffer[data_len -2]) - - # Here if last 2 bytes are end condition bytes we process the data - if (data_buffer[data_len -1] == end_cond2_byte and data_buffer[data_len - 2] == end_cond1_byte): - - logging.debug("GOOD END CONDITION PC") - logging.debug("I UNPACKED RAW DATA RECEIVED FROM THE ROBOT") - Unpack_data(data_buffer, Position_in,Speed_in,Homed_in,InOut_in,Temperature_error_in,Position_error_in,Timeout_error,Timing_data_in, - XTR_data,Gripper_data_in) - logging.debug("DATA UNPACK FINISHED") - # ako su dobri izračunaj crc - # if crc dobar raspakiraj podatke - # ako je dobar paket je dobar i spremi ga u nove variable! - - # Print every byte - #print("podaci u data bufferu su:") - #for i in range(data_len): - #print(data_buffer[i]) - - good_start = 0 - start_cond1 = 0 - start_cond3 = 0 - start_cond2 = 0 - data_len = 0 - data_counter = 0 - else: - data_counter = data_counter + 1 - -# Split data to 3 bytes -def Split_2_3_bytes(var_in): - y = int_to_3_bytes(var_in & 0xFFFFFF) # converts my int value to bytes array - return y - -# Splits byte to bitfield list -def Split_2_bitfield(var_in): - #return [var_in >> i & 1 for i in range(7,-1,-1)] - return [(var_in >> i) & 1 for i in range(7, -1, -1)] - -# Fuses 3 bytes to 1 signed int -def Fuse_3_bytes(var_in): - value = struct.unpack(">I", bytearray(var_in))[0] # converts bytes array to int - - # convert to negative number if it is negative - if value >= 1<<23: - value -= 1<<24 - - return value - -# Fuses 2 bytes to 1 signed int -def Fuse_2_bytes(var_in): - value = struct.unpack(">I", bytearray(var_in))[0] # converts bytes array to int - - # convert to negative number if it is negative - if value >= 1<<15: - value -= 1<<16 - - return value - -# Fuse bitfield list to byte -def Fuse_bitfield_2_bytearray(var_in): - number = 0 - for b in var_in: - number = (2 * number) + b - return bytes([number]) - -# Check if there is element 1 in the list. -# If yes return its index, if no element is 1 return -1 -def check_elements(lst): - for i, element in enumerate(lst): - if element == 1: - return i - return -1 # Return -1 if no element is 1 - -def quintic_scaling(s: float) -> float: - """ - Calculates a smooth 0-to-1 scaling factor for progress 's' - using a quintic polynomial, ensuring smooth start/end accelerations. - """ - return 6 * (s**5) - 15 * (s**4) + 10 * (s**3) - -######################################################################### -# Robot Commands Start Here -######################################################################### - -class HomeCommand: - """ - 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. - """ - def __init__(self): - self.is_valid = True - self.is_finished = False - # 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 - print("Initializing Home command...") - - def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): - """ - Manages the homing command and monitors for completion using a state machine. - """ - if self.is_finished: - return True - - # --- State: START --- - # On the first few executions, continuously send the 'home' (100) command. - if self.state == "START": - print(f" -> Sending home signal (100)... Countdown: {self.start_cmd_counter}") - Command_out.value = 100 - 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 False - - # --- 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": - Command_out.value = 255 - # Check if at least one joint has started homing (is no longer homed) - if any(h == 0 for h in Homed_in[:6]): - print(" -> Homing sequence initiated by robot.") - self.state = "WAITING_FOR_HOMED" - # Check for timeout - self.timeout_counter -= 1 - if self.timeout_counter <= 0: - print(" -> ERROR: Timeout waiting for robot to start homing sequence.") - self.is_finished = True - return self.is_finished - - # --- 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": - Command_out.value = 255 - # Check if all joints have finished homing - if all(h == 1 for h in Homed_in[:6]): - print("Homing sequence complete. All joints reported home.") - self.is_finished = True - Speed_out[:] = [0] * 6 # Ensure robot is stopped - - return self.is_finished - -class JogCommand: - """ - A non-blocking command to jog a joint for a specific duration or distance. - It performs all safety and validity checks upon initialization. - """ - def __init__(self, joint, speed_percentage=None, duration=None, distance_deg=None): - """ - Initializes and validates the jog command. This is the SETUP phase. - """ - self.is_valid = False - self.is_finished = False - self.mode = None - self.command_step = 0 - - # --- 1. Parameter Validation and Mode Selection --- - if duration and distance_deg: - self.mode = 'distance' - print(f"Initializing Jog: Joint {joint}, Distance {distance_deg} deg, Duration {duration}s.") - elif duration: - self.mode = 'time' - print(f"Initializing Jog: Joint {joint}, Speed {speed_percentage}%, Duration {duration}s.") - elif distance_deg: - self.mode = 'distance' - print(f"Initializing Jog: Joint {joint}, Speed {speed_percentage}%, Distance {distance_deg} deg.") - else: - print("Error: JogCommand requires either 'duration', 'distance_deg', or both.") - return - - # --- 2. Store parameters for deferred calculation --- - self.joint = joint - self.speed_percentage = speed_percentage - self.duration = duration - self.distance_deg = distance_deg - - # --- These will be calculated later --- - self.direction = 1 - self.joint_index = 0 - self.speed_out = 0 - self.command_len = 0 - self.target_position = 0 - - self.is_valid = True # Mark as valid for now; preparation step will confirm. - - - def prepare_for_execution(self, current_position_in): - """Pre-computes speeds and target positions using live data.""" - print(f" -> Preparing for Jog command...") - - # Determine direction and joint index - self.direction = 1 if 0 <= self.joint <= 5 else -1 - self.joint_index = self.joint if self.direction == 1 else self.joint - 6 - - distance_steps = 0 - if self.distance_deg: - distance_steps = int(PAROL6_ROBOT.DEG2STEPS(abs(self.distance_deg), self.joint_index)) - # --- MOVED LOGIC: Calculate target using the LIVE position --- - self.target_position = current_position_in[self.joint_index] + (distance_steps * self.direction) - - min_limit, max_limit = PAROL6_ROBOT.Joint_limits_steps[self.joint_index] - if not (min_limit <= self.target_position <= max_limit): - print(f" -> VALIDATION FAILED: Target position {self.target_position} is out of joint limits ({min_limit}, {max_limit}).") - self.is_valid = False - return - - # Calculate speed and duration - speed_steps_per_sec = 0 - if self.mode == 'distance' and self.duration: - speed_steps_per_sec = int(distance_steps / self.duration) if self.duration > 0 else 0 - max_joint_speed = PAROL6_ROBOT.Joint_max_speed[self.joint_index] - if speed_steps_per_sec > max_joint_speed: - print(f" -> VALIDATION FAILED: Required speed ({speed_steps_per_sec} steps/s) exceeds joint's max speed ({max_joint_speed} steps/s).") - self.is_valid = False - return - else: - if self.speed_percentage is None: - print("Error: 'speed_percentage' must be provided if not calculating automatically.") - self.is_valid = False - return - speed_steps_per_sec = int(np.interp(abs(self.speed_percentage), [0, 100], [0, PAROL6_ROBOT.Joint_max_speed[self.joint_index] * 2])) - - self.speed_out = speed_steps_per_sec * self.direction - self.command_len = int(self.duration / INTERVAL_S) if self.duration else float('inf') - print(" -> Jog command is ready.") - - - def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): - """This is the EXECUTION phase. It runs on every loop cycle.""" - if self.is_finished or not self.is_valid: - return True - - stop_reason = None - current_pos = Position_in[self.joint_index] - - if self.mode == 'time': - if self.command_step >= self.command_len: - stop_reason = "Timed jog finished." - elif self.mode == 'distance': - if (self.direction == 1 and current_pos >= self.target_position) or \ - (self.direction == -1 and current_pos <= self.target_position): - stop_reason = "Distance jog finished." - - if not stop_reason: - if (self.direction == 1 and current_pos >= PAROL6_ROBOT.Joint_limits_steps[self.joint_index][1]) or \ - (self.direction == -1 and current_pos <= PAROL6_ROBOT.Joint_limits_steps[self.joint_index][0]): - stop_reason = f"Limit reached on joint {self.joint_index + 1}." - - if stop_reason: - print(stop_reason) - self.is_finished = True - Speed_out[:] = [0] * 6 - Command_out.value = 255 - return True - else: - Speed_out[:] = [0] * 6 - Speed_out[self.joint_index] = self.speed_out - Command_out.value = 123 - self.command_step += 1 - return False - -class MultiJogCommand: - """ - A non-blocking command to jog multiple joints simultaneously for a specific duration. - It performs all safety and validity checks upon initialization. - """ - def __init__(self, joints, speed_percentages, duration): - """ - Initializes and validates the multi-jog command. - """ - self.is_valid = False - self.is_finished = False - self.command_step = 0 - - # --- 1. Parameter Validation --- - if not isinstance(joints, list) or not isinstance(speed_percentages, list): - print("Error: MultiJogCommand requires 'joints' and 'speed_percentages' to be lists.") - return - - if len(joints) != len(speed_percentages): - print("Error: The number of joints must match the number of speed percentages.") - return - - if not duration or duration <= 0: - print("Error: MultiJogCommand requires a positive 'duration'.") - return - - # ========================================================== - # === NEW: Check for conflicting joint commands === - # ========================================================== - base_joints = set() - for joint in joints: - # Normalize the joint index to its base (0-5) - base_joint = joint % 6 - # If the base joint is already in our set, it's a conflict. - if base_joint in base_joints: - print(f" -> VALIDATION FAILED: Conflicting commands for Joint {base_joint + 1} (e.g., J1+ and J1-).") - self.is_valid = False - return - base_joints.add(base_joint) - # ========================================================== - - print(f"Initializing MultiJog for joints {joints} with speeds {speed_percentages}% for {duration}s.") - - # --- 2. Store parameters --- - self.joints = joints - self.speed_percentages = speed_percentages - self.duration = duration - self.command_len = int(self.duration / INTERVAL_S) - - # --- This will be calculated in the prepare step --- - self.speeds_out = [0] * 6 - - self.is_valid = True - - def prepare_for_execution(self, current_position_in): - """Pre-computes the speeds for each joint.""" - print(f" -> Preparing for MultiJog command...") - - for i, joint in enumerate(self.joints): - # Determine direction and joint index (0-5 for positive, 6-11 for negative) - direction = 1 if 0 <= joint <= 5 else -1 - joint_index = joint if direction == 1 else joint - 6 - speed_percentage = self.speed_percentages[i] - - # Check for joint index validity - if not (0 <= joint_index < 6): - print(f" -> VALIDATION FAILED: Invalid joint index {joint_index}.") - self.is_valid = False - return - - # Calculate speed in steps/sec - speed_steps_per_sec = int(np.interp(speed_percentage, [0, 100], [0, PAROL6_ROBOT.Joint_max_speed[joint_index]])) - self.speeds_out[joint_index] = speed_steps_per_sec * direction - - print(" -> MultiJog command is ready.") - - - def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): - """This is the EXECUTION phase. It runs on every loop cycle.""" - if self.is_finished or not self.is_valid: - return True - - # Stop if the duration has elapsed - if self.command_step >= self.command_len: - print("Timed multi-jog finished.") - self.is_finished = True - Speed_out[:] = [0] * 6 - Command_out.value = 255 - return True - else: - # Continuously check for joint limits during the jog - for i in range(6): - if self.speeds_out[i] != 0: - current_pos = Position_in[i] - # Hitting positive limit while moving positively - if self.speeds_out[i] > 0 and current_pos >= PAROL6_ROBOT.Joint_limits_steps[i][1]: - print(f"Limit reached on joint {i + 1}. Stopping jog.") - self.is_finished = True - Speed_out[:] = [0] * 6 - Command_out.value = 255 - return True - # Hitting negative limit while moving negatively - elif self.speeds_out[i] < 0 and current_pos <= PAROL6_ROBOT.Joint_limits_steps[i][0]: - print(f"Limit reached on joint {i + 1}. Stopping jog.") - self.is_finished = True - Speed_out[:] = [0] * 6 - Command_out.value = 255 - return True - - # If no limits are hit, apply the speeds - Speed_out[:] = self.speeds_out - Command_out.value = 123 # Jog command - self.command_step += 1 - return False # Command is still running - -# This dictionary maps descriptive axis names to movement vectors, which is cleaner. -# 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]), -} - -class CartesianJogCommand: - """ - A non-blocking command to jog the robot's end-effector in Cartesian space. - This is the final, refactored version using clean, standard spatial math - operations now that the core unit bug has been fixed. - """ - def __init__(self, frame, axis, speed_percentage=50, duration=1.5, **kwargs): - """ - Initializes and validates the Cartesian jog command. - """ - self.is_valid = False - self.is_finished = False - print(f"Initializing CartesianJog: Frame {frame}, Axis {axis}...") - - if axis not in AXIS_MAP: - print(f" -> VALIDATION FAILED: Invalid axis '{axis}'.") - return - - # Store all necessary parameters for use in execute_step - self.frame = frame - self.axis_vectors = AXIS_MAP[axis] - self.is_rotation = any(self.axis_vectors[1]) - self.speed_percentage = speed_percentage - self.duration = duration - self.end_time = time.time() + self.duration - - self.is_valid = True - print(" -> Command is valid and ready.") - - def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): - if self.is_finished or not self.is_valid: - return True - - # --- A. Check for completion --- - if time.time() >= self.end_time: - print("Cartesian jog finished.") - self.is_finished = True - Speed_out[:] = [0] * 6 - Command_out.value = 255 - return True - - # --- B. Calculate Target Pose using clean vector math --- - Command_out.value = 123 # Set jog command - - q_current = np.array([PAROL6_ROBOT.STEPS2RADS(p, i) for i, p in enumerate(Position_in)]) - T_current = PAROL6_ROBOT.robot.fkine(q_current) - - if not isinstance(T_current, SE3): - return False # Wait for valid pose data - - # Calculate speed and displacement for this cycle - linear_speed_ms = float(np.interp(self.speed_percentage, [0, 100], [PAROL6_ROBOT.Cartesian_linear_velocity_min_JOG, PAROL6_ROBOT.Cartesian_linear_velocity_max_JOG])) - angular_speed_degs = float(np.interp(self.speed_percentage, [0, 100], [PAROL6_ROBOT.Cartesian_angular_velocity_min, PAROL6_ROBOT.Cartesian_angular_velocity_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 - delta_pose = SE3.Rt(SE3.Eul(rot_vec).R, 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, jogging=True) - - if var.success: - q_velocities = (var.q - q_current) / INTERVAL_S - for i in range(6): - Speed_out[i] = int(PAROL6_ROBOT.SPEED_RAD2STEP(q_velocities[i], i)) - else: - print("IK Warning: Could not find solution for jog step. Stopping.") - self.is_finished = True - Speed_out[:] = [0] * 6 - Command_out.value = 255 - return True - - # --- D. Speed Scaling --- - max_scale_factor = 1.0 - for i in range(6): - if abs(Speed_out[i]) > PAROL6_ROBOT.Joint_max_speed[i]: - scale = abs(Speed_out[i]) / PAROL6_ROBOT.Joint_max_speed[i] - if scale > max_scale_factor: - max_scale_factor = scale - - if max_scale_factor > 1.0: - for i in range(6): - Speed_out[i] = int(Speed_out[i] / max_scale_factor) - - return False # Command is still running - -class MovePoseCommand: - """ - A non-blocking command to move the robot to a specific Cartesian pose. - The movement itself is a joint-space interpolation. - """ - def __init__(self, pose, duration=None, velocity_percent=None, accel_percent=50, trajectory_type='poly'): - self.is_valid = True # Assume valid; preparation step will confirm. - self.is_finished = False - self.command_step = 0 - self.trajectory_steps = [] - - print(f"Initializing MovePose to {pose}...") - - # --- MODIFICATION: Store parameters for deferred planning --- - self.pose = pose - self.duration = duration - self.velocity_percent = velocity_percent - self.accel_percent = accel_percent - self.trajectory_type = trajectory_type - - """ - Initializes, validates, and pre-computes the trajectory for a move-to-pose command. - - Args: - pose (list): A list of 6 values [x, y, z, r, p, y] for the target pose. - Positions are in mm, rotations are in degrees. - duration (float, optional): The total time for the movement in seconds. - velocity_percent (float, optional): The target velocity as a percentage (0-100). - accel_percent (float, optional): The target acceleration as a percentage (0-100). - trajectory_type (str, optional): The type of trajectory ('poly' or 'trap'). - """ - - def prepare_for_execution(self, current_position_in): - """Calculates the full trajectory just-in-time before execution.""" - print(f" -> Preparing trajectory for MovePose to {self.pose}...") - - initial_pos_rad = np.array([PAROL6_ROBOT.STEPS2RADS(p, i) for i, p in enumerate(current_position_in)]) - target_pose = SE3(self.pose[0] / 1000.0, self.pose[1] / 1000.0, self.pose[2] / 1000.0) * SE3.RPY(self.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: - print(" -> VALIDATION FAILED: Inverse kinematics failed at execution time.") - self.is_valid = False - return - - target_pos_rad = ik_solution.q - - if self.duration and self.duration > 0: - if self.velocity_percent is not None: - print(" -> INFO: Both duration and velocity were provided. Using duration.") - command_len = int(self.duration / INTERVAL_S) - traj_generator = rp.tools.trajectory.jtraj(initial_pos_rad, target_pos_rad, command_len) - - for i in range(len(traj_generator.q)): - pos_step = [int(PAROL6_ROBOT.RAD2STEPS(p, j)) for j, p in enumerate(traj_generator.q[i])] - self.trajectory_steps.append((pos_step, None)) - - elif self.velocity_percent is not None: - try: - accel_percent = self.accel_percent if self.accel_percent is not None else 50 - - initial_pos_steps = np.array(current_position_in) - target_pos_steps = np.array([int(PAROL6_ROBOT.RAD2STEPS(rad, i)) for i, rad in enumerate(target_pos_rad)]) - - all_joint_times = [] - for i in range(6): - path_to_travel = abs(target_pos_steps[i] - initial_pos_steps[i]) - if path_to_travel == 0: - all_joint_times.append(0) - continue - - v_max_joint = np.interp(self.velocity_percent, [0, 100], [PAROL6_ROBOT.Joint_min_speed[i], PAROL6_ROBOT.Joint_max_speed[i]]) - a_max_rad = np.interp(accel_percent, [0, 100], [PAROL6_ROBOT.Joint_min_acc, PAROL6_ROBOT.Joint_max_acc]) - a_max_steps = PAROL6_ROBOT.SPEED_RAD2STEP(a_max_rad, i) - - if v_max_joint <= 0 or a_max_steps <= 0: - raise ValueError(f"Invalid speed/acceleration for joint {i+1}. Must be positive.") - - t_accel = v_max_joint / a_max_steps - if path_to_travel < v_max_joint * t_accel: - t_accel = np.sqrt(path_to_travel / a_max_steps) - joint_time = 2 * t_accel - else: - joint_time = path_to_travel / v_max_joint + t_accel - all_joint_times.append(joint_time) - - total_time = max(all_joint_times) - - if total_time <= 0: - self.is_finished = True - return - - if total_time < (2 * INTERVAL_S): - total_time = 2 * INTERVAL_S - - execution_time = np.arange(0, total_time, INTERVAL_S) - - all_q, all_qd = [], [] - for i in range(6): - if abs(target_pos_steps[i] - initial_pos_steps[i]) == 0: - all_q.append(np.full(len(execution_time), initial_pos_steps[i])) - all_qd.append(np.zeros(len(execution_time))) - else: - joint_traj = rp.trapezoidal(initial_pos_steps[i], target_pos_steps[i], execution_time) - all_q.append(joint_traj.q) - all_qd.append(joint_traj.qd) - - self.trajectory_steps = list(zip(np.array(all_q).T.astype(int), np.array(all_qd).T.astype(int))) - print(f" -> Command is valid (duration calculated from speed: {total_time:.2f}s).") - - except Exception as e: - print(f" -> VALIDATION FAILED: Could not calculate velocity-based trajectory. Error: {e}") - self.is_valid = False - return - - else: - print(" -> Using conservative values for MovePose.") - command_len = 200 - traj_generator = rp.tools.trajectory.jtraj(initial_pos_rad, target_pos_rad, command_len) - for i in range(len(traj_generator.q)): - pos_step = [int(PAROL6_ROBOT.RAD2STEPS(p, j)) for j, p in enumerate(traj_generator.q[i])] - self.trajectory_steps.append((pos_step, None)) - - if not self.trajectory_steps: - print(" -> Trajectory calculation resulted in no steps. Command is invalid.") - self.is_valid = False - else: - print(f" -> Trajectory prepared with {len(self.trajectory_steps)} steps.") - - def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): - # This method remains unchanged. - if self.is_finished or not self.is_valid: - return True - - if self.command_step >= len(self.trajectory_steps): - print(f"{type(self).__name__} finished.") - self.is_finished = True - Position_out[:] = Position_in[:] - Speed_out[:] = [0] * 6 - Command_out.value = 156 - return True - else: - pos_step, _ = self.trajectory_steps[self.command_step] - Position_out[:] = pos_step - Speed_out[:] = [0] * 6 - Command_out.value = 156 - self.command_step += 1 - return False - -class MoveJointCommand: - """ - A non-blocking command to move the robot's joints to a specific configuration. - It pre-calculates the entire trajectory upon initialization. - """ - def __init__(self, target_angles, duration=None, velocity_percent=None, accel_percent=50, trajectory_type='poly'): - self.is_valid = False # Will be set to True after basic validation - self.is_finished = False - self.command_step = 0 - self.trajectory_steps = [] - - print(f"Initializing MoveJoint to {target_angles}...") - - # --- MODIFICATION: Store parameters for deferred planning --- - self.target_angles = target_angles - self.duration = duration - self.velocity_percent = velocity_percent - self.accel_percent = accel_percent - self.trajectory_type = trajectory_type - - # --- Perform only state-independent validation --- - target_pos_rad = np.array([np.deg2rad(angle) for angle in self.target_angles]) - for i in range(6): - min_rad, max_rad = PAROL6_ROBOT.Joint_limits_radian[i] - if not (min_rad <= target_pos_rad[i] <= max_rad): - print(f" -> VALIDATION FAILED: Target for Joint {i+1} ({self.target_angles[i]} deg) is out of range.") - return - - self.is_valid = True - - def prepare_for_execution(self, current_position_in): - """Calculates the trajectory just before execution begins.""" - print(f" -> Preparing trajectory for MoveJoint to {self.target_angles}...") - - initial_pos_rad = np.array([PAROL6_ROBOT.STEPS2RADS(p, i) for i, p in enumerate(current_position_in)]) - target_pos_rad = np.array([np.deg2rad(angle) for angle in self.target_angles]) - - if self.duration and self.duration > 0: - if self.velocity_percent is not None: - print(" -> INFO: Both duration and velocity were provided. Using duration.") - command_len = int(self.duration / INTERVAL_S) - traj_generator = rp.tools.trajectory.jtraj(initial_pos_rad, target_pos_rad, command_len) - - for i in range(len(traj_generator.q)): - pos_step = [int(PAROL6_ROBOT.RAD2STEPS(p, j)) for j, p in enumerate(traj_generator.q[i])] - self.trajectory_steps.append((pos_step, None)) - - elif self.velocity_percent is not None: - try: - accel_percent = self.accel_percent if self.accel_percent is not None else 50 - initial_pos_steps = np.array(current_position_in) - target_pos_steps = np.array([int(PAROL6_ROBOT.RAD2STEPS(rad, i)) for i, rad in enumerate(target_pos_rad)]) - - all_joint_times = [] - for i in range(6): - path_to_travel = abs(target_pos_steps[i] - initial_pos_steps[i]) - if path_to_travel == 0: - all_joint_times.append(0) - continue - - v_max_joint = np.interp(self.velocity_percent, [0, 100], [PAROL6_ROBOT.Joint_min_speed[i], PAROL6_ROBOT.Joint_max_speed[i]]) - a_max_rad = np.interp(accel_percent, [0, 100], [PAROL6_ROBOT.Joint_min_acc, PAROL6_ROBOT.Joint_max_acc]) - a_max_steps = PAROL6_ROBOT.SPEED_RAD2STEP(a_max_rad, i) - - if v_max_joint <= 0 or a_max_steps <= 0: - raise ValueError(f"Invalid speed/acceleration for joint {i+1}. Must be positive.") - - t_accel = v_max_joint / a_max_steps - if path_to_travel < v_max_joint * t_accel: - t_accel = np.sqrt(path_to_travel / a_max_steps) - joint_time = 2 * t_accel - else: - joint_time = path_to_travel / v_max_joint + t_accel - all_joint_times.append(joint_time) - - total_time = max(all_joint_times) - - if total_time <= 0: - self.is_finished = True - return - - if total_time < (2 * INTERVAL_S): - total_time = 2 * INTERVAL_S - - execution_time = np.arange(0, total_time, INTERVAL_S) - - all_q, all_qd = [], [] - for i in range(6): - if abs(target_pos_steps[i] - initial_pos_steps[i]) == 0: - all_q.append(np.full(len(execution_time), initial_pos_steps[i])) - all_qd.append(np.zeros(len(execution_time))) - else: - joint_traj = rp.trapezoidal(initial_pos_steps[i], target_pos_steps[i], execution_time) - all_q.append(joint_traj.q) - all_qd.append(joint_traj.qd) - - self.trajectory_steps = list(zip(np.array(all_q).T.astype(int), np.array(all_qd).T.astype(int))) - print(f" -> Command is valid (duration calculated from speed: {total_time:.2f}s).") - - except Exception as e: - print(f" -> VALIDATION FAILED: Could not calculate velocity-based trajectory. Error: {e}") - print(f" -> Please check Joint_min/max_speed and Joint_min/max_acc values in PAROL6_ROBOT.py.") - self.is_valid = False - return - - else: - print(" -> Using conservative values for MoveJoint.") - command_len = 200 - traj_generator = rp.tools.trajectory.jtraj(initial_pos_rad, target_pos_rad, command_len) - for i in range(len(traj_generator.q)): - pos_step = [int(PAROL6_ROBOT.RAD2STEPS(p, j)) for j, p in enumerate(traj_generator.q[i])] - self.trajectory_steps.append((pos_step, None)) - - if not self.trajectory_steps: - print(" -> Trajectory calculation resulted in no steps. Command is invalid.") - self.is_valid = False - else: - print(f" -> Trajectory prepared with {len(self.trajectory_steps)} steps.") - - def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): - # This method remains unchanged. - if self.is_finished or not self.is_valid: - return True - - if self.command_step >= len(self.trajectory_steps): - print(f"{type(self).__name__} finished.") - self.is_finished = True - Position_out[:] = Position_in[:] - Speed_out[:] = [0] * 6 - Command_out.value = 156 - return True - else: - pos_step, _ = self.trajectory_steps[self.command_step] - Position_out[:] = pos_step - Speed_out[:] = [0] * 6 - Command_out.value = 156 - self.command_step += 1 - return False - -class MoveCartCommand: - """ - 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. - """ - def __init__(self, pose, duration=None, velocity_percent=None): - self.is_valid = False - self.is_finished = False - - # --- MODIFICATION: Validate that at least one timing parameter is given --- - if duration is None and velocity_percent is None: - print(" -> VALIDATION FAILED: MoveCartCommand requires either 'duration' or 'velocity_percent'.") - return - if duration is not None and velocity_percent is not None: - print(" -> INFO: Both duration and velocity_percent provided. Using duration.") - self.velocity_percent = None # Prioritize duration - else: - self.velocity_percent = velocity_percent - - # --- Store parameters and set placeholders --- - self.duration = duration - self.pose = pose - self.start_time = None - self.initial_pose = None - self.target_pose = None - self.is_valid = True - - def prepare_for_execution(self, current_position_in): - """Captures the initial state and validates the path just before execution.""" - print(f" -> Preparing for MoveCart to {self.pose}...") - - # --- MOVED LOGIC: Capture initial state from live data --- - initial_q_rad = np.array([PAROL6_ROBOT.STEPS2RADS(p, i) for i, p in enumerate(current_position_in)]) - self.initial_pose = PAROL6_ROBOT.robot.fkine(initial_q_rad) - self.target_pose = SE3(self.pose[0]/1000.0, self.pose[1]/1000.0, self.pose[2]/1000.0) * SE3.RPY(self.pose[3:6], unit='deg', order='xyz') - - print(" -> Pre-validating final target pose...") - ik_check = solve_ik_with_adaptive_tol_subdivision( - PAROL6_ROBOT.robot, self.target_pose, initial_q_rad - ) - - if not ik_check.success: - print(" -> VALIDATION FAILED: The final target pose is unreachable.") - if ik_check.violations: - print(f" Reason: Solution violates joint limits: {ik_check.violations}") - self.is_valid = False # Mark as invalid if path fails - return - - # --- NEW BLOCK: Calculate duration from velocity if needed --- - if self.velocity_percent is not None: - print(f" -> Calculating duration for {self.velocity_percent}% speed...") - # Calculate the total distance for translation and rotation - linear_distance = np.linalg.norm(self.target_pose.t - self.initial_pose.t) - angular_distance_rad = self.initial_pose.angdist(self.target_pose) - - # Interpolate the target speeds from percentages, assuming constants exist in PAROL6_ROBOT - target_linear_speed = np.interp(self.velocity_percent, [0, 100], [PAROL6_ROBOT.Cartesian_linear_velocity_min, PAROL6_ROBOT.Cartesian_linear_velocity_max]) - target_angular_speed = np.interp(self.velocity_percent, [0, 100], [PAROL6_ROBOT.Cartesian_angular_velocity_min, PAROL6_ROBOT.Cartesian_angular_velocity_max]) - target_angular_speed_rad = np.deg2rad(target_angular_speed) - - # 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: - print(" -> 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 - print(f" -> Calculated MoveCart duration: {self.duration:.2f}s") - - print(" -> Command is valid and ready for execution.") - - def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): - if self.is_finished or not self.is_valid: - return True - - if self.start_time is None: - self.start_time = time.time() - - elapsed_time = time.time() - self.start_time - s = min(elapsed_time / self.duration, 1.0) - s_scaled = quintic_scaling(s) - - current_target_pose = self.initial_pose.interp(self.target_pose, s_scaled) - - current_q_rad = np.array([PAROL6_ROBOT.STEPS2RADS(p, i) for i, p in enumerate(Position_in)]) - ik_solution = solve_ik_with_adaptive_tol_subdivision( - PAROL6_ROBOT.robot, current_target_pose, current_q_rad - ) - - if not ik_solution.success: - print(" -> ERROR: MoveCart failed. An intermediate point on the path is unreachable.") - if ik_solution.violations: - print(f" Reason: Path violates joint limits: {ik_solution.violations}") - self.is_finished = True - Speed_out[:] = [0] * 6 - Command_out.value = 255 - return True - - current_pos_rad = ik_solution.q - - # --- MODIFIED BLOCK --- - # Send only the target position and let the firmware's P-controller handle speed. - Position_out[:] = [int(PAROL6_ROBOT.RAD2STEPS(p, i)) for i, p in enumerate(current_pos_rad)] - Speed_out[:] = [0] * 6 # Set feed-forward velocity to zero for smooth P-control. - Command_out.value = 156 - # --- END MODIFIED BLOCK --- - - if s >= 1.0: - print(f"MoveCart finished in ~{elapsed_time:.2f}s.") - self.is_finished = True - # The main loop will handle holding the final position. - - return self.is_finished - -class GripperCommand: - """ - 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. - """ - def __init__(self, gripper_type, action=None, position=100, speed=100, current=500, output_port=1): - """ - Initializes the Gripper command and configures its internal state machine - based on the requested action. - """ - self.is_valid = True - self.is_finished = False - self.gripper_type = gripper_type.lower() - self.action = action.lower() if action else 'move' - self.state = "START" - self.timeout_counter = 1000 # 10-second safety timeout for all waiting states - - # --- Configure based on Gripper Type and Action --- - if self.gripper_type == 'electric': - if self.action == 'move': - self.target_position = position - self.speed = speed - self.current = current - if not (0 <= position <= 255 and 0 <= speed <= 255 and 100 <= current <= 1000): - self.is_valid = False - elif self.action == 'calibrate': - self.wait_counter = 200 # 2-second fixed delay for calibration - else: - self.is_valid = False # Invalid action - - elif self.gripper_type == 'pneumatic': - if self.action not in ['open', 'close']: - self.is_valid = False - self.state_to_set = 1 if self.action == 'open' else 0 - self.port_index = 2 if output_port == 1 else 3 - else: - self.is_valid = False - - if not self.is_valid: - print(f" -> VALIDATION FAILED for GripperCommand with action: '{self.action}'") - - def execute_step(self, Gripper_data_out, InOut_out, Gripper_data_in, InOut_in, **kwargs): - if self.is_finished or not self.is_valid: - return True - - self.timeout_counter -= 1 - if self.timeout_counter <= 0: - print(f" -> ERROR: Gripper command timed out in state {self.state}.") - self.is_finished = True - return True - - # --- Pneumatic Logic (Instantaneous) --- - if self.gripper_type == 'pneumatic': - InOut_out[self.port_index] = self.state_to_set - print(" -> Pneumatic gripper command sent.") - self.is_finished = True - return True - - # --- Electric Gripper Logic --- - if self.gripper_type == 'electric': - # On the first run, transition to the correct state for the action - if self.state == "START": - if self.action == 'calibrate': - self.state = "SEND_CALIBRATE" - else: # 'move' - self.state = "WAIT_FOR_POSITION" - - # --- Calibrate Logic (Timed Delay) --- - if self.state == "SEND_CALIBRATE": - print(" -> Sending one-shot calibrate command...") - Gripper_data_out[4] = 1 # Set mode to calibrate - self.state = "WAITING_CALIBRATION" - return False - - if self.state == "WAITING_CALIBRATION": - self.wait_counter -= 1 - if self.wait_counter <= 0: - print(" -> Calibration delay finished.") - Gripper_data_out[4] = 0 # Reset to operation mode - self.is_finished = True - return True - return False - - # --- Move Logic (Position-Based) --- - if self.state == "WAIT_FOR_POSITION": - # Persistently send the move command - Gripper_data_out[0], Gripper_data_out[1], Gripper_data_out[2] = self.target_position, self.speed, self.current - Gripper_data_out[4] = 0 # Operation mode - bitfield = [1, 1, not InOut_in[4], 1, 0, 0, 0, 0] - fused = PAROL6_ROBOT.fuse_bitfield_2_bytearray(bitfield) - Gripper_data_out[3] = int(fused.hex(), 16) - - # Check for completion - current_position = Gripper_data_in[1] - if abs(current_position - self.target_position) <= 5: - print(f" -> Gripper move complete.") - self.is_finished = True - # Set command back to idle - bitfield = [1, 0, not InOut_in[4], 1, 0, 0, 0, 0] - fused = PAROL6_ROBOT.fuse_bitfield_2_bytearray(bitfield) - Gripper_data_out[3] = int(fused.hex(), 16) - return True - return False - - return self.is_finished - -class DelayCommand: - """ - 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. - """ - def __init__(self, duration): - """ - Initializes and validates the Delay command. - - Args: - duration (float): The delay time in seconds. - """ - self.is_valid = False - self.is_finished = False - - # --- 1. Parameter Validation --- - if not isinstance(duration, (int, float)) or duration <= 0: - print(f" -> VALIDATION FAILED: Delay duration must be a positive number, but got {duration}.") - return - - print(f"Initializing Delay for {duration} seconds...") - - self.duration = duration - self.end_time = None # Will be set in prepare_for_execution - self.is_valid = True - - def prepare_for_execution(self, current_position_in): - """Set the end time when the command actually starts.""" - self.end_time = time.time() + self.duration - print(f" -> Delay starting for {self.duration} seconds...") - - def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): - """ - Checks if the delay duration has passed and keeps the robot idle. - This method is called on every loop cycle (~0.01s). - """ - if self.is_finished or not self.is_valid: - return True - - # --- A. Keep the robot idle during the delay --- - Command_out.value = 255 # Set command to idle - Speed_out[:] = [0] * 6 # Set all speeds to zero - - # --- B. Check for completion --- - if self.end_time and time.time() >= self.end_time: - print(f"Delay finished after {self.duration} seconds.") - self.is_finished = True - - return self.is_finished - -######################################################################### -# Smooth Motion Commands Start Here -######################################################################### - -class BaseSmoothMotionCommand: - """ - Base class for all smooth motion commands with proper error tracking. - """ - - def __init__(self, description="smooth motion"): - self.description = description - self.trajectory = None - self.trajectory_command = None - self.transition_command = None - self.is_valid = True - self.is_finished = False - self.specified_start_pose = None - self.transition_complete = False - self.trajectory_prepared = False - self.error_state = False - self.error_message = "" - self.trajectory_generated = False # NEW: Track if trajectory is generated - - def create_transition_command(self, current_pose, target_pose): - """Create a MovePose command for smooth transition to start position.""" - pos_error = np.linalg.norm( - np.array(target_pose[:3]) - np.array(current_pose[:3]) - ) - - # Lower threshold to 2mm for more aggressive transition generation - if pos_error < 2.0: # Changed from 5.0mm to 2.0mm - print(f" -> Already near start position (error: {pos_error:.1f}mm)") - return None - - print(f" -> Creating smooth transition to start ({pos_error:.1f}mm away)") - - # Calculate transition speed based on distance - # Slower for short distances, faster for long distances - 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 - - transition_cmd = MovePoseCommand( - pose=target_pose, - duration=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 = np.array([PAROL6_ROBOT.STEPS2RADS(p, i) - for i, p in enumerate(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]).tolist() - - def prepare_for_execution(self, current_position_in): - """Minimal preparation - just check if we need a transition.""" - print(f" -> Preparing {self.description}...") - - # If there's a specified start pose, prepare transition - if self.specified_start_pose: - actual_current_pose = self.get_current_pose_from_position(current_position_in) - self.transition_command = self.create_transition_command( - actual_current_pose, self.specified_start_pose - ) - - if self.transition_command: - self.transition_command.prepare_for_execution(current_position_in) - if not self.transition_command.is_valid: - print(f" -> ERROR: Cannot reach specified start position") - self.is_valid = False - self.error_state = True - self.error_message = "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 - print(f" -> {self.description} preparation complete (trajectory will be generated at execution)") - - 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, Position_in, Homed_in, Speed_out, Command_out, **kwargs): - """Execute transition first if needed, then generate and execute trajectory.""" - if self.is_finished or not self.is_valid: - return True - - # Execute transition first if needed - if self.transition_command and not self.transition_complete: - is_done = self.transition_command.execute_step( - Position_in, Homed_in, Speed_out, Command_out, **kwargs - ) - - if is_done: - print(f" -> Transition complete") - self.transition_complete = True - return False - - # 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(Position_in) - print(f" -> Generating {self.description} from ACTUAL position: {[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 = np.array([PAROL6_ROBOT.STEPS2RADS(p, i) - for i, p in enumerate(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: - print(f" -> ERROR: Cannot reach first trajectory point") - self.is_finished = True - self.error_state = True - self.error_message = "Cannot reach trajectory start" - Speed_out[:] = [0] * 6 - Command_out.value = 255 - return True - - 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: - print(f" -> WARNING: First trajectory point {distance:.1f}mm from current!") - - # Execute main trajectory - if self.trajectory_command and self.trajectory_prepared: - is_done = self.trajectory_command.execute_step( - Position_in, Homed_in, Speed_out, Command_out, **kwargs - ) - - # Check for errors in trajectory execution - if hasattr(self.trajectory_command, 'error_state') and self.trajectory_command.error_state: - self.error_state = True - self.error_message = self.trajectory_command.error_message - - if is_done: - self.is_finished = True - - return is_done - else: - self.is_finished = True - return True - -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 = "" - - print(f"Initializing smooth {description} with {len(trajectory)} points") - - def prepare_for_execution(self, current_position_in): - """Skip validation - trajectory is already generated from correct position""" - # No validation needed since trajectory was just generated from current position - self.is_valid = True - return - - def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, Position_out=None, **kwargs): - """Execute one step of the smooth trajectory""" - if self.is_finished or not self.is_valid: - return True - - # Get Position_out from kwargs if not provided - if Position_out is None: - Position_out = kwargs.get('Position_out', Position_in) - - if self.trajectory_index >= len(self.trajectory): - print(f"Smooth {self.description} finished.") - self.is_finished = True - Speed_out[:] = [0] * 6 - Command_out.value = 255 - return True - - # 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 = np.array([PAROL6_ROBOT.STEPS2RADS(p, i) - for i, p in enumerate(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: - print(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)}" - Speed_out[:] = [0] * 6 - Command_out.value = 255 - return True - - # Convert to steps - target_steps = [int(PAROL6_ROBOT.RAD2STEPS(q, i)) - for i, q in enumerate(ik_result.q)] - - # ADD VELOCITY LIMITING - This prevents violent movements - if self.trajectory_index > 0: - for i in range(6): - step_diff = abs(target_steps[i] - Position_in[i]) - max_step_diff = PAROL6_ROBOT.Joint_max_speed[i] * 0.01 # Max steps in 10ms - - # Use 1.2x safety margin (not 2x as before) - if step_diff > max_step_diff * 1.2: - #print(f" -> WARNING: Joint {i+1} velocity limit exceeded at point {self.trajectory_index}") - #print(f" Step difference: {step_diff}, Max allowed: {max_step_diff * 1.2:.1f}") - - # Clamp the motion - sign = 1 if target_steps[i] > Position_in[i] else -1 - target_steps[i] = Position_in[i] + sign * int(max_step_diff) - - # Send position command - Position_out[:] = target_steps - Speed_out[:] = [0] * 6 - Command_out.value = 156 - - # Advance to next point - self.trajectory_index += 1 - - return False - -class SmoothCircleCommand(BaseSmoothMotionCommand): - def __init__(self, center, radius, plane, duration, clockwise, frame='WRF', start_pose=None): - super().__init__(f"circle (r={radius}mm, {frame})") - self.center = center - self.radius = radius - self.plane = plane - self.duration = duration - self.clockwise = clockwise - self.frame = frame # Store reference frame - self.specified_start_pose = start_pose - self.normal_vector = None # Will be set if TRF - self.current_position_in = None # Store for TRF transformation - - def prepare_for_execution(self, current_position_in): - """Transform parameters if in TRF, then prepare normally.""" - # Store current position for potential use in generate_main_trajectory - self.current_position_in = current_position_in - - 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', current_position_in - ) - - # Update with transformed values - self.center = transformed['center'] - self.normal_vector = transformed.get('normal_vector') - - print(f" -> TRF Circle: center {self.center[:3]} (WRF), normal {self.normal_vector}") - - # Also transform start_pose if specified - if self.specified_start_pose: - params = {'start_pose': self.specified_start_pose} - transformed = transform_command_params_to_wrf( - 'SMOOTH_CIRCLE', params, 'TRF', current_position_in - ) - self.specified_start_pose = transformed.get('start_pose') - - # Now do normal preparation with transformed parameters - return super().prepare_for_execution(current_position_in) - - 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) - print(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])) # Convert to numpy array - print(f" Using WRF plane {self.plane} with normal: {normal}") - - print(f" Generating circle from position: {[round(p, 1) for p in effective_start_pose[:3]]}") - print(f" Circle center: {[round(c, 1) for c in self.center]}") - - # Add geometry validation - center_np = np.array(self.center) - start_np = np.array(effective_start_pose[:3]) - - # Project start point onto circle plane to check distance - to_start = start_np - center_np - to_start_plane = to_start - np.dot(to_start, normal) * normal - distance_to_center = np.linalg.norm(to_start_plane) - - if abs(distance_to_center - self.radius) > self.radius * 0.3: - print(f" WARNING: Robot is {distance_to_center:.1f}mm from center, but radius is {self.radius:.1f}mm") - print(f" Circle geometry will be auto-corrected") - - # Generate circle that starts at effective_start_pose - trajectory = motion_gen.generate_circle_3d( - self.center, - self.radius, - normal, # This normal now correctly represents the plane - start_point=effective_start_pose[:3], - duration=self.duration - ) - - 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:] - - return trajectory - -class SmoothArcCenterCommand(BaseSmoothMotionCommand): - def __init__(self, end_pose, center, duration, clockwise, frame='WRF', start_pose=None): - super().__init__(f"arc (center-based, {frame})") - self.end_pose = end_pose - self.center = center - self.duration = duration - self.clockwise = clockwise - self.frame = frame - self.specified_start_pose = start_pose - self.normal_vector = None - - def prepare_for_execution(self, current_position_in): - """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', current_position_in - ) - self.end_pose = transformed['end_pose'] - self.center = transformed['center'] - self.normal_vector = transformed.get('normal_vector') - - if self.specified_start_pose: - params = {'start_pose': self.specified_start_pose} - transformed = transform_command_params_to_wrf( - 'SMOOTH_ARC_CENTER', params, 'TRF', current_position_in - ) - self.specified_start_pose = transformed.get('start_pose') - - return super().prepare_for_execution(current_position_in) - - def generate_main_trajectory(self, effective_start_pose): - """Generate arc from actual start to end.""" - motion_gen = CircularMotion() - return motion_gen.generate_arc_3d( - effective_start_pose, self.end_pose, self.center, - normal=self.normal_vector, # Use transformed normal if TRF - clockwise=self.clockwise, duration=self.duration - ) - -class SmoothArcParamCommand(BaseSmoothMotionCommand): - def __init__(self, end_pose, radius, arc_angle, duration, clockwise, frame='WRF', start_pose=None): - super().__init__(f"parametric arc (r={radius}mm, θ={arc_angle}°, {frame})") - self.end_pose = end_pose - self.radius = radius - self.arc_angle = arc_angle - self.duration = duration - self.clockwise = clockwise - self.frame = frame - self.specified_start_pose = start_pose - self.normal_vector = None # Will be set if TRF - self.current_position_in = None - - def prepare_for_execution(self, current_position_in): - """Transform parameters if in TRF, then prepare normally.""" - self.current_position_in = current_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', current_position_in - ) - - # Update with transformed values - self.end_pose = transformed['end_pose'] - self.normal_vector = transformed.get('normal_vector') - - print(f" -> TRF Parametric Arc: end {self.end_pose[:3]} (WRF)") - - # Also transform start_pose if specified - if self.specified_start_pose: - params = {'start_pose': self.specified_start_pose} - transformed = transform_command_params_to_wrf( - 'SMOOTH_ARC_PARAM', params, 'TRF', current_position_in - ) - self.specified_start_pose = transformed.get('start_pose') - - return super().prepare_for_execution(current_position_in) - - 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) - - # Project start and end onto the plane perpendicular to normal - # This ensures the arc stays in the correct plane for TRF - - # 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: - print(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: - print(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 from actual start - motion_gen = CircularMotion() - return motion_gen.generate_arc_3d( - effective_start_pose, self.end_pose, center_3d.tolist(), - normal=normal if self.normal_vector is not None else None, - clockwise=self.clockwise, duration=self.duration - ) - -class SmoothHelixCommand(BaseSmoothMotionCommand): - def __init__(self, center, radius, pitch, height, duration, clockwise, frame='WRF', start_pose=None): - super().__init__(f"helix (h={height}mm, {frame})") - self.center = center - self.radius = radius - self.pitch = pitch - self.height = height - self.duration = duration - self.clockwise = clockwise - self.frame = frame - self.specified_start_pose = start_pose - self.helix_axis = None - self.up_vector = None - - def prepare_for_execution(self, current_position_in): - """Transform parameters if in TRF.""" - if self.frame == 'TRF': - params = {'center': self.center} - transformed = transform_command_params_to_wrf( - 'SMOOTH_HELIX', params, 'TRF', current_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', current_position_in - ) - self.specified_start_pose = transformed.get('start_pose') - - return super().prepare_for_execution(current_position_in) - - def generate_main_trajectory(self, effective_start_pose): - """Generate helix with proper axis orientation.""" - num_revolutions = self.height / self.pitch if self.pitch > 0 else 1 - num_points = int(self.duration * 100) # 100Hz - - # Get helix axis (default Z for WRF, transformed for TRF) - if self.helix_axis is not None: - axis = np.array(self.helix_axis) - else: - axis = np.array([0, 0, 1]) # Default vertical - - # Create orthonormal basis for helix - if self.up_vector is not None: - up = np.array(self.up_vector) - else: - # Find perpendicular to axis - if abs(axis[2]) < 0.9: - up = np.array([0, 0, 1]) - else: - up = np.array([1, 0, 0]) - - # Ensure up is perpendicular to axis - up = up - np.dot(up, axis) * axis - up = up / np.linalg.norm(up) - - # Create right vector - right = np.cross(axis, up) - - # Calculate starting angle based on actual start position - to_start = np.array(effective_start_pose[:3]) - np.array(self.center) - # Project onto plane perpendicular to axis - to_start_plane = to_start - np.dot(to_start, axis) * axis - - 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, up), - np.dot(to_start_normalized, right)) - else: - start_angle = 0 - - trajectory = [] - for i in range(num_points): - t = i / (num_points - 1) if num_points > 1 else 0 - - angle = start_angle + 2 * np.pi * num_revolutions * t - if self.clockwise: - angle = start_angle - 2 * np.pi * num_revolutions * t - - # Position on helix - pos = np.array(self.center) + \ - self.radius * (np.cos(angle) * right + np.sin(angle) * up) + \ - self.height * t * axis - - # Use orientation from effective start - trajectory.append(np.concatenate([pos, effective_start_pose[3:]])) - - return np.array(trajectory) - -class SmoothSplineCommand(BaseSmoothMotionCommand): - def __init__(self, waypoints, duration, frame='WRF', start_pose=None): - super().__init__(f"spline ({len(waypoints)} points, {frame})") - self.waypoints = waypoints - self.duration = duration - self.frame = frame - self.specified_start_pose = start_pose - self.current_position_in = None - - def prepare_for_execution(self, current_position_in): - """Transform parameters if in TRF, then prepare normally.""" - self.current_position_in = current_position_in - - if self.frame == 'TRF': - # Transform waypoints to WRF - params = {'waypoints': self.waypoints} - transformed = transform_command_params_to_wrf( - 'SMOOTH_SPLINE', params, 'TRF', current_position_in - ) - - # Update with transformed values - self.waypoints = transformed['waypoints'] - - print(f" -> TRF Spline: transformed {len(self.waypoints)} waypoints to WRF") - - # Also transform start_pose if specified - if self.specified_start_pose: - params = {'start_pose': self.specified_start_pose} - transformed = transform_command_params_to_wrf( - 'SMOOTH_SPLINE', params, 'TRF', current_position_in - ) - self.specified_start_pose = transformed.get('start_pose') - - return super().prepare_for_execution(current_position_in) - - 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 - print(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:] - print(f" Replaced first waypoint with actual start position") - - timestamps = np.linspace(0, self.duration, len(modified_waypoints)) - - # Generate the spline trajectory - trajectory = motion_gen.generate_cubic_spline(modified_waypoints, timestamps) - - print(f" Generated spline with {len(trajectory)} points") - - return trajectory - -class SmoothBlendCommand(BaseSmoothMotionCommand): - def __init__(self, segment_definitions, blend_time, frame='WRF', start_pose=None): - super().__init__(f"blended ({len(segment_definitions)} segments, {frame})") - self.segment_definitions = segment_definitions - self.blend_time = blend_time - self.frame = frame - self.specified_start_pose = start_pose - self.current_position_in = None - - def prepare_for_execution(self, current_position_in): - """Transform parameters if in TRF, then prepare normally.""" - self.current_position_in = current_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', current_position_in - ) - - # Update with transformed values - self.segment_definitions = transformed['segments'] - - print(f" -> TRF Blend: transformed {len(self.segment_definitions)} segments to WRF") - - # Also transform start_pose if specified - if self.specified_start_pose: - params = {'start_pose': self.specified_start_pose} - transformed = transform_command_params_to_wrf( - 'SMOOTH_BLEND', params, 'TRF', current_position_in - ) - self.specified_start_pose = transformed.get('start_pose') - - return super().prepare_for_execution(current_position_in) - - 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['clockwise'] - - # 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['clockwise'] - - # 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].tolist() - - 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 - if len(trajectories) > 1: - blender = MotionBlender(self.blend_time) - blended = trajectories[0] - blend_samples = int(self.blend_time * 100) - - for i in range(1, len(trajectories)): - blended = blender.blend_trajectories(blended, trajectories[i], blend_samples) - - print(f" Blended {len(trajectories)} segments into {len(blended)} points") - return blended - elif trajectories: - return trajectories[0] - else: - raise ValueError("No trajectories generated in blend") - -def calculate_duration_from_speed(trajectory_length: float, speed_percentage: float) -> float: - """ - Calculate duration based on trajectory length and speed percentage. - - Args: - trajectory_length: Total path length in mm - speed_percentage: Speed as percentage (1-100) - - Returns: - Duration in seconds - """ - # Map speed percentage to mm/s (adjustable based on robot capabilities) - # For example: 100% = 100mm/s, 50% = 50mm/s - speed_mm_s = np.interp(speed_percentage, [0, 100], - [PAROL6_ROBOT.Cartesian_linear_velocity_min * 1000, - PAROL6_ROBOT.Cartesian_linear_velocity_max * 1000]) - - if speed_mm_s > 0: - return trajectory_length / speed_mm_s - else: - return 5.0 # Default fallback - -def parse_smooth_motion_commands(parts): - """ - Parse smooth motion commands received via UDP and create appropriate command objects. - All commands support: - - Reference frame selection (WRF or TRF) - - Optional start position (CURRENT or specified pose) - - Both DURATION and SPEED timing modes - - Args: - parts: List of command parts split by '|' - - Returns: - Command object or None if parsing fails - """ - command_type = parts[0] - - # Helper function for parsing optional start pose - def parse_start_pose(start_str): - """Parse start pose - returns None for CURRENT, or list of floats for specified pose.""" - if start_str == 'CURRENT' or start_str == 'NONE': - return None - else: - try: - return list(map(float, start_str.split(','))) - except: - print(f"Warning: Invalid start pose format: {start_str}") - return None - - # Helper function for calculating duration from speed - def calculate_duration_from_speed(trajectory_length: float, speed_percentage: float) -> float: - """Calculate duration based on trajectory length and speed percentage.""" - # Map speed percentage to mm/s - min_speed = PAROL6_ROBOT.Cartesian_linear_velocity_min * 1000 # Convert to mm/s - max_speed = PAROL6_ROBOT.Cartesian_linear_velocity_max * 1000 # Convert to mm/s - speed_mm_s = np.interp(speed_percentage, [0, 100], [min_speed, max_speed]) - - if speed_mm_s > 0: - return trajectory_length / speed_mm_s - else: - return 5.0 # Default fallback - - try: - if command_type == 'SMOOTH_CIRCLE': - # Format: SMOOTH_CIRCLE|center_x,center_y,center_z|radius|plane|frame|start_pose|timing_type|timing_value|clockwise - center = list(map(float, parts[1].split(','))) - radius = float(parts[2]) - plane = parts[3] - frame = parts[4] # 'WRF' or 'TRF' - start_pose = parse_start_pose(parts[5]) - timing_type = parts[6] # 'DURATION' or 'SPEED' - timing_value = float(parts[7]) - clockwise = parts[8] == '1' - - # Calculate duration - if timing_type == 'DURATION': - duration = timing_value - else: # SPEED - # Circle circumference - path_length = 2 * np.pi * radius - duration = calculate_duration_from_speed(path_length, timing_value) - - print(f" -> Parsed circle: r={radius}mm, plane={plane}, frame={frame}, {timing_type}={timing_value}, duration={duration:.2f}s") - - # Return command object with frame parameter - return SmoothCircleCommand(center, radius, plane, duration, clockwise, frame, start_pose) - - elif command_type == 'SMOOTH_ARC_CENTER': - # Format: SMOOTH_ARC_CENTER|end_pose|center|frame|start_pose|timing_type|timing_value|clockwise - end_pose = list(map(float, parts[1].split(','))) - center = list(map(float, parts[2].split(','))) - frame = parts[3] # 'WRF' or 'TRF' - start_pose = parse_start_pose(parts[4]) - timing_type = parts[5] # 'DURATION' or 'SPEED' - timing_value = float(parts[6]) - clockwise = parts[7] == '1' - - # Calculate duration - if timing_type == 'DURATION': - duration = timing_value - else: # SPEED - # Estimate arc length (will be more accurate when we have actual positions) - # Use a conservative estimate based on radius - radius_estimate = np.linalg.norm(np.array(center) - np.array(end_pose[:3])) - estimated_arc_angle = np.pi / 2 # 90 degrees estimate - arc_length = radius_estimate * estimated_arc_angle - duration = calculate_duration_from_speed(arc_length, timing_value) - - print(f" -> Parsed arc (center): frame={frame}, {timing_type}={timing_value}, duration={duration:.2f}s") - - # Return command with frame - return SmoothArcCenterCommand(end_pose, center, duration, clockwise, frame, start_pose) - - elif command_type == 'SMOOTH_ARC_PARAM': - # Format: SMOOTH_ARC_PARAM|end_pose|radius|angle|frame|start_pose|timing_type|timing_value|clockwise - end_pose = list(map(float, parts[1].split(','))) - radius = float(parts[2]) - arc_angle = float(parts[3]) - frame = parts[4] # 'WRF' or 'TRF' - start_pose = parse_start_pose(parts[5]) - timing_type = parts[6] # 'DURATION' or 'SPEED' - timing_value = float(parts[7]) - clockwise = parts[8] == '1' - - # Calculate duration - if timing_type == 'DURATION': - duration = timing_value - else: # SPEED - # Arc length = radius * angle (in radians) - arc_length = radius * np.deg2rad(arc_angle) - duration = calculate_duration_from_speed(arc_length, timing_value) - - print(f" -> Parsed arc (param): r={radius}mm, θ={arc_angle}°, frame={frame}, duration={duration:.2f}s") - - # Return command object with frame - return SmoothArcParamCommand(end_pose, radius, arc_angle, duration, clockwise, frame, start_pose) - - elif command_type == 'SMOOTH_SPLINE': - # Format: SMOOTH_SPLINE|num_waypoints|frame|start_pose|timing_type|timing_value|waypoint1|waypoint2|... - num_waypoints = int(parts[1]) - frame = parts[2] # 'WRF' or 'TRF' - start_pose = parse_start_pose(parts[3]) - timing_type = parts[4] # 'DURATION' or 'SPEED' - timing_value = float(parts[5]) - - # Parse waypoints - waypoints = [] - idx = 6 - for i in range(num_waypoints): - wp = [] - for j in range(6): # Each waypoint has 6 values (x,y,z,rx,ry,rz) - wp.append(float(parts[idx])) - idx += 1 - waypoints.append(wp) - - # Calculate duration - if timing_type == 'DURATION': - duration = timing_value - else: # SPEED - # Calculate total path length - total_dist = 0 - for i in range(1, len(waypoints)): - dist = np.linalg.norm(np.array(waypoints[i][:3]) - np.array(waypoints[i-1][:3])) - total_dist += dist - - duration = calculate_duration_from_speed(total_dist, timing_value) - - print(f" -> Parsed spline: {num_waypoints} points, frame={frame}, duration={duration:.2f}s") - - # Return command object with frame - return SmoothSplineCommand(waypoints, duration, frame, start_pose) - - elif command_type == 'SMOOTH_HELIX': - # Format: SMOOTH_HELIX|center|radius|pitch|height|frame|start_pose|timing_type|timing_value|clockwise - center = list(map(float, parts[1].split(','))) - radius = float(parts[2]) - pitch = float(parts[3]) - height = float(parts[4]) - frame = parts[5] # 'WRF' or 'TRF' - start_pose = parse_start_pose(parts[6]) - timing_type = parts[7] # 'DURATION' or 'SPEED' - timing_value = float(parts[8]) - clockwise = parts[9] == '1' - - # Calculate duration - if timing_type == 'DURATION': - duration = timing_value - else: # SPEED - # Calculate helix path length - num_revolutions = height / pitch if pitch > 0 else 1 - horizontal_length = 2 * np.pi * radius * num_revolutions - helix_length = np.sqrt(horizontal_length**2 + height**2) - duration = calculate_duration_from_speed(helix_length, timing_value) - - print(f" -> Parsed helix: h={height}mm, pitch={pitch}mm, frame={frame}, duration={duration:.2f}s") - - # Return command object with frame - return SmoothHelixCommand(center, radius, pitch, height, duration, clockwise, frame, start_pose) - - elif command_type == 'SMOOTH_BLEND': - # Format: SMOOTH_BLEND|num_segments|blend_time|frame|start_pose|timing_type|timing_value|segment1||segment2||... - num_segments = int(parts[1]) - blend_time = float(parts[2]) - frame = parts[3] # 'WRF' or 'TRF' - start_pose = parse_start_pose(parts[4]) - timing_type = parts[5] # 'DEFAULT', 'DURATION', or 'SPEED' - - # Parse overall timing - if timing_type == 'DEFAULT': - # Use individual segment durations as-is - overall_duration = None - overall_speed = None - segments_start_idx = 6 - else: - timing_value = float(parts[6]) - if timing_type == 'DURATION': - overall_duration = timing_value - overall_speed = None - else: # SPEED - overall_speed = timing_value - overall_duration = None - segments_start_idx = 7 - - # Parse segments (separated by ||) - segments_data = '|'.join(parts[segments_start_idx:]) - segment_strs = segments_data.split('||') - - # Parse segment definitions - segment_definitions = [] - total_original_duration = 0 - total_estimated_length = 0 - - for seg_str in segment_strs: - if not seg_str: # Skip empty segments - continue - - seg_parts = seg_str.split('|') - seg_type = seg_parts[0] - - if seg_type == 'LINE': - # Format: LINE|end_x,end_y,end_z,end_rx,end_ry,end_rz|duration - end = list(map(float, seg_parts[1].split(','))) - segment_duration = float(seg_parts[2]) - total_original_duration += segment_duration - - # Estimate length (will be refined when we have actual start) - estimated_length = 100 # mm, conservative estimate - total_estimated_length += estimated_length - - segment_definitions.append({ - 'type': 'LINE', - 'end': end, - 'duration': segment_duration, - 'original_duration': segment_duration - }) - - elif seg_type == 'CIRCLE': - # Format: CIRCLE|center_x,center_y,center_z|radius|plane|duration|clockwise - center = list(map(float, seg_parts[1].split(','))) - radius = float(seg_parts[2]) - plane = seg_parts[3] - segment_duration = float(seg_parts[4]) - total_original_duration += segment_duration - clockwise = seg_parts[5] == '1' - - # Circle circumference - estimated_length = 2 * np.pi * radius - total_estimated_length += estimated_length - - segment_definitions.append({ - 'type': 'CIRCLE', - 'center': center, - 'radius': radius, - 'plane': plane, - 'duration': segment_duration, - 'original_duration': segment_duration, - 'clockwise': clockwise - }) - - elif seg_type == 'ARC': - # Format: ARC|end_x,end_y,end_z,end_rx,end_ry,end_rz|center_x,center_y,center_z|duration|clockwise - end = list(map(float, seg_parts[1].split(','))) - center = list(map(float, seg_parts[2].split(','))) - segment_duration = float(seg_parts[3]) - total_original_duration += segment_duration - clockwise = seg_parts[4] == '1' - - # Estimate arc length - estimated_radius = 50 # mm - estimated_arc_angle = np.pi / 2 # 90 degrees - estimated_length = estimated_radius * estimated_arc_angle - total_estimated_length += estimated_length - - segment_definitions.append({ - 'type': 'ARC', - 'end': end, - 'center': center, - 'duration': segment_duration, - 'original_duration': segment_duration, - 'clockwise': clockwise - }) - - elif seg_type == 'SPLINE': - # Format: SPLINE|num_points|waypoint1;waypoint2;...|duration - num_points = int(seg_parts[1]) - waypoints = [] - wp_strs = seg_parts[2].split(';') - for wp_str in wp_strs: - waypoints.append(list(map(float, wp_str.split(',')))) - segment_duration = float(seg_parts[3]) - total_original_duration += segment_duration - - # Estimate spline length - estimated_length = 0 - for i in range(1, len(waypoints)): - estimated_length += np.linalg.norm( - np.array(waypoints[i][:3]) - np.array(waypoints[i-1][:3]) - ) - total_estimated_length += estimated_length - - segment_definitions.append({ - 'type': 'SPLINE', - 'waypoints': waypoints, - 'duration': segment_duration, - 'original_duration': segment_duration - }) - - # Adjust segment durations if overall timing is specified - if overall_duration is not None: - # Scale all segment durations proportionally - if total_original_duration > 0: - scale_factor = overall_duration / total_original_duration - for seg in segment_definitions: - seg['duration'] = seg['original_duration'] * scale_factor - print(f" -> Scaled blend segments to total duration: {overall_duration:.2f}s") - - elif overall_speed is not None: - # Calculate duration from speed and estimated path length - overall_duration = calculate_duration_from_speed(total_estimated_length, overall_speed) - if total_original_duration > 0: - scale_factor = overall_duration / total_original_duration - for seg in segment_definitions: - seg['duration'] = seg['original_duration'] * scale_factor - print(f" -> Calculated blend duration from speed: {overall_duration:.2f}s") - else: - print(f" -> Using original segment durations (total: {total_original_duration:.2f}s)") - - print(f" -> Parsed blend: {num_segments} segments, frame={frame}, blend_time={blend_time}s") - - # Return command with frame - return SmoothBlendCommand(segment_definitions, blend_time, frame, start_pose) - - except Exception as e: - print(f"Error parsing smooth motion command: {e}") - print(f"Command parts: {parts}") - import traceback - traceback.print_exc() - return None - - print(f"Warning: Unknown smooth motion command type: {command_type}") - return None - -def transform_command_params_to_wrf(command_type: str, params: dict, frame: str, current_position_in) -> dict: - """ - Transform command parameters from TRF to WRF. - Handles position, orientation, and directional vectors correctly. - """ - if frame == 'WRF': - return params - - # Get current tool pose - current_q = np.array([PAROL6_ROBOT.STEPS2RADS(p, i) - for i, p in enumerate(current_position_in)]) - tool_pose = PAROL6_ROBOT.robot.fkine(current_q) - - transformed = params.copy() - - # SMOOTH_CIRCLE - Transform center and plane normal - if command_type == 'SMOOTH_CIRCLE': - if 'center' in params: - center_trf = SE3(params['center'][0]/1000, - params['center'][1]/1000, - params['center'][2]/1000) - center_wrf = tool_pose * center_trf - transformed['center'] = (center_wrf.t * 1000).tolist() - - if 'plane' in params: - plane_normals_trf = { - 'XY': [0, 0, 1], # Tool's Z-axis - 'XZ': [0, 1, 0], # Tool's Y-axis - 'YZ': [1, 0, 0] # Tool's X-axis - } - normal_trf = np.array(plane_normals_trf[params['plane']]) - normal_wrf = tool_pose.R @ normal_trf - transformed['normal_vector'] = normal_wrf.tolist() - print(f" -> TRF circle plane {params['plane']} transformed to WRF") - - # SMOOTH_ARC_CENTER - Transform center, end_pose, and implied plane - elif command_type == 'SMOOTH_ARC_CENTER': - if 'center' in params: - center_trf = SE3(params['center'][0]/1000, - params['center'][1]/1000, - params['center'][2]/1000) - center_wrf = tool_pose * center_trf - transformed['center'] = (center_wrf.t * 1000).tolist() - - if 'end_pose' in params: - end_trf = SE3(params['end_pose'][0]/1000, - params['end_pose'][1]/1000, - params['end_pose'][2]/1000) * \ - SE3.RPY(params['end_pose'][3:], unit='deg', order='xyz') - end_wrf = tool_pose * end_trf - transformed['end_pose'] = np.concatenate([ - end_wrf.t * 1000, - end_wrf.rpy(unit='deg', order='xyz') - ]).tolist() - - # Arc plane is determined by start, end, and center points - # But we should transform any specified plane normal - if 'plane' in params: - # Similar to circle plane transformation - plane_normals_trf = { - 'XY': [0, 0, 1], - 'XZ': [0, 1, 0], - 'YZ': [1, 0, 0] - } - normal_trf = np.array(plane_normals_trf[params['plane']]) - normal_wrf = tool_pose.R @ normal_trf - transformed['normal_vector'] = normal_wrf.tolist() - - # SMOOTH_ARC_PARAM - Transform end_pose and arc plane - elif command_type == 'SMOOTH_ARC_PARAM': - if 'end_pose' in params: - end_trf = SE3(params['end_pose'][0]/1000, - params['end_pose'][1]/1000, - params['end_pose'][2]/1000) * \ - SE3.RPY(params['end_pose'][3:], unit='deg', order='xyz') - end_wrf = tool_pose * end_trf - transformed['end_pose'] = np.concatenate([ - end_wrf.t * 1000, - end_wrf.rpy(unit='deg', order='xyz') - ]).tolist() - - # For parametric arc, the plane is usually XY of the tool - # Transform the assumed plane normal - if 'plane' not in params: - params['plane'] = 'XY' # Default to XY plane - - plane_normals_trf = { - 'XY': [0, 0, 1], - 'XZ': [0, 1, 0], - 'YZ': [1, 0, 0] - } - normal_trf = np.array(plane_normals_trf[params.get('plane', 'XY')]) - normal_wrf = tool_pose.R @ normal_trf - transformed['normal_vector'] = normal_wrf.tolist() - - # SMOOTH_HELIX - Transform center and helix axis - elif command_type == 'SMOOTH_HELIX': - if 'center' in params: - center_trf = SE3(params['center'][0]/1000, - params['center'][1]/1000, - params['center'][2]/1000) - center_wrf = tool_pose * center_trf - transformed['center'] = (center_wrf.t * 1000).tolist() - - # Helix axis - default is Z-axis (vertical in TRF) - # In TRF, helix rises along tool's Z-axis - helix_axis_trf = np.array([0, 0, 1]) # Tool's Z-axis - helix_axis_wrf = tool_pose.R @ helix_axis_trf - transformed['helix_axis'] = helix_axis_wrf.tolist() - - # Also need to transform the "up" direction for proper orientation - up_vector_trf = np.array([0, 1, 0]) # Tool's Y-axis - up_vector_wrf = tool_pose.R @ up_vector_trf - transformed['up_vector'] = up_vector_wrf.tolist() - - # SMOOTH_SPLINE - Transform all waypoints - elif command_type == 'SMOOTH_SPLINE': - if 'waypoints' in params: - transformed_waypoints = [] - for wp in params['waypoints']: - wp_trf = SE3(wp[0]/1000, wp[1]/1000, wp[2]/1000) * \ - SE3.RPY(wp[3:], unit='deg', order='xyz') - wp_wrf = tool_pose * wp_trf - wp_transformed = np.concatenate([ - wp_wrf.t * 1000, - wp_wrf.rpy(unit='deg', order='xyz') - ]).tolist() - transformed_waypoints.append(wp_transformed) - transformed['waypoints'] = transformed_waypoints - - # SMOOTH_BLEND - Transform all segments recursively - elif command_type == 'SMOOTH_BLEND': - if 'segments' in params: - transformed_segments = [] - for seg in params['segments']: - seg_copy = seg.copy() - seg_type = seg['type'] - - if seg_type == 'LINE': - if 'end' in seg: - end_trf = SE3(seg['end'][0]/1000, - seg['end'][1]/1000, - seg['end'][2]/1000) * \ - SE3.RPY(seg['end'][3:], unit='deg', order='xyz') - end_wrf = tool_pose * end_trf - seg_copy['end'] = np.concatenate([ - end_wrf.t * 1000, - end_wrf.rpy(unit='deg', order='xyz') - ]).tolist() - - elif seg_type == 'CIRCLE': - if 'center' in seg: - center_trf = SE3(seg['center'][0]/1000, - seg['center'][1]/1000, - seg['center'][2]/1000) - center_wrf = tool_pose * center_trf - seg_copy['center'] = (center_wrf.t * 1000).tolist() - - if 'plane' in seg: - plane_normals_trf = { - 'XY': [0, 0, 1], - 'XZ': [0, 1, 0], - 'YZ': [1, 0, 0] - } - normal_trf = np.array(plane_normals_trf[seg['plane']]) - normal_wrf = tool_pose.R @ normal_trf - seg_copy['normal_vector'] = normal_wrf.tolist() - - elif seg_type == 'ARC': - if 'center' in seg: - center_trf = SE3(seg['center'][0]/1000, - seg['center'][1]/1000, - seg['center'][2]/1000) - center_wrf = tool_pose * center_trf - seg_copy['center'] = (center_wrf.t * 1000).tolist() - - if 'end' in seg: - end_trf = SE3(seg['end'][0]/1000, - seg['end'][1]/1000, - seg['end'][2]/1000) * \ - SE3.RPY(seg['end'][3:], unit='deg', order='xyz') - end_wrf = tool_pose * end_trf - seg_copy['end'] = np.concatenate([ - end_wrf.t * 1000, - end_wrf.rpy(unit='deg', order='xyz') - ]).tolist() - - elif seg_type == 'SPLINE': - if 'waypoints' in seg: - transformed_waypoints = [] - for wp in seg['waypoints']: - wp_trf = SE3(wp[0]/1000, wp[1]/1000, wp[2]/1000) * \ - SE3.RPY(wp[3:], unit='deg', order='xyz') - wp_wrf = tool_pose * wp_trf - wp_transformed = np.concatenate([ - wp_wrf.t * 1000, - wp_wrf.rpy(unit='deg', order='xyz') - ]).tolist() - transformed_waypoints.append(wp_transformed) - seg_copy['waypoints'] = transformed_waypoints - - transformed_segments.append(seg_copy) - transformed['segments'] = transformed_segments - - # Transform start_pose if specified (common to all commands) - if 'start_pose' in params and params['start_pose'] is not None: - start_trf = SE3(params['start_pose'][0]/1000, - params['start_pose'][1]/1000, - params['start_pose'][2]/1000) * \ - SE3.RPY(params['start_pose'][3:], unit='deg', order='xyz') - start_wrf = tool_pose * start_trf - transformed['start_pose'] = np.concatenate([ - start_wrf.t * 1000, - start_wrf.rpy(unit='deg', order='xyz') - ]).tolist() - - return transformed - -######################################################################### -# Smooth Motion Commands and Robot Commands End Here -######################################################################### - -# Acknowledgment system configuration -CLIENT_ACK_PORT = 5002 # Port where clients listen for acknowledgments -ack_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - -# Command tracking -active_command_id = None -command_id_map = {} # Maps command objects to their IDs - -def send_acknowledgment(cmd_id: str, status: str, details: str = "", addr=None): - """Send acknowledgment back to client""" - if not cmd_id: - return - - ack_message = f"ACK|{cmd_id}|{status}|{details}" - - # Send to the original sender if we have their address - if addr: - try: - ack_socket.sendto(ack_message.encode('utf-8'), (addr[0], CLIENT_ACK_PORT)) - except Exception as e: - print(f"Failed to send ack to {addr}: {e}") - - # Also broadcast to localhost in case the client is local - try: - ack_socket.sendto(ack_message.encode('utf-8'), ('127.0.0.1', CLIENT_ACK_PORT)) - except: - pass - -def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: - """ - Parse command ID if present. - Format: [cmd_id|]COMMAND|params... - Returns: (cmd_id or None, command_string) - """ - # Clean up any logging artifacts - if "ID:" in message or "):" in message: - # Extract the actual command after these artifacts - if "):" in message: - message = message[message.rindex("):")+2:].strip() - elif "ID:" in message: - message = message[message.index("ID:")+3:].strip() - # Remove any trailing parentheses or colons - message = message.lstrip('):').strip() - - parts = message.split('|', 1) - - # Check if first part looks like a valid command ID (8 chars, alphanumeric) - # IMPORTANT: Command IDs from uuid.uuid4()[:8] will contain lowercase letters/numbers - # Actual commands are all uppercase, so exclude all-uppercase strings - if (len(parts) > 1 and - len(parts[0]) == 8 and - parts[0].replace('-', '').isalnum() and - not parts[0].isupper()): # This prevents "MOVEPOSE" from being treated as an ID - return parts[0], parts[1] - else: - return None, message - - -# Create a new, empty command queue -command_queue = deque() - -# -------------------------------------------------------------------------- -# --- Test 1: Homing and Initial Setup -# -------------------------------------------------------------------------- - -# 1. Start with the mandatory Home command. -command_queue.append(HomeCommand()) - -# --- State variable for the currently running command --- -active_command = None -e_stop_active = False - -# Use deque for an efficient FIFO queue -incoming_command_buffer = deque() -# Timestamp of the last processed network command -last_command_time = 0 -# Cooldown period in seconds to prevent command flooding -COMMAND_COOLDOWN_S = 0.1 # 100ms - -# Set interval -timer = Timer(interval=INTERVAL_S, warnings=False, precise=True) - -# ============================================================================ -# MODIFIED MAIN LOOP WITH ACKNOWLEDGMENTS -# ============================================================================ - -timer = Timer(interval=INTERVAL_S, warnings=False, precise=True) -prev_time = 0 - -while timer.elapsed_time < 1100000: - - # --- Connection Handling --- - if ser is None or not ser.is_open: - print("Serial port not open. Attempting to reconnect...") - try: - ser = serial.Serial(port=com_port_str, baudrate=3000000, timeout=0) - if ser.is_open: - print(f"Successfully reconnected to {com_port_str}") - except serial.SerialException as e: - ser = None - time.sleep(1) - continue - - # ======================================================================= - # === NETWORK COMMAND RECEPTION WITH ID PARSING === - # ======================================================================= - try: - while sock in select.select([sock], [], [], 0)[0]: - data, addr = sock.recvfrom(1024) - raw_message = data.decode('utf-8').strip() - if raw_message: - # Parse command ID if present - cmd_id, message = parse_command_with_id(raw_message) - - parts = message.split('|') - command_name = parts[0].upper() - - # Handle immediate response commands - if command_name == 'STOP': - print("Received STOP command. Halting all motion and clearing queue.") - - # Cancel active command - if active_command and active_command_id: - send_acknowledgment(active_command_id, "CANCELLED", - "Stopped by user", addr) - active_command = None - active_command_id = None - - # Clear queue and notify about cancelled commands - for queued_cmd in command_id_map.keys(): - if queued_cmd != active_command: - if queued_cmd in command_id_map: - send_acknowledgment(command_id_map[queued_cmd], - "CANCELLED", "Queue cleared by STOP", addr) - - command_queue.clear() - command_id_map.clear() - - # Stop robot - Command_out.value = 255 - Speed_out[:] = [0] * 6 - - # Send acknowledgment for STOP command itself - if cmd_id: - send_acknowledgment(cmd_id, "COMPLETED", "Emergency stop executed", addr) - - elif command_name == 'GET_POSE': - q_current = np.array([PAROL6_ROBOT.STEPS2RADS(p, i) for i, p in enumerate(Position_in)]) - current_pose_matrix = PAROL6_ROBOT.robot.fkine(q_current).A - pose_flat = current_pose_matrix.flatten() - pose_str = ",".join(map(str, pose_flat)) - response_message = f"POSE|{pose_str}" - sock.sendto(response_message.encode('utf-8'), addr) - - if cmd_id: - send_acknowledgment(cmd_id, "COMPLETED", "Pose data sent", addr) - - elif command_name == 'GET_ANGLES': - angles_rad = [PAROL6_ROBOT.STEPS2RADS(p, i) for i, p in enumerate(Position_in)] - angles_deg = np.rad2deg(angles_rad) - angles_str = ",".join(map(str, angles_deg)) - response_message = f"ANGLES|{angles_str}" - sock.sendto(response_message.encode('utf-8'), addr) - - if cmd_id: - send_acknowledgment(cmd_id, "COMPLETED", "Angles data sent", addr) - - elif command_name == 'GET_IO': - io_status_str = ",".join(map(str, InOut_in[:5])) - response_message = f"IO|{io_status_str}" - sock.sendto(response_message.encode('utf-8'), addr) - - if cmd_id: - send_acknowledgment(cmd_id, "COMPLETED", "IO data sent", addr) - - elif command_name == 'GET_GRIPPER': - gripper_status_str = ",".join(map(str, Gripper_data_in)) - response_message = f"GRIPPER|{gripper_status_str}" - sock.sendto(response_message.encode('utf-8'), addr) - - if cmd_id: - send_acknowledgment(cmd_id, "COMPLETED", "Gripper data sent", addr) - - elif command_name == 'GET_SPEEDS': - speeds_str = ",".join(map(str, Speed_in)) - response_message = f"SPEEDS|{speeds_str}" - sock.sendto(response_message.encode('utf-8'), addr) - - if cmd_id: - send_acknowledgment(cmd_id, "COMPLETED", "Speed data sent", addr) - - else: - # Queue command for processing - incoming_command_buffer.append((raw_message, addr)) - - except Exception as e: - print(f"Network receive error: {e}") - - # ======================================================================= - # === PROCESS COMMANDS FROM BUFFER WITH ACKNOWLEDGMENTS === - # ======================================================================= - current_time = time.time() - if incoming_command_buffer and (current_time - last_command_time) > COMMAND_COOLDOWN_S and not e_stop_active: - raw_message, addr = incoming_command_buffer.popleft() - last_command_time = current_time - - # Parse command ID - cmd_id, message = parse_command_with_id(raw_message) - print(f"Processing command{' (ID: ' + cmd_id + ')' if cmd_id else ''}: {message[:50]}...") - - parts = message.split('|') - command_name = parts[0].upper() - - # Variable to track if command was successfully queued - command_queued = False - command_obj = None - error_details = "" - - # Parse and create command objects - try: - if command_name == 'MOVEPOSE' and len(parts) == 9: - pose_vals = [float(p) for p in parts[1:7]] - duration = None if parts[7].upper() == 'NONE' else float(parts[7]) - speed = None if parts[8].upper() == 'NONE' else float(parts[8]) - command_obj = MovePoseCommand(pose=pose_vals, duration=duration, velocity_percent=speed) - command_queued = True - - elif command_name == 'MOVEJOINT' and len(parts) == 9: - joint_vals = [float(p) for p in parts[1:7]] - duration = None if parts[7].upper() == 'NONE' else float(parts[7]) - speed = None if parts[8].upper() == 'NONE' else float(parts[8]) - command_obj = MoveJointCommand(target_angles=joint_vals, duration=duration, velocity_percent=speed) - command_queued = True - - elif command_name in ['SMOOTH_CIRCLE', 'SMOOTH_ARC_CENTER', 'SMOOTH_ARC_PARAM', - 'SMOOTH_SPLINE', 'SMOOTH_HELIX', 'SMOOTH_BLEND']: - command_obj = parse_smooth_motion_commands(parts) - if command_obj: - command_queued = True - else: - error_details = "Failed to parse smooth motion parameters" - - elif command_name == 'MOVECART' and len(parts) == 9: - pose_vals = [float(p) for p in parts[1:7]] - duration = None if parts[7].upper() == 'NONE' else float(parts[7]) - speed = None if parts[8].upper() == 'NONE' else float(parts[8]) - command_obj = MoveCartCommand(pose=pose_vals, duration=duration, velocity_percent=speed) - command_queued = True - - elif command_name == 'DELAY' and len(parts) == 2: - duration = float(parts[1]) - command_obj = DelayCommand(duration=duration) - command_queued = True - - elif command_name == 'HOME': - command_obj = HomeCommand() - command_queued = True - - elif command_name == 'CARTJOG' and len(parts) == 5: - frame, axis, speed, duration = parts[1].upper(), parts[2], float(parts[3]), float(parts[4]) - command_obj = CartesianJogCommand(frame=frame, axis=axis, speed_percentage=speed, duration=duration) - command_queued = True - - elif command_name == 'JOG' and len(parts) == 5: - joint_idx, speed = int(parts[1]), float(parts[2]) - duration = None if parts[3].upper() == 'NONE' else float(parts[3]) - distance = None if parts[4].upper() == 'NONE' else float(parts[4]) - command_obj = JogCommand(joint=joint_idx, speed_percentage=speed, duration=duration, distance_deg=distance) - command_queued = True - - elif command_name == 'MULTIJOG' and len(parts) == 4: - joint_indices = [int(j) for j in parts[1].split(',')] - speeds = [float(s) for s in parts[2].split(',')] - duration = float(parts[3]) - command_obj = MultiJogCommand(joints=joint_indices, speed_percentages=speeds, duration=duration) - command_queued = True - - elif command_name == 'PNEUMATICGRIPPER' and len(parts) == 3: - action, port = parts[1].lower(), int(parts[2]) - command_obj = GripperCommand(gripper_type='pneumatic', action=action, output_port=port) - command_queued = True - - elif command_name == 'ELECTRICGRIPPER' and len(parts) == 5: - action = None if parts[1].upper() == 'NONE' or parts[1].upper() == 'MOVE' else parts[1].lower() - pos, spd, curr = int(parts[2]), int(parts[3]), int(parts[4]) - command_obj = GripperCommand(gripper_type='electric', action=action, position=pos, speed=spd, current=curr) - command_queued = True - - else: - error_details = f"Unknown or malformed command: {command_name}" - - except Exception as e: - error_details = f"Error parsing command: {str(e)}" - command_queued = False - - # Handle command queueing and acknowledgments - if command_queued and command_obj: - # Check if command is initially valid - if hasattr(command_obj, 'is_valid') and not command_obj.is_valid: - if cmd_id: - send_acknowledgment(cmd_id, "INVALID", - "Command failed validation", addr) - else: - # Add to queue - command_queue.append(command_obj) - if cmd_id: - command_id_map[command_obj] = (cmd_id, addr) - send_acknowledgment(cmd_id, "QUEUED", - f"Position {len(command_queue)} in queue", addr) - else: - # Command was not queued - if cmd_id: - send_acknowledgment(cmd_id, "INVALID", error_details, addr) - print(f"Warning: {error_details}") - - # ======================================================================= - # === MAIN EXECUTION LOGIC WITH ACKNOWLEDGMENTS === - # ======================================================================= - try: - # --- E-Stop Handling --- - if InOut_in[4] == 0: # E-Stop pressed - if not e_stop_active: - cancelled_command_info = "None" - if active_command is not None: - cancelled_command_info = type(active_command).__name__ - if active_command_id: - send_acknowledgment(active_command_id, "CANCELLED", - "E-Stop activated") - - # Cancel all queued commands - for cmd_obj in command_queue: - if cmd_obj in command_id_map: - cmd_id, addr = command_id_map[cmd_obj] - send_acknowledgment(cmd_id, "CANCELLED", "E-Stop activated", addr) - - # Cancel all buffered but unprocessed commands - for raw_message, addr in incoming_command_buffer: - cmd_id, _ = parse_command_with_id(raw_message) - if cmd_id: - send_acknowledgment(cmd_id, "CANCELLED", "E-Stop activated - command not processed", addr) - - print(f"E-STOP TRIGGERED! Active command '{cancelled_command_info}' cancelled.") - print("Release E-Stop and press 'e' to re-enable.") - e_stop_active = True - - Command_out.value = 102 - Speed_out[:] = [0] * 6 - Gripper_data_out[3] = 0 - active_command = None - active_command_id = None - command_queue.clear() - command_id_map.clear() - incoming_command_buffer.clear() - - elif e_stop_active: - # Waiting for re-enable - if keyboard.is_pressed('e'): - print("Re-enabling robot...") - Command_out.value = 101 - e_stop_active = False - else: - Command_out.value = 255 - Speed_out[:] = [0] * 6 - Position_out[:] = Position_in[:] - - else: - # --- Normal Command Processing --- - - # Start new command if none active - if active_command is None and command_queue: - new_command = command_queue.popleft() - - # Get command ID and address if tracked - cmd_info = command_id_map.get(new_command, (None, None)) - new_cmd_id, new_addr = cmd_info - - # Initial validation - if hasattr(new_command, 'is_valid') and not new_command.is_valid: - # Command was invalid from the start - if new_cmd_id: - send_acknowledgment(new_cmd_id, "INVALID", - "Initial validation failed", new_addr) - if new_command in command_id_map: - del command_id_map[new_command] - continue # Skip to next command - - # Prepare command - if hasattr(new_command, 'prepare_for_execution'): - try: - new_command.prepare_for_execution(current_position_in=Position_in) - except Exception as e: - print(f"Command preparation failed: {e}") - if hasattr(new_command, 'is_valid'): - new_command.is_valid = False - if hasattr(new_command, 'error_message'): - new_command.error_message = str(e) - - # Check if still valid after preparation - if hasattr(new_command, 'is_valid') and not new_command.is_valid: - # Failed during preparation - error_msg = "Failed during preparation" - if hasattr(new_command, 'error_message'): - error_msg = new_command.error_message - - if new_cmd_id: - send_acknowledgment(new_cmd_id, "FAILED", error_msg, new_addr) - - # Clean up - if new_command in command_id_map: - del command_id_map[new_command] - else: - # Command is valid, make it active - active_command = new_command - active_command_id = new_cmd_id - - if new_cmd_id: - send_acknowledgment(new_cmd_id, "EXECUTING", - f"Starting {type(new_command).__name__}", new_addr) - - # Execute active command - if active_command: - try: - is_done = active_command.execute_step( - Position_in=Position_in, - Homed_in=Homed_in, - Speed_out=Speed_out, - Command_out=Command_out, - Gripper_data_out=Gripper_data_out, - InOut_out=InOut_out, - InOut_in=InOut_in, - Gripper_data_in=Gripper_data_in, - Position_out=Position_out # Add this if needed - ) - - if is_done: - # Command completed - if active_command_id: - # Check for error state in smooth motion commands - if hasattr(active_command, 'error_state') and active_command.error_state: - error_msg = getattr(active_command, 'error_message', 'Command failed during execution') - send_acknowledgment(active_command_id, "FAILED", error_msg) - else: - send_acknowledgment(active_command_id, "COMPLETED", - f"{type(active_command).__name__} finished successfully") - - # Clean up - if active_command in command_id_map: - del command_id_map[active_command] - - active_command = None - active_command_id = None - - except Exception as e: - # Command execution error - print(f"Command execution error: {e}") - if active_command_id: - send_acknowledgment(active_command_id, "FAILED", - f"Execution error: {str(e)}") - - # Clean up - if active_command in command_id_map: - del command_id_map[active_command] - - active_command = None - active_command_id = None - - else: - # No active command - idle - Command_out.value = 255 - Speed_out[:] = [0] * 6 - Position_out[:] = Position_in[:] - - # --- Communication with Robot --- - s = Pack_data(Position_out, Speed_out, Command_out.value, - Affected_joint_out, InOut_out, Timeout_out, Gripper_data_out) - for chunk in s: - ser.write(chunk) - - Get_data(Position_in, Speed_in, Homed_in, InOut_in, Temperature_error_in, - Position_error_in, Timeout_error, Timing_data_in, XTR_data, Gripper_data_in) - - except serial.SerialException as e: - print(f"Serial communication error: {e}") - - # Send failure acknowledgments for active command - if active_command_id: - send_acknowledgment(active_command_id, "FAILED", "Serial communication lost") - - if ser: - ser.close() - ser = None - active_command = None - active_command_id = None - - timer.checkpt() \ No newline at end of file diff --git a/parol6/PAROL6_ROBOT.py b/parol6/PAROL6_ROBOT.py new file mode 100644 index 0000000..baa490e --- /dev/null +++ b/parol6/PAROL6_ROBOT.py @@ -0,0 +1,605 @@ +# 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 new file mode 100644 index 0000000..6997a08 --- /dev/null +++ b/parol6/__init__.py @@ -0,0 +1,29 @@ +""" +PAROL6 Python Package + +A unified library for controlling PAROL6 robot arms with async-first UDP client, +optional sync wrapper, and server management capabilities. + +Key components: +- AsyncRobotClient: Async UDP client for robot operations +- RobotClient: Sync wrapper with automatic event loop handling +- ServerManager: Manages headless controller process lifecycle +- 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 + +__all__ = [ + "__version__", + "AsyncRobotClient", + "RobotClient", + "ServerManager", + "manage_server", + "is_server_running", + "PAROL6_ROBOT", +] diff --git a/parol6/_version.py b/parol6/_version.py new file mode 100644 index 0000000..8545f72 --- /dev/null +++ b/parol6/_version.py @@ -0,0 +1,3 @@ +"""Version information for parol6 package.""" + +__version__ = "0.1.0" diff --git a/parol6/ack_policy.py b/parol6/ack_policy.py new file mode 100644 index 0000000..16b1754 --- /dev/null +++ b/parol6/ack_policy.py @@ -0,0 +1,81 @@ +import os +from collections.abc import Callable + +SYSTEM_COMMANDS: set[str] = { + "STOP", + "ENABLE", + "DISABLE", + "SET_PORT", + "STREAM", + "SIMULATOR", +} + +QUERY_COMMANDS: set[str] = { + "GET_POSE", + "GET_ANGLES", + "GET_IO", + "GET_GRIPPER", + "GET_SPEEDS", + "GET_STATUS", + "GET_GCODE_STATUS", + "GET_LOOP_STATS", + "GET_CURRENT_ACTION", + "GET_QUEUE", + "PING", +} + + +def is_localhost(host: str) -> bool: + h = (host or "").strip().lower() + return h in {"127.0.0.1", "localhost", "::1"} + + +class AckPolicy: + """ + Centralized heuristic for deciding if a command requires an acknowledgment. + + Rules: + - If force_ack is set, it overrides everything. + - Safety-critical commands always require ack. + - If running on localhost/loopback, default to no-ack (low drop risk). + - If stream mode is ON, default to no-ack (high-rate streaming traffic). + - Otherwise default to no-ack. + """ + + def __init__( + self, + get_stream_mode: Callable[[], bool], + force_ack: bool | None = None, + ) -> None: + self._get_stream_mode = get_stream_mode + self._force_ack = force_ack + + @staticmethod + def from_env(get_stream_mode: Callable[[], bool]) -> "AckPolicy": + raw = os.getenv("PAROL6_FORCE_ACK", "").strip().lower() + if raw in {"1", "true", "yes", "on"}: + force = True + elif raw in {"0", "false", "no", "off"}: + force = False + else: + force = None + return AckPolicy(get_stream_mode=get_stream_mode, force_ack=force) + + def requires_ack(self, message: str) -> bool: + # Forced override (e.g., diagnostics) + if self._force_ack is not None: + return bool(self._force_ack) + + name = (message or "").split("|", 1)[0].strip().upper() + + # System commands always require ACKs + if name in SYSTEM_COMMANDS: + return True + + # Query commands use request/response, not ACKs + if name in QUERY_COMMANDS: + return False + + # Motion and other commands: ACKs only when forced + # Localhost and stream mode both favor no-ack by default + return False diff --git a/parol6/cli/__init__.py b/parol6/cli/__init__.py new file mode 100644 index 0000000..e16bac8 --- /dev/null +++ b/parol6/cli/__init__.py @@ -0,0 +1 @@ +"""Command-line interface modules.""" diff --git a/parol6/cli/server.py b/parol6/cli/server.py new file mode 100644 index 0000000..a4933d8 --- /dev/null +++ b/parol6/cli/server.py @@ -0,0 +1,16 @@ +""" +CLI entry point for parol6-server command. + +This module provides the command-line interface for starting the PAROL6 headless controller. +""" + +from parol6.server.controller import main + + +def main_entry(): + """Entry point for the parol6-server command.""" + main() + + +if __name__ == "__main__": + main_entry() diff --git a/parol6/client/__init__.py b/parol6/client/__init__.py new file mode 100644 index 0000000..eb1a788 --- /dev/null +++ b/parol6/client/__init__.py @@ -0,0 +1 @@ +"""Client modules for robot communication.""" diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py new file mode 100644 index 0000000..c431b61 --- /dev/null +++ b/parol6/client/async_client.py @@ -0,0 +1,1352 @@ +""" +Async UDP client for PAROL6 robot control. +""" + +import asyncio +import contextlib +import json +import logging +import random +import time +from collections.abc import AsyncIterator, Callable, Iterable +from typing import Literal, cast + +import numpy as np +from spatialmath import SO3 + +from .. import config as cfg +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]]]): + self.rx_queue = rx_queue + self.transport: asyncio.DatagramTransport | None = None + + def connection_made(self, transport: asyncio.BaseTransport) -> None: + self.transport = cast(asyncio.DatagramTransport, transport) + + def datagram_received(self, data: bytes, addr: tuple[str, int]) -> None: + try: + self.rx_queue.put_nowait((data, addr)) + except Exception: + pass + + def error_received(self, exc: Exception) -> None: + pass + + def connection_lost(self, exc: Exception | None) -> None: + pass + + +class AsyncRobotClient: + """ + Async UDP client for the PAROL6 headless controller. + + Motion/control commands: fire-and-forget via UDP + Query commands: request/response with timeout and simple retry + """ + + def __init__( + self, + host: str = "127.0.0.1", + port: int = 5001, + timeout: float = 1.0, + retries: int = 1, + ) -> None: + # 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._ep_lock = asyncio.Lock() + + # Serialize request/response + self._req_lock = asyncio.Lock() + + # Stream flag for UI convenience + 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() + self._rx_queue = asyncio.Queue(maxsize=256) + transport, protocol = await loop.create_datagram_endpoint( + lambda: _UDPClientProtocol(self._rx_queue), + remote_addr=(self.host, self.port), + ) + 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() + + 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}" + ) + # 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 + ) + 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: + self._status_queue.get_nowait() + except asyncio.QueueEmpty: + pass + try: + 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 unexpectedly, could retry but for now just exit + pass + + # Start listener task + self._multicast_task = asyncio.create_task(_listener()) + + # --------------- Lifecycle / context management --------------- + + async def close(self) -> None: + """Release UDP transport and background tasks. + + Safe to call multiple times. + """ + 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: + item = await self._status_queue.get() + if item is _STATUS_SENTINEL: + break + yield item + + async def _send(self, message: str) -> bool: + """ + Send a command based on AckPolicy: + - System commands: wait for server OK/ERROR, return True on OK + - Motion commands: wait iff policy requires ACK; otherwise fire-and-forget (return True on send) + - Query commands: should use _request path; if invoked here, just fire-and-forget + """ + await self._ensure_endpoint() + assert self._transport is not None + + name = (message or "").split("|", 1)[0].strip().upper() + + # System commands: wait for OK/ERROR + if name in SYSTEM_COMMANDS: + return await self._request_ok(message, self.timeout) + + # Motion and other non-query commands + if name not in QUERY_COMMANDS: + if self._ack_policy.requires_ack(message): + return await self._request_ok(message, self.timeout) + # Fire-and-forget + self._transport.sendto(message.encode("ascii")) + return True + + # Queries: fire-and-forget here (query methods use _request()) + self._transport.sendto(message.encode("ascii")) + return True + + async def _request(self, message: str, bufsize: int = 2048) -> str | None: + """Send a request and wait for a UDP response.""" + await self._ensure_endpoint() + assert self._transport is not None + for attempt in range(self.retries + 1): + 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 + ) + return data.decode("ascii", errors="ignore") + except (asyncio.TimeoutError, TimeoutError): + if attempt < self.retries: + backoff = min(0.5, 0.05 * (2**attempt)) + random.uniform(0, 0.05) + await asyncio.sleep(backoff) + continue + except Exception: + break + return None + + async def _request_ok(self, message: str, timeout: float) -> bool: + """ + Send a command and wait for a simple 'OK' or 'ERROR|...' reply. + Returns True on OK; raises on ERROR or timeout. + """ + await self._ensure_endpoint() + assert self._transport is not None + + end_time = time.time() + timeout + async with self._req_lock: + self._transport.sendto(message.encode("ascii")) + while time.time() < end_time: + try: + data, _addr = await asyncio.wait_for( + self._rx_queue.get(), timeout=max(0.0, end_time - time.time()) + ) + text = data.decode("ascii", errors="ignore").strip() + if text == "OK": + return True + if text.startswith("ERROR|"): + raise RuntimeError(text) + # Ignore unrelated datagrams + except (asyncio.TimeoutError, TimeoutError): + break + except Exception: + break + raise TimeoutError("Timeout waiting for OK") + + # --------------- Motion / Control --------------- + + async def home(self) -> bool: + return await self._send("HOME") + + async def enable(self) -> bool: + return await self._send("ENABLE") + + async def disable(self) -> bool: + return await self._send("DISABLE") + + 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 + return await self._send("STREAM|ON") + + 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") + + async def simulator_off(self) -> bool: + return await self._send("SIMULATOR|OFF") + + async def set_serial_port(self, port_str: str) -> bool: + if not port_str: + raise ValueError("No port provided") + return await self._send(f"SET_PORT|{port_str}") + + # --------------- Status / Queries --------------- + async def ping(self) -> str | None: + """Return raw 'PONG|...' text (e.g., 'PONG|SERIAL=1') or None on timeout.""" + return await self._request("PING", bufsize=256) + + async def get_angles(self) -> list[float] | None: + resp = await self._request("GET_ANGLES", bufsize=1024) + if not resp: + return None + vals = wire.decode_simple(resp, "ANGLES") + 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) + + 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) + + 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) + + async def get_pose( + self, frame: Literal["WRF", "TRF"] = "WRF" + ) -> list[float] | None: + """ + 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" + """ + 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) + + async def get_gripper(self) -> list[int] | None: + """Alias for get_gripper_status for compatibility.""" + return await self.get_gripper_status() + + 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 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) + if not resp: + return None + return cast(dict | None, wire.decode_status(resp)) + + async def get_loop_stats(self) -> dict | None: + """ + Fetch control-loop runtime metrics. + Expected wire format: "LOOP_STATS|{json}" + """ + 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])) + + 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_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: + x, y, z = pose_matrix[3], pose_matrix[7], pose_matrix[11] + # 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: + """Get robot position as [x, y, z] in mm.""" + pose_rpy = await self.get_pose_rpy() + return pose_rpy[:3] if pose_rpy else None + + async def is_estop_pressed(self) -> bool: + """Check if E-stop is pressed. Returns True if pressed.""" + io_status = await self.get_io() + if io_status and len(io_status) >= 5: + return io_status[4] == 0 # E-stop at index 4, 0 means pressed + return False + + 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 + """ + speeds = await self.get_speeds() + if not speeds: + return False + return max(abs(s) for s in speeds) < threshold_speed + + async def wait_until_stopped( + self, + timeout: float = 90.0, + settle_window: float = 1.0, + speed_threshold: float = 2.0, + 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)) + + try: + async for status in self.status_stream(): + if timeout_task.done(): + return False + + # Check speeds from status + 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: + settle_start = time.time() + elif time.time() - settle_start > settle_window: + return True + else: + settle_start = None + + # Also check angles as fallback + 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, strict=False) + ) + if max_change < angle_threshold: + if settle_start is None: + settle_start = time.time() + elif time.time() - settle_start > settle_window: + return True + else: + settle_start = None + last_angles = angles + finally: + timeout_task.cancel() + try: + 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: + """Poll ping() until server responds or timeout.""" + end_time = time.time() + timeout + while time.time() < end_time: + ok = await self.ping() + if ok: + return True + await asyncio.sleep(interval) + return False + + 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, TimeoutError): + break + try: + if predicate(status): + return True + except Exception: + # Ignore predicate exceptions from tests + pass + return False + + 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 + try: + if not await_reply: + self._transport.sendto(message.encode("ascii")) + 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 + ) + return data.decode("ascii", errors="ignore") + except (asyncio.TimeoutError, TimeoutError): + return None + except Exception: + return None + + # --------------- Motion encoders --------------- + + async def move_joints( + self, + joint_angles: list[float], + duration: float | None = None, + speed_percentage: int | None = None, + accel_percentage: int | None = None, # accepted but not sent + profile: str | None = None, # accepted but not sent + 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." + ) + message = wire.encode_move_joint(joint_angles, duration, speed_percentage) + return await self._send(message) + + async def move_pose( + self, + pose: list[float], + duration: float | None = None, + speed_percentage: int | None = None, + accel_percentage: int | None = None, + profile: str | None = None, + 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." + ) + message = wire.encode_move_pose(pose, duration, speed_percentage) + return await self._send(message) + + async def move_cartesian( + self, + pose: list[float], + duration: float | None = None, + speed_percentage: float | None = None, + accel_percentage: int | None = None, + profile: str | None = None, + 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." + ) + message = wire.encode_move_cartesian(pose, duration, speed_percentage) + return await self._send(message) + + async def move_cartesian_rel_trf( + self, + deltas: list[float], # [dx, dy, dz, rx, ry, rz] + duration: float | None = None, + speed_percentage: float | None = None, + accel_percentage: int | None = None, + profile: str | None = None, + tracking: str | None = None, + ) -> bool: + """ + Send a MOVECARTRELTRF (relative straight-line in TRF) command. + 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." + ) + message = wire.encode_move_cartesian_rel_trf( + deltas, duration, speed_percentage, accel_percentage, profile, tracking + ) + return await self._send(message) + + async def jog_joint( + self, + joint_index: int, + speed_percentage: int, + duration: float | None = None, + distance_deg: float | None = None, + ) -> bool: + """ + Send a JOG command for a single joint (0..5 positive, 6..11 negative for reverse). + 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 + ) + return await self._send(message) + + async def jog_cartesian( + self, + frame: Frame, + axis: Axis, + speed_percentage: int, + duration: float, + ) -> bool: + """ + Send a CARTJOG command (frame 'TRF' or 'WRF', axis in {X+/X-/Y+/.../RZ-}). + """ + message = wire.encode_cart_jog(frame, axis, speed_percentage, duration) + return await self._send(message) + + async def jog_multiple( + self, + joints: list[int], + speeds: list[float], + duration: float, + ) -> bool: + """ + 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." + ) + 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}" + return await self._send(message) + + async def set_io(self, index: int, value: int) -> bool: + """ + Set digital I/O bit. + index: 0..7, value: 0 or 1 + """ + if index < 0 or index > 7: + raise ValueError("I/O index must be 0..7") + if value not in (0, 1): + raise ValueError("I/O value must be 0 or 1") + return await self._send(f"SET_IO|{index}|{value}") + + async def delay(self, seconds: float) -> bool: + """ + Insert a non-blocking delay in the motion queue. + """ + if seconds <= 0: + raise ValueError("Delay must be positive") + return await self._send(f"DELAY|{seconds}") + + # --------------- IO / Gripper --------------- + + async def control_pneumatic_gripper(self, action: str, port: int) -> bool: + """ + Control pneumatic gripper via digital outputs. + action: 'open' or 'close' + port: 1 or 2 + """ + action = action.lower() + if action not in ("open", "close"): + raise ValueError("Invalid pneumatic action") + if port not in (1, 2): + raise ValueError("Invalid pneumatic port") + message = f"PNEUMATICGRIPPER|{action}|{port}" + return await self._send(message) + + async def control_electric_gripper( + self, + action: str, + position: int | None = 255, + speed: int | None = 150, + current: int | None = 500, + ) -> bool: + """ + Control electric gripper. + action: 'move' or 'calibrate' + position: 0..255 + speed: 0..255 + current: 100..1000 (mA) + """ + action = action.lower() + if action not in ("move", "calibrate"): + raise ValueError("Invalid electric gripper action") + pos = 0 if position is None else int(position) + spd = 0 if speed is None else int(speed) + cur = 100 if current is None else int(current) + message = f"ELECTRICGRIPPER|{action}|{pos}|{spd}|{cur}" + return await self._send(message) + + # --------------- GCODE --------------- + + async def execute_gcode(self, gcode_line: str) -> bool: + """ + Execute a single GCODE line. + """ + message = wire.encode_gcode(gcode_line) + return await self._send(message) + + async def execute_gcode_program(self, program_lines: list[str]) -> bool: + """ + Execute a GCODE program from a list of lines. + """ + for i, line in enumerate(program_lines): + if "|" in line: + raise SyntaxError(f"Line {i + 1} contains invalid '|'") + message = wire.encode_gcode_program_inline(program_lines) + return await self._send(message) + + async def load_gcode_file(self, filepath: str) -> bool: + """ + Load and execute a GCODE program from a file. + """ + message = f"GCODE_PROGRAM|FILE|{filepath}" + return await self._send(message) + + async def get_gcode_status(self) -> dict | None: + """ + Get the current status of the GCODE interpreter. + """ + 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] + return json.loads(status_json) + + async def pause_gcode_program(self) -> bool: + """Pause the currently running GCODE program.""" + return await self._send("GCODE_PAUSE") + + async def resume_gcode_program(self) -> bool: + """Resume a paused GCODE program.""" + return await self._send("GCODE_RESUME") + + async def stop_gcode_program(self) -> bool: + """Stop the currently running GCODE program.""" + return await self._send("GCODE_STOP") + + # --------------- Smooth motion --------------- + + async def smooth_circle( + self, + 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: 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: float | None = None, + ) -> bool: + """ + Execute a smooth circular motion. + + Args: + center: [x, y, z] center point in mm + radius: Circle radius in mm + plane: Plane of the circle ('XY', 'XZ', or 'YZ') + frame: Reference frame ('WRF' for World, 'TRF' for Tool) + center_mode: How to interpret center point + entry_mode: How to approach circle if not on perimeter + start_pose: Optional [x, y, z, rx, ry, rz] start pose + duration: Time to complete the circle in seconds + speed_percentage: Speed as percentage (1-100) + clockwise: Direction of motion + trajectory_type: Type of trajectory + 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." + ) + 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}" + ) + traj_params = f"|{trajectory_type}" + if trajectory_type == "s_curve" and jerk_limit is not None: + traj_params += f"|{jerk_limit}" + elif trajectory_type != "cubic": + traj_params += "|DEFAULT" + mode_params = f"|{center_mode}|{entry_mode}" + command = ( + f"SMOOTH_CIRCLE|{center_str}|{radius}|{plane}|{frame}|{start_str}|" + f"{timing_str}|{clockwise_str}{traj_params}{mode_params}" + ) + return await self._send(command) + + async def smooth_arc_center( + self, + end_pose: list[float], + center: list[float], + frame: Literal["WRF", "TRF"] = "WRF", + 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: 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 + frame: Reference frame ('WRF' for World, 'TRF' for Tool) + start_pose: Optional [x, y, z, rx, ry, rz] start pose + duration: Time to complete the arc in seconds + speed_percentage: Speed as percentage (1-100) + clockwise: Direction of motion + trajectory_type: Type of trajectory + 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." + ) + 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}" + ) + traj_params = f"|{trajectory_type}" + if trajectory_type == "s_curve" and jerk_limit is not None: + traj_params += f"|{jerk_limit}" + elif trajectory_type != "cubic": + traj_params += "|DEFAULT" + command = ( + f"SMOOTH_ARC_CENTER|{end_str}|{center_str}|{frame}|{start_str}|" + f"{timing_str}|{clockwise_str}{traj_params}" + ) + return await self._send(command) + + async def smooth_arc_param( + self, + end_pose: list[float], + radius: float, + arc_angle: float, + frame: Literal["WRF", "TRF"] = "WRF", + 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: 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." + ) + 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}" + ) + parts = [ + "SMOOTH_ARC_PARAM", + end_str, + str(radius), + str(arc_angle), + frame, + start_str, + timing_str, + trajectory_type, + ] + if trajectory_type == "s_curve" and jerk_limit is not None: + parts.append(str(jerk_limit)) + if clockwise: + parts.append("CW") + return await self._send("|".join(parts)) + + async def smooth_spline( + self, + waypoints: list[list[float]], + frame: Literal["WRF", "TRF"] = "WRF", + 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: 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) + start_pose: Optional [x, y, z, rx, ry, rz] start pose + duration: Total time for the motion in seconds + speed_percentage: Speed as percentage (1-100) + trajectory_type: Type of trajectory + jerk_limit: Optional jerk limit for s_curve trajectory + wait_for_ack: Enable command tracking + timeout: Timeout for acknowledgment + 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." + ) + 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}" + ) + 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, + ] + if trajectory_type == "s_curve" and jerk_limit is not None: + parts.append(str(jerk_limit)) + elif trajectory_type == "s_curve": + parts.append("DEFAULT") + parts.extend(waypoint_strs) + return await self._send("|".join(parts)) + + async def smooth_helix( + self, + center: list[float], + radius: float, + pitch: float, + height: float, + frame: Literal["WRF", "TRF"] = "WRF", + trajectory_type: Literal["cubic", "quintic", "s_curve"] = "cubic", + 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 + pitch: Vertical distance per revolution in mm + height: Total height of helix in mm + frame: Reference frame ('WRF' for World, 'TRF' for Tool) + trajectory_type: Type of trajectory + jerk_limit: Optional jerk limit for s_curve trajectory + start_pose: Optional [x, y, z, rx, ry, rz] start pose + duration: Time to complete the helix in seconds + speed_percentage: Speed as percentage (1-100) + clockwise: Direction of motion + """ + if duration is None and speed_percentage is None: + 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}" + ) + traj_params = f"|{trajectory_type}" + if trajectory_type == "s_curve" and jerk_limit is not None: + traj_params += f"|{jerk_limit}" + elif trajectory_type != "cubic": + traj_params += "|DEFAULT" + command = ( + f"SMOOTH_HELIX|{center_str}|{radius}|{pitch}|{height}|{frame}|{start_str}|" + f"{timing_str}|{clockwise_str}{traj_params}" + ) + return await self._send(command) + + async def smooth_blend( + self, + segments: list[dict], + blend_time: float = 0.5, + frame: Literal["WRF", "TRF"] = "WRF", + 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 + frame: Reference frame ('WRF' for World, 'TRF' for Tool) + start_pose: Optional [x, y, z, rx, ry, rz] start pose + duration: Total time for entire motion + speed_percentage: Speed as percentage (1-100) for entire motion + wait_for_ack: Enable command tracking + timeout: Timeout for acknowledgment + non_blocking: Return immediately with command ID + """ + num_segments = len(segments) + start_str = ",".join(map(str, start_pose)) if start_pose else "CURRENT" + if duration is None and speed_percentage is None: + timing_str = "DEFAULT" + elif duration is not None: + timing_str = f"DURATION|{duration}" + else: + timing_str = f"SPEED|{speed_percentage}" + + segment_strs = [] + for seg in segments: + seg_type = seg["type"] + if seg_type == "LINE": + end_str = ",".join(map(str, seg["end"])) + seg_str = f"LINE|{end_str}|{seg.get('duration', 2.0)}" + elif seg_type == "CIRCLE": + center_str = ",".join(map(str, seg["center"])) + clockwise_str = "1" if seg.get("clockwise", False) else "0" + seg_str = f"CIRCLE|{center_str}|{seg['radius']}|{seg['plane']}|{seg.get('duration', 3.0)}|{clockwise_str}" + elif seg_type == "ARC": + end_str = ",".join(map(str, seg["end"])) + center_str = ",".join(map(str, seg["center"])) + 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"]] + ) + seg_str = f"SPLINE|{len(seg['waypoints'])}|{waypoints_str}|{seg.get('duration', 3.0)}" + else: + continue + segment_strs.append(seg_str) + + command = ( + f"SMOOTH_BLEND|{num_segments}|{blend_time}|{frame}|{start_str}|{timing_str}|" + + "||".join(segment_strs) + ) + return await self._send(command) + + async def smooth_waypoints( + self, + waypoints: list[list[float]], + blend_radii: Literal["AUTO"] | list[float] = "AUTO", + blend_mode: Literal["parabolic", "circular", "none"] = "parabolic", + 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: float | None = None, + ) -> bool: + """ + Execute a waypoint trajectory with blending. + """ + if not waypoints or any(len(wp) != 6 for wp in waypoints): + raise ValueError("Waypoints must be a non-empty list of [x,y,z,rx,ry,rz]") + wp_str = ";".join(",".join(map(str, wp)) for wp in waypoints) + if blend_radii == "AUTO": + 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)}" + ) + radii_str = ",".join(map(str, blend_radii)) + if via_modes is None: + via_modes_list: list[str] = ["via"] * len(waypoints) + else: + via_modes_list = list(via_modes) + if len(via_modes_list) != len(waypoints): + raise ValueError("via_modes length must match waypoints length") + if any(vm not in ("via", "stop") for vm in via_modes_list): + raise ValueError("via_modes entries must be 'via' or 'stop'") + via_str = ",".join(via_modes_list) + parts = [ + "SMOOTH_WAYPOINTS", + wp_str, + radii_str, + blend_mode, + via_str, + str(max_velocity), + str(max_acceleration), + frame, + trajectory_type, + ] + if duration is not None: + parts.append(str(duration)) + return await self._send("|".join(parts)) + + # --------------- Work coordinate helpers --------------- + + async def set_work_coordinate_offset( + self, + coordinate_system: str, + x: float | None = None, + y: float | None = None, + z: float | None = None, + ) -> 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}" + ) + + coord_num = int(coordinate_system[1:]) - 53 # G54=1, G55=2, etc. + offset_params = [] + if x is not None: + offset_params.append(f"X{x}") + if y is not None: + offset_params.append(f"Y{y}") + if z is not None: + offset_params.append(f"Z{z}") + + # Always select CS first, then apply offset if any + ok = await self.execute_gcode(coordinate_system) + if not ok: + return False + if offset_params: + offset_cmd = f"G10 L2 P{coord_num} {' '.join(offset_params)}" + return await self.execute_gcode(offset_cmd) + return True + + async def zero_work_coordinates( + self, + coordinate_system: str = "G54", + ) -> 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) diff --git a/parol6/client/manager.py b/parol6/client/manager.py new file mode 100644 index 0000000..d7ac6e7 --- /dev/null +++ b/parol6/client/manager.py @@ -0,0 +1,383 @@ +""" +Server management for PAROL6 controller. + +Provides lifecycle management and automatic spawning of the controller process. +""" + +import asyncio +import contextlib +import logging +import os +import re +import socket +import subprocess +import sys +import threading +import time +from typing import Tuple +from dataclasses import dataclass +from pathlib import Path + +# 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+(.*)$" +) + + +@dataclass +class ServerOptions: + """Options for launching the controller.""" + + com_port: str | None = None + no_autohome: bool = True # Set PAROL6_NOAUTOHOME=1 by default + extra_env: dict | None = None + + +class ServerManager: + """ + Manages the lifecycle of the PAROL6 controller. + + - Writes com_port.txt in the controller working directory to preselect the port. + - Spawns the controller as a subprocess using sys.executable. + - Provides stop and liveness checks. + - 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: + """ + 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 + timestamp/level/module info. Set to True when used from web GUI. + """ + if controller_path: + self.controller_path = Path(controller_path).resolve() + if not self.controller_path.exists(): + raise FileNotFoundError( + f"Controller script not found: {self.controller_path}" + ) + 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() + self.normalize_logs = normalize_logs + + @property + def pid(self) -> int | None: + return self._proc.pid if self._proc and self._proc.poll() is None else None + + def is_running(self) -> bool: + return self._proc is not None and self._proc.poll() is None + + def start_controller( + self, + com_port: str | None = None, + no_autohome: bool = True, + extra_env: dict | None = None, + server_host: str | None = None, + server_port: int | None = None, + ) -> None: + """Start the controller if not already running.""" + if self.is_running(): + 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 + + env = os.environ.copy() + # Disable autohome unless explicitly overridden + if no_autohome: + env["PAROL6_NOAUTOHOME"] = "1" + if extra_env: + env.update(extra_env) + # Explicitly bind controller (if provided) + if server_host: + env["PAROL6_CONTROLLER_IP"] = server_host + if server_port is not None: + 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) + ) + + # Launch the controller as a module to ensure package imports resolve + args = [sys.executable, "-u", "-m", "parol6.server.controller"] + + # 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, + stderr=subprocess.STDOUT, + text=True, + bufsize=1, # line-buffered + ) + except Exception as e: + raise RuntimeError(f"Failed to start controller: {e}") from e + + # Start background reader thread to stream server stdout/stderr to logging + if self._proc and self._proc.stdout is not None: + self._stop_reader.clear() + self._reader_thread = threading.Thread( + target=self._stream_output, + args=(self._proc,), + name="ServerOutputReader", + daemon=True, + ) + self._reader_thread.start() + + def _stream_output(self, proc: subprocess.Popen) -> None: + """Read controller stdout and forward to logging.""" + try: + assert proc.stdout is not None + # Maintain last logger/level for multi-line tracebacks + last_logger = "parol6.server" + + for raw_line in iter(proc.stdout.readline, ""): + if self._stop_reader.is_set(): + break + line = raw_line.rstrip("\r\n") + if not line: + continue + + if self.normalize_logs: + # Normalize child log line and route to embedded module logger + level = logging.INFO + logger_name: str | None = None + msg = line + + s = _SIMPLE_FORMAT_RE.match(line) + if s: + _, 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 + ) + elif line.startswith("Traceback"): + # Traceback and continuation lines -> keep last context, escalate on Traceback + level = logging.ERROR + + # Choose target logger + target_logger_name = logger_name or last_logger or "parol6.server" + target_logger = logging.getLogger(target_logger_name) + target_logger.log(level, msg) + + # Update last context if we identified a module + if logger_name: + last_logger = logger_name + else: + # No normalization - forward line as-is to root logger + print(line) + except Exception as e: + logging.warning("ServerManager: output reader stopped: %s", e) + + def stop_controller(self, timeout: float = 2.0) -> None: + """Stop the controller process if running. + + 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(): + self._reader_thread.join(timeout=timeout) + self._reader_thread = 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, + host: str = "127.0.0.1", + port: int = 5001, + timeout: float = 10.0, + poll_interval: float = 0.2, + ) -> bool: + """ + Wait until the controller responds to PING over UDP, asynchronously. + + Returns: + True if the server becomes ready within timeout, else False. + """ + loop = asyncio.get_running_loop() + deadline = loop.time() + max(0.0, float(timeout)) + addr: Tuple[str, int] = (host, port) + + 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 + + +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, +) -> ServerManager: + """Synchronously start a PAROL6 controller at host:port and block until ready. + + Fast-fails if a server is already running there. + + Returns a ServerManager that owns the spawned 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) + manager.start_controller( + com_port=com_port, + no_autohome=True, + extra_env=env_to_pass, + server_host=host, + server_port=port, + ) + + # 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") + 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 new file mode 100644 index 0000000..b6b3bef --- /dev/null +++ b/parol6/client/status_subscriber.py @@ -0,0 +1,208 @@ +import asyncio +import contextlib +import logging +import socket +import struct +import time +from collections.abc import AsyncIterator + +from parol6 import config as cfg +from parol6.protocol.types import StatusAggregate +from parol6.protocol.wire import decode_status + +logger = logging.getLogger(__name__) + + +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 RX + self._rx_count = 0 + self._rx_last_time = time.monotonic() + 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 RX rate with EMA + now = time.monotonic() + if self._rx_count > 0: # Skip first sample for period calculation + period = now - self._rx_last_time + if period > 0: + # EMA update: 0.1 * new + 0.9 * old + 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"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: + # Drop oldest packet if queue is full + try: + self.queue.get_nowait() + self.queue.put_nowait((data, addr)) + 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}") + + +def _create_multicast_socket(group: str, port: int, iface_ip: str) -> socket.socket: + """Create and configure a multicast socket with loopback-first semantics and robust joins.""" + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + + # Allow multiple listeners on same port; prefer SO_REUSEPORT where available + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + try: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) + except Exception: + # Not available or not permitted on this platform; continue + pass + sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 1 << 20) + # Bind to port (try wildcard first, then iface_ip) + try: + sock.bind(("", port)) + except OSError: + sock.bind((iface_ip, port)) + + # Helper to determine active NIC IP (no packets sent) + def _detect_primary_ip() -> 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: + with contextlib.suppress(Exception): + tmp.close() + + # Join multicast group on specified interface (loopback preferred), with fallbacks + try: + mreq = struct.pack("=4s4s", socket.inet_aton(group), socket.inet_aton(iface_ip)) + sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq) + except Exception: + # Retry using primary NIC IP + try: + primary_ip = _detect_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 + mreq_any = struct.pack("=4sl", socket.inet_aton(group), socket.INADDR_ANY) + sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq_any) + + # Non-blocking for asyncio + sock.setblocking(False) + 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 +) -> AsyncIterator[StatusAggregate]: + """ + Async generator that yields decoded STATUS dicts from the UDP broadcaster. + + Uses create_datagram_endpoint for uvloop compatibility. + + Notes: + - Uses loopback multicast by default (cfg.MCAST_* values). + - Yields only messages that decode successfully via decode_status; otherwise skips. + """ + 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: 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 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: 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, 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 + finally: + if transport: + logger.info("Closing transport...") + transport.close() + try: + sock.close() + except Exception: + pass diff --git a/parol6/client/sync_client.py b/parol6/client/sync_client.py new file mode 100644 index 0000000..384a93f --- /dev/null +++ b/parol6/client/sync_client.py @@ -0,0 +1,677 @@ +""" +Synchronous facade for AsyncRobotClient. + +- In sync code: use RobotClient and call methods directly. +- In async code (event loop running): use AsyncRobotClient and `await` the methods. +""" + +import asyncio +import atexit +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 + +T = TypeVar("T") + + +# Persistent background event loop for sync wrapper +_SYNC_LOOP: asyncio.AbstractEventLoop | None = None +_SYNC_THREAD: threading.Thread | None = None +_SYNC_LOOP_READY = threading.Event() + + +def _loop_worker(loop: asyncio.AbstractEventLoop) -> None: + asyncio.set_event_loop(loop) + _SYNC_LOOP_READY.set() + loop.run_forever() + + +def _stop_sync_loop() -> None: + global _SYNC_LOOP, _SYNC_THREAD + if _SYNC_LOOP is not None: + try: + _SYNC_LOOP.call_soon_threadsafe(_SYNC_LOOP.stop) + except Exception: + pass + _SYNC_LOOP = None + _SYNC_THREAD = None + + +def _ensure_sync_loop() -> None: + """Start a persistent background event loop if not started yet.""" + global _SYNC_LOOP, _SYNC_THREAD + 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, + ) + _SYNC_THREAD.start() + _SYNC_LOOP_READY.wait(timeout=1.0) + atexit.register(_stop_sync_loop) + + +def _run(coro: Coroutine[Any, Any, T]) -> T: + """ + Run an async coroutine to completion using a persistent background event loop. + If a loop is already running in this thread, raise to avoid deadlocks. + """ + try: + asyncio.get_running_loop() + except RuntimeError: + # No running loop in this thread -> submit to persistent loop + _ensure_sync_loop() + assert _SYNC_LOOP is not None + fut = asyncio.run_coroutine_threadsafe(coro, _SYNC_LOOP) + return fut.result() + # A loop is running in this thread; blocking would be unsafe. + raise RuntimeError( + "RobotClient was used while an event loop is running.\n" + "Use AsyncRobotClient and `await` the method instead." + ) + + +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 ---------- + + def __init__( + self, + host: str = "127.0.0.1", + port: int = 5001, + timeout: float = 2.0, + retries: int = 1, + ) -> None: + self._inner = AsyncRobotClient( + 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.""" + return self._inner + + # Expose common configuration attributes + @property + def host(self) -> str: + return self._inner.host + + @property + def port(self) -> int: + return self._inner.port + + # ---------- motion / control ---------- + + def home(self) -> bool: + return _run(self._inner.home()) + + def enable(self) -> bool: + return _run(self._inner.enable()) + + def disable(self) -> bool: + return _run(self._inner.disable()) + + 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()) + + def simulator_off(self) -> bool: + return _run(self._inner.simulator_off()) + + def set_serial_port(self, port_str: str) -> bool: + return _run(self._inner.set_serial_port(port_str)) + + # ---------- status / queries ---------- + def ping(self) -> str | None: + return _run(self._inner.ping()) + + def get_angles(self) -> list[float] | None: + return _run(self._inner.get_angles()) + + def get_io(self) -> list[int] | None: + return _run(self._inner.get_io()) + + def get_gripper_status(self) -> list[int] | None: + return _run(self._inner.get_gripper_status()) + + def get_speeds(self) -> list[float] | None: + return _run(self._inner.get_speeds()) + + def get_pose(self) -> list[float] | None: + return _run(self._inner.get_pose()) + + def get_gripper(self) -> list[int] | None: + return _run(self._inner.get_gripper()) + + def get_status(self) -> dict | None: + return _run(self._inner.get_status()) + + 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: + return _run(self._inner.get_pose_rpy()) + + def get_pose_xyz(self) -> list[float] | None: + return _run(self._inner.get_pose_xyz()) + + def is_estop_pressed(self) -> bool: + return _run(self._inner.is_estop_pressed()) + + def is_robot_stopped(self, threshold_speed: float = 2.0) -> bool: + return _run(self._inner.is_robot_stopped(threshold_speed)) + + def wait_until_stopped( + self, + timeout: float = 90.0, + settle_window: float = 1.0, + speed_threshold: float = 2.0, + angle_threshold: float = 0.5, + ) -> bool: + return _run( + self._inner.wait_until_stopped( + timeout=timeout, + settle_window=settle_window, + speed_threshold=speed_threshold, + angle_threshold=angle_threshold, + ) + ) + + # ---------- responsive waits / raw send ---------- + + 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) + ) + + 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: + """ + 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) + ) + + # ---------- extended controls / motion ---------- + + def move_joints( + self, + joint_angles: list[float], + duration: float | None = None, + speed_percentage: int | None = None, + accel_percentage: int | None = None, + profile: str | None = None, + tracking: str | None = None, + ) -> bool: + return _run( + self._inner.move_joints( + joint_angles, + duration, + speed_percentage, + accel_percentage, + profile, + tracking, + ) + ) + + def move_pose( + self, + pose: list[float], + duration: float | None = None, + speed_percentage: int | None = None, + accel_percentage: int | None = None, + profile: str | None = None, + tracking: str | None = None, + ) -> bool: + return _run( + self._inner.move_pose( + pose, + duration, + speed_percentage, + accel_percentage, + profile, + tracking, + ) + ) + + def move_cartesian( + self, + pose: list[float], + duration: float | None = None, + speed_percentage: float | None = None, + accel_percentage: int | None = None, + profile: str | None = None, + tracking: str | None = None, + ) -> bool: + return _run( + self._inner.move_cartesian( + pose, + duration, + speed_percentage, + accel_percentage, + profile, + tracking, + ) + ) + + def move_cartesian_rel_trf( + self, + deltas: list[float], + duration: float | None = None, + speed_percentage: float | None = None, + accel_percentage: int | None = None, + profile: str | None = None, + tracking: str | None = None, + ) -> bool: + return _run( + self._inner.move_cartesian_rel_trf( + deltas, + duration, + speed_percentage, + accel_percentage, + profile, + tracking, + ) + ) + + def jog_joint( + self, + joint_index: int, + speed_percentage: int, + duration: float | None = None, + distance_deg: float | None = None, + ) -> bool: + return _run( + self._inner.jog_joint( + joint_index, + speed_percentage, + duration, + distance_deg, + ) + ) + + def jog_cartesian( + self, + frame: Frame, + axis: Axis, + speed_percentage: int, + duration: float, + ) -> bool: + return _run(self._inner.jog_cartesian(frame, axis, speed_percentage, duration)) + + def jog_multiple( + self, + joints: list[int], + speeds: list[float], + duration: float, + ) -> bool: + return _run(self._inner.jog_multiple(joints, speeds, duration)) + + def set_io(self, index: int, value: int) -> bool: + """Set digital I/O bit (0..7) to 0 or 1.""" + return _run(self._inner.set_io(index, value)) + + def delay(self, seconds: float) -> bool: + """Insert a non-blocking delay in the motion queue.""" + return _run(self._inner.delay(seconds)) + + # ---------- IO / gripper ---------- + + def control_pneumatic_gripper( + self, + action: str, + port: int, + ) -> bool: + return _run(self._inner.control_pneumatic_gripper(action, port)) + + def control_electric_gripper( + self, + action: str, + position: int | None = 255, + speed: int | None = 150, + current: int | None = 500, + ) -> bool: + return _run( + self._inner.control_electric_gripper(action, position, speed, current) + ) + + # ---------- GCODE ---------- + + def execute_gcode( + self, + gcode_line: str, + ) -> bool: + return _run(self._inner.execute_gcode(gcode_line)) + + def execute_gcode_program( + self, + program_lines: list[str], + ) -> bool: + return _run(self._inner.execute_gcode_program(program_lines)) + + def load_gcode_file( + self, + filepath: str, + ) -> bool: + return _run(self._inner.load_gcode_file(filepath)) + + def get_gcode_status(self) -> dict | None: + return _run(self._inner.get_gcode_status()) + + def pause_gcode_program(self) -> bool: + return _run(self._inner.pause_gcode_program()) + + def resume_gcode_program(self) -> bool: + return _run(self._inner.resume_gcode_program()) + + def stop_gcode_program(self) -> bool: + return _run(self._inner.stop_gcode_program()) + + # ---------- smooth motion ---------- + + def smooth_circle( + self, + 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: 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: float | None = None, + ) -> bool: + return _run( + self._inner.smooth_circle( + center=center, + radius=radius, + plane=plane, + frame=frame, + center_mode=center_mode, + entry_mode=entry_mode, + start_pose=start_pose, + duration=duration, + speed_percentage=speed_percentage, + clockwise=clockwise, + trajectory_type=trajectory_type, + jerk_limit=jerk_limit, + ) + ) + + def smooth_arc_center( + self, + end_pose: list[float], + center: list[float], + frame: Literal["WRF", "TRF"] = "WRF", + 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: float | None = None, + ) -> bool: + return _run( + self._inner.smooth_arc_center( + end_pose=end_pose, + center=center, + frame=frame, + start_pose=start_pose, + duration=duration, + speed_percentage=speed_percentage, + clockwise=clockwise, + trajectory_type=trajectory_type, + jerk_limit=jerk_limit, + ) + ) + + def smooth_arc_param( + self, + end_pose: list[float], + radius: float, + arc_angle: float, + frame: Literal["WRF", "TRF"] = "WRF", + 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: float | None = None, + clockwise: bool = False, + ) -> bool: + return _run( + self._inner.smooth_arc_param( + end_pose=end_pose, + radius=radius, + arc_angle=arc_angle, + frame=frame, + start_pose=start_pose, + duration=duration, + speed_percentage=speed_percentage, + trajectory_type=trajectory_type, + jerk_limit=jerk_limit, + clockwise=clockwise, + ) + ) + + def smooth_spline( + self, + waypoints: list[list[float]], + frame: Literal["WRF", "TRF"] = "WRF", + 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: float | None = None, + ) -> bool: + return _run( + self._inner.smooth_spline( + waypoints=waypoints, + frame=frame, + start_pose=start_pose, + duration=duration, + speed_percentage=speed_percentage, + trajectory_type=trajectory_type, + jerk_limit=jerk_limit, + ) + ) + + def smooth_helix( + self, + center: list[float], + radius: float, + pitch: float, + height: float, + frame: Literal["WRF", "TRF"] = "WRF", + trajectory_type: Literal["cubic", "quintic", "s_curve"] = "cubic", + 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( + self._inner.smooth_helix( + center=center, + radius=radius, + pitch=pitch, + height=height, + frame=frame, + trajectory_type=trajectory_type, + jerk_limit=jerk_limit, + start_pose=start_pose, + duration=duration, + speed_percentage=speed_percentage, + clockwise=clockwise, + ) + ) + + def smooth_blend( + self, + segments: list[dict], + blend_time: float = 0.5, + frame: Literal["WRF", "TRF"] = "WRF", + start_pose: list[float] | None = None, + duration: float | None = None, + speed_percentage: float | None = None, + ) -> bool: + return _run( + self._inner.smooth_blend( + segments=segments, + blend_time=blend_time, + frame=frame, + start_pose=start_pose, + duration=duration, + speed_percentage=speed_percentage, + ) + ) + + def smooth_waypoints( + self, + waypoints: list[list[float]], + blend_radii: Literal["AUTO"] | list[float] = "AUTO", + blend_mode: Literal["parabolic", "circular", "none"] = "parabolic", + 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: float | None = None, + ) -> bool: + return _run( + self._inner.smooth_waypoints( + waypoints=waypoints, + blend_radii=blend_radii, + blend_mode=blend_mode, + via_modes=via_modes, + max_velocity=max_velocity, + max_acceleration=max_acceleration, + frame=frame, + trajectory_type=trajectory_type, + duration=duration, + ) + ) + + # ---------- work coordinate helpers ---------- + + def set_work_coordinate_offset( + self, + coordinate_system: str, + x: float | None = None, + y: float | None = None, + z: float | None = None, + ) -> bool: + return _run( + self._inner.set_work_coordinate_offset( + coordinate_system=coordinate_system, + x=x, + y=y, + z=z, + ) + ) + + def zero_work_coordinates( + self, + coordinate_system: str = "G54", + ) -> bool: + return _run( + self._inner.zero_work_coordinates( + coordinate_system=coordinate_system, + ) + ) diff --git a/parol6/commands/__init__.py b/parol6/commands/__init__.py new file mode 100644 index 0000000..0910028 --- /dev/null +++ b/parol6/commands/__init__.py @@ -0,0 +1,18 @@ +""" +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 new file mode 100644 index 0000000..2ba229c --- /dev/null +++ b/parol6/commands/base.py @@ -0,0 +1,646 @@ +""" +Base abstractions and helpers for command implementations. +""" + +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 numpy as np +import roboticstoolbox as rp + +import parol6.PAROL6_ROBOT as PAROL6_ROBOT +from parol6.config import INTERVAL_S, TRACE +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" + FAILED = "FAILED" + CANCELLED = "CANCELLED" + + +@dataclass +class ExecutionStatus: + """ + Status returned from command execution steps. + """ + + code: ExecutionStatusCode + message: str + error: Exception | None = None + details: dict[str, Any] | None = None + error_type: str | None = None + + @classmethod + 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: dict[str, Any] | None = None + ) -> "ExecutionStatus": + return cls(ExecutionStatusCode.COMPLETED, message, error=None, details=details) + + @classmethod + 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, + ) + + +# ----- Shared context and small utilities ----- + + +@dataclass +class CommandContext: + """Shared dynamic execution context for commands.""" + + udp_transport: Any = None + addr: tuple | None = None + gcode_interpreter: Any = None + dt: float = INTERVAL_S + + +# Parsing utilities (lightweight, shared) +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) -> int | None: + t = _noneify(token) + return None if t is None else int(t) + + +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]: + 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]: + t = _noneify(token) + return [] if t is None else [float(x) for x in t.split(",") if x != ""] + + +def parse_bool(token: Any) -> bool: + t = (str(token or "")).strip().lower() + return t in ("1", "true", "yes", "on") + + +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: + return None + if type_ is bool: + return parse_bool(t) + return type_(t) + + +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}") + + +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}" + ) + + +def parse_frame(token: Any) -> str: + """Parse and validate frame token (WRF/TRF).""" + t = (str(token or "")).strip().upper() + if t not in ("WRF", "TRF"): + raise ValueError(f"Invalid frame: {token}") + return t + + +def parse_axis(token: Any) -> str: + """Parse and validate axis token against AXIS_MAP.""" + t = (str(token or "")).strip().upper() + # Convert to match AXIS_MAP format (e.g., +X -> X+, -Y -> Y-) + if len(t) == 2 and t[0] in "+-" and t[1] in "XYZ": + t = t[1] + t[0] # Swap sign and axis + elif len(t) == 3 and t[0] == "R" and t[2] in "+-": + t = "R" + t[1] + t[2] # Keep RX+ format + if t not in AXIS_MAP: + raise ValueError(f"Invalid axis: {token}") + return t + + +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: + self.count -= 1 + return self.count == 0 + + +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 + + def reset(self) -> None: + self.count = self.count_init + + def tick(self, active: bool) -> bool: + """ + Returns True exactly once when 'active' stays non-zero for 'count_init' ticks. + Resets when 'active' becomes False. + """ + if active: + if self.count > 0: + self.count -= 1 + return self.count == 0 + else: + self.reset() + return False + + +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", + ) + + def __init__(self) -> None: + self.is_valid: bool = True + self.is_finished: bool = False + self.error_state: bool = False + self.error_message: str = "" + self.udp_transport: Any = None + self.addr: Any = None + self.gcode_interpreter: Any = 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: + # Identity-based hash is appropriate for ephemeral command instances + return id(self) + + @property + def name(self) -> str: + return self._registered_name or type(self).__name__ + + # Logging helpers (uniform, include command identity) + def log_trace(self, msg: str, *args: Any) -> None: + logger.log(TRACE, "[%s] " + msg, self.name, *args) + + def log_debug(self, msg: str, *args: Any) -> None: + logger.debug("[%s] " + msg, self.name, *args) + + def log_info(self, msg: str, *args: Any) -> None: + logger.info("[%s] " + msg, self.name, *args) + + def log_warning(self, msg: str, *args: Any) -> None: + logger.warning("[%s] " + msg, self.name, *args) + + def log_error(self, msg: str, *args: Any) -> None: + logger.error("[%s] " + msg, self.name, *args) + + @staticmethod + def stop_and_idle(state: ControllerState) -> None: + state.Speed_out.fill(0) + state.Command_out = CommandCode.IDLE + + def bind(self, context: CommandContext) -> None: + """ + Bind dynamic execution context. Controller should call this prior to setup(). + """ + self.udp_transport = context.udp_transport + self.addr = context.addr + self.gcode_interpreter = context.gcode_interpreter + + @abstractmethod + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Check if this command can handle the given message parts. + + Args: + parts: Pre-split message parts (e.g., ['JOG', '0', '50', '2.0', 'None']) + + Returns: + Tuple of (can_handle, error_message) + - can_handle: True if this command can process the message + - error_message: Optional error message if the message is invalid + """ + raise NotImplementedError + + 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. + """ + try: + return self.do_match(parts) + except Exception as e: + # Do not log here to avoid duplicate noise; registry/controller provide lifecycle TRACE. + return False, str(e) + + def do_setup(self, state: ControllerState) -> None: + """Subclass hook for preparation; override in subclasses.""" + return + + def setup(self, state: ControllerState) -> None: + """Public setup wrapper providing centralized logging and error handling.""" + self.log_trace("setup start") + try: + self.do_setup(state) + self.log_trace("setup ok") + except Exception as e: + # Mark invalid and propagate for higher-level lifecycle logging + self.fail(f"Setup error: {e}") + self.log_error("Setup error: %s", e) + raise + + @abstractmethod + def tick(self, state: ControllerState) -> ExecutionStatus: + """ + Template-method wrapper that centralizes lifecycle/error handling and calls do_execute(). + Controllers should prefer tick() over calling execute_step() directly. + """ + raise NotImplementedError + + @abstractmethod + def execute_step(self, state: "ControllerState") -> ExecutionStatus: + """ + Execute one control-loop step and return an ExecutionStatus. + + Commands MUST interact with state.* arrays/buffers directly (Position_in/out, Speed_out, Command_out, etc.). + """ + raise NotImplementedError + + # ----- lifecycle helpers ----- + + def finish(self) -> None: + """Mark command as finished.""" + self.is_finished = True + + def fail(self, message: str) -> None: + """Mark command as invalid/failed with an error message.""" + self.is_valid = False + self.error_state = True + self.error_message = message + self.is_finished = True + + # ---- timing helpers ---- + def start_timer(self, duration_s: float) -> None: + """Start a timer for the given duration in seconds.""" + self._t_end = time.perf_counter() + max(0.0, duration_s) + + def timer_expired(self) -> bool: + """Check if the timer has expired.""" + return self._t_end is not None and time.perf_counter() >= self._t_end + + def progress01(self, duration_s: float) -> float: + """Get progress as a value between 0 and 1.""" + if self._t0 is None: + self._t0 = time.perf_counter() + if duration_s <= 0.0: + return 1.0 + 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. + + Query commands are read-only operations that return information about the robot state. + They execute immediately without waiting in the command queue. + """ + + def reply_text(self, message: str) -> None: + """Send an opaque ASCII message via UDP.""" + if self.udp_transport and self.addr: + try: + self.udp_transport.send_response(message, self.addr) + except Exception as e: + self.log_warning("Failed to send UDP reply: %s", e) + + 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. + """ + if payload is None: + self.reply_text(prefix_or_message) + else: + self.reply_text(f"{prefix_or_message}|{payload}") + + def reply_json(self, prefix: str, obj: Any) -> None: + """Reply with JSON payload.""" + try: + s = json.dumps(obj) + except Exception: + s = "{}" + self.reply_ascii(prefix, s) + + def tick(self, state: ControllerState) -> ExecutionStatus: + """ + Template-method wrapper that centralizes lifecycle/error handling and calls do_execute(). + 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") + ) + if not self.udp_transport or not self.addr: + self.fail("Missing UDP transport or address") + return ExecutionStatus.failed("Missing UDP transport or address") + try: + status = self.execute_step(state) + except Exception as e: + # Hard failure safeguards + self.fail(str(e)) + return ExecutionStatus.failed("Execution error", error=e) + return status + + +class MotionCommand(CommandBase): + """ + Base class for motion commands that require the controller to be enabled. + + 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 + LIMS_STEPS: ClassVar[np.ndarray] = PAROL6_ROBOT.joint.limits.steps + J_MIN: ClassVar[np.ndarray] = PAROL6_ROBOT.joint.speed.min + J_MAX: ClassVar[np.ndarray] = PAROL6_ROBOT.joint.speed.max + JOG_MIN: ClassVar[np.ndarray] = PAROL6_ROBOT.joint.speed.jog.min + JOG_MAX: ClassVar[np.ndarray] = PAROL6_ROBOT.joint.speed.jog.max + ACC_MIN_RAD: ClassVar[float] = PAROL6_ROBOT.joint.acc.min_rad + ACC_MAX_RAD: ClassVar[float] = PAROL6_ROBOT.joint.acc.max_rad + CART_LIN_JOG_MIN: ClassVar[float] = PAROL6_ROBOT.cart.vel.jog.min + CART_LIN_JOG_MAX: ClassVar[float] = PAROL6_ROBOT.cart.vel.jog.max + CART_ANG_JOG_MIN: ClassVar[float] = PAROL6_ROBOT.cart.vel.angular.min # deg/s + CART_ANG_JOG_MAX: ClassVar[float] = PAROL6_ROBOT.cart.vel.angular.max # deg/s + + def __init__(self) -> None: + super().__init__() + + # ---- mapping ---- + @staticmethod + def linmap_pct(pct: float, lo: float, hi: float) -> float: + if pct < 0.0: + pct = 0.0 + elif pct > 100.0: + pct = 100.0 + return lo + (hi - lo) * (pct / 100.0) + + # ---- 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 + ) + + 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 + ) + + # ---- 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)) + 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]) + ) + + # ---- 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]: + n = int(tgrid.size) + q = np.empty((n, 6), dtype=float) + qd = np.empty((n, 6), dtype=float) + stationary = target_steps == start_steps + if np.any(stationary): + q[:, stationary] = start_steps[stationary] + qd[:, stationary] = 0.0 + for i in np.flatnonzero(~stationary): + jt = rp.trapezoidal(float(start_steps[i]), float(target_steps[i]), tgrid) + q[:, i] = jt.q + qd[:, i] = jt.qd + return q, qd + + def fail_and_idle(self, state: ControllerState, message: str) -> None: + self.fail(message) + self.stop_and_idle(state) + + # ---- 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") + state.Speed_out.fill(0) + state.Command_out = CommandCode.MOVE + + def tick(self, state: ControllerState) -> ExecutionStatus: + """ + Template-method wrapper that centralizes lifecycle/error handling and calls do_execute(). + 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") + ) + try: + status = self.execute_step(state) + except Exception as e: + # Hard failure safeguards + 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: + """ + Utilities to build motion profiles in step-space using consistent trapezoids. + """ + + @staticmethod + def from_duration_steps( + start_steps: np.ndarray, + target_steps: np.ndarray, + duration_s: float, + dt: float = INTERVAL_S, + ) -> np.ndarray: + """ + Build per-joint trapezoids to reach target in given duration. + Returns array of shape (N, 6) steps (int32). + """ + dur = float(max(0.0, duration_s)) + if dur == 0.0: + # Degenerate: single step + return np.asarray(target_steps, dtype=np.int32).reshape(1, -1) + 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, + ) + return cast(np.ndarray, q.astype(np.int32, copy=False)) + + @staticmethod + def from_velocity_percent( + start_steps: np.ndarray, + target_steps: np.ndarray, + velocity_percent: float, + accel_percent: float, + dt: float = INTERVAL_S, + ) -> np.ndarray: + """ + Build per-joint trapezoids sized by per-joint vmax and accel derived from percent settings. + """ + start_steps = np.asarray(start_steps, dtype=float) + target_steps = np.asarray(target_steps, dtype=float) + + # 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 + ) + + # 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 + ) + + if np.any(v_max_joint <= 0) or np.any(a_steps_vec <= 0): + raise ValueError("Invalid speed/acceleration (must be positive).") + + path = np.abs(target_steps - start_steps) + t_accel = v_max_joint / a_steps_vec # time to reach vmax per joint + short_path = path < (v_max_joint * t_accel) + + # Safe accel time for short paths + t_accel_adj = t_accel.copy() + mask = short_path + t_accel_adj[mask] = 0.0 + 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 + ) + 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) + ) + if total_time < (2 * dt): + total_time = 2 * dt + + n = max(2, int(np.ceil(total_time / max(dt, 1e-9)))) + tgrid = np.linspace(0.0, total_time, n, dtype=float) + q, _qd = MotionCommand.plan_trapezoids(start_steps, target_steps, tgrid) + return cast(np.ndarray, q.astype(np.int32, copy=False)) + + +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. + """ + + 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") + ) + try: + status = self.execute_step(state) + except Exception as 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 new file mode 100644 index 0000000..c2e7d0e --- /dev/null +++ b/parol6/commands/basic_commands.py @@ -0,0 +1,509 @@ +""" +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 new file mode 100644 index 0000000..2dfb8fe --- /dev/null +++ b/parol6/commands/cartesian_commands.py @@ -0,0 +1,506 @@ +""" +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 new file mode 100644 index 0000000..cb37638 --- /dev/null +++ b/parol6/commands/gcode_commands.py @@ -0,0 +1,181 @@ +""" +GCODE command wrappers for robot control. + +These commands integrate the GCODE interpreter with the robot command system. +""" + +from typing import TYPE_CHECKING + +from parol6.commands.base import CommandBase, ExecutionStatus +from parol6.gcode import GcodeInterpreter +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 + + +@register_command("GCODE") +class GcodeCommand(CommandBase): + """Execute a single GCODE line.""" + + __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:]) + return True, None + return False, 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() + ) + assert self.interpreter is not None + # Update interpreter position with current robot position + 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, + } + ) + # 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: + """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 + self.finish() + return ExecutionStatus.completed("GCODE parsed", details=details) + + +@register_command("GCODE_PROGRAM") +class GcodeProgramCommand(CommandBase): + """Load and execute a GCODE program.""" + + __slots__ = ("program_type", "program_data", "interpreter") + 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:]) + 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() + ) + 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(";") + 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: + """Signal that the program was loaded; controller will fetch commands.""" + self.finish() + return ExecutionStatus.completed("GCODE program loaded") + + +@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, 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: + """Stop the GCODE program.""" + if self.gcode_interpreter: + self.gcode_interpreter.stop_program() + self.finish() + return ExecutionStatus.completed("GCODE stopped") + + +@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, 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: + """Pause the GCODE program.""" + if self.gcode_interpreter: + self.gcode_interpreter.pause_program() + self.finish() + return ExecutionStatus.completed("GCODE paused") + + +@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, 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: + """Resume the GCODE program.""" + if self.gcode_interpreter: + self.gcode_interpreter.start_program() # resumes if already loaded + self.finish() + return ExecutionStatus.completed("GCODE resumed") diff --git a/parol6/commands/gripper_commands.py b/parol6/commands/gripper_commands.py new file mode 100644 index 0000000..b8a2cf3 --- /dev/null +++ b/parol6/commands/gripper_commands.py @@ -0,0 +1,259 @@ +""" +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 new file mode 100644 index 0000000..792f985 --- /dev/null +++ b/parol6/commands/joint_commands.py @@ -0,0 +1,167 @@ +""" +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 new file mode 100644 index 0000000..597f7cb --- /dev/null +++ b/parol6/commands/query_commands.py @@ -0,0 +1,332 @@ +""" +Query commands that return immediate status information. +""" + +from typing import TYPE_CHECKING + +import numpy as np + +import parol6.PAROL6_ROBOT as PAROL6_ROBOT +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 + + +@register_command("GET_POSE") +class GetPoseCommand(QueryCommand): + """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 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") + + +@register_command("GET_ANGLES") +class GetAnglesCommand(QueryCommand): + """Get current joint angles in degrees.""" + + __slots__ = () + + 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: + """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") + + +@register_command("GET_IO") +class GetIOCommand(QueryCommand): + """Get current I/O status.""" + + __slots__ = () + + 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: + """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") + + +@register_command("GET_GRIPPER") +class GetGripperCommand(QueryCommand): + """Get current gripper status.""" + + __slots__ = () + + 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: + """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") + + +@register_command("GET_SPEEDS") +class GetSpeedsCommand(QueryCommand): + """Get current joint speeds.""" + + __slots__ = () + + 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: + """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") + + +@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, 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: + """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") + + +@register_command("GET_GCODE_STATUS") +class GetGcodeStatusCommand(QueryCommand): + """Get GCODE interpreter status.""" + + __slots__ = () + + 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: + """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": {}, + } + + 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, str | None]: + if parts[0].upper() == "GET_LOOP_STATS": + return True, None + return False, None + + 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 = { + "target_hz": float(target_hz), + "loop_count": int(state.loop_count), + "overrun_count": int(state.overrun_count), + "last_period_s": float(state.last_period_s), + "ema_period_s": float(state.ema_period_s), + "ema_hz": float(ema_hz), + } + self.reply_json("LOOP_STATS", payload) + self.finish() + return ExecutionStatus.completed("Loop stats sent") + + +@register_command("PING") +class PingCommand(QueryCommand): + """Respond to ping requests.""" + + __slots__ = () + + 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 (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 new file mode 100644 index 0000000..394a754 --- /dev/null +++ b/parol6/commands/smooth_commands.py @@ -0,0 +1,2155 @@ +""" +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 new file mode 100644 index 0000000..0c1ac22 --- /dev/null +++ b/parol6/commands/system_commands.py @@ -0,0 +1,294 @@ +""" +System control commands that can execute regardless of controller enable state. + +These commands control the overall state of the robot controller (enable/disable, stop, etc.) +and can execute even when the controller is disabled. +""" + +from __future__ import annotations + +import logging +import os +from typing import TYPE_CHECKING + +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 + +logger = logging.getLogger(__name__) + + +@register_command("STOP") +class StopCommand(SystemCommand): + """Emergency stop command - immediately stops all motion.""" + + __slots__ = () + + 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: + """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") + + +@register_command("ENABLE") +class EnableCommand(SystemCommand): + """Enable the robot controller.""" + + __slots__ = () + + 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: + """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") + + +@register_command("DISABLE") +class DisableCommand(SystemCommand): + """Disable the robot controller.""" + + __slots__ = () + + 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 - 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 + + self.finish() + 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, 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: + """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}" + ) + + +@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, str | None]: + """ + Parse SET_PORT command. + + Format: SET_PORT|serial_port + Example: SET_PORT|/dev/ttyACM0 + """ + if parts[0].upper() != "SET_PORT": + return False, None + + if len(parts) != 2: + return False, "SET_PORT requires 1 parameter: serial_port" + + port = (parts[1] or "").strip() + if not port: + return False, "Serial port cannot be empty" + + self.port_str = port + logger.info(f"Parsed SET_PORT: serial_port={self.port_str}") + return True, None + + 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") + return ExecutionStatus.failed("No serial port provided") + + ok = save_com_port(self.port_str) + if not ok: + self.fail("Failed to save COM port") + return ExecutionStatus.failed("Failed to save COM port") + + self.finish() + # Include details so the controller can reconnect immediately + 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, 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"): + 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: + """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'}" + ) + + +@register_command("SIMULATOR") +class SimulatorCommand(SystemCommand): + """Toggle simulator (fake serial) mode on/off.""" + + __slots__ = ("mode_on",) + + def do_match(self, parts: list[str]) -> tuple[bool, str | None]: + """ + Parse SIMULATOR command. + + Format: SIMULATOR|on/off + Example: SIMULATOR|on + """ + if parts[0].upper() != "SIMULATOR": + return False, None + + if len(parts) != 2: + 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", + ): + 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: + """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") + return ExecutionStatus.failed("Simulator mode not set") + + # Set environment variable used by transport factory + os.environ["PAROL6_FAKE_SERIAL"] = "1" if self.mode_on else "0" + logger.info(f"SIMULATOR command executed: {'ON' if self.mode_on else 'OFF'}") + + self.finish() + return ExecutionStatus.completed( + f"Simulator {'ON' if self.mode_on else 'OFF'}", + details={"simulator_mode": "on" if self.mode_on else "off"}, + ) diff --git a/parol6/commands/utility_commands.py b/parol6/commands/utility_commands.py new file mode 100644 index 0000000..4c54b49 --- /dev/null +++ b/parol6/commands/utility_commands.py @@ -0,0 +1,78 @@ +""" +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 new file mode 100644 index 0000000..f42db81 --- /dev/null +++ b/parol6/config.py @@ -0,0 +1,170 @@ +""" +Central configuration for PAROL6 tunables and shared constants. +""" + +import logging +import os +from pathlib import Path + +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] + +TRACE_ENABLED = str(os.getenv("PAROL_TRACE", "0")).lower() in ("1", "true", "yes", "on") + +logger = logging.getLogger(__name__) + +# Default control/sample rates (Hz) +CONTROL_RATE_HZ: float = float(os.getenv("PAROL6_CONTROL_RATE_HZ", "250")) + +# Velocity/acceleration safety margins +VELOCITY_SAFETY_SCALE: float = 1.2 # e.g., clamp at 1.2x of budget + +DEFAULT_ACCEL_PERCENT: float = 50.0 + +# Motion thresholds (mm) +NEAR_MM_TOL_MM: float = 2.0 # Proximity threshold for considering positions "near" (mm) +ENTRY_MM_TOL_MM: float = 5.0 # Entry trajectory threshold for smooth motion (mm) + +# Centralized loop interval (seconds). +INTERVAL_S: float = max(1e-6, 1.0 / max(CONTROL_RATE_HZ, 1.0)) + +# Server/runtime defaults (overridable by env/CLI in headless commander) +SERVER_IP: str = "127.0.0.1" +SERVER_PORT: int = 5001 +SERVER_STREAM_DEFAULT: bool = False +FAKE_SERIAL: bool = False +SERIAL_BAUD: int = 3_000_000 +AUTO_HOME_DEFAULT: bool = True +LOG_LEVEL_DEFAULT: str = "INFO" + +# COM port persistence file stored in user config directory by default (cross-platform). +_default_com_file = Path.home() / ".parol6" / "com_port.txt" +COM_PORT_FILE: str = os.getenv("PAROL6_COM_FILE", str(_default_com_file)) + +# Multicast/broadcast status configuration (all overridable via env) +# These defaults implement local-only multicast on loopback by default. +MCAST_GROUP: str = os.getenv("PAROL6_MCAST_GROUP", "239.255.0.101") +MCAST_PORT: int = int(os.getenv("PAROL6_MCAST_PORT", "50510")) +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") + if not raw: + return [90.0, -90.0, 180.0, 0.0, 0.0, 180.0] + try: + parts = [p.strip() for p in raw.split(",")] + vals = [float(p) for p in parts] + # Ensure length 6 + if len(vals) != 6: + return [90.0, -90.0, 180.0, 0.0, 0.0, 180.0] + return vals + 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) -> bool | None: + raw = os.getenv(name) + if raw is None: + return None + s = raw.strip().lower() + if s in ("1", "true", "yes", "on"): + return True + if s in ("0", "false", "no", "off"): + return False + return None + + +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 + """ + try: + com_port_path = Path(COM_PORT_FILE) + com_port_path.parent.mkdir(parents=True, exist_ok=True) + com_port_path.write_text(port.strip()) + logger.info(f"Saved COM port {port} to {COM_PORT_FILE}") + return True + except Exception as e: + logger.error(f"Failed to save COM port: {e}") + return False + + +def load_com_port() -> str | None: + """ + Load saved COM port from file. + + Returns: + COM port string if found, None otherwise + """ + try: + com_port_path = Path(COM_PORT_FILE) + if com_port_path.exists(): + port = com_port_path.read_text().strip() + if port: + logger.info(f"Loaded COM port {port} from {COM_PORT_FILE}") + return port + except Exception as e: + logger.error(f"Failed to load COM port: {e}") + return None + + +def get_com_port_with_fallback() -> str: + """ + Resolve COM port from environment or file. + + Priority: + 1) Environment variables: PAROL6_COM_PORT or PAROL6_SERIAL + 2) com_port.txt (if present and non-empty) + + Returns: + Port string if available, otherwise an empty string "". + """ + # 1) Environment variables + env_port = os.getenv("PAROL6_COM_PORT") or os.getenv("PAROL6_SERIAL") + if env_port and env_port.strip(): + port = env_port.strip() + logger.info(f"Using COM port from environment: {port}") + return port + + # 2) Persistence file + saved_port = load_com_port() + if saved_port: + return saved_port + + return "" diff --git a/parol6/gcode/__init__.py b/parol6/gcode/__init__.py new file mode 100644 index 0000000..60b1676 --- /dev/null +++ b/parol6/gcode/__init__.py @@ -0,0 +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 .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 new file mode 100644 index 0000000..774accb --- /dev/null +++ b/parol6/gcode/commands.py @@ -0,0 +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 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 new file mode 100644 index 0000000..2be00ad --- /dev/null +++ b/parol6/gcode/coordinates.py @@ -0,0 +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 + + +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 new file mode 100644 index 0000000..befb078 --- /dev/null +++ b/parol6/gcode/interpreter.py @@ -0,0 +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 .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 new file mode 100644 index 0000000..b2316d1 --- /dev/null +++ b/parol6/gcode/parser.py @@ -0,0 +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 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 new file mode 100644 index 0000000..a31be41 --- /dev/null +++ b/parol6/gcode/state.py @@ -0,0 +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 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 new file mode 100644 index 0000000..a79cfd9 --- /dev/null +++ b/parol6/gcode/utils.py @@ -0,0 +1,360 @@ +""" +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/__init__.py b/parol6/protocol/__init__.py new file mode 100644 index 0000000..d00b764 --- /dev/null +++ b/parol6/protocol/__init__.py @@ -0,0 +1 @@ +"""Protocol types and definitions.""" diff --git a/parol6/protocol/types.py b/parol6/protocol/types.py new file mode 100644 index 0000000..2f098f7 --- /dev/null +++ b/parol6/protocol/types.py @@ -0,0 +1,109 @@ +""" +Type definitions for PAROL6 protocol. + +Defines enums, TypedDicts, and dataclasses used across the public API. +""" + +from datetime import datetime +from enum import Enum +from typing import Literal, TypedDict + + +# 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) + + +# Frame literals +Frame = Literal["WRF", "TRF"] + +# 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", +] + + +class IOStatus(TypedDict): + """Digital I/O status.""" + + in1: int + in2: int + out1: int + out2: int + estop: int + + +class GripperStatus(TypedDict): + """Electric gripper status.""" + + id: int + position: int + speed: int + current: int + status_byte: int + object_detect: int + + +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 + completed: bool + ack_time: datetime | None + + +class SendResult(TypedDict): + """Standardized result for command-sending APIs.""" + + command_id: str | None + status: AckStatus + details: str + completed: bool + ack_time: datetime | None + + +class WireResponse(TypedDict): + """Typed wrapper for parsed wire responses.""" + + 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 new file mode 100644 index 0000000..b95dbd0 --- /dev/null +++ b/parol6/protocol/wire.py @@ -0,0 +1,540 @@ +""" +Wire protocol helpers for UDP encoding/decoding. + +This module centralizes encoding of command strings and decoding of common +response payloads used by the headless controller. +""" + +import logging +from collections.abc import Sequence + +# 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" +) +START = b"\xff\xff\xff" +END = b"\x01\x02" +PAYLOAD_LEN = 52 # matches existing firmware expectation + +__all__ = [ + "CommandCode", + "pack_tx_frame_into", + "unpack_rx_frame_into", + "encode_move_joint", + "encode_move_pose", + "encode_move_cartesian", + "encode_move_cartesian_rel_trf", + "encode_jog_joint", + "encode_cart_jog", + "encode_gcode", + "encode_gcode_program_inline", + "decode_simple", + "decode_status", + "split_to_3_bytes", + "fuse_3_bytes", + "fuse_2_bytes", +] + + +class CommandCode(IntEnum): + """Unified command codes for firmware interface.""" + + HOME = 100 + ENABLE = 101 + DISABLE = 102 + JOG = 123 + MOVE = 156 + IDLE = 255 + + +def split_bitfield(byte_val: int) -> list[int]: + """Split an 8-bit integer into a big-endian list of bits (MSB..LSB).""" + return [(byte_val >> i) & 1 for i in range(7, -1, -1)] + + +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. + """ + number = 0 + for b in bits[:8]: + number = (number << 1) | (1 if b else 0) + return bytes([number]) + + +def split_to_3_bytes(n: int) -> tuple[int, int, int]: + """ + Convert int to signed 24-bit big-endian (two's complement) encoded bytes (b0,b1,b2). + """ + n24 = n & 0xFFFFFF + return ((n24 >> 16) & 0xFF, (n24 >> 8) & 0xFF, n24 & 0xFF) + + +def fuse_3_bytes(b0: int, b1: int, b2: int) -> int: + """ + Convert 3 bytes (big-endian) into a signed 24-bit integer. + """ + val = (b0 << 16) | (b1 << 8) | b2 + return val - 0x1000000 if (val & 0x800000) else val + + +def fuse_2_bytes(b0: int, b1: int) -> int: + """ + Convert 2 bytes (big-endian) into a signed 16-bit integer. + """ + val = (b0 << 8) | b1 + return val - 0x10000 if (val & 0x8000) else val + + +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. + """ + try: + if index < len(arr): + return int(arr[index]) + return default + except (IndexError, TypeError): + return default + + +def pack_tx_frame_into( + out: memoryview, + position_out: np.ndarray, + speed_out: np.ndarray, + command_code: int | CommandCode, + affected_joint_out: np.ndarray, + inout_out: np.ndarray, + timeout_out: int, + gripper_data_out: np.ndarray, +) -> None: + """ + Pack a full TX frame into the provided memoryview without allocations. + + Expects 'out' to be a writable buffer of length >= 56 bytes: + - 3 start bytes + 1 length byte + 52-byte payload + + Layout of the 52-byte payload: + - 6x position (3 bytes each) = 18 + - 6x speed (3 bytes each) = 18 + - 1 byte command + - 1 byte affected joint bitfield + - 1 byte in/out bitfield + - 1 byte timeout + - 2 bytes reserved (legacy) + - 2 bytes gripper position + - 2 bytes gripper speed + - 2 bytes gripper current + - 1 byte gripper command + - 1 byte gripper mode + - 1 byte gripper id + - 1 byte CRC (placeholder 228) + - 2 bytes end markers (0x01, 0x02) + """ + # Header + out[0:3] = START + out[3] = PAYLOAD_LEN + offset = 4 + + # Positions: 6 * 3 bytes + for i in range(6): + val = _get_array_value(position_out, i, 0) + b0, b1, b2 = split_to_3_bytes(val) + out[offset] = b0 + out[offset + 1] = b1 + out[offset + 2] = b2 + offset += 3 + + # Speeds: 6 * 3 bytes + for i in range(6): + val = _get_array_value(speed_out, i, 0) + b0, b1, b2 = split_to_3_bytes(val) + out[offset] = b0 + out[offset + 1] = b1 + out[offset + 2] = b2 + offset += 3 + + # Command + out[offset] = int(command_code) + offset += 1 + + # Affected joints as bitfield byte + bitfield_val = 0 + for i in range(8): + if _get_array_value(affected_joint_out, i, 0): + bitfield_val |= 1 << (7 - i) + out[offset] = bitfield_val + offset += 1 + + # In/Out as bitfield byte + bitfield_val = 0 + for i in range(8): + if _get_array_value(inout_out, i, 0): + bitfield_val |= 1 << (7 - i) + out[offset] = bitfield_val + offset += 1 + + # Timeout + 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 + out[offset] = (v >> 8) & 0xFF + out[offset + 1] = v & 0xFF + offset += 2 + + # Gripper command, mode, id + out[offset] = _get_array_value(gripper_data_out, 3, 0) & 0xFF + out[offset + 1] = _get_array_value(gripper_data_out, 4, 0) & 0xFF + out[offset + 2] = _get_array_value(gripper_data_out, 5, 0) & 0xFF + offset += 3 + + # CRC (placeholder - unchanged from legacy) + out[offset] = 228 + offset += 1 + + # End bytes + out[offset] = 0x01 + out[offset + 1] = 0x02 + + +def unpack_rx_frame_into( + data: memoryview, + *, + 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. + Expects: + - pos_out, spd_out: shape (6,), dtype=int32 + - homed_out, io_out, temp_out, poserr_out: shape (8,), dtype=uint8 + - timing_out: shape (1,), dtype=int32 + - grip_out: shape (6,), dtype=int32 [device_id, pos, spd, cur, status, obj] + """ + try: + if len(data) < 52: + logger.warning( + f"unpack_rx_frame_into: payload too short ({len(data)} bytes)" + ) + return False + + mv = memoryview(data) + + # Positions (0..17) and speeds (18..35), 3 bytes per value, big-endian signed 24-bit + b = np.frombuffer(mv[:36], dtype=np.uint8).reshape(12, 3) + 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) + ) + + # Sign-correct 24-bit to int32 + 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") + + homed_byte = mv[36] + io_byte = mv[37] + temp_err_byte = mv[38] + pos_err_byte = mv[39] + timing_b0 = mv[40] + timing_b1 = mv[41] + # indices 42..43 exist in some variants (timeout/xtr), legacy code ignores + + device_id = mv[44] + grip_pos_b0, grip_pos_b1 = mv[45], mv[46] + grip_spd_b0, grip_spd_b1 = mv[47], mv[48] + grip_cur_b0, grip_cur_b1 = mv[49], mv[50] + status_byte = mv[51] + + # Bitfields (MSB..LSB) via LUT (no per-call Python loops) + homed_out[:] = _BIT_UNPACK[int(homed_byte)] + io_out[:] = _BIT_UNPACK[int(io_byte)] + temp_out[:] = _BIT_UNPACK[int(temp_err_byte)] + poserr_out[:] = _BIT_UNPACK[int(pos_err_byte)] + + # Timing (legacy semantics: fuse_3_bytes(0, b0, b1)) + timing_val = fuse_3_bytes(0, int(timing_b0), int(timing_b1)) + timing_out[0] = int(timing_val) + + # Gripper values + grip_pos = fuse_2_bytes(int(grip_pos_b0), int(grip_pos_b1)) + grip_spd = fuse_2_bytes(int(grip_spd_b0), int(grip_spd_b1)) + grip_cur = fuse_2_bytes(int(grip_cur_b0), int(grip_cur_b1)) + + sbits = _BIT_UNPACK[int(status_byte)] + obj_detection = (int(sbits[4]) << 1) | int(sbits[5]) + + grip_out[0] = int(device_id) + grip_out[1] = int(grip_pos) + grip_out[2] = int(grip_spd) + grip_out[3] = int(grip_cur) + grip_out[4] = int(status_byte) + grip_out[5] = int(obj_detection) + + return True + except Exception as e: + logger.error(f"unpack_rx_frame_into: exception {e}") + return False + + +# ========================= +# 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) + + +def encode_move_joint( + angles: Sequence[float], + duration: float | None, + speed: float | None, +) -> str: + """ + MOVEJOINT|j1|j2|j3|j4|j5|j6|DUR|SPD + Use "NONE" for omitted duration/speed. + Note: Validation (requiring one of duration/speed) is left to caller. + """ + angles_str = "|".join(str(a) for a in angles) + return f"MOVEJOINT|{angles_str}|{_opt(duration)}|{_opt(speed)}" + + +def encode_move_pose( + pose: Sequence[float], + duration: float | None, + speed: float | None, +) -> str: + """ + MOVEPOSE|x|y|z|rx|ry|rz|DUR|SPD + Use "NONE" for omitted duration/speed. + """ + pose_str = "|".join(str(v) for v in pose) + return f"MOVEPOSE|{pose_str}|{_opt(duration)}|{_opt(speed)}" + + +def encode_move_cartesian( + pose: Sequence[float], + duration: float | None, + speed: float | None, +) -> str: + """ + MOVECART|x|y|z|rx|ry|rz|DUR|SPD + Use "NONE" for omitted duration/speed. + """ + pose_str = "|".join(str(v) for v in pose) + return f"MOVECART|{pose_str}|{_opt(duration)}|{_opt(speed)}" + + +def encode_move_cartesian_rel_trf( + deltas: Sequence[float], # [dx, dy, dz, rx, ry, rz] in mm/deg relative to TRF + duration: float | None, + speed: float | None, + accel: int | None, + profile: str | None, + tracking: str | None, +) -> str: + """ + MOVECARTRELTRF|dx|dy|dz|rx|ry|rz|DUR|SPD|ACC|PROFILE|TRACKING + Non-required fields should use "NONE". + """ + delta_str = "|".join(str(v) for v in deltas) + prof_str = (profile or "NONE").upper() + track_str = (tracking or "NONE").upper() + return ( + f"MOVECARTRELTRF|{delta_str}|{_opt(duration)}|{_opt(speed)}|" + f"{_opt(accel)}|{prof_str}|{track_str}" + ) + + +def encode_jog_joint( + joint_index: int, + speed_percentage: int, + duration: float | None, + distance_deg: float | None, +) -> str: + """ + JOG|joint_index|speed_pct|DUR|DIST + duration and distance_deg are optional; use "NONE" if omitted. + """ + return f"JOG|{joint_index}|{speed_percentage}|{_opt(duration)}|{_opt(distance_deg)}" + + +def encode_cart_jog( + frame: Frame, + axis: Axis, + speed_percentage: int, + duration: float, +) -> str: + """ + CARTJOG|FRAME|AXIS|speed_pct|duration + """ + return f"CARTJOG|{frame}|{axis}|{speed_percentage}|{duration}" + + +def encode_gcode(line: str) -> str: + """ + GCODE| + The caller should ensure that '|' is not present in the line. + """ + return f"GCODE|{line}" + + +def encode_gcode_program_inline(lines: Sequence[str]) -> str: + """ + GCODE_PROGRAM|INLINE|line1;line2;... + The caller should ensure that '|' is not present inside any line. + """ + program_str = ";".join(lines) + return f"GCODE_PROGRAM|INLINE|{program_str}" + + +# ========================= +# Decoding helpers +# ========================= +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 + IO|in1,in2,out1,out2,estop + GRIPPER|id,pos,spd,cur,status,obj + SPEEDS|s0,s1,s2,s3,s4,s5 + POSE|p0,p1,...,p15 + + 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}'" + ) + 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}'" + ) + return None + payload = parts[1] + tokens = [t for t in payload.split(",") if t != ""] + + # IO and GRIPPER are integer-based; others default to float + if expected_prefix in ("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}" + ) + 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}" + ) + 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| + ACTION_CURRENT=...|ACTION_STATE=... + + Returns a dict matching StatusAggregate or None on parse failure. + """ + if not resp or not resp.startswith("STATUS|"): + return None + + # Split top-level sections after "STATUS|" + sections = resp.split("|")[1:] + result: dict[str, object] = { + "pose": None, + "angles": 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] + result["pose"] = vals + elif sec.startswith("ANGLES="): + 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] + result["speeds"] = vals + elif sec.startswith("IO="): + 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] + result["gripper"] = vals + 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/__init__.py b/parol6/server/__init__.py new file mode 100644 index 0000000..8b1b3a8 --- /dev/null +++ b/parol6/server/__init__.py @@ -0,0 +1 @@ +"""Server management modules.""" diff --git a/parol6/server/command_registry.py b/parol6/server/command_registry.py new file mode 100644 index 0000000..7641a11 --- /dev/null +++ b/parol6/server/command_registry.py @@ -0,0 +1,279 @@ +""" +Command registration system with decorator support. + +This module provides a centralized registry for all commands, enabling +auto-discovery and registration through decorators. This eliminates the +need for manual command factory maintenance. +""" + +from __future__ import annotations + +import logging +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 + +logger = logging.getLogger(__name__) + + +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: 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"): + self._commands = {} + self._class_to_name = {} + self._discovered = False + self._initialized = True + + 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 + """ + if name in self._commands: + existing = self._commands[name] + if existing != command_class: + raise ValueError( + f"Command '{name}' is already registered with class {existing.__name__}. " + f"Cannot register with {command_class.__name__}" + ) + else: + self._commands[name] = command_class + # 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) -> 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]) -> str | None: + """ + Retrieve the registered command name for a given command class. + Returns None if the class is not registered. + """ + # Ensure commands are discovered + if not self._discovered: + self.discover_commands() + # 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]: + """ + 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") + 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}" + + # Skip the base module + if modname == "base": + continue + + try: + # Import the module (this triggers decorators) + import_module(full_module_name) + logger.debug(f"Imported command module: {full_module_name}") + except ImportError as e: + 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[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 + - (None, None) if command name not registered + - (None, error_message) if command is recognized but has invalid parameters + """ + # 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.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.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() + self._discovered = False + logger.debug("Command registry cleared") + + +# Global registry instance +_registry = CommandRegistry() + + +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]: + # 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 +discover_commands = _registry.discover_commands +clear_registry = _registry.clear +create_command_from_parts = _registry.create_command_from_parts +get_name_for_class = _registry.get_name_for_class diff --git a/parol6/server/controller.py b/parol6/server/controller.py new file mode 100644 index 0000000..33b54e1 --- /dev/null +++ b/parol6/server/controller.py @@ -0,0 +1,1440 @@ +""" +Main controller for PAROL6 robot server. +""" + +import argparse +import logging +import os +import socket +import threading +import time +from collections import deque +from dataclasses import dataclass, field +from typing import Any + +import numpy as np + +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.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") + + +@dataclass +class ExecutionContext: + """Context passed to commands during execution.""" + + 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: str | None = None + address: tuple[str, int] | None = None + queued_time: float = field(default_factory=time.time) + activated: bool = False + initialized: bool = False + first_tick_logged: bool = False + + +@dataclass +class ControllerConfig: + """Configuration for the controller.""" + + udp_host: str = "0.0.0.0" + udp_port: int = 5001 + serial_port: str | None = None + serial_baudrate: int = 3000000 + loop_interval: float = INTERVAL_S + estop_recovery_delay: float = 1.0 + auto_home: bool = False + + +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 + """ + self.config = config + self.running = False + self.shutdown_event = threading.Event() + self._initialized = False + + # Core components + self.state_manager = StateManager() + self.udp_transport: UDPTransport | None = None + self.serial_transport: SerialTransport | MockSerialTransport | None = None + + # ACK management + 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: QueuedCommand | None = None + + # Command tracking + self.current_command: CommandBase | None = None + self.command_id_map: dict[str, Any] = {} + + # E-stop recovery + 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: threading.Thread | None = None + + # GCODE interpreter + self.gcode_interpreter = GcodeInterpreter() + + # Status services (updater + multicast broadcaster) + 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: + """ + Send an acknowledgment message. + + Args: + cmd_id: Command ID to acknowledge + status: Status (QUEUED, EXECUTING, COMPLETED, FAILED, CANCELLED) + details: Optional details message + addr: Address tuple (host, port) to send to + """ + 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, + ) + + 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 + ) + 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: + persisted = get_com_port_with_fallback() + if persisted: + self.config.serial_port = persisted + logger.info(f"Using persisted serial port: {persisted}") + except Exception as e: + logger.debug(f"Failed to load persisted COM port: {e}") + + # Initialize Serial transport using factory + if self.config.serial_port or is_simulation_mode(): + self.serial_transport = create_and_connect_transport( + port=self.config.serial_port, + baudrate=self.config.serial_baudrate, + 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 + ) + 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, + ) + else: + 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: + home_cmd, _ = create_command_from_parts(["HOME"]) + if home_cmd: + # Queue without address/id for auto-home + self._queue_command(("127.0.0.1", 0), home_cmd, 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}" + ) + broadcaster = StatusBroadcaster( + state_mgr=self.state_manager, + group=MCAST_GROUP, + port=MCAST_PORT, + ttl=MCAST_TTL, + iface_ip=MCAST_IF, + rate_hz=STATUS_RATE_HZ, + stale_s=STATUS_STALE_S, + ) + + broadcaster.start() + logger.info("Status updater and broadcaster started") + except Exception as e: + logger.warning(f"Failed to start status services: {e}") + + 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: + self._status_broadcaster.stop() + if self._status_updater: + self._status_updater.stop() + except Exception: + pass + + # 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: + 1. Reads from firmware (serial) + 2. Handles E-stop and recovery + 3. Executes active command or fetches from GCODE + 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 + prev_print = next_t + + 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: + mv, ver, ts = self.serial_transport.get_latest_frame_view() + if mv is not None and ver != self._serial_last_version: + ok = unpack_rx_frame_into( + mv, + pos_out=state.Position_in, + spd_out=state.Speed_in, + homed_out=state.Homed_in, + io_out=state.InOut_in, + temp_out=state.Temperature_error_in, + poserr_out=state.Position_error_in, + timing_out=state.Timing_data_in, + grip_out=state.Gripper_data_in, + ) + if ok: + get_cache().mark_serial_observed() + if not self.first_frame_received: + self.first_frame_received = True + logger.info("First frame received from robot") + 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 + # 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 + ) + + # 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 not self.estop_active: # Not already in E-stop state + logger.warning("E-STOP activated") + self.estop_active = True + # Cancel active command + if self.active_command: + self._cancel_active_command("E-Stop activated") + # Clear queue + self._clear_queue("E-Stop activated") + # Stop robot + 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: # Was in E-stop state + # E-stop was released - automatic recovery + logger.info("E-STOP released - automatic recovery") + self.estop_active = False + # Re-enable immediately per policy + state.enabled = True + 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 ( + 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() + # Check for GCODE commands if program is running + elif self.gcode_interpreter.is_running: + self._fetch_gcode_commands() + else: + # No commands - idle + state.Command_out = CommandCode.IDLE + state.Speed_out.fill(0) + np.copyto(state.Position_out, state.Position_in, casting="no") + + # 4. Write to firmware + 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"] + ) + ) + ) + + # 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() + # Update period metrics + period = now - prev_t + prev_t = now + state.loop_count += 1 + state.last_period_s = float(period) + if state.ema_period_s <= 0.0: + 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 + ) + + next_t += tick + sleep = next_t - time.perf_counter() + if sleep > 0: + time.sleep(sleep) + else: + # Overrun; catch up + state.overrun_count += 1 + next_t = time.perf_counter() + + 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] + ) + if time_span > 0: + short_term_cmd_hz = ( + len(state.command_timestamps) - 1 + ) / time_span + + logger.debug( + "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() + break + 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. + """ + while self.running and self.udp_transport: + try: + # Check for new commands from UDP (blocking with short timeout) + tup = self.udp_transport.receive_one() + if tup is None: + continue + cmd_str, addr = tup + try: + _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.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_name = cmd_parts[0].upper() + # Build server-side ack/wait policy + policy = AckPolicy.from_env(lambda: state.stream_mode) + + # 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, + ) + active_inst = self.active_command.command + 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.setup(state) + except Exception as _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, + ) + 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, + ) + + # 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}" + ) + 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 + ) + 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.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, + ) + + # 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 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, + ) + ) + if self.serial_transport and hasattr( + self.serial_transport, "start_reader" + ): + self.serial_transport.start_reader( + self.shutdown_event + ) + self.first_frame_received = False + # 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}" + ) + + # 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() + 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" + ) + + # Pre-switch safety: stop and clear motion before transport switch + try: + state = self.state_manager.get_state() + # Immediately stop motion + state.Command_out = CommandCode.IDLE + state.Speed_out.fill(0) + # Cancel active and clear queued streamable/non-streamable commands + 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, + ) + + # Disconnect any existing transport + if self.serial_transport: + try: + self.serial_transport.disconnect() + except Exception: + pass + # Recreate transport according to new mode; auto_find_port for real serial + self.serial_transport = create_and_connect_transport( + port=self.config.serial_port, + baudrate=self.config.serial_baudrate, + 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 + ) + except Exception as _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.first_frame_received = False + # 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}" + ) + except Exception as e: + logger.debug(f"System command side-effect handling failed: {e}") + + # Always respond for system commands + if self.udp_transport: + if status.code == ExecutionStatusCode.COMPLETED: + self.udp_transport.send_response("OK", addr) + else: + msg = status.message or "Failed" + self.udp_transport.send_response(f"ERROR|{msg}", addr) + continue + + # Check if controller is enabled for motion commands + if isinstance(command, MotionCommand) and not state.enabled: + # Inform client only if policy requires ACK for motion + 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}" + ) + 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.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 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) + ): + 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 + ): + self.command_queue.remove(queued_cmd) + removed += 1 + if removed: + logger.log(TRACE, "queued_streamables_removed count=%d", removed) + + # Queue the command + status = self._queue_command(addr, command, None) + 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: + if policy.requires_ack(cmd_str): + if status.code == ExecutionStatusCode.QUEUED: + self.udp_transport.send_response("OK", addr) + else: + msg = status.message or "Queue error" + self.udp_transport.send_response(f"ERROR|{msg}", addr) + + # Start execution if no active command + if not self.active_command: + self._execute_active_command() + except Exception as e: + logger.error(f"Error in command processing: {e}", exc_info=True) + + 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("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 + ) + + # 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, + ) + ) + + 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 + ) + + return ExecutionStatus( + code=ExecutionStatusCode.QUEUED, + message=f"Command queued at position {len(self.command_queue)}", + ) + + 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 + """ + # Check if we need to activate a new command from queue + if self.active_command is None: + if not self.command_queue: + return None + # Get next command from queue + 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() + + # 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.activated = True + 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", + ) + + # Execute command step + if not getattr(ac, "first_tick_logged", False): + logger.log(TRACE, "tick_start name=%s", type(ac.command).__name__) + ac.first_tick_logged = True + 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 + ): + try: + 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(), + ) + + # Send completion acknowledgment + if cid and 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}" + ) + + # Send failure acknowledgment + if cid and 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: {e!s}", addr) + self.active_command = None + + 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}" + ) + + # 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, + ) + + # 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]]: + """ + Clear all queued commands. + + Args: + reason: Reason for clearing the queue + + Returns: + List of (command_id, status) for cleared commands + """ + cleared = [] + # 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 + ) + + # Record cleared command + if queued_cmd.command_id: + status = ExecutionStatus( + 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. + Converts GCODE output to command objects and queues them. + """ + 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 + ) + 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(): + """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)" + ) + + # 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", + ) + 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": + log_level = TRACE + cfg.TRACE_ENABLED = True + else: + log_level = getattr(logging, args.log_level) + elif args.verbose >= 3: + log_level = TRACE + 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", + ) + + # Create configuration (env vars may override defaults) + env_host = os.getenv("PAROL6_CONTROLLER_IP") + env_port = os.getenv("PAROL6_CONTROLLER_PORT") + udp_host = env_host.strip() if env_host else args.host + try: + udp_port = int(env_port) if env_port else args.port + except (TypeError, ValueError): + udp_port = args.port + + logger.info(f"Controller bind: host={udp_host} port={udp_port}") + + config = ControllerConfig( + udp_host=udp_host, + udp_port=udp_port, + serial_port=args.serial, + serial_baudrate=args.baudrate, + auto_home=bool(args.auto_home), + ) + + # Create and run controller + try: + controller = Controller(config) + + if controller.is_initialized(): + try: + controller.start() + except KeyboardInterrupt: + logger.info("Shutting down...") + finally: + controller.stop() + else: + logger.error("Controller not properly initialized") + return 1 + except RuntimeError as e: + logger.error(f"Failed to create controller: {e}") + return 1 + + return 0 + + +if __name__ == "__main__": + exit(main()) diff --git a/parol6/server/state.py b/parol6/server/state.py new file mode 100644 index 0000000..79ad91d --- /dev/null +++ b/parol6/server/state.py @@ -0,0 +1,350 @@ +from __future__ import annotations + +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 + + +@dataclass +class ControllerState: + """ + Centralized mutable state for the headless controller. + + Buffers use preallocated NumPy ndarrays for zero-copy, in-place operations. + """ + + # Serial/transport + ser: Any = None + last_reconnect_attempt: float = 0.0 + + # Safety and control flags + enabled: bool = True + soft_error: bool = False + disabled_reason: str = "" + 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 + start_cond2: int = 0 + start_cond3: int = 0 + good_start: int = 0 + data_len: int = 0 + 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) + ) + 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) + ) + + # 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) + ) + 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) + ) + + 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) + active_command: Any = 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 + ) + + # 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_period_s: float = 0.0 + ema_command_period_s: float = 0.0 + 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__) + + +class StateManager: + """ + Singleton manager for ControllerState with thread-safe operations. + + This class ensures that all state access is synchronized and provides + convenience methods for common state operations. + """ + + _instance: StateManager | None = None + _lock: threading.Lock = threading.Lock() + _state: ControllerState | None = None + + def __new__(cls) -> StateManager: + """Ensure singleton pattern with thread safety.""" + if cls._instance is None: + with cls._lock: + # Double-check locking pattern + if cls._instance is None: + cls._instance = super().__new__(cls) + return cls._instance + + def __init__(self): + """Initialize the state manager (only runs once due to singleton).""" + if not hasattr(self, "_initialized"): + self._state = ControllerState() + self._state_lock = threading.RLock() # Use RLock for re-entrant locking + self._initialized = True + logger.info("StateManager initialized with NumPy buffers") + + def get_state(self) -> ControllerState: + """ + Get the current controller state. + + Note: This returns the actual state object. Modifications to it + should be done through StateManager methods to ensure thread safety. + + Returns: + The current ControllerState instance + """ + with self._state_lock: + if self._state is None: + self._state = ControllerState() + return self._state + + def reset_state(self) -> None: + """ + Reset the controller state to a fresh instance. + + This is useful at controller startup to ensure buffers are initialized + to known defaults. Callers must ensure they hold appropriate locks in + higher layers if concurrent access is possible. + """ + with self._state_lock: + self._state = ControllerState() + logger.info("Controller state reset") + + +# Global singleton instance accessor +_state_manager: StateManager | None = None + + +def get_instance() -> StateManager: + """ + Get the global StateManager instance. + """ + global _state_manager + if _state_manager is None: + _state_manager = StateManager() + return _state_manager + + +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 new file mode 100644 index 0000000..3104acd --- /dev/null +++ b/parol6/server/status_broadcast.py @@ -0,0 +1,283 @@ +from __future__ import annotations + +import logging +import socket +import threading +import time + +from parol6 import config as cfg +from parol6.server.state import StateManager +from parol6.server.status_cache import get_cache + +logger = logging.getLogger(__name__) + + +class StatusBroadcaster(threading.Thread): + """ + Broadcasts ASCII STATUS frames via UDP. + + Transport: + - cfg.STATUS_TRANSPORT: "MULTICAST" (default) or "UNICAST" + + 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") + + 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 + """ + + def __init__( + self, + state_mgr: StateManager, + group: str = "239.255.0.101", + port: int = 50510, + ttl: int = 1, + iface_ip: str = "127.0.0.1", + rate_hz: float = cfg.STATUS_RATE_HZ, + stale_s: float = cfg.STATUS_STALE_S, + ) -> None: + super().__init__(daemon=True) + self._state_mgr = state_mgr + self.group = group + self.port = port + self.ttl = ttl + self.iface_ip = iface_ip + self._period = 1.0 / max(rate_hz, 1.0) + self._stale_s = stale_s + + # 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 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) + + 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: + 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: + sock.close() + except Exception: + pass + 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() + + # 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: + state = self._state_mgr.get_state() + cache.update_from_state(state) + except Exception as e: + logger.debug("StatusBroadcaster cache refresh failed: %s", e) + + # 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") + # 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: + time.sleep(sleep_time) + next_deadline += self._period + + def stop(self) -> None: + self._running.clear() + try: + if self._sock: + self._sock.close() + except Exception: + pass diff --git a/parol6/server/status_cache.py b/parol6/server/status_cache.py new file mode 100644 index 0000000..2ab6a43 --- /dev/null +++ b/parol6/server/status_cache.py @@ -0,0 +1,285 @@ +import threading +import time + +import numpy as np +from numpy.typing import ArrayLike + +import parol6.PAROL6_ROBOT as PAROL6_ROBOT +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: + """ + Thread-safe cache of the aggregate STATUS payload components and formatted ASCII. + + Fields: + - angles_deg: 6 floats + - speeds: 6 ints (steps/sec) + - io: 5 ints [in1,in2,out1,out2,estop] + - gripper: >=6 ints [id,pos,spd,cur,status,obj] + - pose: 16 floats (flattened transform) + - last_update_s: wall clock time of last cache update + """ + + def __init__(self) -> None: + self._lock = threading.RLock() + + # Public snapshots (materialized only when they change) + self.angles_deg: np.ndarray = np.zeros((6,), dtype=np.float64) + self.speeds: np.ndarray = np.zeros((6,), dtype=np.int32) + self.io: np.ndarray = np.zeros((5,), dtype=np.uint8) + self.gripper: np.ndarray = np.zeros((6,), dtype=np.int32) + self.pose: np.ndarray = np.zeros((16,), dtype=np.float64) + + 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" + self._speeds_ascii: str = "0,0,0,0,0,0" + 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 + self._last_pos_in: np.ndarray = np.zeros((6,), dtype=np.int32) + + 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 + + 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: + """ + Update cache from current controller state with change gating: + - Only recompute angles/pose when Position_in changes (and beyond optional deadband) + - Only refresh IO/speeds/gripper when their inputs actually change + """ + now = time.time() + changed_any = False + + with self._lock: + # 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,) + # Publish angles list and ASCII + self._angles_ascii = self._format_csv_from_list(self.angles_deg) + changed_any = True + + # 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]) + self._io_ascii = self._format_csv_from_list(self.io) + changed_any = True + + # 3) Speeds (steps/sec from Speed_in) + if not np.array_equal(self.speeds, state.Speed_in): + np.copyto(self.speeds, state.Speed_in) + self._speeds_ascii = self._format_csv_from_list(self.speeds) + changed_any = True + + # 4) Gripper + if not np.array_equal(self.gripper, state.Gripper_data_in): + np.copyto(self.gripper, state.Gripper_data_in) + self._gripper_ascii = self._format_csv_from_list(self.gripper) + changed_any = True + + # 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}" + 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}" + ) + self.last_update_s = now + + def to_ascii(self) -> str: + """Return the full ASCII STATUS payload.""" + with self._lock: + return self._ascii_full + + def mark_serial_observed(self) -> None: + """Mark that a fresh serial frame was observed just now.""" + with self._lock: + self.last_serial_s = time.time() + + def age_s(self) -> float: + """Seconds since last fresh serial observation (used to gate broadcasting).""" + with self._lock: + if self.last_serial_s <= 0: + return 1e9 + return time.time() - self.last_serial_s + + +# Module-level singleton +_status_cache: StatusCache | None = None + + +def get_cache() -> StatusCache: + global _status_cache + if _status_cache is None: + _status_cache = StatusCache() + return _status_cache diff --git a/parol6/server/transports/__init__.py b/parol6/server/transports/__init__.py new file mode 100644 index 0000000..1cafe02 --- /dev/null +++ b/parol6/server/transports/__init__.py @@ -0,0 +1,24 @@ +""" +Transport modules for PAROL6 server. + +This package provides different transport implementations for +communicating with the robot hardware or simulation. +""" + +from .mock_serial_transport import MockSerialTransport +from .serial_transport import SerialTransport +from .transport_factory import ( + create_and_connect_transport, + create_transport, + is_simulation_mode, +) +from .udp_transport import UDPTransport + +__all__ = [ + "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 new file mode 100644 index 0000000..0bbc7d8 --- /dev/null +++ b/parol6/server/transports/mock_serial_transport.py @@ -0,0 +1,545 @@ +""" +Mock serial transport for simulation and testing. + +This module provides a complete serial port simulation that generates +realistic robot responses without requiring hardware. The simulation +operates at the wire protocol level, making it transparent to the +controller code. +""" + +import logging +import threading +import time +from dataclasses import dataclass, field + +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 + +logger = logging.getLogger(__name__) + + +@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) + ) + # 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 + # 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) + ) + # Gripper state + 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) + ) + + # 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) + ) + + def __post_init__(self): + """Initialize robot to standby position.""" + # Set initial positions to standby position for better IK + for i in range(6): + 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 + + +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: 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) + timeout: Ignored (for interface compatibility) + """ + 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._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 + + # Latest-frame infrastructure (simulation publishes into this buffer) + self._frame_buf = bytearray(64) # payload 52B + headroom + self._frame_mv = memoryview(self._frame_buf)[:52] + self._frame_version = 0 + self._frame_ts = 0.0 + 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: 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 + + # Allow controller to sync the simulator pose/homing from live controller state + def sync_from_controller_state(self, state: ControllerState) -> None: + """ + Synchronize the mock robot internal state from a controller state snapshot. + Expects arrays compatible with ControllerState (Position_in, Homed_in). + """ + 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.perf_counter() + self._state.homing_countdown = 0 + + # Clear speeds and hold position + self._state.speed_in = state.Speed_in.copy() + 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 + ) + + 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: + """ + 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 + command_out: Command code + affected_joint_out: Affected joint flags + 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 = 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 + self._state.gripper_data_in[0] = gripper_data_out[5] # Set device ID + self._state.gripper_data_in[4] = 0x40 # Set calibrated bit + 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 + if state.homing_countdown == 0: + # Homing complete - mark all joints as homed and move to configured home posture + target_deg = cfg.HOME_ANGLES_DEG + # Mark all 8 homed bits as 1 to satisfy status bitfield expectations + state.homed_in.fill(1) + 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 + if state.homing_countdown == 0: + for i in range(6): + state.homed_in[i] = 0 # Mark as not homed + # Schedule homing completion after ~0.2s (use fixed frame interval for determinism) + state.homing_countdown = max(1, int(0.2 / self._frame_interval + 0.5)) + # 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 (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 (float-accumulated and per-tick speed clamp) + prev_pos_f = state.position_f.copy() + for i in range(6): + 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_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) + # ================================ + def start_reader(self, shutdown_event: threading.Event) -> threading.Thread: + """ + Start simulated latest-frame publisher thread. + """ + if self._reader_thread and self._reader_thread.is_alive(): + return self._reader_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.perf_counter() + if now >= next_deadline: + # Advance simulation before publishing a new frame + 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 = 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 + + t = threading.Thread(target=_run, name="MockSerialReader", daemon=True) + self._reader_thread = t + t.start() + return t + + def _encode_payload_into(self, out_mv: memoryview) -> None: + """ + 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 = 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 + 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 + off += 3 + + def bits_to_byte(bits: np.ndarray) -> int: + val = 0 + for b in bits[:8]: + val = (val << 1) | (1 if b else 0) + return val & 0xFF + + # Bitfields + out[36] = bits_to_byte(st.homed_in) + out[37] = bits_to_byte(st.io_in) + out[38] = bits_to_byte(st.temperature_error_in) + out[39] = bits_to_byte(st.position_error_in) + + # Timing (two bytes) + t = int(st.timing_data_in[0]) if st.timing_data_in else 0 + out[40] = (t >> 8) & 0xFF + out[41] = t & 0xFF + + # Reserved + out[42] = 0 + out[43] = 0 + + # Gripper + gd = st.gripper_data_in + dev_id = int(gd[0]) if gd.all() else 0 + pos = int(gd[1]) & 0xFFFF + spd = int(gd[2]) & 0xFFFF + cur = int(gd[3]) & 0xFFFF + 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[51] = status & 0xFF + + def get_latest_frame_view(self) -> tuple[memoryview | None, int, float]: + """ + Return latest 52-byte payload memoryview, version, timestamp. + """ + 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 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, + }, + } + + +def create_mock_serial_transport() -> MockSerialTransport: + """ + Factory function to create a mock serial transport. + + Returns: + Configured MockSerialTransport instance + """ + transport = MockSerialTransport() + transport.connect() + logger.info("Mock serial transport created and connected") + return transport diff --git a/parol6/server/transports/serial_transport.py b/parol6/server/transports/serial_transport.py new file mode 100644 index 0000000..86169be --- /dev/null +++ b/parol6/server/transports/serial_transport.py @@ -0,0 +1,495 @@ +""" +Serial transport implementation for PAROL6 controller. + +This module handles serial port communication, frame parsing, and +data exchange with the robot hardware. +""" + +import logging +import os +import threading +import time + +import numpy as np +import serial + +from parol6.config import INTERVAL_S, get_com_port_with_fallback +from parol6.protocol.wire import pack_tx_frame_into + +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]) + END_BYTES = bytes([0x01, 0x02]) + + 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 + timeout: Read timeout in seconds (0 for non-blocking) + """ + self.port = port + self.baudrate = baudrate + self.timeout = timeout + self.serial: serial.Serial | None = None + self.last_reconnect_attempt = 0.0 + self.reconnect_interval = 1.0 # seconds between reconnect attempts + + # Reduced-copy latest-frame infrastructure (reader thread will publish here) + self._scratch = bytearray(4096) + self._scratch_mv = memoryview(self._scratch) + # Fixed-size ring buffer for RX stream (drop-oldest on overflow) + _cap = int(os.getenv("PAROL6_SERIAL_RX_RING_CAP", "262144")) + self._ring = bytearray(_cap) + self._r_cap = _cap + self._r_head = 0 + self._r_tail = 0 + self._frame_buf = bytearray(64) # 52-byte payload + headroom + self._frame_mv = memoryview(self._frame_buf)[:52] + self._frame_version = 0 + self._frame_ts = 0.0 + 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: 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 + ) + + 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 + return False + except Exception as e: + 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: + try: + if self.serial.is_open: + self.serial.close() + logger.info(f"Disconnected from serial port: {self.port}") + except Exception as e: + 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: + """ + 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 + command_out: Command code + affected_joint_out: Affected joint flags + 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 + if ser is None: + return False + + pack_tx_frame_into( + self._tx_mv, + position_out, + speed_out, + command_out, + affected_joint_out, + inout_out, + timeout_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 + self.disconnect() + return False + except Exception as e: + logger.error(f"Unexpected error writing frame: {e}") + return False + + # ================================ + # Latest-frame API (reduced-copy) + # ================================ + def start_reader(self, shutdown_event: threading.Event) -> threading.Thread: + """ + Start a dedicated reader thread that parses incoming frames from the serial port + and publishes the latest 52-byte payload into an internal stable buffer. + + 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" + ) + + if self._reader_thread and self._reader_thread.is_alive(): + return self._reader_thread + + # Ensure a short timeout for responsive shutdown + if self.serial: + # Small block so as not to busy loop, but can't use a larger timout + # because serial will read until buffer is full or timeout. + self.serial.timeout = INTERVAL_S / 2 + + def _run() -> None: + self._reader_running = True + try: + while not shutdown_event.is_set(): + if not self.is_connected(): + # Backoff a bit to avoid busy loop if disconnected + time.sleep(0.1) + continue + # Race-safe read: hold local ref and check is_open + ser = self.serial + if not ser or not getattr(ser, "is_open", False): + # Disconnected between iterations; back off briefly + time.sleep(0.1) + continue + try: + # 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, + ) + try: + self.disconnect() + except Exception: + pass + break + except Exception: + logger.exception("Serial reader unexpected exception") + break + + if not n: + # Timeout or no data; loop to check shutdown_event + continue + + # 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 + + # 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() + finally: + self._reader_running = False + + t = threading.Thread(target=_run, name="SerialReader", daemon=True) + self._reader_thread = t + t.start() + return t + + def _parse_ring_for_frames(self) -> None: + """ + Parse as many complete frames as possible from the RX ring buffer in-place. + + Frame format: + [0xFF,0xFF,0xFF] [LEN] [LEN bytes data ...] + """ + START0, START1, START2 = 0xFF, 0xFF, 0xFF + END0, END1 = self.END_BYTES[0], self.END_BYTES[1] + cap = self._r_cap + head = self._r_head + tail = self._r_tail + rb = self._ring + + def available(h: int, t: int) -> int: + return (h - t + cap) % cap + + while available(head, tail) >= 4: + # Find start sequence 0xFF 0xFF 0xFF by advancing tail + found = False + while available(head, tail) >= 3: + b0 = rb[tail] + b1 = rb[(tail + 1) % cap] + b2 = rb[(tail + 2) % cap] + if b0 == START0 and b1 == START1 and b2 == START2: + found = True + break + tail = (tail + 1) % cap + if not found or available(head, tail) < 4: + break + + length = rb[(tail + 3) % cap] + total_needed = 4 + length + if available(head, tail) < total_needed: + # Wait for more data + break + + # Validate end markers if possible + if length >= 2: + e0 = rb[(tail + 4 + length - 2) % cap] + e1 = rb[(tail + 4 + length - 1) % cap] + if not (e0 == END0 and e1 == END1): + # Bad frame; skip one byte to resync + tail = (tail + 1) % cap + continue + + # Publish first 52 bytes if available + 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] + else: + first = cap - start + self._frame_buf[:first] = rb[start:cap] + 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() + + # Consume this frame + tail = (tail + total_needed) % cap + + # Publish updated tail + self._r_tail = tail + + 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, + } + + if self.serial and self.serial.is_open: + try: + 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})" + ) + else: + 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: 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) + else: + # Try to load and connect to saved port + 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 new file mode 100644 index 0000000..f4d11b7 --- /dev/null +++ b/parol6/server/transports/transport_factory.py @@ -0,0 +1,126 @@ +""" +Transport factory for creating appropriate transport instances. + +This module provides a factory pattern for creating transport instances +based on configuration and environment. It automatically selects between +real serial, mock serial, or other transport types. +""" + +import logging +import os +from typing import Any + +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__) + + +def is_simulation_mode() -> bool: + """ + Check if simulation mode is enabled. + + Returns: + True if simulation mode is enabled via environment variable + """ + fake_serial = str(os.getenv("PAROL6_FAKE_SERIAL", "0")).lower() + return fake_serial in ("1", "true", "yes", "on") + + +def create_transport( + transport_type: str | None = None, + port: str | None = None, + baudrate: int = 2000000, + **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) + """ + # Determine transport type + if transport_type is None: + # Auto-detect based on environment + if is_simulation_mode(): + transport_type = "mock" + else: + transport_type = "serial" + + # Create appropriate transport + if transport_type == "mock": + logger.info("Creating MockSerialTransport for simulation") + 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: str | None = None, + port: str | None = None, + baudrate: int = 2000000, + auto_find_port: bool = True, + **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 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 new file mode 100644 index 0000000..1baef3f --- /dev/null +++ b/parol6/server/transports/udp_transport.py @@ -0,0 +1,282 @@ +""" +UDP transport implementation for PAROL6 controller. + +This module handles UDP socket communication, message parsing, and +response handling for the controller's network interface. +""" + +from __future__ import annotations + +import logging +import socket +import time + +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 + ): + """ + Initialize the UDP transport. + + Args: + ip: IP address to bind to + port: Port number to listen on + buffer_size: Size of the receive buffer in bytes + """ + self.ip = ip + self.port = port + self.buffer_size = buffer_size + 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) + 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 + for _i in range(_attempts): + try: + self.socket.bind((self.ip, self.port)) + break + except OSError as _e: + 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 OSError as e: + logger.error(f"Failed to create UDP socket: {e}") + self.socket = None + return False + except Exception as e: + 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() + logger.info("UDP socket closed") + except Exception as e: + logger.error(f"Error closing UDP socket: {e}") + finally: + self.socket = None + + 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. + """ + if not self.socket or not self._running: + return None + try: + nbytes, address = self.socket.recvfrom_into(self._rxv) + if nbytes <= 0: + 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") + ) + except Exception: + logger.warning(f"Failed to decode UDP datagram from {address}") + return None + return (message_str, address) + except TimeoutError: + return None + 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 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") + self.socket.sendto(data, address) + return True + + 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: + """ + 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, + } + + if self.socket: + try: + sockname = self.socket.getsockname() + 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 +) -> 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: + return None diff --git a/parol6/smooth_motion/__init__.py b/parol6/smooth_motion/__init__.py new file mode 100644 index 0000000..8b99f6f --- /dev/null +++ b/parol6/smooth_motion/__init__.py @@ -0,0 +1,15 @@ +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 + +__all__ = [ + "CircularMotion", + "SplineMotion", + "HelixMotion", + "WaypointTrajectoryPlanner", + "MotionBlender", + "AdvancedMotionBlender", +] diff --git a/parol6/smooth_motion/advanced.py b/parol6/smooth_motion/advanced.py new file mode 100644 index 0000000..febc785 --- /dev/null +++ b/parol6/smooth_motion/advanced.py @@ -0,0 +1,347 @@ +""" +Advanced trajectory blending utilities (C2 continuity, minimum-jerk, etc.). +""" + +import numpy as np + + +class AdvancedMotionBlender: + """ + Advanced trajectory blender with C2 continuity support. + + Provides multiple blending methods including quintic polynomials for true + smooth motion with continuous position, velocity, and acceleration. + """ + + def __init__(self, sample_rate: float = 100.0): + """ + Initialize advanced motion blender. + + Args: + sample_rate: Trajectory sampling rate in Hz + """ + self.sample_rate = sample_rate + self.dt = 1.0 / sample_rate + + # Blend method registry + self.blend_methods = { + "quintic": self._blend_quintic, + "s_curve": self._blend_scurve, + "smoothstep": self._blend_smoothstep, # Legacy compatibility + "minimum_jerk": self._blend_minimum_jerk, + "cubic": self._blend_cubic, + } + + 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. + + Args: + trajectory: Trajectory array + index: Point index (-1 for last point) + + Returns: + Tuple of (position, velocity, acceleration) + """ + if len(trajectory) < 3: + pos = trajectory[index] + vel = np.zeros_like(pos) + acc = np.zeros_like(pos) + return pos, vel, acc + + # Position + pos = trajectory[index].copy() + + # Velocity + if index == 0 or (index == -1 and len(trajectory) == 1): + vel = (trajectory[1] - trajectory[0]) / self.dt + elif index == -1 or index == len(trajectory) - 1: + vel = (trajectory[-1] - trajectory[-2]) / self.dt + else: + vel = (trajectory[index + 1] - trajectory[index - 1]) / (2 * self.dt) + + # Acceleration + if len(trajectory) < 3: + acc = np.zeros_like(pos) + elif index == 0: + v1 = (trajectory[1] - trajectory[0]) / self.dt + v2 = (trajectory[2] - trajectory[1]) / self.dt + acc = (v2 - v1) / self.dt + elif index == -1 or index == len(trajectory) - 1: + v1 = (trajectory[-2] - trajectory[-3]) / self.dt + 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) + + return pos, vel, acc + + 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. + + Args: + traj1: First trajectory + traj2: Second trajectory + max_accel: Maximum allowed acceleration (mm/s² or deg/s²) + + Returns: + Number of samples for blend region + """ + _, v1, _ = self.extract_trajectory_state(traj1, -1) + _, v2, _ = self.extract_trajectory_state(traj2, 0) + + delta_v = np.linalg.norm(v2[:3] - v1[:3]) # Focus on position components + + if delta_v < 1.0: + return 20 # Minimal blend + + t_blend = delta_v / max_accel + t_blend *= 1.5 # safety + + blend_samples = int(t_blend * self.sample_rate) + blend_samples = max(20, min(blend_samples, 200)) + 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, + ) -> np.ndarray: + """ + Solve for quintic polynomial coefficients given boundary conditions. + + Returns: + 6xN array of coefficients [a0, a1, a2, a3, a4, a5] for each dimension + """ + M = np.array( + [ + [1, 0, 0, 0, 0, 0], # p(0) + [1, T, T**2, T**3, T**4, T**5], # p(T) + [0, 1, 0, 0, 0, 0], # v(0) + [0, 1, 2 * T, 3 * T**2, 4 * T**3, 5 * T**4], # v(T) + [0, 0, 2, 0, 0, 0], # a(0) + [0, 0, 2, 6 * T, 12 * T**2, 20 * T**3], # a(T) + ] + ) + + num_dims = len(p0) + coeffs = np.zeros((6, num_dims)) + + for i in range(num_dims): + b = np.array([p0[i], pf[i], v0[i], vf[i], a0[i], af[i]]) + coeffs[:, i] = np.linalg.solve(M, b) + + return coeffs + + 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 + ) + return pos, vel, acc + + def _blend_quintic( + self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int + ) -> np.ndarray: + """ + Generate quintic polynomial blend with C2 continuity. + + This ensures smooth position, velocity, and acceleration transitions. + """ + p0, v0, a0 = self.extract_trajectory_state(traj1, -1) + pf, vf, af = self.extract_trajectory_state(traj2, 0) + + T = blend_samples * self.dt + coeffs = self.solve_quintic_coefficients(p0, pf, v0, vf, a0, af, T) + + blend_traj = [] + for i in range(blend_samples): + t = i * T / (blend_samples - 1) if blend_samples > 1 else 0.0 + pos, _, _ = self.evaluate_quintic(coeffs, t) + blend_traj.append(pos) + + return np.array(blend_traj) + + def _blend_scurve( + self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int + ) -> np.ndarray: + """ + Generate S-curve blend with jerk limiting. + """ + p0, v0, _ = self.extract_trajectory_state(traj1, -1) + pf, vf, _ = self.extract_trajectory_state(traj2, 0) + + try: + from .scurve import MultiAxisSCurveTrajectory + + scurve = MultiAxisSCurveTrajectory( + 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) + + blend_traj = [] + for i in range(len(points["position"])): + 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 + ) + pose[6:] = p0[6:] * (1 - alpha) + pf[6:] * alpha + blend_traj.append(pose) + + return np.array(blend_traj) + 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: + """ + Legacy smoothstep blend for backward compatibility. + """ + p0 = traj1[-1] if len(traj1) > 0 else traj2[0] + pf = traj2[0] if len(traj2) > 0 else traj1[-1] + + blend_traj = [] + for i in range(blend_samples): + t = i / (blend_samples - 1) if blend_samples > 1 else 0.0 + s = t * t * (3 - 2 * t) + pos = p0 * (1 - s) + pf * s + blend_traj.append(pos) + + return np.array(blend_traj) + + def _blend_minimum_jerk( + self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int + ) -> np.ndarray: + """ + Minimum jerk trajectory blend. + + Uses the minimum jerk trajectory equation for smooth motion. + """ + p0, _, _ = self.extract_trajectory_state(traj1, -1) + pf, _, _ = self.extract_trajectory_state(traj2, 0) + + blend_traj = [] + for i in range(blend_samples): + tau = i / (blend_samples - 1) if blend_samples > 1 else 0.0 + pos = p0 + (pf - p0) * (10 * tau**3 - 15 * tau**4 + 6 * tau**5) + blend_traj.append(pos) + + return np.array(blend_traj) + + def _blend_cubic( + self, traj1: np.ndarray, traj2: np.ndarray, blend_samples: int + ) -> np.ndarray: + """ + Cubic spline blend with C1 continuity. + """ + p0, v0, _ = self.extract_trajectory_state(traj1, -1) + pf, vf, _ = self.extract_trajectory_state(traj2, 0) + + T = blend_samples * self.dt + blend_traj = [] + + for i in range(blend_samples): + t = i * T / (blend_samples - 1) if blend_samples > 1 else 0.0 + tau = t / T if T > 0 else 0.0 + + h00 = 2 * tau**3 - 3 * tau**2 + 1 + h10 = tau**3 - 2 * tau**2 + tau + h01 = -2 * tau**3 + 3 * tau**2 + h11 = tau**3 - tau**2 + + pos = h00 * p0 + h10 * T * v0 + h01 * pf + h11 * T * vf + blend_traj.append(pos) + + return np.array(blend_traj) + + def blend_trajectories( + self, + traj1: np.ndarray, + traj2: np.ndarray, + method: str = "quintic", + blend_samples: int | None = None, + auto_size: bool = True, + ) -> np.ndarray: + """ + Blend two trajectory segments with specified method. + + Args: + traj1: First trajectory segment + traj2: Second trajectory segment + method: Blend method ('quintic', 's_curve', 'smoothstep', 'minimum_jerk', 'cubic') + blend_samples: Number of blend samples (auto-calculated if None) + auto_size: Automatically calculate blend region size + + Returns: + Combined trajectory with smooth blend + """ + if len(traj1) == 0 and len(traj2) == 0: + return np.array([]) + if len(traj1) == 0: + return traj2 + if len(traj2) == 0: + return traj1 + + if blend_samples is None or auto_size: + blend_samples = self.calculate_blend_region_size(traj1, traj2) + + blend_samples = max(4, blend_samples) + + if method not in self.blend_methods: + print(f"[WARNING] Unknown blend method '{method}', using quintic") + method = "quintic" + + blend_func = self.blend_methods[method] + blend_traj = blend_func(traj1, traj2, blend_samples) + + overlap_start = max(0, len(traj1) - 1) + overlap_end = min(1, len(traj2)) + + result = np.vstack([traj1[:overlap_start], blend_traj, traj2[overlap_end:]]) + return result diff --git a/parol6/smooth_motion/base.py b/parol6/smooth_motion/base.py new file mode 100644 index 0000000..8e6c9d7 --- /dev/null +++ b/parol6/smooth_motion/base.py @@ -0,0 +1,32 @@ +""" +Base trajectory generator. + +Provides common timing utilities and constraints for derived generators. +""" + +from typing import Any + +import numpy as np + +from .motion_constraints import MotionConstraints + + +class TrajectoryGenerator: + """Base class for trajectory generation with caching support""" + + def __init__(self, control_rate: float = 100.0): + """ + Initialize trajectory generator + + Args: + control_rate: Control loop frequency in Hz + """ + self.control_rate = control_rate + self.dt = 1.0 / control_rate + self.trajectory_cache: dict[str, Any] = {} + self.constraints = MotionConstraints() # Add constraints + + 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 new file mode 100644 index 0000000..10c9a7c --- /dev/null +++ b/parol6/smooth_motion/circle.py @@ -0,0 +1,618 @@ +""" +Circle trajectory generator. +""" + +from collections.abc import Sequence + +import numpy as np +from numpy.typing import NDArray +from scipy.spatial.transform import Rotation, Slerp + +from .base import TrajectoryGenerator + + +class CircularMotion(TrajectoryGenerator): + """Generate circular and arc trajectories in 3D space""" + + def generate_arc_3d( + self, + start_pose: Sequence[float], + end_pose: Sequence[float], + center: Sequence[float] | NDArray, + normal: Sequence[float] | NDArray | None = None, + clockwise: bool = True, + duration: float | np.floating = 2.0, + ) -> np.ndarray: + """ + Generate a 3D circular arc trajectory + + Args: + start_pose: Starting pose [x, y, z, rx, ry, rz] (mm and degrees) + end_pose: Ending pose [x, y, z, rx, ry, rz] (mm and degrees) + center: Center point of arc [x, y, z] (mm) + normal: Normal vector to arc plane (default: z-axis) + clockwise: Direction of rotation + duration: Time to complete arc (seconds) + + Returns: + Array of poses along the arc trajectory + """ + # Convert to numpy arrays + start_pos = np.array(start_pose[:3]) + end_pos = np.array(end_pose[:3]) + center_pt = np.array(center) + + # Arc geometry vectors + r1 = start_pos - center_pt + r2 = end_pos - center_pt + + # Arc plane normal computation + if normal is None: + 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: 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) + r2_norm = r2 / np.linalg.norm(r2) + cos_angle = np.clip(np.dot(r1_norm, r2_norm), -1, 1) + arc_angle = np.arccos(cos_angle) + + # Arc direction validation + cross = np.cross(r1_norm, r2_norm) + if np.dot(cross, normal_np) < 0: + arc_angle = 2 * np.pi - arc_angle + + if clockwise: + arc_angle = -arc_angle + + # Generate trajectory points + timestamps = self.generate_timestamps(duration) + trajectory = [] + + for i, t in enumerate(timestamps): + # Interpolation factor + s = t / duration + + # Exact start position for trajectory begin + if i == 0: + current_pos = start_pos + else: + # Rotate radius vector + angle = s * arc_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( + 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]) + trajectory.append(pose) + + return np.array(trajectory) + + def generate_arc_with_profile( + self, + start_pose: Sequence[float], + end_pose: Sequence[float], + center: Sequence[float] | NDArray, + normal: Sequence[float] | NDArray | None = None, + clockwise: bool = True, + duration: float | np.floating = 2.0, + trajectory_type: str = "cubic", + jerk_limit: float | None = None, + ) -> np.ndarray: + """ + Generate arc trajectory with specified velocity profile. + + This method generates arcs with direct velocity profiles instead of + re-interpolating, ensuring smooth motion without jerking. + + Args: + start_pose: Starting pose [x, y, z, rx, ry, rz] + end_pose: Ending pose [x, y, z, rx, ry, rz] + center: Arc center point [x, y, z] + normal: Normal vector to arc plane + clockwise: Direction of rotation + duration: Time to complete arc (seconds) + trajectory_type: 'cubic', 'quintic', or 's_curve' + jerk_limit: Maximum jerk for s_curve (mm/s³) + + Returns: + Array of poses with velocity profile applied + """ + if trajectory_type == "cubic": + # Use existing cubic implementation + return self.generate_arc_3d( + start_pose, end_pose, center, normal, clockwise, duration + ) + + # Convert to numpy arrays + start_pos = np.array(start_pose[:3]) + end_pos = np.array(end_pose[:3]) + center_pt = np.array(center) + + # Arc geometry + r1 = start_pos - center_pt + r2 = end_pos - center_pt + + # Arc plane normal + if normal is None: + normal = np.cross(r1, r2) + if np.linalg.norm(normal) < 1e-6: + normal = np.array([0, 0, 1]) + 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) + r2_norm = r2 / np.linalg.norm(r2) + cos_angle = np.clip(np.dot(r1_norm, r2_norm), -1, 1) + arc_angle = np.arccos(cos_angle) + + # Determine arc direction + cross = np.cross(r1_norm, r2_norm) + if np.dot(cross, normal_np) < 0: + arc_angle = 2 * np.pi - arc_angle + if clockwise: + arc_angle = -arc_angle + + # Generate trajectory points with profile + num_points = int(duration * self.control_rate) + trajectory = [] + + for i in range(num_points): + # Normalized time [0, 1] + t = i / (num_points - 1) if num_points > 1 else 0.0 + + # Apply velocity profile + if trajectory_type == "quintic": + # Quintic polynomial for smooth acceleration + s = 10 * t**3 - 15 * t**4 + 6 * t**5 + elif trajectory_type == "s_curve": + # S-curve (smoothstep) for jerk-limited motion + if t <= 0.0: + s = 0.0 + elif t >= 1.0: + s = 1.0 + else: + s = t * t * (3.0 - 2.0 * t) + else: + s = t # Linear/cubic fallback + + # Current angle along arc + angle = s * arc_angle + + # Rotation matrix for arc + 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( + 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]) + trajectory.append(pose) + + return np.array(trajectory) + + def generate_circle_3d( + self, + center: Sequence[float] | NDArray, + radius: float, + 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 + """ + timestamps = self.generate_timestamps(duration) + trajectory = [] + + # Circle coordinate system + 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) + + # CRITICAL FIX: Validate and handle geometry + if start_point is not None: + 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_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" + ) + start_angle = 0 + actual_start = center_np + radius * u + else: + # Start point angular position + to_start_normalized = to_start_plane / dist_in_plane + u_comp = np.dot(to_start_normalized, u) + v_comp = np.dot(to_start_normalized, v) + start_angle = np.arctan2(v_comp, u_comp) + + # 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)" + ) + if radius_error > radius * 0.3: # More than 30% off + print( + " WARNING: Large distance from circle - consider using entry trajectory" + ) + + actual_start = start_pos + else: + start_angle = 0 if start_angle is None else start_angle + actual_start = None + + # Generate the circle + for i, t in enumerate(timestamps): + if i == 0 and actual_start is not None: + # First point MUST be exactly the start point + pos = actual_start + else: + # Generate circle points + 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) + orient = [0, 0, 0] + trajectory.append(np.concatenate([pos, orient])) + + return np.array(trajectory) + + def generate_circle_with_profile( + self, + center: Sequence[float] | NDArray, + radius: float, + normal: Sequence[float] | NDArray = [0, 0, 1], + duration: float | np.floating = 4.0, + trajectory_type: str = "cubic", + jerk_limit: float | None = None, + start_angle: float | None = None, + start_point: Sequence[float] | None = None, + ) -> np.ndarray: + """ + Generate circle with specified trajectory profile. + + Args: + center: Center of circle [x, y, z] + radius: Circle radius in mm + normal: Normal vector to circle plane + duration: Time to complete circle (seconds) + trajectory_type: 'cubic', 'quintic', or 's_curve' + jerk_limit: Maximum jerk for s_curve profile (mm/s^3) + start_angle: Starting angle in radians + start_point: Starting position [x, y, z, rx, ry, rz] + + Returns: + Array of waypoints with shape (N, 6) + """ + # Calculate adaptive control rate for small circles + circumference = 2 * np.pi * radius + min_arc_length = 2.0 # Minimum 2mm between points for smoothness + min_points = int(circumference / min_arc_length) + + # Calculate control rate (100-200Hz range) + base_rate = self.control_rate + required_rate = min_points / duration + adaptive_rate = float(min(200, max(base_rate, required_rate))) + + # Temporarily override control rate for small circles + if radius < 30 and adaptive_rate > base_rate: + original_rate = self.control_rate + original_dt = self.dt + 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" + ) + else: + original_rate = None + original_dt = None + + try: + if trajectory_type == "cubic": + # Use existing implementation + 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 + ) + elif trajectory_type == "s_curve": + 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: + # Restore original control rate if we changed it + if original_rate is not None and original_dt is not None: + self.control_rate = original_rate + self.dt = original_dt + + return result + + def generate_quintic_circle( + self, + center: Sequence[float] | NDArray, + radius: float, + 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. + Provides smooth acceleration and deceleration in Cartesian space. + """ + # First generate uniform angular points + num_points = int(duration * self.control_rate) + + # Setup coordinate system + 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_np) * normal_np + dist_in_plane = np.linalg.norm(to_start_plane) + + if dist_in_plane < 0.001: + start_angle = 0 + else: + to_start_normalized = to_start_plane / dist_in_plane + u_comp = np.dot(to_start_normalized, u) + v_comp = np.dot(to_start_normalized, v) + start_angle = np.arctan2(v_comp, u_comp) + + # 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)" + ) + if radius_error > radius * 0.2: # More than 20% off + 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( + 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: + pos = center_np + radius * (np.cos(angle) * u + np.sin(angle) * v) + uniform_positions.append(pos) + + # Step 2: Apply quintic velocity profile to Cartesian motion + trajectory = [] + timestamps = np.linspace(0, duration, num_points) + + for i, t in enumerate(timestamps): + tau = t / duration # Normalized time [0, 1] + + # Quintic profile for smooth acceleration/deceleration + # Applied to arc length parameterization, not angular + s_normalized = 10 * tau**3 - 15 * tau**4 + 6 * tau**5 + + # Map to position index + position_index = s_normalized * (num_points - 1) + + # Interpolate between positions + if position_index <= 0: + pos = uniform_positions[0] + elif position_index >= num_points - 1: + pos = uniform_positions[-1] + else: + lower_idx = int(position_index) + upper_idx = min(lower_idx + 1, num_points - 1) + alpha = position_index - lower_idx + pos = uniform_positions[lower_idx] + alpha * ( + uniform_positions[upper_idx] - uniform_positions[lower_idx] + ) + + # Placeholder orientation + orient = [0, 0, 0] + trajectory.append(np.concatenate([pos, orient])) + + return np.array(trajectory) + + def generate_scurve_circle( + self, + center: Sequence[float] | NDArray, + radius: float, + 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. + Provides jerk-limited motion in Cartesian space for maximum smoothness. + """ + if jerk_limit is None: + jerk_limit = 5000.0 # Default jerk limit in mm/s^3 + + # Generate timestamps at control rate + num_points = int(duration * self.control_rate) + + # Setup coordinate system + 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_np) * normal_np + dist_in_plane = np.linalg.norm(to_start_plane) + + if dist_in_plane < 0.001: + start_angle = 0 + else: + to_start_normalized = to_start_plane / dist_in_plane + u_comp = np.dot(to_start_normalized, u) + v_comp = np.dot(to_start_normalized, v) + start_angle = np.arctan2(v_comp, u_comp) + + # 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)" + ) + if radius_error > radius * 0.2: # More than 20% off + 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( + 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: + pos = center_np + radius * (np.cos(angle) * u + np.sin(angle) * v) + uniform_positions.append(pos) + + # Step 2: Apply S-curve velocity profile to Cartesian motion + trajectory = [] + timestamps = np.linspace(0, duration, num_points) + + for i, t in enumerate(timestamps): + tau = t / duration # Normalized time [0, 1] + + # S-curve (smoothstep) profile for smooth acceleration + # Applied to arc length, not angle + if tau <= 0.0: + s_normalized = 0.0 + elif tau >= 1.0: + s_normalized = 1.0 + else: + # Smoothstep: 3t² - 2t³ for smooth acceleration and deceleration + s_normalized = tau * tau * (3.0 - 2.0 * tau) + + # Map to position index + position_index = s_normalized * (num_points - 1) + + # Interpolate between positions + if position_index <= 0: + pos = uniform_positions[0] + elif position_index >= num_points - 1: + pos = uniform_positions[-1] + else: + lower_idx = int(position_index) + upper_idx = min(lower_idx + 1, num_points - 1) + alpha = position_index - lower_idx + pos = uniform_positions[lower_idx] + alpha * ( + uniform_positions[upper_idx] - uniform_positions[lower_idx] + ) + + # Placeholder orientation + orient = [0, 0, 0] + trajectory.append(np.concatenate([pos, orient])) + + return np.array(trajectory) + + 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]] + ) + + # Rodrigues' formula + R = np.eye(3) + sin_a * K + (1 - cos_a) * K @ K + return R + + def _get_perpendicular_vector(self, v: np.ndarray) -> np.ndarray: + """Find a vector perpendicular to the given vector""" + v = np.array(v, dtype=float) # Ensure it's a numpy array + if abs(v[0]) < 0.9: + cross = np.cross(v, [1, 0, 0]) + return cross / np.linalg.norm(cross) + else: + cross = np.cross(v, [0, 1, 0]) + return cross / np.linalg.norm(cross) + + def _slerp_orientation( + self, + start_orient: NDArray[np.floating], + end_orient: NDArray[np.floating], + t: float, + ) -> np.ndarray: + """Spherical linear interpolation for orientation""" + # Convert to quaternions + r1 = Rotation.from_euler("xyz", start_orient, degrees=True) + r2 = Rotation.from_euler("xyz", end_orient, degrees=True) + + # Quaternion interpolation setup + 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 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 new file mode 100644 index 0000000..7003986 --- /dev/null +++ b/parol6/smooth_motion/helix.py @@ -0,0 +1,295 @@ +""" +Helix trajectory generator. +""" + +from collections.abc import Sequence + +import numpy as np +from numpy.typing import NDArray + +from .base import TrajectoryGenerator + + +class HelixMotion(TrajectoryGenerator): + """Generate helical trajectories with various velocity profiles""" + + def _get_perpendicular_vector(self, v: np.ndarray) -> np.ndarray: + """Find a vector perpendicular to the given vector""" + v = np.array(v, dtype=float) # Ensure it's a numpy array + if abs(v[0]) < 0.9: + return np.cross(v, [1, 0, 0]) / np.linalg.norm(np.cross(v, [1, 0, 0])) + else: + return np.cross(v, [0, 1, 0]) / np.linalg.norm(np.cross(v, [0, 1, 0])) + + def generate_helix_with_profile( + self, + center: Sequence[float] | NDArray, + radius: float, + pitch: float, + height: float, + axis: Sequence[float] | NDArray = [0, 0, 1], + duration: float | np.floating = 4.0, + trajectory_type: str = "cubic", + jerk_limit: float | None = None, + start_point: Sequence[float] | None = None, + clockwise: bool = False, + ) -> np.ndarray: + """ + Generate helix with specified trajectory profile. + + Args: + center: Helix center point [x, y, z] + radius: Helix radius in mm + pitch: Vertical distance per revolution in mm + height: Total height of helix in mm + axis: Helix axis vector (normalized internally) + duration: Time to complete helix in seconds + trajectory_type: 'cubic', 'quintic', or 's_curve' + jerk_limit: Maximum jerk for s_curve profile + start_point: Starting position [x, y, z, rx, ry, rz] + clockwise: Rotation direction + + Returns: + Array of waypoints with shape (N, 6) for [x, y, z, rx, ry, rz] + """ + if trajectory_type == "cubic": + return self.generate_cubic_helix( + center, radius, pitch, height, axis, duration, start_point, clockwise + ) + if trajectory_type == "quintic": + return self.generate_quintic_helix( + center, radius, pitch, height, axis, duration, start_point, clockwise + ) + if trajectory_type == "s_curve": + return self.generate_scurve_helix( + 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: Sequence[float] | NDArray, + radius: float, + pitch: float, + height: float, + axis: Sequence[float] | NDArray = [0, 0, 1], + duration: float | np.floating = 4.0, + start_point: Sequence[float] | None = None, + clockwise: bool = False, + ) -> np.ndarray: + """ + Generate helix with cubic (linear) interpolation. + This is the simplest profile with constant angular velocity. + """ + # Calculate number of revolutions + num_revolutions = height / pitch if pitch > 0 else 1 + total_angle = 2 * np.pi * num_revolutions + + # Setup coordinate system + 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 + start_angle = 0.0 + 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_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) + ) + + # Generate trajectory points + num_points = int(duration * self.control_rate) + timestamps = np.linspace(0, duration, num_points) + trajectory = [] + + for t in timestamps: + # Linear interpolation for simple cubic profile + progress = t / duration + + # Angular position (constant velocity) + if clockwise: + theta = start_angle - total_angle * progress + else: + theta = start_angle + total_angle * progress + + # Vertical position (constant velocity) + z_offset = height * progress + + # Calculate 3D position + 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] + + trajectory.append(np.concatenate([pos, orient])) + + return np.array(trajectory) + + def generate_quintic_helix( + self, + center: Sequence[float] | NDArray, + radius: float, + pitch: float, + height: float, + axis: Sequence[float] | NDArray = [0, 0, 1], + duration: float | np.floating = 4.0, + start_point: Sequence[float] | None = None, + clockwise: bool = False, + ) -> np.ndarray: + """ + Generate helix with quintic polynomial profile. + Provides smooth acceleration and deceleration. + """ + # Calculate total angle + num_revolutions = height / pitch if pitch > 0 else 1 + total_angle = 2 * np.pi * num_revolutions + + # Setup coordinate system + 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 + start_angle = 0.0 + 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_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) + ) + + # Generate trajectory with quintic profile + num_points = int(duration * self.control_rate) + timestamps = np.linspace(0, duration, num_points) + trajectory = [] + + for t in timestamps: + # Quintic polynomial: 10τ³ - 15τ⁴ + 6τ⁵ + tau = t / duration + progress = 10 * tau**3 - 15 * tau**4 + 6 * tau**5 + + # Apply to both angular and vertical motion + if clockwise: + theta = start_angle - total_angle * progress + else: + theta = start_angle + total_angle * progress + + z_offset = height * progress + + # Calculate position + 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] + + trajectory.append(np.concatenate([pos, orient])) + + return np.array(trajectory) + + def generate_scurve_helix( + self, + center: Sequence[float] | NDArray, + radius: float, + pitch: float, + height: float, + 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: + """ + Generate helix with S-curve (smoothstep) profile. + Provides jerk-limited motion. + """ + # Calculate total angle + num_revolutions = height / pitch if pitch > 0 else 1 + total_angle = 2 * np.pi * num_revolutions + + # Setup coordinate system + 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 + start_angle = 0.0 + 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_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) + ) + + # Generate trajectory with S-curve profile + num_points = int(duration * self.control_rate) + timestamps = np.linspace(0, duration, num_points) + trajectory = [] + + for t in timestamps: + # S-curve (smoothstep): 3τ² - 2τ³ + tau = t / duration + if tau <= 0.0: + progress = 0.0 + elif tau >= 1.0: + progress = 1.0 + else: + progress = tau * tau * (3.0 - 2.0 * tau) + + # Apply to motion + if clockwise: + theta = start_angle - total_angle * progress + else: + theta = start_angle + total_angle * progress + + z_offset = height * progress + + # Calculate position + 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] + + trajectory.append(np.concatenate([pos, orient])) + + return np.array(trajectory) diff --git a/parol6/smooth_motion/motion_blender.py b/parol6/smooth_motion/motion_blender.py new file mode 100644 index 0000000..0900877 --- /dev/null +++ b/parol6/smooth_motion/motion_blender.py @@ -0,0 +1,65 @@ +""" +Motion blending utilities. +""" + +from __future__ import annotations + +import numpy as np +from scipy.spatial.transform import Rotation, Slerp + + +class MotionBlender: + """Blend between different motion segments for smooth transitions""" + + 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: + """Blend two trajectory segments with improved velocity continuity""" + + if blend_samples < 4: + return np.vstack([traj1, traj2]) + + # Use more samples for smoother blending + blend_samples = max(blend_samples, 20) # Minimum 20 samples for smooth blend + + # Trajectory overlap region analysis + overlap_start = max(0, len(traj1) - blend_samples // 3) + 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_end_pose = traj2[overlap_end] if overlap_end < len(traj2) else traj2[0] + + # Generate smooth transition using S-curve + blended: list[np.ndarray] = [] + for i in range(blend_samples): + t = i / (blend_samples - 1) + # Use smoothstep function for smoother acceleration + s = t * t * (3 - 2 * t) # Smoothstep + + # Blend position + pos_blend = blend_start_pose * (1 - s) + blend_end_pose * s + + # 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:]] + ) + + return result diff --git a/parol6/smooth_motion/motion_constraints.py b/parol6/smooth_motion/motion_constraints.py new file mode 100644 index 0000000..aa0c661 --- /dev/null +++ b/parol6/smooth_motion/motion_constraints.py @@ -0,0 +1,108 @@ +""" +Motion constraints for PAROL6 robot. + +Defines per-joint limits for velocity, acceleration, and jerk +based on gear ratios and mechanical properties. +""" + +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 = [float(x) for x in PAROL6_ROBOT.joint.ratio.tolist()] + + # Use jerk limits from PAROL6_ROBOT.py + self.max_jerk = [float(x) for x in PAROL6_ROBOT.joint.jerk.max.tolist()] + + # Use maximum velocities from PAROL6_ROBOT.py + 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²) + # 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], + } + + 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, + } + + return scaled + + 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, + } + + # Calculate derivatives numerically + velocities = np.diff(trajectory) / dt + accelerations = np.diff(velocities) / dt + jerks = np.diff(accelerations) / dt + + 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 new file mode 100644 index 0000000..6ef76b0 --- /dev/null +++ b/parol6/smooth_motion/quintic.py @@ -0,0 +1,392 @@ +""" +Quintic polynomial primitives and multi-axis quintic trajectory. +""" + +from typing import Any, Optional + +import numpy as np + +from .motion_constraints import MotionConstraints + + +class QuinticPolynomial: + """ + Single-axis quintic polynomial trajectory primitive. + + Provides C² continuous trajectories (continuous position, velocity, acceleration) + with zero jerk at boundaries. This is the building block for S-curve profiles + and advanced multi-segment trajectories. + """ + + def __init__( + self, + q0: float, + qf: float, + v0: float = 0, + vf: float = 0, + a0: float = 0, + af: float = 0, + T: float = 1.0, + ): + """ + Generate quintic polynomial trajectory. + + Args: + q0: Initial position + qf: Final position + v0: Initial velocity (default 0) + vf: Final velocity (default 0) + a0: Initial acceleration (default 0) + af: Final acceleration (default 0) + T: Duration of trajectory (must be > 0) + """ + if T <= 0: + raise ValueError(f"Duration must be positive, got T={T}") + + self.T = T + self.q0 = q0 + self.qf = qf + + # Store boundary conditions + 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) + + # Pre-compute coefficient derivatives for faster evaluation + self._prepare_derivative_coeffs() + + def _solve_coefficients_analytical(self, q0, qf, v0, vf, a0, af, T): + """ + Analytical solution for quintic polynomial coefficients. + + Uses closed-form solution to avoid numerical issues with matrix inversion. + Works in normalized time [0,1] then scales back to actual time. + + The quintic polynomial is: q(t) = a0 + a1*t + a2*t² + a3*t³ + a4*t⁴ + a5*t⁵ + + Returns: + numpy array of coefficients [a0, a1, a2, a3, a4, a5] + """ + # Scale boundary conditions for normalized time τ = t/T + v0_norm = v0 * T + vf_norm = vf * T + a0_norm = a0 * T**2 + af_norm = af * T**2 + + # Coefficients in normalized time domain + a0_ = q0 + a1_ = v0_norm + 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 + ) + 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] + ) + 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.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.""" + if t < 0: + return self.q0 + if t > self.T: + return self.qf + result = self.coeffs[5] + for i in range(4, -1, -1): + result = result * t + self.coeffs[i] + return result + + def velocity(self, t: float) -> float: + """Evaluate velocity at time t using Horner's method.""" + if t < 0: + return self.boundary_conditions["v0"] + if t > self.T: + return self.boundary_conditions["vf"] + result = self.vel_coeffs[4] if len(self.vel_coeffs) > 4 else 0 + for i in range(min(3, len(self.vel_coeffs) - 1), -1, -1): + result = result * t + self.vel_coeffs[i] + return result + + def acceleration(self, t: float) -> float: + """Evaluate acceleration at time t using Horner's method.""" + if t < 0: + return self.boundary_conditions["a0"] + if t > self.T: + return self.boundary_conditions["af"] + result = self.acc_coeffs[3] if len(self.acc_coeffs) > 3 else 0 + for i in range(min(2, len(self.acc_coeffs) - 1), -1, -1): + result = result * t + self.acc_coeffs[i] + return result + + def jerk(self, t: float) -> float: + """Evaluate jerk at time t using Horner's method.""" + if t < 0 or t > self.T: + return 0 + result = self.jerk_coeffs[2] if len(self.jerk_coeffs) > 2 else 0 + for i in range(min(1, len(self.jerk_coeffs) - 1), -1, -1): + result = result * t + self.jerk_coeffs[i] + return result + + def evaluate(self, t: float, derivative: int = 0) -> float: + """ + Unified evaluation function for any derivative order. + + Args: + t: Time point to evaluate + derivative: 0=position, 1=velocity, 2=acceleration, 3=jerk + """ + if derivative == 0: + return self.position(t) + if derivative == 1: + return self.velocity(t) + if derivative == 2: + return self.acceleration(t) + if derivative == 3: + 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]: + """ + Generate trajectory points at specified time interval. + + Args: + dt: Time step + """ + time_points = np.arange(0, self.T + dt, dt) + trajectory = { + "time": time_points, + "position": np.array([self.position(t) for t in time_points]), + "velocity": np.array([self.velocity(t) for t in time_points]), + "acceleration": np.array([self.acceleration(t) for t in time_points]), + "jerk": np.array([self.jerk(t) for t in time_points]), + } + return trajectory + + 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, + "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, + } + return validation + + def validate_numerical_stability(self) -> dict[str, Any]: + """ + Check for potential numerical stability issues. + """ + 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 + metrics["time_distance_ratio"] = time_distance_ratio + if time_distance_ratio > 100: + 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] + max_coeff = max(coeff_magnitudes) + nz = [m for m in coeff_magnitudes if m > 1e-10] + min_nonzero_coeff = min(nz) if nz else 0.0 + + if min_nonzero_coeff > 0: + coeff_ratio = max_coeff / min_nonzero_coeff + metrics["coefficient_ratio"] = coeff_ratio + if coeff_ratio > 1e6: + warnings.append(f"Large coefficient ratio: {coeff_ratio:.2e}") + + if self.T < 0.001: + 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: + warnings.append(f"Very large jerk values: {max_jerk:.2e}") + + return {"is_stable": is_stable, "warnings": warnings, "metrics": metrics} + + +class MultiAxisQuinticTrajectory: + """ + Multi-axis synchronized quintic trajectory generator. + + Ensures all axes complete their motion simultaneously using a + time-scaling approach. + """ + + def __init__( + self, + 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, + ): + """ + Generate synchronized quintic trajectories for multiple axes. + + Args: + q0: Initial positions for each axis + qf: Final positions for each axis + v0: Initial velocities (default zeros) + vf: Final velocities (default zeros) + a0: Initial accelerations (default zeros) + af: Final accelerations (default zeros) + T: Duration (if None, calculated from constraints) + constraints: Motion constraints for time calculation + """ + self.num_axes = len(q0) + + # 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] = [] + for i in range(self.num_axes): + 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: list[float], + qf: list[float], + v0: list[float], + vf: list[float], + constraints: Optional["MotionConstraints"], + ) -> float: + """ + Calculate minimum time based on velocity and acceleration constraints. + """ + if constraints is None: + # Default time based on distance + 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] = [] + for i in range(self.num_axes): + distance = abs(qf[i] - q0[i]) + + # Time based on velocity limit + if constraints.max_velocity and i < len(constraints.max_velocity): + t_vel = distance / constraints.max_velocity[i] + min_times.append(t_vel) + + # Time based on acceleration limit (triangular profile) + if constraints.max_acceleration and i < len(constraints.max_acceleration): + t_acc = 2 * np.sqrt(distance / constraints.max_acceleration[i]) + min_times.append(t_acc) + + # 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]]: + """ + Evaluate all axes at time t. + """ + 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)) + result["acceleration"].append(traj.acceleration(t)) + result["jerk"].append(traj.jerk(t)) + return result + + 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) + + positions = np.zeros((num_points, self.num_axes)) + velocities = np.zeros((num_points, self.num_axes)) + accelerations = np.zeros((num_points, self.num_axes)) + jerks = np.zeros((num_points, self.num_axes)) + + for i, t in enumerate(time_points): + values = self.evaluate_all(t) + positions[i] = values["position"] + velocities[i] = values["velocity"] + accelerations[i] = values["acceleration"] + jerks[i] = values["jerk"] + + return { + "time": time_points, + "position": positions, + "velocity": velocities, + "acceleration": accelerations, + "jerk": jerks, + } diff --git a/parol6/smooth_motion/scurve.py b/parol6/smooth_motion/scurve.py new file mode 100644 index 0000000..cbc6b04 --- /dev/null +++ b/parol6/smooth_motion/scurve.py @@ -0,0 +1,729 @@ +""" +S-curve profile generator and multi-axis S-curve trajectory. +""" + +from typing import TYPE_CHECKING, TypedDict + +import numpy as np + +if TYPE_CHECKING: + # For type checkers and to satisfy linters referencing QuinticPolynomial in annotations + from .quintic import QuinticPolynomial + +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. + + Creates jerk-limited trajectories with smooth acceleration transitions. + The seven segments are: + 1. Acceleration buildup (constant positive jerk) + 2. Constant acceleration + 3. Acceleration ramp-down (constant negative jerk) + 4. Constant velocity (cruise) + 5. Deceleration buildup (constant negative jerk) + 6. Constant deceleration + 7. Deceleration ramp-down (constant positive jerk) + """ + + def __init__( + self, + distance: float, + v_max: float, + a_max: float, + j_max: float, + v_start: float = 0, + v_end: float = 0, + ): + """ + Calculate S-curve profile for given motion parameters. + + Args: + distance: Total distance to travel + v_max: Maximum velocity constraint + a_max: Maximum acceleration constraint + j_max: Maximum jerk constraint + v_start: Initial velocity (default 0) + v_end: Final velocity (default 0) + """ + self.distance = distance + self.v_max = float(v_max) + self.a_max = float(a_max) + self.j_max = float(j_max) + 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() + + # Calculate profile type and segment durations + self.profile_type, self.segments = self._calculate_profile() + + # Pre-calculate segment boundaries for proper evaluation + self._calculate_segment_boundaries() + + def _calculate_profile(self) -> tuple[str, dict[str, float]]: + """ + Calculate the S-curve profile type and segment durations. + + Returns: + profile_type: 'full', 'triangular', 'reduced', or 'asymmetric' + segments: Dictionary with segment durations + """ + # For symmetric profiles with v_start = v_end = 0 + if self.v_start == 0 and self.v_end == 0: + return self._calculate_symmetric_profile() + # Asymmetric profiles for non-zero boundary velocities + return self._calculate_asymmetric_profile() + + 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 + + # Distance covered during jerk phases + 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") + ) + + 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 + ) + 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_j2": T_j_actual, # Jerk down + "T_v": 0.0, # No cruise + "T_j3": T_j_actual, # Jerk down (decel) + "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, + } + elif self.distance < 2 * d_to_vmax: + # 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 + ) + + segments = { + "T_j1": T_j, + "T_a": T_a, + "T_j2": T_j, + "T_v": 0.0, # No cruise phase + "T_j3": T_j, + "T_d": T_a, + "T_j4": T_j, + "v_reached": v_reached, + } + else: + # Case 3: Full S-curve with cruise phase + 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 + ) + + # 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 + ) + + # Distance at cruise velocity + d_cruise = self.distance - 2 * d_accel + + # Time at cruise velocity + T_v = d_cruise / self.v_max if self.v_max > 0 else 0.0 + + segments = { + "T_j1": T_j, + "T_a": max(T_a, 0.0), + "T_j2": T_j, + "T_v": max(T_v, 0.0), + "T_j3": T_j, + "T_d": max(T_a, 0.0), + "T_j4": T_j, + "v_reached": self.v_max, + } + + # Calculate total time + total_time = ( + segments.get("T_j1", 0.0) + + segments.get("T_a", 0.0) + + segments.get("T_j2", 0.0) + + segments.get("T_v", 0.0) + + segments.get("T_j3", 0.0) + + segments.get("T_d", 0.0) + + segments.get("T_j4", 0.0) + ) + segments["T_total"] = total_time + + return profile_type, segments + + 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 + + # Time to change between velocities at max acceleration + T_vel_change = v_diff / self.a_max if self.a_max > 0 else 0.0 + + # Jerk time (time to reach max acceleration) + 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)) + ) + + 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) + + 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_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 + + if d_cruise > 0 and v_peak > 0: + 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, + ) + ) + else: + # Simple case - just decelerate + T_a = 0.0 + T_v = 0.0 + T_d = T_vel_change + v_peak = max(self.v_start, self.v_end) + + segments = { + "T_j1": T_j, + "T_a": T_a, + "T_j2": T_j, + "T_v": T_v, + "T_j3": T_j, + "T_d": T_d, + "T_j4": T_j, + "v_reached": v_peak, + "T_total": 2 * T_j + T_a + T_v + 2 * T_j + T_d, + } + return "asymmetric", segments + + def _check_feasibility(self) -> FeasibilityInfo: + """ + Check if S-curve profile is achievable with given constraints. + + Returns: + Dictionary with feasibility status and adjusted parameters + """ + # Minimum distance to reach maximum acceleration + d_min_jerk = (self.a_max**3) / (self.j_max**2) if self.j_max > 0 else 0.0 + + # Minimum distance to reach maximum velocity + d_min_vel = ( + (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") + ) + + 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 + ) + 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 + ) + feasibility["status"] = "triangular_velocity" + feasibility["achievable_v"] = achievable_v + feasibility["warnings"].append( + f"Distance too short to reach full velocity. Max achievable: {achievable_v:.2f}" + ) + + if self.distance > 0 and self.a_max > 0: + time_estimate = 2 * np.sqrt(self.distance / self.a_max) + if time_estimate > 100: + feasibility["warnings"].append( + f"Very long trajectory time ({time_estimate:.1f}s) may cause numerical issues" + ) + + 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" + ) + + return feasibility + + def _calculate_segment_boundaries(self) -> None: + """ + Pre-calculate position, velocity, and acceleration at segment boundaries. + This ensures proper continuity across segments. + """ + boundary_list: list[dict[str, float]] = [] + + # Initial state + pos = 0.0 + vel = self.v_start + acc = 0.0 + + # Segment 1: Jerk up (acceleration buildup) + T_j1 = self.segments["T_j1"] + if T_j1 > 0: + j = self.j_max + 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 + boundary_list.append( + { + "pos_start": pos, + "vel_start": vel, + "acc_start": acc, + "pos_end": pos_end, + "vel_end": vel_end, + "acc_end": acc_end, + "jerk": j, + "duration": T_j1, + } + ) + pos, vel, acc = pos_end, vel_end, acc_end + + # Segment 2: Constant acceleration + T_a = self.segments["T_a"] + if T_a > 0: + j = 0.0 + acc_end = acc + vel_end = vel + acc * T_a + pos_end = pos + vel * T_a + 0.5 * acc * T_a**2 + boundary_list.append( + { + "pos_start": pos, + "vel_start": vel, + "acc_start": acc, + "pos_end": pos_end, + "vel_end": vel_end, + "acc_end": acc_end, + "jerk": j, + "duration": T_a, + } + ) + pos, vel, acc = pos_end, vel_end, acc_end + + # Segment 3: Jerk down (acceleration ramp-down) + T_j2 = self.segments["T_j2"] + if T_j2 > 0: + j = -self.j_max + 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 + boundary_list.append( + { + "pos_start": pos, + "vel_start": vel, + "acc_start": acc, + "pos_end": pos_end, + "vel_end": vel_end, + "acc_end": acc_end, + "jerk": j, + "duration": T_j2, + } + ) + pos, vel, acc = pos_end, vel_end, acc_end + + # Segment 4: Constant velocity (cruise) + T_v = self.segments["T_v"] + if T_v > 0: + j = 0.0 + acc_end = 0.0 + vel_end = vel + pos_end = pos + vel * T_v + boundary_list.append( + { + "pos_start": pos, + "vel_start": vel, + "acc_start": acc, + "pos_end": pos_end, + "vel_end": vel_end, + "acc_end": acc_end, + "jerk": j, + "duration": T_v, + } + ) + pos, vel, acc = pos_end, vel_end, acc_end + + # Segment 5: Jerk down (deceleration buildup) + T_j3 = self.segments["T_j3"] + if T_j3 > 0: + j = -self.j_max + 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 + boundary_list.append( + { + "pos_start": pos, + "vel_start": vel, + "acc_start": acc, + "pos_end": pos_end, + "vel_end": vel_end, + "acc_end": acc_end, + "jerk": j, + "duration": T_j3, + } + ) + pos, vel, acc = pos_end, vel_end, acc_end + + # Segment 6: Constant deceleration + T_d = self.segments["T_d"] + if T_d > 0: + j = 0.0 + acc_end = acc + vel_end = vel + acc * T_d + pos_end = pos + vel * T_d + 0.5 * acc * T_d**2 + boundary_list.append( + { + "pos_start": pos, + "vel_start": vel, + "acc_start": acc, + "pos_end": pos_end, + "vel_end": vel_end, + "acc_end": acc_end, + "jerk": j, + "duration": T_d, + } + ) + pos, vel, acc = pos_end, vel_end, acc_end + + # Segment 7: Jerk up (deceleration ramp-down) + T_j4 = self.segments["T_j4"] + if T_j4 > 0: + j = self.j_max + 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 + boundary_list.append( + { + "pos_start": pos, + "vel_start": vel, + "acc_start": acc, + "pos_end": pos_end, + "vel_end": vel_end, + "acc_end": acc_end, + "jerk": j, + "duration": T_j4, + } + ) + pos, vel, acc = pos_end, vel_end, acc_end + # finalize + self.segment_boundaries = boundary_list + + def generate_trajectory_quintics(self) -> list["QuinticPolynomial"]: + """ + Generate quintic polynomials for each segment of the S-curve. + + Returns: + List of QuinticPolynomial objects representing each segment + """ + from .quintic import QuinticPolynomial # Local import to avoid cycle + + quintics: list[QuinticPolynomial] = [] + for seg in self.segment_boundaries: + if seg["duration"] > 0: + q = QuinticPolynomial( + seg["pos_start"], + seg["pos_end"], + seg["vel_start"], + seg["vel_end"], + seg["acc_start"], + seg["acc_end"], + seg["duration"], + ) + quintics.append(q) + + return quintics + + 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]: + """ + Evaluate S-curve at specific time. + + Returns: + Dictionary with position, velocity, acceleration, jerk + """ + if t <= 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, + } + + # Find which segment we're in + cumulative_time = 0.0 + for seg in self.segment_boundaries: + if t <= cumulative_time + seg["duration"]: + t_in_segment = t - cumulative_time + return self._evaluate_in_segment(seg, t_in_segment) + cumulative_time += seg["duration"] + + # Fallback + 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]: + """ + Evaluate motion within a specific segment using proper kinematics. + """ + p0 = segment["pos_start"] + v0 = segment["vel_start"] + a0 = segment["acc_start"] + j = segment["jerk"] + + # Kinematics within segment + acc = a0 + j * t + vel = v0 + a0 * t + 0.5 * j * t**2 + pos = p0 + v0 * t + 0.5 * a0 * t**2 + (1 / 6) * j * t**3 + + return {"position": pos, "velocity": vel, "acceleration": acc, "jerk": j} + + +class MultiAxisSCurveTrajectory: + """ + Multi-axis synchronized S-curve trajectory generator. + + Coordinates multiple S-curve profiles (one per axis) and synchronizes them + to ensure all axes complete their motion simultaneously while respecting + individual axis constraints. + """ + + def __init__( + self, + 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. + + Args: + start_pose: Starting position for each axis + end_pose: Target position for each axis + v0: Initial velocities (defaults to zeros) + vf: Final velocities (defaults to zeros) + T: Desired duration (if None, calculates minimum time) + jerk_limit: Maximum jerk limit to apply to all axes + """ + self.start_pose = np.array(start_pose, dtype=float) + 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.constraints = MotionConstraints() + + self.axis_profiles: list[SCurveProfile | None] = [] + self.max_time = 0.0 + + for i in range(self.num_axes): + distance = self.end_pose[i] - self.start_pose[i] + + if abs(distance) < 1e-6: + self.axis_profiles.append(None) + continue + + 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, + } + + j_max = jerk_limit if jerk_limit is not None else joint_constraints["j_max"] + + axis_profile = SCurveProfile( + float(distance), + float(joint_constraints["v_max"]), + float(joint_constraints["a_max"]), + float(j_max), + v_start=float(self.v0[i]), + v_end=float(self.vf[i]), + ) + 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 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]: + """ + Generate synchronized trajectory points for all axes. + + Args: + dt: Time step for sampling + """ + num_points = int(self.T / dt) + 1 if self.T > 0 else 1 + timestamps = np.linspace(0, self.T, num_points) + + positions = np.zeros((num_points, self.num_axes)) + velocities = np.zeros((num_points, self.num_axes)) + accelerations = np.zeros((num_points, self.num_axes)) + + for idx, t in enumerate(timestamps): + for axis in range(self.num_axes): + 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 = 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, + } + + def evaluate_at_time(self, t: float) -> dict[str, np.ndarray]: + """ + Evaluate trajectory at specific time. + + Args: + t: Time point to evaluate + """ + position = np.zeros(self.num_axes) + velocity = np.zeros(self.num_axes) + acceleration = np.zeros(self.num_axes) + + for axis in range(self.num_axes): + 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], 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, + } diff --git a/parol6/smooth_motion/spline.py b/parol6/smooth_motion/spline.py new file mode 100644 index 0000000..b26979d --- /dev/null +++ b/parol6/smooth_motion/spline.py @@ -0,0 +1,356 @@ +""" +Spline trajectory generator. +""" + +from __future__ import annotations + +from typing import Any + +import numpy as np +from scipy.interpolate import CubicSpline +from scipy.spatial.transform import Rotation, Slerp + +from .base import TrajectoryGenerator +from .quintic import MultiAxisQuinticTrajectory +from .scurve import SCurveProfile + + +class SplineMotion(TrajectoryGenerator): + """Generate smooth spline trajectories through waypoints""" + + def generate_quintic_spline_true( + self, + waypoints: list[list[float]], + waypoint_behavior: str = "stop", + profile_type: str = "s_curve", + optimization: str = "jerk", + time_optimal: bool = False, + jerk_limit: float | None = None, + ) -> np.ndarray: + """ + Generate true quintic spline trajectory with optional S-curve profiles. + + This is the enhanced version using our quintic polynomial implementation + instead of the cubic-based version. + + Args: + waypoints: List of poses [x, y, z, rx, ry, rz] + waypoint_behavior: 'stop' or 'continuous' at waypoints + profile_type: 's_curve', 'quintic', or 'cubic' + optimization: 'time', 'jerk', or 'energy' + time_optimal: Calculate minimum time if True + """ + if profile_type == "s_curve": + 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 + return self.generate_cubic_spline(waypoints) + + def _generate_pure_quintic_waypoints(self, waypoints, behavior): + """Generate quintic trajectories between waypoints.""" + waypoints = np.array(waypoints) + num_waypoints = len(waypoints) + + if num_waypoints < 2: + return waypoints + + # Calculate timing for each segment + segment_times = [] + for i in range(num_waypoints - 1): + distance = np.linalg.norm(waypoints[i + 1, :3] - waypoints[i, :3]) + time = float(max(1.0, float(distance) / 50.0)) # 50 mm/s average + segment_times.append(time) + + # Generate trajectory segments + full_trajectory: list[list[float]] = [] + prev_vf: list[float] | None = None + + for i in range(num_waypoints - 1): + start_pose = waypoints[i] + end_pose = waypoints[i + 1] + T = segment_times[i] + + # Determine boundary velocities based on behavior + if behavior == "stop": + v0 = [0.0] * 6 + vf = [0.0] * 6 + else: # continuous + if i == 0: + v0 = [0.0] * 6 + else: + v0 = prev_vf if prev_vf is not None else [0.0] * 6 + + if i == num_waypoints - 2: + 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] + ) + current_direction = waypoints[i + 1] - waypoints[i] + 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 + ) + + # 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"].tolist()) + else: + full_trajectory.extend(segment_points["position"][1:].tolist()) + + return np.array(full_trajectory) + + 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) + + if num_waypoints < 2: + return waypoints + + full_trajectory: list[list[float]] = [] + + for i in range(num_waypoints - 1): + start_pose = waypoints[i] + end_pose = waypoints[i + 1] + + # For each joint, calculate S-curve profile + segment_trajectories = [] + max_time = 0.0 + + for j in range(6): # 6 joints + distance = end_pose[j] - start_pose[j] + + # Get joint constraints + 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 + ) + + max_time = max(max_time, scurve.get_total_time()) + segment_trajectories.append(scurve) + + # Synchronize all joints to slowest + if optimization == "time": + sync_time = max_time + else: + sync_time = max_time * 1.2 # margin + + # Generate synchronized points + timestamps = self.generate_timestamps(sync_time) + + for t in timestamps: + pose = [] + for j in range(6): + joint_scurve = segment_trajectories[j] + t_normalized = t / sync_time # Normalize to [0, 1] + t_joint = t_normalized * joint_scurve.get_total_time() + + values = joint_scurve.evaluate_at_time(float(t_joint)) + pose.append(float(start_pose[j] + values["position"])) + full_trajectory.append(pose) + + return np.array(full_trajectory) + + def generate_cubic_spline( + self, + 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 + + Args: + waypoints: List of poses [x, y, z, rx, ry, rz] + timestamps: Time for each waypoint (auto-generated if None) + velocity_start: Initial velocity (zero if None) + velocity_end: Final velocity (zero if None) + + Returns: + Array of interpolated poses + """ + 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_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_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_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): + # Provide boundary conditions per component + bc: Any + if velocity_start is not None and velocity_end is not None: + 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 + ] + quats = np.array([r.as_quat() for r in rotations]) + key_rots = Rotation.from_quat(quats) + slerp = Slerp(timestamps_arr, key_rots) + + # Generate dense trajectory + 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 = [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: list[float] | None = None + ) -> np.ndarray: + """ + Generate quintic (5th order) spline with zero velocity and acceleration at endpoints + + Args: + waypoints: List of poses [x, y, z, rx, ry, rz] + 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] + ) + + def generate_scurve_spline( + self, + waypoints: list[list[float]], + duration: float | None = None, + jerk_limit: float = 1000.0, + timestamps: list[float] | None = None, + ) -> np.ndarray: + """ + Generate S-curve spline with jerk-limited motion profile + + Args: + waypoints: List of poses [x, y, z, rx, ry, rz] + duration: Total duration for the trajectory (optional) + jerk_limit: Maximum jerk value in mm/s^3 + timestamps: Time for each waypoint (optional) + + 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] + ) + + if len(basic_trajectory) < 2: + return basic_trajectory + + # 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]) + ) + path_length += float(segment_length) + + if path_length < 0.001: + return basic_trajectory + + # Determine duration if not provided + if duration is None: + avg_speed = 50.0 # mm/s conservative speed + duration = path_length / avg_speed + + # Generate S-curve profile for the motion + num_points = int(duration * self.control_rate) + if num_points < 2: + num_points = 2 + + # Create S-curve time parameterization + time_points = np.linspace(0, duration, num_points) + s_curve_params: list[float] = [] + + for t in time_points: + tau = t / duration + if tau <= 0.0: + s = 0.0 + elif tau >= 1.0: + s = 1.0 + else: + s = tau * tau * (3.0 - 2.0 * tau) # smoothstep + s_curve_params.append(float(s)) + + # 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]] = [] + for new_idx in new_indices: + if new_idx <= 0: + resampled_trajectory.append(basic_trajectory[0].tolist()) + elif new_idx >= len(basic_trajectory) - 1: + resampled_trajectory.append(basic_trajectory[-1].tolist()) + else: + lower_idx = int(new_idx) + upper_idx = min(lower_idx + 1, len(basic_trajectory) - 1) + alpha = float(new_idx - lower_idx) + + lower_point = np.array(basic_trajectory[lower_idx]) + upper_point = np.array(basic_trajectory[upper_idx]) + interpolated = lower_point + alpha * (upper_point - lower_point) + resampled_trajectory.append(interpolated.tolist()) + + return np.array(resampled_trajectory) diff --git a/parol6/smooth_motion/waypoints.py b/parol6/smooth_motion/waypoints.py new file mode 100644 index 0000000..0062003 --- /dev/null +++ b/parol6/smooth_motion/waypoints.py @@ -0,0 +1,709 @@ +""" +Waypoint trajectory planner. +""" + +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. + + Implements mstraj-style parabolic blending at waypoints to avoid acceleration + spikes and ensure smooth motion through complex paths. + """ + + def __init__( + self, + waypoints: list[list[float]], + constraints: dict | None = None, + sample_rate: float = 100.0, + ): + """ + Initialize waypoint trajectory planner. + + Args: + waypoints: List of waypoint poses [x, y, z, rx, ry, rz] + constraints: Motion constraints (max_velocity, max_acceleration, max_jerk) + sample_rate: Trajectory sampling rate in Hz + """ + self.waypoints = np.array(waypoints, dtype=np.float64) + self.num_waypoints = len(waypoints) + self.sample_rate = sample_rate + self.dt = 1.0 / sample_rate + + # Default constraints + default_constraints = { + "max_velocity": 100.0, # mm/s + "max_acceleration": 500.0, # mm/s² + "max_jerk": 5000.0, # mm/s³ + } + self.constraints = constraints if constraints else default_constraints + + # Blend planning data + 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: + """ + Calculate the angle between incoming and outgoing path segments. + + Args: + idx: Waypoint index + + Returns: + Angle in radians (0 to π) + """ + if idx <= 0 or idx >= self.num_waypoints - 1: + return 0.0 # No corner at start/end + + # Vectors for path segments + v_in = self.waypoints[idx] - self.waypoints[idx - 1] + v_out = self.waypoints[idx + 1] - self.waypoints[idx] + + # Normalize (use only position components) + v_in_norm = v_in[:3] / (np.linalg.norm(v_in[:3]) + 1e-9) + v_out_norm = v_out[:3] / (np.linalg.norm(v_out[:3]) + 1e-9) + + # Calculate angle + cos_angle = np.clip(np.dot(v_in_norm, v_out_norm), -1, 1) + angle = np.arccos(cos_angle) + + return float(angle) + + def calculate_safe_blend_radius(self, idx: int, approach_velocity: float) -> float: + """ + Calculate maximum safe blend radius for a waypoint. + + Args: + idx: Waypoint index + approach_velocity: Velocity approaching the waypoint + + Returns: + Safe blend radius in mm + """ + angle = self.calculate_corner_angle(idx) + + if angle < 0.01: # Nearly straight (< 0.5 degrees) + return 0.0 + + # Dynamic blend radius based on velocity and angle + a_max = float(self.constraints["max_acceleration"]) + + # Centripetal acceleration constraint + # r = v² / (a_max * sin(θ/2)) + sin_half_angle = float(np.sin(angle / 2)) + if sin_half_angle > 0: + 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 = 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.0, min(r_safe, 100.0)) # Cap at 100mm + + return float(r_safe) + + def get_max_geometric_radius(self, idx: int) -> float: + """ + Get maximum blend radius based on segment geometry. + + Args: + idx: Waypoint index + + Returns: + Maximum geometric radius in mm + """ + if idx <= 0 or idx >= self.num_waypoints - 1: + return 0.0 + + # Distance to previous waypoint + 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] + ) + + # Maximum radius is 40% of shortest segment + max_radius = 0.4 * min(dist_prev, dist_next) + + return float(max_radius) + + def calculate_auto_blend_radii(self): + """ + Automatically calculate blend radius for each waypoint. + """ + self.blend_radii = [] + + for i in range(self.num_waypoints): + if i == 0 or i == self.num_waypoints - 1: + # No blending at start/end + self.blend_radii.append(0.0) + else: + # Estimate approach velocity + segment_length = np.linalg.norm( + self.waypoints[i][:3] - self.waypoints[i - 1][:3] + ) + + # Simple velocity estimation + if segment_length > 0: + approach_velocity = min( + self.constraints["max_velocity"], + np.sqrt(self.constraints["max_acceleration"] * segment_length), + ) + else: + approach_velocity = 0 + + # Calculate safe radius + 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]: + """ + Calculate blend entry and exit points for a waypoint. + + Args: + idx: Waypoint index + blend_radius: Blend radius in mm + + Returns: + Tuple of (entry_point, exit_point) + """ + if blend_radius <= 0 or idx <= 0 or idx >= self.num_waypoints - 1: + return self.waypoints[idx], self.waypoints[idx] + + # Get path vectors + v_in = self.waypoints[idx] - self.waypoints[idx - 1] + v_out = self.waypoints[idx + 1] - self.waypoints[idx] + + # Normalize position components + v_in_norm = np.zeros_like(v_in) + v_out_norm = np.zeros_like(v_out) + + v_in_norm[:3] = v_in[:3] / (np.linalg.norm(v_in[:3]) + 1e-9) + v_out_norm[:3] = v_out[:3] / (np.linalg.norm(v_out[:3]) + 1e-9) + + # Calculate blend entry point (along incoming path) + blend_entry = self.waypoints[idx].copy() + blend_entry[:3] -= v_in_norm[:3] * blend_radius + + # Calculate blend exit point (along outgoing path) + blend_exit = self.waypoints[idx].copy() + blend_exit[:3] += v_out_norm[:3] * blend_radius + + # Interpolate orientations + angle = self.calculate_corner_angle(idx) + if angle > 0: + # Weighted average for orientations at blend points + 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 + ) + + return blend_entry, blend_exit + + def generate_parabolic_blend( + self, + entry_point: np.ndarray, + exit_point: np.ndarray, + v_entry: np.ndarray, + v_exit: np.ndarray, + blend_time: float | None = None, + ) -> list[np.ndarray]: + """ + Generate parabolic trajectory for corner blend with acceleration limits. + + Parabolic blends have constant acceleration, providing smooth motion. + + Args: + entry_point: Blend entry position + exit_point: Blend exit position + v_entry: Velocity at blend entry + v_exit: Velocity at blend exit + blend_time: Blend duration (auto-calculated if None) + + Returns: + List of trajectory points through the blend + """ + # Calculate blend parameters + delta_p = exit_point - entry_point + distance = np.linalg.norm(delta_p[:3]) + + if distance < 1e-6: + return [entry_point] # No blend needed + + # Calculate velocity change needed + delta_v = v_exit - v_entry + delta_v_mag = np.linalg.norm(delta_v[:3]) + + # Minimum blend time from acceleration constraint + 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 = ( + 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 = 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) + + bt = float(max(blend_time, 0.01)) # Numerical stability + + # Acceleration (bounded) + 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 + bt = float(bt / (scale + 1e-9)) + + # Generate trajectory using cubic Hermite interpolation (C1 continuity) + num_points = max(5, int(bt * self.sample_rate)) # At least 5 points + blend_traj = [] + + for i in range(num_points): + # Normalized time from 0 to 1 + s = i / (num_points - 1) if num_points > 1 else 0.0 + + # Cubic Hermite basis functions + h00 = 2 * s**3 - 3 * s**2 + 1 + h10 = s**3 - 2 * s**2 + s + h01 = -2 * s**3 + 3 * s**2 + h11 = s**3 - s**2 + + # Interpolate position using hermite spline + # Scale velocities by blend_time to get tangents + 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: float | None = None + ) -> list[np.ndarray]: + """ + Generate linear trajectory segment between two points. + + Args: + start: Start position + end: End position + velocity: Desired velocity (uses max if None) + + Returns: + List of trajectory points + """ + distance = np.linalg.norm(end[:3] - start[:3]) + + if distance < 1e-6: + return [start] + + # Determine velocity + if velocity is None: + velocity = float(self.constraints["max_velocity"]) + + # Calculate duration and number of points + duration = float(distance) / float(velocity) + num_points = max(2, int(duration * self.sample_rate)) + + # Generate trajectory + segment = [] + for i in range(num_points): + alpha = i / (num_points - 1) if num_points > 1 else 0.0 + pos = start * (1 - alpha) + end * alpha + segment.append(pos) + + return segment + + def compute_blend_regions(self): + """ + Compute all blend regions for the trajectory. + """ + self.blend_regions = [] + + for i in range(1, self.num_waypoints - 1): + if self.blend_radii[i] > 0 and self.via_modes[i] == "via": + entry, exit = self.compute_blend_points(i, self.blend_radii[i]) + + # 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_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_full = np.zeros(len(exit)) + v_exit_full[:3] = v_exit + + 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) + + def plan_trajectory( + self, + blend_mode: str = "auto", + blend_radii: list[float] | None = None, + via_modes: list[str] | None = None, + trajectory_type: str = "cubic", + jerk_limit: float | None = None, + ) -> np.ndarray: + """ + Plan complete trajectory through waypoints with blending. + + Args: + blend_mode: 'auto', 'manual', or 'none' + blend_radii: Manual blend radii (if blend_mode='manual') + via_modes: 'via' or 'stop' for each waypoint + trajectory_type: 'cubic', 'quintic', or 's_curve' velocity profile + jerk_limit: Maximum jerk for s_curve profile (mm/s^3) + + Returns: + Complete trajectory as numpy array + """ + if self.num_waypoints < 2: + return self.waypoints + + # Set blend radii + if blend_mode in ["auto", "parabolic", "circular"]: + self.calculate_auto_blend_radii() + elif blend_mode == "manual" and blend_radii is not None: + self.blend_radii = blend_radii + elif blend_mode == "none": + self.blend_radii = [0.0] * self.num_waypoints + else: + # Default to auto for unrecognized modes + self.calculate_auto_blend_radii() + + # Set via modes + if via_modes is not None: + self.via_modes = via_modes + + # 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] + ) + # Simple velocity planning + if self.via_modes[i] == "stop" or self.via_modes[i + 1] == "stop": + # Trapezoid profile with acceleration + v_max = min( + float(self.constraints["max_velocity"]), + float( + np.sqrt( + float(self.constraints["max_acceleration"]) + * float(segment_length) + ) + ), + ) + else: + v_max = float(self.constraints["max_velocity"]) + + self.segment_velocities.append(v_max) + + # Compute blend regions + self.compute_blend_regions() + + # Generate full trajectory + full_trajectory: list[np.ndarray] = [] + + for i in range(self.num_waypoints - 1): + # Determine segment start and end + if i == 0: + 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 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 is not None: + segment_end = blend_region_next["entry"] + else: + segment_end = self.waypoints[i + 1] + else: + segment_end = self.waypoints[i + 1] + + # Generate linear segment + segment = self.generate_linear_segment( + segment_start, segment_end, self.segment_velocities[i] + ) + + # Add segment to trajectory + if i == 0: + full_trajectory.extend(segment) + else: + # Skip first point to avoid duplication + full_trajectory.extend(segment[1:]) + + # Add blend if needed + 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( + br["entry"], + br["exit"], + br["v_entry"], + br["v_exit"], + ) + # Skip first point to avoid duplication + full_trajectory.extend(blend_traj[1:]) + + trajectory_array = np.array(full_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 + ) + + return trajectory_array + + def apply_velocity_profile( + self, + trajectory: np.ndarray, + profile_type: str = "quintic", + jerk_limit: float | None = None, + ) -> np.ndarray: + """ + Apply velocity profile to existing trajectory points. + + Instead of re-interpolating with sparse waypoints, this method + applies the velocity profile to ALL trajectory points, preserving + the geometric path while adjusting the timing. + + Args: + trajectory: Input trajectory points + profile_type: 'quintic' or 's_curve' + jerk_limit: Maximum jerk for s_curve (mm/s^3) + + Returns: + Trajectory with velocity profile applied + """ + if len(trajectory) < 2: + return trajectory + + # Calculate cumulative arc length + arc_lengths = [0.0] + for i in range(1, len(trajectory)): + dist = float(np.linalg.norm(trajectory[i][:3] - trajectory[i - 1][:3])) + arc_lengths.append(arc_lengths[-1] + dist) + + total_length = arc_lengths[-1] + if total_length < 1e-6: + return trajectory + + # Generate new time mapping based on profile + num_points = len(trajectory) + new_trajectory = np.zeros_like(trajectory) + + for i in range(num_points): + # Get normalized position along path + s = i / (num_points - 1) if num_points > 1 else 0.0 + + # Apply velocity profile to get new arc length position + if profile_type == "quintic": + # Quintic polynomial: smooth acceleration/deceleration + s_new = 10 * s**3 - 15 * s**4 + 6 * s**5 + elif profile_type == "s_curve": + # S-curve (smoothstep): smooth jerk-limited motion + if s <= 0.0: + s_new = 0.0 + elif s >= 1.0: + s_new = 1.0 + else: + # Smoothstep function for smooth acceleration + s_new = s * s * (3.0 - 2.0 * s) + else: + # Default to linear (no change) + s_new = s + + # Find the corresponding point on original trajectory + # Use linear interpolation between trajectory points + target_arc_length = s_new * total_length + + # Find the segment containing this arc length + for j in range(len(arc_lengths) - 1): + if arc_lengths[j] <= target_arc_length <= arc_lengths[j + 1]: + # Interpolate within this segment + segment_length = arc_lengths[j + 1] - arc_lengths[j] + if segment_length > 1e-6: + alpha = (target_arc_length - arc_lengths[j]) / segment_length + else: + alpha = 0.0 + + # Linear interpolation between points + 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 + new_trajectory[i] = trajectory[-1] + + return new_trajectory + + def validate_trajectory(self, trajectory: np.ndarray) -> dict[str, float | bool]: + """ + Validate that trajectory respects all constraints. + + Args: + trajectory: Trajectory to validate + + Returns: + Dictionary of validation results with detailed information + """ + results: dict[str, float | bool] = { + "velocity_ok": True, + "acceleration_ok": True, + "jerk_ok": True, + "continuity_ok": True, + "max_velocity": 0.0, + "max_acceleration": 0.0, + "max_jerk": 0.0, + "max_step": 0.0, + } + + if len(trajectory) < 2: + return results + + dt = self.dt + n_dims = min(trajectory.shape[1], 6) + + # Check continuity - maximum step size between points + position_diffs = np.diff(trajectory[:, :3], axis=0) + step_sizes = np.linalg.norm(position_diffs, axis=1) + max_step = float(np.max(step_sizes)) if step_sizes.size else 0.0 + results["max_step"] = max_step + + expected_max_step = self.constraints["max_velocity"] * dt * 1.5 # 50% tolerance + results["continuity_ok"] = max_step <= expected_max_step + + # 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 + ) + 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 + ) + results["max_acceleration"] = max_acc + 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 + ) + results["max_jerk"] = max_jerk + results["jerk_ok"] = max_jerk <= self.constraints["max_jerk"] * 1.5 + + return results 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/__init__.py b/parol6/utils/__init__.py new file mode 100644 index 0000000..183c974 --- /dev/null +++ b/parol6/utils/__init__.py @@ -0,0 +1 @@ +"""Utility modules.""" diff --git a/parol6/utils/errors.py b/parol6/utils/errors.py new file mode 100644 index 0000000..d5b297d --- /dev/null +++ b/parol6/utils/errors.py @@ -0,0 +1,26 @@ +""" +Custom exception types for PAROL6 command/control pipeline. +Keep this focused and non-redundant; prefer built-ins where appropriate. +""" + + +class IKError(RuntimeError): + """Inverse kinematics failure (no solution, constraints violated, etc.).""" + + 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.""" + + 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 new file mode 100644 index 0000000..217f4f7 --- /dev/null +++ b/parol6/utils/frames.py @@ -0,0 +1,223 @@ +""" +Shared TRF/WRF transformation utilities used by commands. + +These helpers convert tool/frame-relative inputs (TRF) into world reference frame (WRF) +using the current tool pose derived from the robot's forward kinematics. +""" + +from __future__ import annotations + +import logging +from collections.abc import Sequence +from typing import Any, cast + +import numpy as np +from numpy.typing import NDArray +from spatialmath import SE3 + +from parol6.server.state import get_fkine_se3 + +logger = logging.getLogger(__name__) + +# Constants for TRF plane normal vectors +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) -> 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).tolist() + + +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_wrf = cast(SE3, tool_pose * pose_trf) + return np.concatenate( + [pose_wrf.t * 1000.0, pose_wrf.rpy(unit="deg", order="xyz")] + ).tolist() + + +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")]).tolist() + + +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_wrf = cast(SE3, tool_pose * center_trf) + transformed["center"] = (center_wrf.t * 1000.0).tolist() + + +def transform_start_pose_if_needed( + 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: + tool_pose = get_fkine_se3() + return pose6_trf_to_wrf(start_pose, tool_pose) + 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]: + """ + Transform command parameters from TRF to WRF. + Handles position, orientation, and directional vectors correctly. + """ + if frame == "WRF": + return params + + # Get current tool pose + tool_pose = get_fkine_se3() + + transformed = params.copy() + + # SMOOTH_CIRCLE - Transform center and plane normal + if command_type == "SMOOTH_CIRCLE": + if "center" in params: + transform_center_trf_to_wrf(params, tool_pose, transformed) + + if "plane" in params: + normal_trf = PLANE_NORMALS_TRF[params["plane"]] + normal_wrf = tool_pose.R @ normal_trf + 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 + elif command_type == "SMOOTH_ARC_CENTER": + if "center" in params: + transform_center_trf_to_wrf(params, tool_pose, transformed) + + if "end_pose" in params: + transformed["end_pose"] = pose6_trf_to_wrf(params["end_pose"], tool_pose) + + if "plane" in params: + normal_trf = PLANE_NORMALS_TRF[params["plane"]] + normal_wrf = tool_pose.R @ normal_trf + transformed["normal_vector"] = normal_wrf.tolist() + + # SMOOTH_ARC_PARAM - Transform end_pose and arc plane + elif command_type == "SMOOTH_ARC_PARAM": + if "end_pose" in params: + transformed["end_pose"] = pose6_trf_to_wrf(params["end_pose"], tool_pose) + + # For parametric arc, the plane is usually XY of the tool + if "plane" not in params: + params["plane"] = "XY" # Default to XY plane + + normal_trf = PLANE_NORMALS_TRF[params.get("plane", "XY")] + normal_wrf = tool_pose.R @ normal_trf + transformed["normal_vector"] = normal_wrf.tolist() + + # SMOOTH_HELIX - Transform center and helix axis + elif command_type == "SMOOTH_HELIX": + if "center" in params: + transform_center_trf_to_wrf(params, tool_pose, transformed) + + # 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.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.tolist() + + # SMOOTH_SPLINE - Transform waypoints + elif command_type == "SMOOTH_SPLINE": + if "waypoints" in params: + transformed_waypoints = [] + for wp in params["waypoints"]: + transformed_waypoints.append(pose6_trf_to_wrf(wp, tool_pose)) + transformed["waypoints"] = transformed_waypoints + + # SMOOTH_BLEND - Transform all segment definitions + elif command_type == "SMOOTH_BLEND": + if "segments" in params: + transformed_segments = [] + for seg in params["segments"]: + seg_transformed = seg.copy() + + # Transform based on segment type + if seg["type"] == "LINE": + if "end" in seg: + seg_transformed["end"] = pose6_trf_to_wrf(seg["end"], tool_pose) + + elif seg["type"] == "ARC": + if "end" in seg: + seg_transformed["end"] = pose6_trf_to_wrf(seg["end"], tool_pose) + + 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 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.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 + ) + + if "plane" in seg: + normal_trf = PLANE_NORMALS_TRF[seg["plane"]] + normal_wrf = tool_pose.R @ normal_trf + seg_transformed["normal_vector"] = normal_wrf.tolist() + + elif seg["type"] == "SPLINE": + if "waypoints" in seg: + transformed_wps = [] + for wp in seg["waypoints"]: + transformed_wps.append(pose6_trf_to_wrf(wp, tool_pose)) + seg_transformed["waypoints"] = transformed_wps + + transformed_segments.append(seg_transformed) + transformed["segments"] = transformed_segments + + # Generic transformations for any command with these parameters + if "start_pose" in params: + transformed["start_pose"] = pose6_trf_to_wrf(params["start_pose"], tool_pose) + + return transformed + + +__all__ = [ + "PLANE_NORMALS_TRF", + "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", +] diff --git a/parol6/utils/ik.py b/parol6/utils/ik.py new file mode 100644 index 0000000..ca45e28 --- /dev/null +++ b/parol6/utils/ik.py @@ -0,0 +1,176 @@ +""" +IK Helper Functions and Utilities +Shared functions used by multiple command classes for inverse kinematics calculations. +""" + +import logging +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 + +import parol6.PAROL6_ROBOT as PAROL6_ROBOT + +logger = logging.getLogger(__name__) + +# 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]), +} + + +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) + diff = qs - qc + q_unwrapped = qs.copy() + q_unwrapped[diff > np.pi] -= 2 * np.pi + q_unwrapped[diff < -np.pi] += 2 * np.pi + return q_unwrapped + + +class IKResult(NamedTuple): + success: bool + q: NDArray[np.float64] | None + iterations: int + residual: float + violations: str | None + + +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: + """ + IK solver + + Parameters + ---------- + robot : DHRobot + Robot model + target_pose : SE3 + Target pose to reach + current_q : array_like + 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 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 + """ + 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] + + violations = None + + 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: + 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: + """ + Calculates a smooth 0-to-1 scaling factor for progress 's' + using a quintic polynomial, ensuring smooth start/end accelerations. + """ + return 6 * (s**5) - 15 * (s**4) + 10 * (s**3) diff --git a/parol6/utils/trajectory.py b/parol6/utils/trajectory.py new file mode 100644 index 0000000..ef75193 --- /dev/null +++ b/parol6/utils/trajectory.py @@ -0,0 +1,172 @@ +""" +Shared trajectory planning utilities. +""" + +from collections.abc import Sequence + +import numpy as np + +from parol6.config import CONTROL_RATE_HZ + + +def _samples_for_duration(duration: float, sample_rate: float) -> int: + if duration <= 0: + return 2 + n = int(round(duration * sample_rate)) + 1 + return max(2, n) + + +def plan_linear_quintic( + start: Sequence[float], + end: Sequence[float], + duration: float, + sample_rate: float | None = None, +) -> np.ndarray: + """ + Quintic time-scaling with zero velocity and acceleration at endpoints. + Applies the scalar blend S(τ) = 10τ^3 - 15τ^4 + 6τ^5 to each dimension. + + Returns: array of shape (N, D) + """ + sr = CONTROL_RATE_HZ if sample_rate is None else float(sample_rate) + start_arr = np.asarray(start, dtype=float) + end_arr = np.asarray(end, dtype=float) + if start_arr.shape != end_arr.shape: + raise ValueError("start and end must have the same shape") + + if duration <= 0: + return np.vstack([start_arr, end_arr]) + + n = _samples_for_duration(duration, sr) + tau = np.linspace(0.0, 1.0, n) + s = 10 * tau**3 - 15 * tau**4 + 6 * tau**5 # [0..1] + s = s.reshape(-1, 1) + delta = (end_arr - start_arr).reshape(1, -1) + traj = start_arr.reshape(1, -1) + s * delta + return traj + + +def plan_linear_cubic( + start: Sequence[float], + end: Sequence[float], + duration: float, + sample_rate: float | None = None, +) -> np.ndarray: + """ + Cubic time-scaling with zero velocity at endpoints. + Applies S(τ) = 3τ^2 - 2τ^3 to each dimension. + + Returns: array of shape (N, D) + """ + sr = CONTROL_RATE_HZ if sample_rate is None else float(sample_rate) + start_arr = np.asarray(start, dtype=float) + end_arr = np.asarray(end, dtype=float) + if start_arr.shape != end_arr.shape: + raise ValueError("start and end must have the same shape") + + if duration <= 0: + return np.vstack([start_arr, end_arr]) + + n = _samples_for_duration(duration, sr) + tau = np.linspace(0.0, 1.0, n) + s = 3 * tau**2 - 2 * tau**3 + s = s.reshape(-1, 1) + delta = (end_arr - start_arr).reshape(1, -1) + traj = start_arr.reshape(1, -1) + s * delta + return traj + + +def _trapezoid_timings( + distance: float, v_max: float, a_max: float +) -> tuple[float, float, float, float, bool]: + """ + Compute trapezoid or triangular profile timing. + + Returns: (T, t_a, t_c, v_peak, triangular) + - T: total time + - t_a: accel time + - t_c: constant velocity time (0 for triangular) + - v_peak: peak velocity reached + - triangular: True if triangular profile (no cruise), else False + """ + if distance <= 0 or v_max <= 0 or a_max <= 0: + return 0.0, 0.0, 0.0, 0.0, True + + t_a = v_max / a_max + s_a = 0.5 * a_max * t_a**2 # distance covered during accel + + if 2 * s_a < distance: + # Trapezoidal: accel, cruise, decel + s_c = distance - 2 * s_a + t_c = s_c / v_max + T = 2 * t_a + t_c + return T, t_a, t_c, v_max, False + else: + # Triangular: peak velocity determined by distance + v_peak = np.sqrt(a_max * distance) + t_a = v_peak / a_max + T = 2 * t_a + return T, t_a, 0.0, v_peak, True + + +def plan_trapezoid_position_1d( + start: float, + end: float, + v_max: float, + a_max: float, + sample_rate: float | None = None, +) -> np.ndarray: + """ + Generate 1D position samples following a trapezoidal (or triangular) velocity profile. + Returns positions of shape (N,), including start and end. + + Notes: + - start and end can be any floats; profile is computed along the line with correct sign. + """ + sr = CONTROL_RATE_HZ if sample_rate is None else float(sample_rate) + d = float(end) - float(start) + sign = 1.0 if d >= 0 else -1.0 + L = abs(d) + + if L == 0 or v_max <= 0 or a_max <= 0: + return np.array([start, end], dtype=float) + + T, t_a, t_c, v_peak, triangular = _trapezoid_timings(L, v_max, a_max) + if T <= 0: + return np.array([start, end], dtype=float) + + n = _samples_for_duration(T, sr) + t = np.linspace(0.0, T, n) + + # Piecewise position function along positive axis (0..L) + pos = np.zeros_like(t) + if triangular: + # accel then decel with peak v_peak + for i, ti in enumerate(t): + if ti <= t_a: + pos[i] = 0.5 * a_max * ti**2 + else: + # decel phase mirrored + td = ti - t_a + pos[i] = (0.5 * a_max * t_a**2) + v_peak * td - 0.5 * a_max * td**2 + else: + # trapezoid: accel (0..t_a), cruise (t_a..t_a+t_c), decel (t_a+t_c..T) + for i, ti in enumerate(t): + if ti <= t_a: + pos[i] = 0.5 * a_max * ti**2 + elif ti <= (t_a + t_c): + 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 + ) + + # Clamp last sample to exact L to avoid drift + pos[-1] = L + # Apply direction and offset + world_pos = float(start) + sign * pos + return world_pos.astype(float) diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..372af52 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,120 @@ +[build-system] +requires = ["setuptools>=61.0"] +build-backend = "setuptools.build_meta" + +[project] +name = "parol6" +version = "0.1.0" +description = "Python library for controlling PAROL6 robot arms" + +requires-python = ">=3.10,<3.12" +dependencies = [ + # 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", + "scipy==1.11.4" +] + +[tool.setuptools.packages.find] +include = ["parol6*"] + +[project.optional-dependencies] +dev = [ + "ruff", + "mypy", + "pytest", + "pytest-asyncio", + "pre-commit" +] + +[project.scripts] +parol6-server = "parol6.cli.server:main_entry" + +[tool.pytest.ini_options] +# Limit discovery to the tests/ directory only +testpaths = ["tests"] + +# Test discovery patterns +python_files = ["test_*.py", "*_test.py"] +python_classes = ["Test*"] +python_functions = ["test_*"] + +# Output configuration +addopts = [ + "-v", + "--tb=short", + "--strict-markers", + "--disable-warnings", + "-p", "no:cacheprovider" +] + +# Timeout configuration (requires pytest-timeout) +timeout = 300 +timeout_method = "thread" + +# Logging configuration for tests +log_cli = true +log_cli_level = "INFO" +log_cli_format = "%(asctime)s [%(levelname)8s] %(name)s: %(message)s" +log_cli_date_format = "%Y-%m-%d %H:%M:%S" + +# Pytest markers for different test types +markers = [ + "unit: Unit tests that test individual components in isolation", + "integration: Integration tests that test component interactions with FAKE_SERIAL", + "hardware: Hardware tests that require actual robot hardware and human confirmation", + "slow: Slow-running tests (typically hardware or complex integration tests)", + "e2e: End-to-end tests that exercise complete workflows", + "gcode: Tests specifically for GCODE parsing and interpretation functionality" +] + +# Filter warnings +filterwarnings = [ + "ignore::DeprecationWarning", + "ignore::PendingDeprecationWarning", + "ignore::UserWarning:roboticstoolbox.*", + "ignore::UserWarning:spatialmath.*" +] + +[[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/robot_api.py b/robot_api.py deleted file mode 100644 index 4a75a71..0000000 --- a/robot_api.py +++ /dev/null @@ -1,1477 +0,0 @@ -""" -Zero-Overhead Robot API with Optional Acknowledgments -====================================================== -This version guarantees ZERO resource overhead when tracking is not used. -The tracking system is only initialized when explicitly requested. -""" - -import socket -from typing import List, Optional, Literal, Dict, Tuple, Union -import time -import threading -import queue -import uuid -from collections import deque -from datetime import datetime, timedelta - -# Global configuration -SERVER_IP = "127.0.0.1" -SERVER_PORT = 5001 - -# Global tracker - starts as None (no resources) -_command_tracker = None -_tracker_lock = threading.Lock() - -# ============================================================================ -# ORIGINAL SEND FUNCTION - ZERO OVERHEAD -# ============================================================================ - -def send_robot_command(command_string: str): - """ - Original send function - NO TRACKING, NO OVERHEAD. - This is what gets called for all backward-compatible operations. - - Resource usage: - - No threads - - No extra sockets - - No memory allocation - - Exactly the same as your original implementation - """ - try: - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: - sock.sendto(command_string.encode('utf-8'), (SERVER_IP, SERVER_PORT)) - return f"Successfully sent command: '{command_string[:50]}...'" - except Exception as e: - return f"Error sending command: {e}" - -# ============================================================================ -# TRACKING SYSTEM - ONLY LOADED WHEN NEEDED -# ============================================================================ - -class LazyCommandTracker: - """ - Command tracker with lazy initialization. - Resources are ONLY allocated when tracking is actually used. - """ - - def __init__(self, listen_port=5002, history_size=100): - self.listen_port = listen_port - self.history_size = history_size - self.command_history = {} - self.lock = threading.Lock() - - # Lazy initialization flags - self._initialized = False - self._thread = None - self._socket = None - self._running = False - - def _lazy_init(self): - """ - Initialize resources only when first tracking is requested. - This is called ONLY when someone uses tracking features. - """ - if self._initialized: - return True - - try: - print("[Tracker] First tracking request - initializing resources...") - - # Create socket - self._socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - self._socket.bind(('', self.listen_port)) - self._socket.settimeout(0.1) - - # Start thread - self._running = True - self._thread = threading.Thread(target=self._listen_loop, daemon=True) - self._thread.start() - - self._initialized = True - print(f"[Tracker] Initialized on port {self.listen_port}") - return True - - except Exception as e: - print(f"[Tracker] Failed to initialize: {e}") - self._cleanup() - return False - - def _cleanup(self): - """Clean up resources""" - self._running = False - if self._thread: - self._thread.join(timeout=0.5) - self._thread = None - if self._socket: - self._socket.close() - self._socket = None - self._initialized = False - - def _listen_loop(self): - """Listener thread - only runs if tracking is used""" - while self._running: - try: - data, addr = self._socket.recvfrom(2048) - message = data.decode('utf-8') - - parts = message.split('|', 3) - if parts[0] == 'ACK' and len(parts) >= 3: - cmd_id = parts[1] - status = parts[2] - details = parts[3] if len(parts) > 3 else "" - - with self.lock: - if cmd_id in self.command_history: - self.command_history[cmd_id].update({ - 'status': status, - 'details': details, - 'ack_time': datetime.now(), - 'completed': status in ['COMPLETED', 'FAILED', 'INVALID', 'CANCELLED'] - }) - - # Clean old entries (only if we have many) - if len(self.command_history) > self.history_size: - self._cleanup_old_entries() - - except socket.timeout: - continue - except Exception: - if self._running: - pass # Silently continue - - def _cleanup_old_entries(self): - """Remove old entries to prevent memory growth""" - with self.lock: - now = datetime.now() - expired = [cmd_id for cmd_id, info in self.command_history.items() - if now - info['sent_time'] > timedelta(seconds=30)] - for cmd_id in expired: - del self.command_history[cmd_id] - - def track_command(self, command: str) -> Tuple[str, str]: - """ - Track a command - initializes tracker if needed. - Returns (modified_command, cmd_id) - """ - # Initialize on first use - if not self._initialized: - if not self._lazy_init(): - # Initialization failed - fall back to non-tracking - return command, None - - # Generate ID and modify command - cmd_id = str(uuid.uuid4())[:8] - tracked_command = f"{cmd_id}|{command}" - - # Register in history - with self.lock: - self.command_history[cmd_id] = { - 'command': command, - 'sent_time': datetime.now(), - 'status': 'SENT', - 'details': '', - 'completed': False - } - - return tracked_command, cmd_id - - def get_status(self, cmd_id: str) -> Optional[Dict]: - """Get status if tracker is initialized""" - if not self._initialized: - return None - with self.lock: - return self.command_history.get(cmd_id, None) - - def wait_for_completion(self, cmd_id: str, timeout: float = 5.0) -> Dict: - """Wait for completion if tracker is initialized""" - if not self._initialized: - return {'status': 'NO_TRACKING', 'details': 'Tracker not initialized', 'completed': True} - - start_time = time.time() - while time.time() - start_time < timeout: - status = self.get_status(cmd_id) - if status and status['completed']: - return status - time.sleep(0.01) - - return self.get_status(cmd_id) or { - 'status': 'TIMEOUT', - 'details': 'No acknowledgment received', - 'completed': True - } - - def is_active(self) -> bool: - """Check if tracker is initialized and running""" - return self._initialized and self._running - -# ============================================================================ -# LAZY TRACKER ACCESS -# ============================================================================ - -def _get_tracker_if_needed() -> Optional[LazyCommandTracker]: - """ - Get tracker ONLY if tracking is requested. - This ensures zero overhead for non-tracking operations. - """ - global _command_tracker, _tracker_lock - - # Fast path - tracker already exists - if _command_tracker is not None: - return _command_tracker - - # Slow path - create tracker (only happens once) - with _tracker_lock: - if _command_tracker is None: - _command_tracker = LazyCommandTracker() - return _command_tracker - -# ============================================================================ -# ENHANCED SEND WITH OPTIONAL TRACKING -# ============================================================================ - -def send_robot_command_tracked(command_string: str) -> Tuple[str, Optional[str]]: - """ - Send with tracking - initializes tracker on first use. - - Resource impact: - - First call: Starts tracker thread - - Subsequent calls: Minimal overhead (UUID generation) - """ - tracker = _get_tracker_if_needed() - if tracker: - tracked_cmd, cmd_id = tracker.track_command(command_string) - if cmd_id: - # Send tracked command - try: - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: - sock.sendto(tracked_cmd.encode('utf-8'), (SERVER_IP, SERVER_PORT)) - return f"Command sent with tracking (ID: {cmd_id})", cmd_id - except Exception as e: - return f"Error: {e}", None - - # Fall back to non-tracked - return send_robot_command(command_string), None - -def send_and_wait( - command_string: str, - timeout: float = 2.0, - non_blocking: bool = False - ) -> Union[Dict, str, None]: - """ - Send and wait for acknowledgment OR return a command_id immediately. - First use initializes tracker. - """ - result, cmd_id = send_robot_command_tracked(command_string) - - if cmd_id: - # If non_blocking is True, return the ID right away - if non_blocking: - return cmd_id - - # Otherwise, proceed with the original blocking logic - tracker = _get_tracker_if_needed() - if tracker: - status_dict = tracker.wait_for_completion(cmd_id, timeout) - # Add the command_id to the returned dictionary - status_dict['command_id'] = cmd_id - return status_dict - - # Handle cases where a command_id could not be generated - if non_blocking: - return None - else: - return {'status': 'NO_TRACKING', 'details': result, 'completed': True, 'command_id': None} - -# ============================================================================ -# BACKWARD COMPATIBLE MOVEMENT FUNCTIONS - ZERO OVERHEAD BY DEFAULT -# ============================================================================ - -def move_robot_joints( - joint_angles: List[float], - duration: Optional[float] = None, - speed_percentage: Optional[int] = None, - wait_for_ack: bool = False, # Default: No tracking, no overhead - timeout: float = 2.0, - non_blocking: bool = False -): - """ - Move robot joints. - - Resource usage: - - wait_for_ack=False (default): ZERO overhead, no tracking - - wait_for_ack=True: Initializes tracker on first use - """ - # Validation - if duration is None and speed_percentage is None: - error = "Error: You must provide either a duration or a speed_percentage." - return {'status': 'INVALID', 'details': error} if wait_for_ack else error - - # Build command - angles_str = "|".join(map(str, joint_angles)) - duration_str = str(duration) if duration is not None else "None" - speed_str = str(speed_percentage) if speed_percentage is not None else "None" - command = f"MOVEJOINT|{angles_str}|{duration_str}|{speed_str}" - - # Send with or without tracking - if wait_for_ack: - # User explicitly requested tracking - initialize if needed - return send_and_wait(command, timeout, non_blocking) - else: - # Default path - NO TRACKING, NO OVERHEAD - return send_robot_command(command) - -def move_robot_pose( - pose: List[float], - duration: Optional[float] = None, - speed_percentage: Optional[int] = None, - wait_for_ack: bool = False, # Default: No tracking - timeout: float = 2., - non_blocking: bool = False -): - """ - Move to pose - zero overhead by default. - """ - if duration is None and speed_percentage is None: - error = "Error: You must provide either a duration or a speed_percentage." - return {'status': 'INVALID', 'details': error} if wait_for_ack else error - - pose_str = "|".join(map(str, pose)) - duration_str = str(duration) if duration is not None else "None" - speed_str = str(speed_percentage) if speed_percentage is not None else "None" - command = f"MOVEPOSE|{pose_str}|{duration_str}|{speed_str}" - - if wait_for_ack: - return send_and_wait(command, timeout, non_blocking) - else: - return send_robot_command(command) - -def jog_robot_joint( - joint_index: int, - speed_percentage: int, - duration: Optional[float] = None, - distance_deg: Optional[float] = None, - wait_for_ack: bool = False, - timeout: float = 2.0, - non_blocking: bool = False -): - """ - Jogs a single robot joint for a specified time or distance. - - Resource usage: - - wait_for_ack=False (default): ZERO overhead, no tracking - - wait_for_ack=True: Initializes tracker on first use - """ - if duration is None and distance_deg is None: - error = "Error: You must provide either a duration or a distance_deg." - return {'status': 'INVALID', 'details': error} if wait_for_ack else error - - if duration is not None: - try: - duration = float(duration) - except (ValueError, TypeError): - error = "Error: Duration must be a valid number." - return {'status': 'INVALID', 'details': error} if wait_for_ack else error - - duration_str = str(duration) if duration is not None else "None" - distance_str = str(distance_deg) if distance_deg is not None else "None" - command = f"JOG|{joint_index}|{speed_percentage}|{duration_str}|{distance_str}" - - if wait_for_ack: - return send_and_wait(command, timeout, non_blocking) - else: - return send_robot_command(command) - -def jog_multiple_joints( - joints: List[int], - speeds: List[float], - duration: float, - wait_for_ack: bool = False, - timeout: float = 2.0, - non_blocking: bool = False -) -> str: - """ - Jogs multiple robot joints simultaneously for a specified duration. - - Args: - joints: List of joint indices (0-5 for positive, 6-11 for negative) - speeds: List of corresponding speeds (1-100%) - duration: Duration of the jog in seconds - wait_for_ack: Enable command tracking (default False) - timeout: Timeout for acknowledgment - - Resource usage: - - wait_for_ack=False (default): ZERO overhead, no tracking - - wait_for_ack=True: Initializes tracker on first use - """ - if len(joints) != len(speeds): - error = "Error: The number of joints must match the number of speeds." - return {'status': 'INVALID', 'details': error} if wait_for_ack else error - - joints_str = ",".join(map(str, joints)) - speeds_str = ",".join(map(str, speeds)) - command = f"MULTIJOG|{joints_str}|{speeds_str}|{duration}" - - if wait_for_ack: - return send_and_wait(command, timeout, non_blocking) - else: - return send_robot_command(command) - -def jog_cartesian( - frame: Literal['TRF', 'WRF'], - axis: Literal['X+', 'X-', 'Y+', 'Y-', 'Z+', 'Z-', 'RX+', 'RX-', 'RY+', 'RY-', 'RZ+', 'RZ-'], - speed_percentage: int, - duration: float, - wait_for_ack: bool = False, - timeout: float = 2.0, - non_blocking: bool = False -): - """ - Jogs the robot's end-effector continuously in Cartesian space. - - Resource usage: - - wait_for_ack=False (default): ZERO overhead, no tracking - - wait_for_ack=True: Initializes tracker on first use - """ - if duration is not None: - try: - duration = float(duration) - except (ValueError, TypeError): - error = "Error: Duration must be a valid number." - return {'status': 'INVALID', 'details': error} if wait_for_ack else error - - command = f"CARTJOG|{frame}|{axis}|{speed_percentage}|{duration}" - - if wait_for_ack: - return send_and_wait(command, timeout, non_blocking) - else: - return send_robot_command(command) - -def move_robot_cartesian( - pose: List[float], - duration: Optional[float] = None, - speed_percentage: Optional[float] = None, - wait_for_ack: bool = False, - timeout: float = 2.0, - non_blocking: bool = False -) -> str: - """ - Moves the robot's end-effector to a specific Cartesian pose in a straight line. - - Args: - pose: Target pose as [x, y, z, r, p, y] (mm and degrees) - duration: Total time for the movement in seconds - speed_percentage: Movement speed as a percentage (1-100) - wait_for_ack: Enable command tracking (default False) - timeout: Timeout for acknowledgment - - Resource usage: - - wait_for_ack=False (default): ZERO overhead, no tracking - - wait_for_ack=True: Initializes tracker on first use - """ - # Validate timing arguments - if (duration is None and speed_percentage is None): - error = "Error: You must provide either 'duration' or 'speed_percentage'." - return {'status': 'INVALID', 'details': error} if wait_for_ack else error - - if (duration is not None and speed_percentage is not None): - error = "Error: Please provide either 'duration' or 'speed_percentage', not both." - return {'status': 'INVALID', 'details': error} if wait_for_ack else error - - # Prepare command arguments - duration_arg = 'NONE' - speed_arg = 'NONE' - - if duration is not None: - try: - if float(duration) <= 0: - error = "Error: Duration must be a positive number." - return {'status': 'INVALID', 'details': error} if wait_for_ack else error - duration_arg = str(duration) - except (ValueError, TypeError): - error = "Error: Duration must be a valid number." - return {'status': 'INVALID', 'details': error} if wait_for_ack else error - - if speed_percentage is not None: - try: - speed_val = float(speed_percentage) - if not (0 < speed_val <= 100): - error = "Error: Speed percentage must be between 1 and 100." - return {'status': 'INVALID', 'details': error} if wait_for_ack else error - speed_arg = str(speed_val) - except (ValueError, TypeError): - error = "Error: Speed percentage must be a valid number." - return {'status': 'INVALID', 'details': error} if wait_for_ack else error - - # Construct command - pose_str = "|".join(map(str, pose)) - command = f"MOVECART|{pose_str}|{duration_arg}|{speed_arg}" - - if wait_for_ack: - return send_and_wait(command, timeout, non_blocking) - else: - return send_robot_command(command) - -def control_pneumatic_gripper( - action: Literal['open', 'close'], - port: Literal[1, 2], - wait_for_ack: bool = False, - timeout: float = 2.0, - non_blocking: bool = False -): - """ - Controls the pneumatic gripper. - - Resource usage: - - wait_for_ack=False (default): ZERO overhead, no tracking - - wait_for_ack=True: Initializes tracker on first use - """ - command = f"PNEUMATICGRIPPER|{action}|{port}" - - if wait_for_ack: - return send_and_wait(command, timeout, non_blocking) - else: - return send_robot_command(command) - -def control_electric_gripper( - action: Literal['move', 'calibrate'], - position: Optional[int] = 255, - speed: Optional[int] = 150, - current: Optional[int] = 500, - wait_for_ack: bool = False, - timeout: float = 2.0, - non_blocking: bool = False -): - """ - Controls the electric gripper. - - Resource usage: - - wait_for_ack=False (default): ZERO overhead, no tracking - - wait_for_ack=True: Initializes tracker on first use - """ - action_str = "move" if action == 'move' else 'calibrate' - command = f"ELECTRICGRIPPER|{action_str}|{position}|{speed}|{current}" - - if wait_for_ack: - return send_and_wait(command, timeout, non_blocking) - else: - return send_robot_command(command) - -# ============================================================================ -# SMOOTH MOTION COMMANDS - WITH START POSITION AND DUAL TIMING SUPPORT -# ============================================================================ - -def smooth_circle( - center: List[float], - radius: float, - plane: Literal['XY', 'XZ', 'YZ'] = 'XY', - frame: Literal['WRF', 'TRF'] = 'WRF', - start_pose: Optional[List[float]] = None, - duration: Optional[float] = None, - speed_percentage: Optional[float] = None, - clockwise: bool = False, - wait_for_ack: bool = False, - timeout: float = 10.0, - non_blocking: bool = False -): - """ - Execute a smooth circular motion. - - Args: - center: [x, y, z] center point in mm - radius: Circle radius in mm - plane: Plane of the circle ('XY', 'XZ', or 'YZ') - frame: Reference frame ('WRF' for World, 'TRF' for Tool) - start_pose: Optional [x, y, z, rx, ry, rz] start pose (mm and degrees). - If None, starts from current position. - duration: Time to complete the circle in seconds - speed_percentage: Speed as percentage (1-100) - clockwise: Direction of motion - wait_for_ack: Enable command tracking (default False) - timeout: Timeout for acknowledgment - non_blocking: Return immediately with command ID - - Resource usage: - - wait_for_ack=False (default): ZERO overhead - - wait_for_ack=True: Initializes tracker on first use - """ - if duration is None and speed_percentage is None: - error = "Error: You must provide either duration or speed_percentage." - return {'status': 'INVALID', 'details': error} if wait_for_ack else error - - 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" - - # Format timing - if duration is not None: - timing_str = f"DURATION|{duration}" - else: - timing_str = f"SPEED|{speed_percentage}" - - command = f"SMOOTH_CIRCLE|{center_str}|{radius}|{plane}|{frame}|{start_str}|{timing_str}|{clockwise_str}" - - if wait_for_ack: - return send_and_wait(command, timeout, non_blocking) - else: - return send_robot_command(command) - -def smooth_arc_center( - 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, - clockwise: bool = False, - wait_for_ack: bool = False, - timeout: float = 10.0, - non_blocking: bool = False -): - """ - 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 - frame: Reference frame ('WRF' for World, 'TRF' for Tool) - start_pose: Optional [x, y, z, rx, ry, rz] start pose. - If None, starts from current position. - If specified, adds smooth transition from current position. - duration: Time to complete the arc in seconds - speed_percentage: Speed as percentage (1-100) - clockwise: Direction of motion - wait_for_ack: Enable command tracking - timeout: Timeout for acknowledgment - non_blocking: Return immediately with command ID - """ - if duration is None and speed_percentage is None: - error = "Error: You must provide either duration or speed_percentage." - return {'status': 'INVALID', 'details': error} if wait_for_ack else error - - 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" - - # Format timing - if duration is not None: - timing_str = f"DURATION|{duration}" - else: - timing_str = f"SPEED|{speed_percentage}" - - command = f"SMOOTH_ARC_CENTER|{end_str}|{center_str}|{frame}|{start_str}|{timing_str}|{clockwise_str}" - - if wait_for_ack: - return send_and_wait(command, timeout, non_blocking) - else: - return send_robot_command(command) - -def smooth_arc_parametric( - 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, - clockwise: bool = False, - wait_for_ack: bool = False, - timeout: float = 10.0, - non_blocking: bool = False -): - """ - Execute a smooth arc motion defined by radius and angle. - - Args: - end_pose: [x, y, z, rx, ry, rz] end pose (mm and degrees) - radius: Arc radius in mm - arc_angle: Arc angle in degrees - frame: Reference frame ('WRF' for World, 'TRF' for Tool) - start_pose: Optional [x, y, z, rx, ry, rz] start pose. - If None, starts from current position. - duration: Time to complete the arc in seconds - speed_percentage: Speed as percentage (1-100) - clockwise: Direction of motion - wait_for_ack: Enable command tracking - timeout: Timeout for acknowledgment - non_blocking: Return immediately with command ID - """ - if duration is None and speed_percentage is None: - error = "Error: You must provide either duration or speed_percentage." - return {'status': 'INVALID', 'details': error} if wait_for_ack else error - - end_str = ",".join(map(str, end_pose)) - start_str = ",".join(map(str, start_pose)) if start_pose else "CURRENT" - clockwise_str = "1" if clockwise else "0" - - # Format timing - if duration is not None: - timing_str = f"DURATION|{duration}" - else: - timing_str = f"SPEED|{speed_percentage}" - - command = f"SMOOTH_ARC_PARAM|{end_str}|{radius}|{arc_angle}|{frame}|{start_str}|{timing_str}|{clockwise_str}" - - if wait_for_ack: - return send_and_wait(command, timeout, non_blocking) - else: - return send_robot_command(command) - -def smooth_spline( - waypoints: List[List[float]], - frame: Literal['WRF', 'TRF'] = 'WRF', - start_pose: Optional[List[float]] = None, - duration: Optional[float] = None, - speed_percentage: Optional[float] = None, - wait_for_ack: bool = False, - timeout: float = 10.0, - non_blocking: bool = False -): - """ - 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) - start_pose: Optional [x, y, z, rx, ry, rz] start pose. - If None, starts from current position. - If specified and different from first waypoint, adds transition. - duration: Total time for the motion in seconds - speed_percentage: Speed as percentage (1-100) - wait_for_ack: Enable command tracking - timeout: Timeout for acknowledgment - non_blocking: Return immediately with command ID - """ - if duration is None and speed_percentage is None: - error = "Error: You must provide either duration or speed_percentage." - return {'status': 'INVALID', 'details': error} if wait_for_ack else error - - num_waypoints = len(waypoints) - start_str = ",".join(map(str, start_pose)) if start_pose else "CURRENT" - - # Format timing - if duration is not None: - timing_str = f"DURATION|{duration}" - else: - timing_str = f"SPEED|{speed_percentage}" - - # Format waypoints - flatten each waypoint's 6 values - waypoint_strs = [] - for wp in waypoints: - waypoint_strs.extend(map(str, wp)) - - # Build command - command_parts = [f"SMOOTH_SPLINE", str(num_waypoints), frame, start_str, timing_str] - command_parts.extend(waypoint_strs) - command = "|".join(command_parts) - - if wait_for_ack: - return send_and_wait(command, timeout, non_blocking) - else: - return send_robot_command(command) - -def smooth_helix( - center: List[float], - radius: float, - pitch: float, - height: float, - frame: Literal['WRF', 'TRF'] = 'WRF', - start_pose: Optional[List[float]] = None, - duration: Optional[float] = None, - speed_percentage: Optional[float] = None, - clockwise: bool = False, - wait_for_ack: bool = False, - timeout: float = 10.0, - non_blocking: bool = False -): - """ - Execute a smooth helical motion. - - Args: - center: [x, y, z] helix center point in mm - radius: Helix radius in mm - pitch: Vertical distance per revolution in mm - height: Total height of helix in mm - frame: Reference frame ('WRF' for World, 'TRF' for Tool) - start_pose: Optional [x, y, z, rx, ry, rz] start pose. - If None, starts from current position on helix perimeter. - duration: Time to complete the helix in seconds - speed_percentage: Speed as percentage (1-100) - clockwise: Direction of motion - wait_for_ack: Enable command tracking - timeout: Timeout for acknowledgment - non_blocking: Return immediately with command ID - """ - if duration is None and speed_percentage is None: - error = "Error: You must provide either duration or speed_percentage." - return {'status': 'INVALID', 'details': error} if wait_for_ack else error - - 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" - - # Format timing - if duration is not None: - timing_str = f"DURATION|{duration}" - else: - timing_str = f"SPEED|{speed_percentage}" - - command = f"SMOOTH_HELIX|{center_str}|{radius}|{pitch}|{height}|{frame}|{start_str}|{timing_str}|{clockwise_str}" - - if wait_for_ack: - return send_and_wait(command, timeout, non_blocking) - else: - return send_robot_command(command) - -def smooth_blend( - 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, - wait_for_ack: bool = False, - timeout: float = 15.0, - non_blocking: bool = False -): - """ - Execute a blended motion through multiple segments. - - Args: - segments: List of segment dictionaries, each containing: - - 'type': 'LINE', 'CIRCLE', 'ARC', or 'SPLINE' - - Additional parameters based on type - blend_time: Time to blend between segments in seconds - frame: Reference frame ('WRF' for World, 'TRF' for Tool) - start_pose: Optional [x, y, z, rx, ry, rz] start pose for first segment. - If None, starts from current position. - duration: Total time for entire motion (scales all segments proportionally) - speed_percentage: Speed as percentage (1-100) for entire motion - wait_for_ack: Enable command tracking - timeout: Timeout for acknowledgment - non_blocking: Return immediately with command ID - - Example: - segments = [ - {'type': 'LINE', 'end': [x,y,z,rx,ry,rz], 'duration': 2.0}, - {'type': 'CIRCLE', 'center': [x,y,z], 'radius': 50, 'plane': 'XY', - 'duration': 3.0, 'clockwise': False}, - {'type': 'ARC', 'end': [x,y,z,rx,ry,rz], 'center': [x,y,z], - 'duration': 2.0, 'clockwise': True} - ] - """ - num_segments = len(segments) - start_str = ",".join(map(str, start_pose)) if start_pose else "CURRENT" - - # Format timing - if duration is None and speed_percentage is None: - # Use individual segment durations - timing_str = "DEFAULT" - elif duration is not None: - timing_str = f"DURATION|{duration}" - else: - timing_str = f"SPEED|{speed_percentage}" - - # Format segments - segment_strs = [] - for seg in segments: - seg_type = seg['type'] - - if seg_type == 'LINE': - end_str = ",".join(map(str, seg['end'])) - seg_str = f"LINE|{end_str}|{seg.get('duration', 2.0)}" - - elif seg_type == 'CIRCLE': - center_str = ",".join(map(str, seg['center'])) - clockwise_str = "1" if seg.get('clockwise', False) else "0" - seg_str = f"CIRCLE|{center_str}|{seg['radius']}|{seg['plane']}|{seg.get('duration', 3.0)}|{clockwise_str}" - - elif seg_type == 'ARC': - end_str = ",".join(map(str, seg['end'])) - center_str = ",".join(map(str, seg['center'])) - 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']]) - seg_str = f"SPLINE|{len(seg['waypoints'])}|{waypoints_str}|{seg.get('duration', 3.0)}" - - else: - continue - - segment_strs.append(seg_str) - - # Build command with || separators between segments - command = f"SMOOTH_BLEND|{num_segments}|{blend_time}|{frame}|{start_str}|{timing_str}|" + "||".join(segment_strs) - - if wait_for_ack: - return send_and_wait(command, timeout, non_blocking) - else: - return send_robot_command(command) - -# ============================================================================ -# CONVENIENCE FUNCTIONS FOR SMOOTH MOTION CHAINS -# ============================================================================ - -def chain_smooth_motions( - motions: List[Dict], - ensure_continuity: bool = True, - frame: Literal['WRF', 'TRF'] = 'WRF', # ADD THIS - wait_for_ack: bool = True, - timeout: float = 30.0 -): - """ - Chain multiple smooth motions together with automatic continuity. - - Args: - motions: List of motion dictionaries, each with 'type' and parameters - ensure_continuity: If True, automatically sets start_pose of each motion - to end of previous motion for perfect continuity - frame: Reference frame for all motions ('WRF' or 'TRF') # ADD THIS - wait_for_ack: Enable command tracking - timeout: Timeout per motion - - Example: - chain_smooth_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}, - {'type': 'helix', 'center': [250, 50, 150], 'radius': 30, 'pitch': 20, 'height': 100, 'duration': 8} - ], frame='TRF') # Can now specify frame - """ - results = [] - last_end_pose = None - - for i, motion in enumerate(motions): - motion_type = motion.get('type', '').lower() - - # Add frame to motion parameters - motion['frame'] = frame - - # Add start_pose from previous motion if ensuring continuity - if ensure_continuity and last_end_pose and i > 0: - motion['start_pose'] = last_end_pose - - # Execute the appropriate motion (add frame parameter to each call) - if motion_type == 'circle': - result = smooth_circle(**{k: v for k, v in motion.items() if k != 'type'}, - wait_for_ack=wait_for_ack, timeout=timeout) - last_end_pose = None # Circles return to start - - elif motion_type == 'arc' or motion_type == 'arc_center': - result = smooth_arc_center(**{k: v for k, v in motion.items() if k != 'type'}, - wait_for_ack=wait_for_ack, timeout=timeout) - last_end_pose = motion.get('end_pose') - - elif motion_type == 'arc_param' or motion_type == 'arc_parametric': - result = smooth_arc_parametric(**{k: v for k, v in motion.items() if k != 'type'}, - wait_for_ack=wait_for_ack, timeout=timeout) - last_end_pose = motion.get('end_pose') - - elif motion_type == 'spline': - result = smooth_spline(**{k: v for k, v in motion.items() if k != 'type'}, - wait_for_ack=wait_for_ack, timeout=timeout) - waypoints = motion.get('waypoints', []) - last_end_pose = waypoints[-1] if waypoints else None - - elif motion_type == 'helix': - result = smooth_helix(**{k: v for k, v in motion.items() if k != 'type'}, - wait_for_ack=wait_for_ack, timeout=timeout) - last_end_pose = None - - else: - result = {'status': 'INVALID', 'details': f'Unknown motion type: {motion_type}'} - - results.append(result) - - # Check for failures if tracking - if wait_for_ack and isinstance(result, dict) and result.get('status') == 'FAILED': - print(f"Motion {i+1} failed: {result.get('details')}") - break - - return results - -def execute_trajectory( - trajectory: List[List[float]], - timing_mode: Literal['duration', 'speed'] = 'duration', - timing_value: float = 5.0, - motion_type: Literal['spline', 'linear'] = 'spline', - frame: Literal['WRF', 'TRF'] = 'WRF', # ADD THIS - wait_for_ack: bool = True, - timeout: float = 30.0, -): - """ - High-level function to execute a trajectory using the best method. - - Args: - trajectory: List of poses [x, y, z, rx, ry, rz] - timing_mode: 'duration' for total time, 'speed' for percentage - timing_value: Duration in seconds or speed percentage - motion_type: 'spline' for smooth curves, 'linear' for point-to-point - frame: Reference frame ('WRF' or 'TRF') # ADD THIS - wait_for_ack: Enable command tracking (recommended for trajectories) - timeout: Timeout for acknowledgment - """ - if motion_type == 'spline': - if timing_mode == 'duration': - return smooth_spline(trajectory, frame=frame, duration=timing_value, # ADD frame - wait_for_ack=wait_for_ack, timeout=timeout) - else: - return smooth_spline(trajectory, frame=frame, speed_percentage=timing_value, # ADD frame - wait_for_ack=wait_for_ack, timeout=timeout) - else: - # Linear motion - send as individual move commands - results = [] - for pose in trajectory: - if timing_mode == 'duration': - segment_duration = timing_value / len(trajectory) - # Note: move_robot_cartesian doesn't support TRF, only smooth motions do - result = move_robot_cartesian(pose, duration=segment_duration, - wait_for_ack=wait_for_ack, timeout=timeout) - else: - result = move_robot_cartesian(pose, speed_percentage=timing_value, - wait_for_ack=wait_for_ack, timeout=timeout) - results.append(result) - - # Check for failures if tracking - if wait_for_ack and result.get('status') == 'FAILED': - break - - return results - -# ============================================================================ -# BASIC FUNCTIONS -# ============================================================================ - -def delay_robot(duration: float, wait_for_ack: bool = False, timeout: float = 2.0, non_blocking: bool = False): - """Delay - optional tracking""" - command = f"DELAY|{duration}" - if wait_for_ack: - return send_and_wait(command, timeout, non_blocking) - else: - return send_robot_command(command) - -def home_robot(wait_for_ack: bool = False, timeout: float = 30.0, non_blocking: bool = False): - """Home robot - optional tracking (longer timeout for homing)""" - command = "HOME" - if wait_for_ack: - return send_and_wait(command, timeout, non_blocking) - else: - return send_robot_command(command) - -def stop_robot_movement(wait_for_ack: bool = False, timeout: float = 2.0, non_blocking: bool = False): - """Stop robot - optional tracking""" - command = "STOP" - if wait_for_ack: - return send_and_wait(command, timeout, non_blocking) - else: - return send_robot_command(command) - -# ============================================================================ -# GET FUNCTIONS - ZERO OVERHEAD, IMMEDIATE RESPONSE -# ============================================================================ - -def get_robot_pose(): - """ - Get the robot's current end-effector pose. - Returns [x, y, z, roll, pitch, yaw] or None if it fails. - - Resource usage: ZERO overhead - simple request/response - """ - try: - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as client_socket: - client_socket.settimeout(2.0) - - request_message = "GET_POSE" - client_socket.sendto(request_message.encode('utf-8'), (SERVER_IP, SERVER_PORT)) - - data, _ = client_socket.recvfrom(2048) - response_str = data.decode('utf-8') - - parts = response_str.split('|') - if parts[0] == 'POSE' and len(parts) == 2: - pose_values = [float(v) for v in parts[1].split(',')] - if len(pose_values) == 16: - # Convert 4x4 matrix to [x,y,z,r,p,y] - import numpy as np - from spatialmath import SE3 - - pose_matrix = np.array(pose_values).reshape((4, 4)) - T = SE3(pose_matrix, check=False) - xyz_mm = T.t * 1000 # Convert to mm - rpy_deg = T.rpy(unit='deg', order='xyz') - - # Convert numpy float64 to regular Python floats - return [float(x) for x in xyz_mm] + [float(r) for r in rpy_deg] - - return None - - except socket.timeout: - print("Timeout waiting for pose response") - return None - except Exception as e: - print(f"Error getting robot pose: {e}") - return None - -def get_robot_joint_angles(): - """ - Get the robot's current joint angles in degrees. - Returns list of 6 angles or None if it fails. - - Resource usage: ZERO overhead - simple request/response - """ - try: - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as client_socket: - client_socket.settimeout(2.0) - - request_message = "GET_ANGLES" - client_socket.sendto(request_message.encode('utf-8'), (SERVER_IP, SERVER_PORT)) - - data, _ = client_socket.recvfrom(1024) - response_str = data.decode('utf-8') - - parts = response_str.split('|') - if parts[0] == 'ANGLES' and len(parts) == 2: - angles = [float(v) for v in parts[1].split(',')] - return angles - - return None - - except socket.timeout: - print("Timeout waiting for angles response") - return None - except Exception as e: - print(f"Error getting robot angles: {e}") - return None - -def get_robot_io(verbose = False): - """ - Get the robot's current digital I/O status. - Returns [IN1, IN2, OUT1, OUT2, ESTOP] or None if it fails. - - Resource usage: ZERO overhead - simple request/response - """ - try: - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as client_socket: - client_socket.settimeout(2.0) - - request_message = "GET_IO" - client_socket.sendto(request_message.encode('utf-8'), (SERVER_IP, SERVER_PORT)) - - data, _ = client_socket.recvfrom(1024) - response_str = data.decode('utf-8') - - parts = response_str.split('|') - if parts[0] == 'IO' and len(parts) == 2: - io_values = [int(v) for v in parts[1].split(',')] - - if verbose: - print("--- I/O Status ---") - print(f" IN1: {io_values[0]} | {'ON' if io_values[0] else 'OFF'}") - print(f" IN2: {io_values[1]} | {'ON' if io_values[1] else 'OFF'}") - print(f" OUT1: {io_values[2]} | {'ON' if io_values[2] else 'OFF'}") - print(f" OUT2: {io_values[3]} | {'ON' if io_values[3] else 'OFF'}") - # More intuitive E-stop display - if io_values[4] == 0: - print(f" ESTOP: {io_values[4]} | PRESSED (Emergency Stop Active!)") - else: - print(f" ESTOP: {io_values[4]} | OK (Normal Operation)") - print("--------------------------") - - return io_values - - return None - - except socket.timeout: - print("Timeout waiting for I/O response") - return None - except Exception as e: - print(f"Error getting robot I/O: {e}") - return None - -def get_electric_gripper_status(verbose = False): - """ - Get the electric gripper's current status. - Returns [ID, Position, Speed, Current, StatusByte, ObjectDetected] or None. - - Resource usage: ZERO overhead - simple request/response - """ - try: - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as client_socket: - client_socket.settimeout(2.0) - - request_message = "GET_GRIPPER" - client_socket.sendto(request_message.encode('utf-8'), (SERVER_IP, SERVER_PORT)) - - data, _ = client_socket.recvfrom(1024) - response_str = data.decode('utf-8') - - parts = response_str.split('|') - if parts[0] == 'GRIPPER' and len(parts) == 2: - gripper_values = [int(v) for v in parts[1].split(',')] - - # Decode the status byte - status_byte = gripper_values[4] if len(gripper_values) > 4 else 0 - is_active = (status_byte & 0b00000001) != 0 - is_moving = (status_byte & 0b00000010) != 0 - is_calibrated = (status_byte & 0b10000000) != 0 - - # Interpret object detection - object_detection = gripper_values[5] if len(gripper_values) > 5 else 0 - if object_detection == 1: - detection_text = "Yes (closing)" - elif object_detection == 2: - detection_text = "Yes (opening)" - else: - detection_text = "No" - - - if verbose: - # Print formatted status - print("--- Electric Gripper Status ---") - print(f" Device ID: {gripper_values[0]}") - print(f" Current Position: {gripper_values[1]}") - print(f" Current Speed: {gripper_values[2]}") - print(f" Current Current: {gripper_values[3]}") - print(f" Object Detected: {detection_text}") - print(f" Status Byte: {bin(status_byte)}") - print(f" - Calibrated: {is_calibrated}") - print(f" - Active: {is_active}") - print(f" - Moving: {is_moving}") - print("-------------------------------") - - return gripper_values - - return None - - except socket.timeout: - print("Timeout waiting for gripper response") - return None - except Exception as e: - print(f"Error getting gripper status: {e}") - return None - -def get_robot_joint_speeds(): - """ - Get the robot's current joint speeds in steps/sec. - Returns list of 6 speed values or None if it fails. - - Resource usage: ZERO overhead - simple request/response - """ - try: - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as client_socket: - client_socket.settimeout(2.0) - - request_message = "GET_SPEEDS" - client_socket.sendto(request_message.encode('utf-8'), (SERVER_IP, SERVER_PORT)) - - data, _ = client_socket.recvfrom(1024) - response_str = data.decode('utf-8') - - parts = response_str.split('|') - if parts[0] == 'SPEEDS' and len(parts) == 2: - speeds = [float(v) for v in parts[1].split(',')] - return speeds - - return None - - except socket.timeout: - print("Timeout waiting for speeds response") - return None - except Exception as e: - print(f"Error getting robot speeds: {e}") - return None - -def get_robot_pose_matrix(): - """ - Get the robot's current pose as a 4x4 transformation matrix. - Returns 4x4 numpy array or None if it fails. - - Resource usage: ZERO overhead - simple request/response - """ - try: - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as client_socket: - client_socket.settimeout(2.0) - - request_message = "GET_POSE" - client_socket.sendto(request_message.encode('utf-8'), (SERVER_IP, SERVER_PORT)) - - data, _ = client_socket.recvfrom(2048) - response_str = data.decode('utf-8') - - parts = response_str.split('|') - if parts[0] == 'POSE' and len(parts) == 2: - pose_values = [float(v) for v in parts[1].split(',')] - if len(pose_values) == 16: - import numpy as np - return np.array(pose_values).reshape((4, 4)) - - return None - - except socket.timeout: - print("Timeout waiting for pose response") - return None - except Exception as e: - print(f"Error getting robot pose matrix: {e}") - return None - -def is_robot_stopped(threshold_speed: float = 2.0) -> bool: - """ - Check if the robot has stopped moving. - - Args: - threshold_speed: Speed threshold in steps/sec - - Returns: - True if all joints below threshold, False otherwise - - Resource usage: ZERO overhead - simple request/response - """ - speeds = get_robot_joint_speeds() - if not speeds: - return False - - max_speed = max(abs(s) for s in speeds) - return max_speed < threshold_speed - -def is_estop_pressed() -> bool: - """ - Check if the E-stop is currently pressed. - - Returns: - True if E-stop is pressed, False otherwise - - Resource usage: ZERO overhead - simple request/response - """ - io_status = get_robot_io() - if io_status and len(io_status) >= 5: - return io_status[4] == 0 # E-stop is at index 4, 0 means pressed - return False - -def get_robot_status() -> Dict: - """ - Get comprehensive robot status in one call. - - Returns: - Dictionary with pose, angles, speeds, IO, gripper status - - Resource usage: Multiple requests but still zero overhead - """ - return { - 'pose': get_robot_pose(), - 'angles': get_robot_joint_angles(), - 'speeds': get_robot_joint_speeds(), - 'io': get_robot_io(), - 'gripper': get_electric_gripper_status(), - 'stopped': is_robot_stopped(), - 'estop': is_estop_pressed() - } - -# ============================================================================ -# TRACKING FUNCTIONS - ONLY FOR EXPLICIT USE -# ============================================================================ - -def check_command_status(command_id: str) -> Optional[Dict]: - """ - Check status - returns None if tracker not initialized. - Does NOT initialize tracker (read-only). - """ - if _command_tracker and _command_tracker.is_active(): - return _command_tracker.get_status(command_id) - return None - -def is_tracking_active() -> bool: - """ - Check if tracking is active. - Returns False if never used (zero overhead check). - """ - return _command_tracker is not None and _command_tracker.is_active() - -def get_tracking_stats() -> Dict: - """ - Get resource usage statistics. - """ - if _command_tracker and _command_tracker.is_active(): - with _command_tracker.lock: - return { - 'active': True, - 'commands_tracked': len(_command_tracker.command_history), - 'memory_bytes': len(str(_command_tracker.command_history)), - 'thread_active': _command_tracker._thread.is_alive() if _command_tracker._thread else False - } - else: - return { - 'active': False, - 'commands_tracked': 0, - 'memory_bytes': 0, - 'thread_active': False - } - -# ============================================================================ -# CONVENIENCE FUNCTIONS FOR COMMON OPERATIONS -# ============================================================================ - -def wait_for_robot_stopped(timeout: float = 10.0, poll_rate: float = 0.1) -> bool: - """ - Wait for the robot to stop moving. - - Args: - timeout: Maximum time to wait in seconds - poll_rate: How often to check in seconds - - Returns: - True if robot stopped, False if timeout - """ - import time - start_time = time.time() - - while time.time() - start_time < timeout: - if is_robot_stopped(): - return True - time.sleep(poll_rate) - - return False - -def safe_move_with_retry( - move_func, - *args, - max_retries: int = 3, - retry_delay: float = 1.0, - **kwargs -): - """ - Execute a move command with automatic retry on failure. - - Args: - move_func: The movement function to call - *args: Arguments for the movement function - max_retries: Maximum number of retry attempts - retry_delay: Delay between retries in seconds - **kwargs: Keyword arguments for the movement function - - Returns: - Result from the movement function or error dict - """ - import time - - # Ensure tracking is enabled for retry logic - kwargs['wait_for_ack'] = True - - for attempt in range(max_retries): - result = move_func(*args, **kwargs) - - if isinstance(result, dict): - if result.get('status') in ['COMPLETED', 'QUEUED', 'EXECUTING']: - return result - elif result.get('status') in ['FAILED', 'TIMEOUT', 'CANCELLED']: - if attempt < max_retries - 1: - print(f"Attempt {attempt + 1} failed: {result.get('details', 'Unknown error')}") - print(f"Retrying in {retry_delay} seconds...") - time.sleep(retry_delay) - else: - print(f"All {max_retries} attempts failed") - return result - else: - # Non-tracked response, assume success - return result - - return {'status': 'FAILED', 'details': f'Failed after {max_retries} attempts'} \ No newline at end of file diff --git a/smooth_motion.py b/smooth_motion.py deleted file mode 100644 index d1daad7..0000000 --- a/smooth_motion.py +++ /dev/null @@ -1,819 +0,0 @@ -""" -Smooth Motion Module for PAROL6 Robotic Arm -============================================ -This module provides advanced trajectory generation capabilities including: -- Circular and arc movements -- Cubic spline trajectories -- Motion blending -- Pre-computed and real-time trajectory generation - -Compatible with: -- numpy==1.23.4 -- scipy==1.11.4 -- roboticstoolbox-python==1.0.3 -""" - -import sys -import warnings -from collections import namedtuple -from roboticstoolbox import DHRobot -from spatialmath.base import trinterp - -# Version compatibility check -try: - import numpy as np - # Check numpy version - np_version = tuple(map(int, np.__version__.split('.')[:2])) - if np_version < (1, 23): - warnings.warn(f"NumPy version {np.__version__} detected. Recommended: 1.23.4") - - from scipy.interpolate import CubicSpline - from scipy.spatial.transform import Rotation, Slerp - import scipy - # Check scipy version - scipy_version = tuple(map(int, scipy.__version__.split('.')[:2])) - if scipy_version < (1, 11): - warnings.warn(f"SciPy version {scipy.__version__} detected. Recommended: 1.11.4") - -except ImportError as e: - print(f"Error importing required packages: {e}") - print("Please install: pip3 install numpy==1.23.4 scipy==1.11.4") - sys.exit(1) - -from spatialmath import SE3 -import time -from typing import List, Tuple, Optional, Dict, Union -from collections import deque - -# Import PAROL6 specific modules (these should be in your path) -try: - import GUI.files.PAROL6_ROBOT -except ImportError: - print("Warning: PAROL6 modules not found. Some functions may not work.") - PAROL6_ROBOT = None - -# Global variable to track previous tolerance for logging changes -_prev_tolerance = None - -# IK Result structure -IKResult = namedtuple('IKResult', 'success q iterations residual tolerance_used violations') - -def normalize_angle(angle): - """Normalize angle to [-pi, pi] range to handle angle wrapping""" - while angle > np.pi: - angle -= 2 * np.pi - while angle < -np.pi: - angle += 2 * np.pi - return angle - -def unwrap_angles(q_solution, q_current): - """ - Unwrap angles in the solution to be closest to current position. - This handles the angle wrapping issue where -179° and 181° are close but appear far. - """ - q_unwrapped = q_solution.copy() - - for i in range(len(q_solution)): - # Calculate the difference - diff = q_solution[i] - q_current[i] - - # If the difference is more than pi, we need to unwrap - if diff > np.pi: - # Solution is too far in positive direction, subtract 2*pi - q_unwrapped[i] = q_solution[i] - 2 * np.pi - elif diff < -np.pi: - # Solution is too far in negative direction, add 2*pi - q_unwrapped[i] = q_solution[i] + 2 * np.pi - - return q_unwrapped - -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.array(q, dtype=float) - - # Calculate manipulability measure (closer to 0 = closer to singularity) - manip = robot.manipulability(q_array) - singularity_threshold = 0.001 - - sing_normalized = np.clip(manip / singularity_threshold, 0.0, 1.0) - adaptive_tol = loose_tol + (strict_tol - loose_tol) * sing_normalized - - # Log tolerance changes (only log significant changes to avoid spam) - if _prev_tolerance is None or abs(adaptive_tol - _prev_tolerance) / _prev_tolerance > 0.5: - tol_category = "LOOSE" if adaptive_tol > 1e-7 else "MODERATE" if adaptive_tol > 5e-10 else "STRICT" - print(f"Adaptive IK tolerance: {adaptive_tol:.2e} ({tol_category}) - Manipulability: {manip:.8f} (threshold: {singularity_threshold})") - _prev_tolerance = adaptive_tol - - return adaptive_tol - -def calculate_configuration_dependent_max_reach(q_seed): - """ - 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. - """ - 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: - # Calculate reduction factor based on proximity to 90° - 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, - max_depth: int = 4, - ilimit: int = 100, - jogging: bool = False -): - """ - Uses adaptive tolerance based on proximity to singularities. - If necessary, recursively subdivide the motion until ikine_LMS converges - on every segment. Finally check that solution respects joint limits. - """ - 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).""" - # Calculate current and target reach - current_reach = np.linalg.norm(Ta.t) - target_reach = np.linalg.norm(Tb.t) - - # Check if this is an inward movement (recovery) - is_recovery = target_reach < current_reach - - # Calculate configuration-dependent maximum reach based on joint 5 position - max_reach_threshold = calculate_configuration_dependent_max_reach(q_seed) - - # Determine damping based on reach and movement direction - if is_recovery: - # Recovery mode - always use low damping - damping = 0.0000001 - else: - # Check if we're near configuration-dependent max reach - if current_reach >= max_reach_threshold: - print(f"Reach limit exceeded: {current_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 - ) - - # ── 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) - - # Check if solution respects joint limits - if PAROL6_ROBOT is not None: - target_q = path[-1] if len(path) != 0 else None - solution_valid, violations = PAROL6_ROBOT.check_joint_limits(current_q, target_q) - if ok and solution_valid: - return IKResult(True, path[-1], its, resid, adaptive_tol, violations) - else: - return IKResult(False, None, its, resid, adaptive_tol, violations) - else: - # If PAROL6_ROBOT not available, skip joint limit checking - if ok and len(path) > 0: - return IKResult(True, path[-1], its, resid, adaptive_tol, None) - else: - return IKResult(False, None, its, resid, adaptive_tol, None) - -# ============================================================================ -# END OF IK SOLVER FUNCTIONS -# ============================================================================ - -class TrajectoryGenerator: - """Base class for trajectory generation with caching support""" - - def __init__(self, control_rate: float = 100.0): - """ - Initialize trajectory generator - - Args: - control_rate: Control loop frequency in Hz (default 100Hz for PAROL6) - """ - self.control_rate = control_rate - self.dt = 1.0 / control_rate - self.trajectory_cache = {} - - def generate_timestamps(self, duration: float) -> np.ndarray: - """Generate evenly spaced timestamps for trajectory""" - num_points = int(duration * self.control_rate) - return np.linspace(0, duration, num_points) - -class CircularMotion(TrajectoryGenerator): - """Generate circular and arc trajectories in 3D space""" - - def generate_arc_3d(self, - start_pose: List[float], - end_pose: List[float], - center: List[float], - normal: Optional[List[float]] = None, - clockwise: bool = True, - duration: float = 2.0) -> np.ndarray: - """ - Generate a 3D circular arc trajectory - - Args: - start_pose: Starting pose [x, y, z, rx, ry, rz] (mm and degrees) - end_pose: Ending pose [x, y, z, rx, ry, rz] (mm and degrees) - center: Center point of arc [x, y, z] (mm) - normal: Normal vector to arc plane (default: z-axis) - clockwise: Direction of rotation - duration: Time to complete arc (seconds) - - Returns: - Array of poses along the arc trajectory - """ - # Convert to numpy arrays - start_pos = np.array(start_pose[:3]) - end_pos = np.array(end_pose[:3]) - center_pt = np.array(center) - - # Calculate radius vectors - r1 = start_pos - center_pt - r2 = end_pos - center_pt - radius = np.linalg.norm(r1) - - # Determine arc plane normal if not provided - if normal is None: - 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 = normal / np.linalg.norm(normal) - - # Calculate arc angle - r1_norm = r1 / np.linalg.norm(r1) - r2_norm = r2 / np.linalg.norm(r2) - cos_angle = np.clip(np.dot(r1_norm, r2_norm), -1, 1) - arc_angle = np.arccos(cos_angle) - - # Check direction using cross product - cross = np.cross(r1_norm, r2_norm) - if np.dot(cross, normal) < 0: - arc_angle = 2 * np.pi - arc_angle - - if clockwise: - arc_angle = -arc_angle - - # Generate trajectory points - timestamps = self.generate_timestamps(duration) - trajectory = [] - - for i, t in enumerate(timestamps): - # Interpolation factor - s = t / duration - - # For first point, use exact start position - if i == 0: - current_pos = start_pos - else: - # Rotate radius vector - angle = s * arc_angle - rot_matrix = self._rotation_matrix_from_axis_angle(normal, angle) - current_pos = center_pt + rot_matrix @ r1 - - # Interpolate orientation (SLERP) - current_orient = self._slerp_orientation(start_pose[3:], end_pose[3:], s) - - # Combine position and orientation - pose = np.concatenate([current_pos, current_orient]) - trajectory.append(pose) - - return np.array(trajectory) - - def generate_circle_3d(self, - center: List[float], - radius: float, - normal: List[float] = [0, 0, 1], - start_angle: float = None, - duration: float = 4.0, - start_point: List[float] = None) -> np.ndarray: - """ - Generate a complete circle trajectory that starts at start_point - """ - timestamps = self.generate_timestamps(duration) - trajectory = [] - - # Create orthonormal basis for circle plane - normal = np.array(normal) / np.linalg.norm(normal) - u = self._get_perpendicular_vector(normal) - v = np.cross(normal, u) - - center_np = np.array(center) - - # CRITICAL FIX: Validate and handle geometry - if start_point is not None: - 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 - - # Get distance from center in the plane - dist_in_plane = np.linalg.norm(to_start_plane) - - if dist_in_plane < 0.001: - # Start point is at center - can't determine angle - print(f" WARNING: Start point is at circle center, using default position") - start_angle = 0 - actual_start = center_np + radius * u - else: - # Calculate the angle of the start point - to_start_normalized = to_start_plane / dist_in_plane - u_comp = np.dot(to_start_normalized, u) - v_comp = np.dot(to_start_normalized, v) - start_angle = np.arctan2(v_comp, u_comp) - - # CHECK FOR INVALID GEOMETRY - radius_error = abs(dist_in_plane - radius) - if radius_error > radius * 0.3: # More than 30% off - print(f" WARNING: Start point is {dist_in_plane:.1f}mm from center,") - print(f" but circle radius is {radius:.1f}mm!") - - # AUTO-CORRECT: Adjust center to make geometry valid - print(f" AUTO-CORRECTING: Moving center to maintain {radius}mm radius from start") - direction = to_start_plane / dist_in_plane - center_np = start_pos - direction * radius - print(f" New center: {center_np.round(1)}") - - # Recalculate with new center - to_start = start_pos - center_np - to_start_plane = to_start - np.dot(to_start, normal) * normal - dist_in_plane = np.linalg.norm(to_start_plane) - - actual_start = start_pos - else: - start_angle = 0 if start_angle is None else start_angle - actual_start = None - - # Generate the circle - for i, t in enumerate(timestamps): - if i == 0 and actual_start is not None: - # First point MUST be exactly the start point - pos = actual_start - else: - # Generate circle points - angle = start_angle + (2 * np.pi * t / duration) - pos = center_np + radius * (np.cos(angle) * u + np.sin(angle) * v) - - # Placeholder orientation (will be overridden) - orient = [0, 0, 0] - trajectory.append(np.concatenate([pos, orient])) - - return np.array(trajectory) - - 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]]) - - # Rodrigues' formula - R = np.eye(3) + sin_a * K + (1 - cos_a) * K @ K - return R - - def _get_perpendicular_vector(self, v: np.ndarray) -> np.ndarray: - """Find a vector perpendicular to the given vector""" - v = np.array(v) # Ensure it's a numpy array - if abs(v[0]) < 0.9: - return np.cross(v, [1, 0, 0]) / np.linalg.norm(np.cross(v, [1, 0, 0])) - else: - return np.cross(v, [0, 1, 0]) / np.linalg.norm(np.cross(v, [0, 1, 0])) - - def _slerp_orientation(self, start_orient: List[float], - end_orient: List[float], - t: float) -> np.ndarray: - """Spherical linear interpolation for orientation""" - # Convert to quaternions - r1 = Rotation.from_euler('xyz', start_orient, degrees=True) - r2 = Rotation.from_euler('xyz', end_orient, degrees=True) - - # Create slerp object - compatible with scipy 1.11.4 - # Stack rotations into a single Rotation object - key_rots = Rotation.from_quat([r1.as_quat(), r2.as_quat()]) - slerp = Slerp([0, 1], key_rots) - - # Interpolate - interp_rot = slerp(t) - return interp_rot.as_euler('xyz', degrees=True) - -class SplineMotion(TrajectoryGenerator): - """Generate smooth spline trajectories through waypoints""" - - 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) -> np.ndarray: - """ - Generate cubic spline trajectory through waypoints - - Args: - waypoints: List of poses [x, y, z, rx, ry, rz] - timestamps: Time for each waypoint (auto-generated if None) - velocity_start: Initial velocity (zero if None) - velocity_end: Final velocity (zero if None) - - Returns: - Array of interpolated poses - """ - waypoints = np.array(waypoints) - num_waypoints = len(waypoints) - - # Auto-generate timestamps if not provided - if timestamps is None: - # Estimate based on distance - total_dist = 0 - for i in range(1, num_waypoints): - dist = np.linalg.norm(waypoints[i, :3] - waypoints[i-1, :3]) - total_dist += dist - - # Assume average speed of 50 mm/s - total_time = total_dist / 50.0 - timestamps = np.linspace(0, total_time, num_waypoints) - - # Create splines for position - pos_splines = [] - for i in range(3): - bc_type = 'not-a-knot' # Default boundary condition - - # Apply velocity boundary conditions if specified - if velocity_start is not None and velocity_end is not None: - bc_type = ((1, velocity_start[i]), (1, velocity_end[i])) - - spline = CubicSpline(timestamps, waypoints[:, i], bc_type=bc_type) - pos_splines.append(spline) - - # Create splines for orientation (convert to quaternions for smooth interpolation) - rotations = [Rotation.from_euler('xyz', wp[3:], degrees=True) for wp in waypoints] - # Stack quaternions for scipy 1.11.4 compatibility - quats = np.array([r.as_quat() for r in rotations]) - key_rots = Rotation.from_quat(quats) - slerp = Slerp(timestamps, key_rots) - - # Generate dense trajectory - t_eval = self.generate_timestamps(timestamps[-1]) - trajectory = [] - - for t in t_eval: - # Evaluate position splines - pos = [spline(t) for spline in pos_splines] - - # Evaluate orientation - rot = slerp(t) - orient = rot.as_euler('xyz', degrees=True) - - trajectory.append(np.concatenate([pos, orient])) - - return np.array(trajectory) - - def generate_quintic_spline(self, - waypoints: List[List[float]], - timestamps: Optional[List[float]] = None) -> np.ndarray: - """ - Generate quintic (5th order) spline with zero velocity and acceleration at endpoints - - Args: - waypoints: List of poses [x, y, z, rx, ry, rz] - timestamps: Time for each waypoint - - Returns: - Array of interpolated poses - """ - # For quintic spline, we need to ensure zero velocity and acceleration - # at the endpoints for smooth motion - return self.generate_cubic_spline( - waypoints, - timestamps, - velocity_start=[0, 0, 0], - velocity_end=[0, 0, 0] - ) - -class MotionBlender: - """Blend between different motion segments for smooth transitions""" - - def __init__(self, blend_time: float = 0.5): - self.blend_time = blend_time - - def blend_trajectories(self, traj1, traj2, blend_samples=50): - """Blend two trajectory segments with improved velocity continuity""" - - if blend_samples < 4: - return np.vstack([traj1, traj2]) - - # Use more samples for smoother blending - blend_samples = max(blend_samples, 20) # Minimum 20 samples for smooth blend - - # Calculate overlap region more carefully - overlap_start = max(0, len(traj1) - blend_samples // 3) - 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_end_pose = traj2[overlap_end] if overlap_end < len(traj2) else traj2[0] - - # Generate smooth transition using S-curve - blended = [] - for i in range(blend_samples): - t = i / (blend_samples - 1) - # Use smoothstep function for smoother acceleration - s = t * t * (3 - 2 * t) # Smoothstep - - # Blend position - pos_blend = blend_start_pose * (1 - s) + blend_end_pose * s - - # For orientation, use 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) - - 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:] - ]) - - return result - -class SmoothMotionCommand: - """Command class for executing smooth motions on PAROL6""" - - def __init__(self, trajectory: np.ndarray, speed_factor: float = 1.0): - """ - Initialize smooth motion command - - Args: - trajectory: Pre-computed trajectory array - speed_factor: Speed scaling factor (1.0 = normal speed) - """ - self.trajectory = trajectory - self.speed_factor = speed_factor - self.current_index = 0 - self.is_finished = False - self.is_valid = True - - def prepare_for_execution(self, current_position_in): - """Validate trajectory is reachable from current position""" - # Check if IK solver is available - if solve_ik_with_adaptive_tol_subdivision is None: - print("Warning: IK solver not available, skipping validation") - self.is_valid = True - return True - - try: - # Convert current position to radians - current_q = np.array([PAROL6_ROBOT.STEPS2RADS(p, i) - for i, p in enumerate(current_position_in)]) - - # Check first waypoint is reachable - 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=20 - ) - - if not ik_result.success: - print(f"Smooth motion validation failed: Cannot reach first waypoint") - self.is_valid = False - return False - - print(f"Smooth motion prepared with {len(self.trajectory)} waypoints") - return True - - except Exception as e: - print(f"Smooth motion preparation error: {e}") - self.is_valid = False - return False - - def execute_step(self, Position_in, Speed_out, Command_out, **kwargs): - """Execute one step of the smooth motion""" - if self.is_finished or not self.is_valid: - return True - - # Check if required modules are available - if PAROL6_ROBOT is None or solve_ik_with_adaptive_tol_subdivision is None: - print("Error: Required PAROL6 modules not available") - self.is_finished = True - Speed_out[:] = [0] * 6 - Command_out.value = 255 - return True - - # Apply speed scaling - step_increment = max(1, int(self.speed_factor)) - self.current_index += step_increment - - if self.current_index >= len(self.trajectory): - print("Smooth motion completed") - self.is_finished = True - Speed_out[:] = [0] * 6 - Command_out.value = 255 - return True - - # Get current target pose - target_pose = self.trajectory[self.current_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 = np.array([PAROL6_ROBOT.STEPS2RADS(p, i) - for i, p in enumerate(Position_in)]) - - # Solve IK - ik_result = solve_ik_with_adaptive_tol_subdivision( - PAROL6_ROBOT.robot, target_se3, current_q, ilimit=20 - ) - - if not ik_result.success: - print(f"IK failed at trajectory point {self.current_index}") - self.is_finished = True - Speed_out[:] = [0] * 6 - Command_out.value = 255 - return True - - # Convert to steps and send - target_steps = [int(PAROL6_ROBOT.RAD2STEPS(q, i)) - for i, q in enumerate(ik_result.q)] - - # Calculate velocities for smooth following - for i in range(6): - Speed_out[i] = int((target_steps[i] - Position_in[i]) * 10) # P-control factor - - Command_out.value = 156 # Smooth motion command - return False - -# Helper functions for integration with robot_api.py - -def execute_circle(center: List[float], - radius: float, - duration: float = 4.0, - normal: List[float] = [0, 0, 1]) -> str: - """ - Execute a circular motion on PAROL6 - - Args: - center: Center point [x, y, z] in mm - radius: Circle radius in mm - duration: Time to complete circle - normal: Normal vector to circle plane - - Returns: - Command string for robot_api - """ - motion_gen = CircularMotion() - trajectory = motion_gen.generate_circle_3d(center, radius, normal, 0, duration) - - # Convert to command string format - traj_str = "|".join([",".join(map(str, pose)) for pose in trajectory]) - command = f"SMOOTH_MOTION|CIRCLE|{traj_str}" - - return command - -def execute_arc(start_pose: List[float], - end_pose: List[float], - center: List[float], - clockwise: bool = True, - duration: float = 2.0) -> str: - """ - Execute an arc motion on PAROL6 - - Args: - start_pose: Starting pose [x, y, z, rx, ry, rz] - end_pose: Ending pose [x, y, z, rx, ry, rz] - center: Arc center point [x, y, z] - clockwise: Direction of rotation - duration: Time to complete arc - - Returns: - Command string for robot_api - """ - motion_gen = CircularMotion() - trajectory = motion_gen.generate_arc_3d(start_pose, end_pose, center, - clockwise=clockwise, duration=duration) - - # Convert to command string format - traj_str = "|".join([",".join(map(str, pose)) for pose in trajectory]) - command = f"SMOOTH_MOTION|ARC|{traj_str}" - - return command - -def execute_spline(waypoints: List[List[float]], - total_time: Optional[float] = None) -> str: - """ - Execute a spline motion through waypoints - - Args: - waypoints: List of poses [x, y, z, rx, ry, rz] - total_time: Total time for motion (auto-calculated if None) - - Returns: - Command string for robot_api - """ - motion_gen = SplineMotion() - - # Generate timestamps if total_time is provided - timestamps = None - if total_time: - timestamps = np.linspace(0, total_time, len(waypoints)) - - trajectory = motion_gen.generate_cubic_spline(waypoints, timestamps) - - # Convert to command string format - traj_str = "|".join([",".join(map(str, pose)) for pose in trajectory]) - command = f"SMOOTH_MOTION|SPLINE|{traj_str}" - - return command - -# Example usage -if __name__ == "__main__": - # Example: Generate a circle trajectory - circle_gen = CircularMotion() - circle_traj = circle_gen.generate_circle_3d( - center=[200, 0, 200], # mm - radius=50, # mm - duration=4.0 # seconds - ) - print(f"Generated circle with {len(circle_traj)} points") - - # Example: Generate arc trajectory - arc_traj = circle_gen.generate_arc_3d( - start_pose=[250, 0, 200, 0, 0, 0], - end_pose=[200, 50, 200, 0, 0, 90], - center=[200, 0, 200], - duration=2.0 - ) - print(f"Generated arc with {len(arc_traj)} points") - - # Example: Generate spline through waypoints - spline_gen = SplineMotion() - waypoints = [ - [200, 0, 100, 0, 0, 0], - [250, 50, 150, 0, 15, 45], - [200, 100, 200, 0, 30, 90], - [150, 50, 150, 0, 15, 45], - [200, 0, 100, 0, 0, 0] - ] - spline_traj = spline_gen.generate_cubic_spline(waypoints) - print(f"Generated spline with {len(spline_traj)} points") \ No newline at end of file diff --git a/test_script.py b/test_script.py deleted file mode 100644 index 24a9be1..0000000 --- a/test_script.py +++ /dev/null @@ -1,33 +0,0 @@ -from robot_api import move_robot_joints, home_robot, delay_robot, get_robot_joint_angles, control_pneumatic_gripper,get_robot_pose, control_electric_gripper, move_robot_pose,move_robot_cartesian,get_electric_gripper_status,get_robot_io -import time -print("Homing robot...") -time.sleep(2) -control_electric_gripper(action = "calibrate") -time.sleep(2) -control_electric_gripper(action='move', position=100, speed=150, current = 200) -time.sleep(2) -control_electric_gripper(action='move', position=200, speed=150, current = 200) -time.sleep(2) -print(get_robot_joint_angles()) -print(get_robot_pose()) -print("Moving to new position...") -control_pneumatic_gripper("open",1) -time.sleep(0.3) -control_pneumatic_gripper("close",1) -time.sleep(0.3) -control_pneumatic_gripper("open",1) -time.sleep(0.3) -control_pneumatic_gripper("close",1) -time.sleep(0.3) -move_robot_joints([90, -90, 160, 12, 12, 180], duration=5.5) -time.sleep(6) -move_robot_joints([50, -60, 180, -12, 32, 0], duration=5.5) -time.sleep(6) -move_robot_joints([90, -90, 160, 12, 12, 180], duration=5.5) -time.sleep(6) -move_robot_pose([7, 250, 200, -100, 0, -90], duration=5.5) -time.sleep(6) -move_robot_cartesian([7, 250, 150, -100, 0, -90], speed_percentage=50) -delay_robot(0.2) -print(get_electric_gripper_status()) -print(get_robot_io()) diff --git a/test_smooth_motion.py b/test_smooth_motion.py deleted file mode 100644 index fe997bc..0000000 --- a/test_smooth_motion.py +++ /dev/null @@ -1,842 +0,0 @@ -import time -import robot_api -import numpy as np - -# Define the safe starting joint configuration for all smooth motion tests. -# This ensures consistency and repeatability for each test. -# Angles: [J1, J2, J3, J4, J5, J6] in degrees. -SAFE_SMOOTH_START_JOINTS = [42.697,-89.381,144.831,-0.436,31.528,180.0] - -def initialize_test_position(): - """ - Moves the robot to the predefined safe starting joint angles and waits. - This function is called before every smooth motion test. - - Returns: - list: The robot's Cartesian pose [x, y, z, rx, ry, rz] after moving, - or None if the move fails or the pose cannot be retrieved. - """ - print("\n" + "="*60) - print(f"MOVING TO SAFE STARTING POSITION: {SAFE_SMOOTH_START_JOINTS}") - print("="*60) - - # Move to the joint position with a 4-second duration and wait for acknowledgment. - result = robot_api.move_robot_joints( - SAFE_SMOOTH_START_JOINTS, - duration=4, - wait_for_ack=True, - timeout=5 - ) - print(f"--> Move command result: {result}") - - # Wait until the robot has physically stopped moving. - if robot_api.wait_for_robot_stopped(timeout=10): - print("--> Robot has reached the starting position.") - time.sleep(1) - start_pose = robot_api.get_robot_pose() - if start_pose: - print(f"--> Starting Pose confirmed at: {[round(p, 2) for p in start_pose]}") - return start_pose - else: - print("--> ERROR: Could not retrieve robot pose after moving.") - return None - else: - print("--> ERROR: Robot did not stop in time. Aborting test.") - return None - -def test_smooth_circle_basic(start_pose): - """Tests the smooth_circle command with different planes, directions, and timing modes.""" - print("\n--- TESTING SMOOTH CIRCLE (BASIC) ---") - - # Define a center point relative to the starting Z-height - radius = 30.0 # 30mm radius - - center_point = [start_pose[0], start_pose[1] + radius, start_pose[2]] # Changed from +50 to +radius - - # Test 1: XY plane, counter-clockwise with DURATION - print("\n[1/4] Testing Circle: XY Plane, Counter-Clockwise (Duration mode)") - result = robot_api.smooth_circle( - center=center_point, - radius=radius, - plane='XY', - duration=5.0, # Using duration - clockwise=False, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - time.sleep(2) - - # Test 1: XY plane, counter-clockwise with DURATION in TRF - print("\n[2/4] Testing Circle: XY Plane, Counter-Clockwise (Duration mode)") - result = robot_api.smooth_circle( - center=center_point, - radius=radius, - frame='TRF', # NEW: Test in TRF - plane='XY', - duration=5.0, # Using duration - clockwise=False, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - time.sleep(2) - - # Test 2: XZ plane, clockwise with SPEED PERCENTAGE - print("\n[3/4] Testing Circle: XZ Plane, Clockwise (Speed percentage mode)") - result = robot_api.smooth_circle( - center=center_point, - radius=radius, - plane='XZ', - speed_percentage=30, # Using speed percentage (30% speed) - clockwise=True, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - time.sleep(2) - - # Test 3: YZ plane with specified start position (NEW) - print("\n[4/4] Testing Circle: YZ Plane with SPECIFIED START POSITION") - # Define a start position slightly offset from current - specified_start = [start_pose[0] + 10, start_pose[1] + 10, start_pose[2], - start_pose[3], start_pose[4], start_pose[5]] - result = robot_api.smooth_circle( - center=center_point, - radius=radius, - plane='YZ', - start_pose=specified_start, # NEW: Will transition here first - duration=5.0, - clockwise=False, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - -def test_smooth_arc_with_start_positions(start_pose): - """Tests smooth arc commands with specified start positions and transitions.""" - print("\n--- TESTING SMOOTH ARC WITH START POSITIONS ---") - - # Test 1: Arc with FAR start position (should see smooth transition) - print("\n[1/4] Testing Arc with FAR START POSITION (big transition)") - far_start = [start_pose[0] + 40, start_pose[1] - 20, start_pose[2] + 10, - start_pose[3], start_pose[4], start_pose[5]] - arc_center = [far_start[0] - 20, far_start[1], far_start[2]] - end_pose_arc = [arc_center[0], arc_center[1] + 20, far_start[2], - far_start[3], far_start[4], far_start[5] + 45] - - print(f" Current position: {[round(p, 1) for p in start_pose[:3]]}") - print(f" Transition to: {[round(p, 1) for p in far_start[:3]]}") - print(f" Then arc to: {[round(p, 1) for p in end_pose_arc[:3]]}") - - result = robot_api.smooth_arc_center( - end_pose=end_pose_arc, - center=arc_center, - start_pose=far_start, # Will transition here first - duration=6.0, - clockwise=True, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=12) - time.sleep(2) - - # Re-initialize for next test - current_pose = initialize_test_position() - if not current_pose: return - - # Test 2: Arc with CLOSE start position (minimal transition) - print("\n[2/4] Testing Arc with CLOSE START POSITION (minimal transition)") - close_start = [current_pose[0] + 2, current_pose[1] + 2, current_pose[2], - current_pose[3], current_pose[4], current_pose[5]] - arc_center = [close_start[0] - 15, close_start[1], close_start[2]] - end_pose_arc = [arc_center[0], arc_center[1] + 15, close_start[2], - close_start[3], close_start[4], close_start[5] + 30] - - print(f" Current position: {[round(p, 1) for p in current_pose[:3]]}") - print(f" Transition to: {[round(p, 1) for p in close_start[:3]]}") - print(f" Then arc to: {[round(p, 1) for p in end_pose_arc[:3]]}") - - result = robot_api.smooth_arc_center( - end_pose=end_pose_arc, - center=arc_center, - start_pose=close_start, # Very close, minimal transition - speed_percentage=40, - clockwise=False, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - time.sleep(2) - - # Test 3: Parametric arc with specified start - print("\n[3/4] Testing PARAMETRIC Arc with specified start") - param_start = [current_pose[0] - 10, current_pose[1] + 5, current_pose[2], - current_pose[3], current_pose[4], current_pose[5]] - end_pose_param = [param_start[0] + 20, param_start[1] - 10, param_start[2], - param_start[3], param_start[4], param_start[5]] - - result = robot_api.smooth_arc_parametric( - end_pose=end_pose_param, - radius=20.0, - arc_angle=60.0, - start_pose=param_start, - duration=4.0, - clockwise=False, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - - # Test 4: Arc in TRF - arc plane follows tool orientation - print("\n[4/4] Testing Arc in TOOL REFERENCE FRAME (TRF)") - # In TRF, the arc is defined relative to the tool's coordinate system - trf_start = [10, 10, 10, 0, 0, 0] # Position relative to tool - trf_center = [0, 0, 0] # Center at tool origin - trf_end = [10, -10, 10, 0, 0, 45] # End position in tool frame - - print(f" TRF Arc - all coordinates relative to tool position/orientation") - print(f" If tool is tilted, the arc plane will be tilted too!") - - result = robot_api.smooth_arc_center( - end_pose=trf_end, - center=trf_center, - frame='TRF', # NEW: Using Tool Reference Frame - start_pose=trf_start, - duration=5.0, - clockwise=False, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - -def test_motion_chaining(start_pose): - """Tests precise motion chaining using end pose of one motion as start of next.""" - print("\n--- TESTING MOTION CHAINING (NEW) ---") - print("This tests using the exact end pose of one motion as the start of the next") - - # Motion 1: Arc to a specific end pose - print("\n[1/4] First Motion: Arc") - arc_center = [start_pose[0] - 20, start_pose[1], start_pose[2]] - arc_end = [arc_center[0], arc_center[1] + 30, start_pose[2], - start_pose[3], start_pose[4] + 15, start_pose[5] + 45] - - result = robot_api.smooth_arc_center( - end_pose=arc_end, - center=arc_center, - duration=4.0, - clockwise=True, - wait_for_ack=True - ) - print(f"--> Arc ended at: {[round(p, 1) for p in arc_end[:3]]}") - robot_api.wait_for_robot_stopped(timeout=8) - time.sleep(1) - - # Motion 2: Circle in TRF starting exactly where arc ended - print("\n[2/4] Second Motion: Circle in TRF starting at arc's end position") - # In TRF, center is relative to current tool position - trf_circle_center = [0, 25, 0] # 25mm forward in tool Y-axis - - result = robot_api.smooth_circle( - center=trf_circle_center, - radius=25.0, - plane='XY', # This is the tool's XY plane, not world XY! - frame='TRF', # NEW: Circle plane follows tool orientation - start_pose=arc_end, # Start exactly where arc ended - speed_percentage=35, - clockwise=False, - wait_for_ack=True - ) - print(f"--> Circle in TRF completed (plane followed tool orientation)") - robot_api.wait_for_robot_stopped(timeout=10) - time.sleep(1) - - # Since circle returns to start, we know where we are - circle_end = arc_end # Circle returns to its start point - - # Motion 3: Helix starting where circle ended - print("\n[3/4] Third Motion: Helix starting at circle's position") - # Calculate actual radius from circle end position - helix_center = [circle_end[0], circle_end[1], circle_end[2] - 30] - # Use the actual distance as radius - actual_radius = np.sqrt((circle_end[0] - helix_center[0])**2 + - (circle_end[1] - helix_center[1])**2) - radius = max(actual_radius, 1.0) # Use actual distance, minimum 1mm - - result = robot_api.smooth_helix( - center=helix_center, - radius=15.0, - pitch=10.0, - height=30.0, - start_pose=circle_end, # Start where circle ended - duration=6.0, - clockwise=True, - wait_for_ack=True - ) - print(f"--> Helix completed") - robot_api.wait_for_robot_stopped(timeout=10) - time.sleep(1) - - # Calculate helix end position (approximately) - helix_end = [helix_center[0] + 15, helix_center[1], helix_center[2] + 30, - circle_end[3], circle_end[4], circle_end[5]] - - # Motion 4: Spline back to near start - print("\n[4/4] Fourth Motion: Spline path back near start") - waypoints = [ - helix_end, # Start from helix end - [helix_end[0] - 10, helix_end[1] - 10, helix_end[2] - 10, - helix_end[3], helix_end[4], helix_end[5] - 20], - [start_pose[0] + 5, start_pose[1] + 5, start_pose[2], - start_pose[3], start_pose[4], start_pose[5]] - ] - - result = robot_api.smooth_spline( - waypoints=waypoints[1:], # Skip first since we specify start_pose - start_pose=waypoints[0], # Explicitly start from helix end - speed_percentage=30, - wait_for_ack=True - ) - print(f"--> Spline completed - returned near start") - robot_api.wait_for_robot_stopped(timeout=10) - -def test_smooth_spline_with_starts(start_pose): - """Tests smooth_spline with various start position scenarios.""" - print("\n--- TESTING SMOOTH SPLINE WITH START POSITIONS ---") - - # Test 1: Spline with default start (current position) - print("\n[1/4] Spline with DEFAULT start (from current position)") - waypoints = [] - for i in range(4): - x = start_pose[0] + i * 15 - y = start_pose[1] + (15 if i % 2 else -15) - z = start_pose[2] - waypoints.append([x, y, z, start_pose[3], start_pose[4], start_pose[5]]) - - result = robot_api.smooth_spline( - waypoints=waypoints, - # No start_pose specified - uses current - duration=5.0, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - time.sleep(2) - - # Re-initialize - current_pose = initialize_test_position() - if not current_pose: return - - # Test 2: Spline with specified start far from first waypoint - print("\n[2/4] Spline with SPECIFIED start (different from first waypoint)") - specified_start = [current_pose[0] - 20, current_pose[1] + 15, current_pose[2], - current_pose[3], current_pose[4], current_pose[5]] - - waypoints = [ - [specified_start[0] + 30, specified_start[1], specified_start[2], - specified_start[3], specified_start[4], specified_start[5]], - [specified_start[0] + 40, specified_start[1] + 20, specified_start[2], - specified_start[3], specified_start[4], specified_start[5]], - [specified_start[0] + 20, specified_start[1] + 30, specified_start[2], - specified_start[3], specified_start[4], specified_start[5]] - ] - - print(f" Current: {[round(p, 1) for p in current_pose[:3]]}") - print(f" Will transition to: {[round(p, 1) for p in specified_start[:3]]}") - print(f" Then follow spline through waypoints") - - result = robot_api.smooth_spline( - waypoints=waypoints, - start_pose=specified_start, # Will transition here first - speed_percentage=40, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=12) - time.sleep(2) - - # Re-initialize - current_pose = initialize_test_position() - if not current_pose: return - - # Test 3: Spline with start matching first waypoint (no transition needed) - print("\n[3/4] Spline with start MATCHING first waypoint (no transition)") - first_waypoint = [current_pose[0] + 5, current_pose[1] + 5, current_pose[2], - current_pose[3], current_pose[4], current_pose[5]] - - waypoints = [ - first_waypoint, # Same as start_pose - [first_waypoint[0] + 20, first_waypoint[1] + 10, first_waypoint[2], - first_waypoint[3], first_waypoint[4], first_waypoint[5]], - [first_waypoint[0] + 10, first_waypoint[1] + 25, first_waypoint[2], - first_waypoint[3], first_waypoint[4], first_waypoint[5]] - ] - - result = robot_api.smooth_spline( - waypoints=waypoints[1:], # Skip first since we use it as start_pose - start_pose=first_waypoint, # Same as would-be first waypoint - duration=4.0, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=8) - - # Test 4: Spline in TRF - waypoints relative to tool - print("\n[4/4] Spline in TOOL REFERENCE FRAME (TRF)") - # In TRF, all waypoints are relative to the tool's coordinate system - trf_waypoints = [ - [20, 0, 0, 0, 0, 0], # 20mm forward in tool X - [20, 20, 0, 0, 0, 15], # Add 20mm in tool Y - [0, 20, 10, 0, 0, 30], # Move to tool Y=20, Z=10 - [0, 0, 0, 0, 0, 0] # Return to tool origin - ] - - print(f" TRF Spline - all waypoints relative to tool coordinate system") - print(f" If tool is rotated, entire spline path rotates with it!") - - result = robot_api.smooth_spline( - waypoints=trf_waypoints, - frame='TRF', # NEW: Waypoints interpreted in tool frame - duration=6.0, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - -def test_smooth_helix_with_starts(start_pose): - """Tests smooth_helix with specified start positions.""" - print("\n--- TESTING SMOOTH HELIX WITH START POSITIONS ---") - - # Test 1: Helix with default start - print("\n[1/3] Helix with DEFAULT start (from current position)") - center = [start_pose[0], start_pose[1] + 30, start_pose[2] - 40] - - result = robot_api.smooth_helix( - center=center, - radius=30.0, - pitch=12.0, - height=36.0, # 3 revolutions - # No start_pose - uses current - duration=10.0, - clockwise=True, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=10) - time.sleep(2) - - # Re-initialize - current_pose = initialize_test_position() - if not current_pose: return - - # Test 2: Helix with specified start on the helix perimeter - print("\n[2/3] Helix with SPECIFIED start on perimeter") - center = [current_pose[0], current_pose[1] + 30, current_pose[2] - 40] - # Start position on the helix perimeter (different angle) - start_on_perimeter = [ - center[0] + 20, # radius * cos(0) - center[1], # radius * sin(0) - center[2], # Starting height - current_pose[3], current_pose[4], current_pose[5] - ] - - print(f" Current: {[round(p, 1) for p in current_pose[:3]]}") - print(f" Will transition to helix start: {[round(p, 1) for p in start_on_perimeter[:3]]}") - - result = robot_api.smooth_helix( - center=center, - radius=20.0, - pitch=12.0, - height=36.0, - start_pose=start_on_perimeter, - speed_percentage=30, - clockwise=False, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=12) - - # Test 3: Helix in TRF - helix axis follows tool Z-axis - print("\n[3/3] Helix in TOOL REFERENCE FRAME (TRF)") - # In TRF, the helix rises along the tool's Z-axis, not world Z - trf_center = [0, 30, -40] # Center relative to tool - trf_start = [20, 30, -40, 0, 0, 0] # Start on perimeter - - print(f" TRF Helix - rises along TOOL'S Z-axis") - print(f" If tool is horizontal, helix will be horizontal too!") - - result = robot_api.smooth_helix( - center=trf_center, - radius=20.0, - pitch=12.0, - height=36.0, - frame='TRF', # NEW: Helix axis follows tool orientation - start_pose=trf_start, - duration=8.0, - clockwise=True, - wait_for_ack=True - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=12) - -def test_smooth_blend_with_starts(start_pose): - """Tests smooth_blend with specified start position for first segment.""" - print("\n--- TESTING SMOOTH BLEND WITH START POSITIONS ---") - - # Test 1: Blend with default start - print("\n[1/4] Blend with DEFAULT start") - p1 = start_pose - p2 = [p1[0] + 25, p1[1] + 10, p1[2], p1[3], p1[4], p1[5] + 20] - arc_center = [p2[0] - 10, p2[1] + 10, p2[2]] - p3 = [arc_center[0], arc_center[1] + 15, arc_center[2], p1[3], p1[4], p1[5] + 40] - - segments = [ - {'type': 'LINE', 'end': p2, 'duration': 2.0}, - {'type': 'ARC', 'end': p3, 'center': arc_center, 'duration': 3.0, 'clockwise': False}, - ] - - result = robot_api.smooth_blend( - segments=segments, - blend_time=0.5, - # No start_pose - uses current - duration=6.0, - wait_for_ack=True, - timeout=15 - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=15) - time.sleep(2) - - # Re-initialize - current_pose = initialize_test_position() - if not current_pose: return - - # Test 2: Blend with specified start for first segment - print("\n[2/4] Blend with SPECIFIED start (adds transition)") - specified_start = [current_pose[0] + 15, current_pose[1] - 10, current_pose[2], - current_pose[3], current_pose[4], current_pose[5]] - p2 = [specified_start[0] + 20, specified_start[1] + 15, specified_start[2], - specified_start[3], specified_start[4], specified_start[5] + 30] - circle_center = [p2[0], p2[1] + 20, p2[2]] - - segments = [ - {'type': 'LINE', 'end': p2, 'duration': 2.5}, - {'type': 'CIRCLE', 'center': circle_center, 'radius': 20, 'plane': 'XY', - 'duration': 4.0, 'clockwise': True}, - ] - - print(f" Current: {[round(p, 1) for p in current_pose[:3]]}") - print(f" Will transition to: {[round(p, 1) for p in specified_start[:3]]}") - print(f" Then execute blend segments") - - result = robot_api.smooth_blend( - segments=segments, - blend_time=0.75, - start_pose=specified_start, # First segment starts here - speed_percentage=35, - wait_for_ack=True, - timeout=20 - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=20) - time.sleep(2) - - # Re-initialize - current_pose = initialize_test_position() - if not current_pose: return - - # Test 3: Complex blend with spline segment and specified start - print("\n[3/4] Complex blend with SPLINE segment and specified start") - blend_start = [current_pose[0] - 10, current_pose[1] + 10, current_pose[2], - current_pose[3], current_pose[4], current_pose[5]] - - # Define waypoints for spline segment - spline_waypoints = [ - [blend_start[0] + 30, blend_start[1], blend_start[2], - blend_start[3], blend_start[4], blend_start[5]], - [blend_start[0] + 35, blend_start[1] + 15, blend_start[2], - blend_start[3], blend_start[4], blend_start[5] + 15], - [blend_start[0] + 25, blend_start[1] + 25, blend_start[2], - blend_start[3], blend_start[4], blend_start[5] + 30] - ] - - segments = [ - {'type': 'LINE', 'end': spline_waypoints[0], 'duration': 2.0}, - {'type': 'SPLINE', 'waypoints': spline_waypoints, 'duration': 4.0}, - {'type': 'LINE', 'end': [blend_start[0], blend_start[1] + 20, blend_start[2], - blend_start[3], blend_start[4], blend_start[5]], - 'duration': 2.0} - ] - - result = robot_api.smooth_blend( - segments=segments, - blend_time=0.5, - start_pose=blend_start, - duration=10.0, # Overall duration - wait_for_ack=True, - timeout=20 - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=20) - - # Test 4: Blend in TRF - all segments relative to tool - print("\n[4/4] Blend in TOOL REFERENCE FRAME (TRF)") - # All segment coordinates are relative to tool position/orientation - trf_segments = [ - {'type': 'LINE', 'end': [30, 0, 0, 0, 0, 0], 'duration': 2.0}, - {'type': 'CIRCLE', 'center': [30, 20, 0], 'radius': 20, 'plane': 'XY', - 'duration': 4.0, 'clockwise': False}, # Tool's XY plane - {'type': 'LINE', 'end': [0, 20, 0, 0, 0, 0], 'duration': 2.0} - ] - - print(f" TRF Blend - all segments in tool coordinate system") - print(f" Circle plane is tool's XY, not world XY!") - - result = robot_api.smooth_blend( - segments=trf_segments, - blend_time=0.5, - frame='TRF', # NEW: All segments in tool frame - duration=10.0, - wait_for_ack=True, - timeout=20 - ) - print(f"--> Command result: {result}") - robot_api.wait_for_robot_stopped(timeout=20) - -def test_transition_distances(): - """Test transitions with various distances to verify smooth transition behavior.""" - print("\n--- TESTING TRANSITION DISTANCES ---") - - # Get current position - start_pose = initialize_test_position() - if not start_pose: return - - # Define test distances: very close, medium, far - test_cases = [ - ("Very Close (3mm)", 3), - ("Close (10mm)", 10), - ("Medium (30mm)", 30), - ("Far (50mm)", 50) - ] - - for description, distance in test_cases: - print(f"\n[{test_cases.index((description, distance)) + 1}/{len(test_cases)}] Testing transition: {description}") - - # Create a start position at the specified distance - transition_start = [ - start_pose[0] + distance, - start_pose[1], - start_pose[2], - start_pose[3], start_pose[4], start_pose[5] - ] - - # Use a simple circle to observe the transition - circle_center = [transition_start[0], transition_start[1] + 30, transition_start[2]] - - print(f" Current position: {[round(p, 1) for p in start_pose[:3]]}") - print(f" Transition to: {[round(p, 1) for p in transition_start[:3]]}") - print(f" Distance: {distance}mm") - - start_time = time.time() - result = robot_api.smooth_circle( - center=circle_center, - radius=30.0, - plane='XY', - start_pose=transition_start, - duration=5.0, - clockwise=False, - wait_for_ack=True - ) - - # Note the transition time - robot_api.wait_for_robot_stopped(timeout=10) - total_time = time.time() - start_time - - print(f" Total execution time: {total_time:.2f}s") - if distance <= 5: - print(f" -> Minimal transition expected and observed") - else: - transition_time = distance / 30.0 # Assuming 30mm/s transition speed - print(f" -> Estimated transition time: {transition_time:.2f}s") - - time.sleep(2) - - # Return to start for next test - if test_cases.index((description, distance)) < len(test_cases) - 1: - initialize_test_position() - - # Additional test: Transition in TRF - print("\n[BONUS] Testing transition in TRF") - print("In TRF, transition is relative to tool, not world") - - # TRF start position (30mm forward in tool X) - trf_transition_start = [30, 0, 0, 0, 0, 0] - trf_circle_center = [30, 30, 0] # Center in tool frame - - result = robot_api.smooth_circle( - center=trf_circle_center, - radius=30.0, - plane='XY', # Tool's XY plane - frame='TRF', # NEW: Transition happens in tool space - start_pose=trf_transition_start, - duration=5.0, - clockwise=False, - wait_for_ack=True - ) - print(f" -> TRF transition completed") - robot_api.wait_for_robot_stopped(timeout=10) - -def test_timing_comparison_with_starts(): - """Compare timing modes with specified start positions.""" - print("\n--- TESTING TIMING MODES WITH START POSITIONS ---") - - # Initialize - start_pose = initialize_test_position() - if not start_pose: return - - # Define a specific start position for both tests - test_start = [start_pose[0] + 20, start_pose[1] - 10, start_pose[2], - start_pose[3], start_pose[4], start_pose[5]] - center = [test_start[0], test_start[1] + 30, test_start[2]] - radius = 30.0 - - print("\n[1/3] Circle with specified start + 5-second DURATION") - print(f" Transition from: {[round(p, 1) for p in start_pose[:3]]}") - print(f" To start position: {[round(p, 1) for p in test_start[:3]]}") - - start_time = time.time() - result = robot_api.smooth_circle( - center=center, - radius=radius, - plane='XY', - start_pose=test_start, - duration=5.0, - clockwise=False, - wait_for_ack=True - ) - robot_api.wait_for_robot_stopped(timeout=12) - elapsed = time.time() - start_time - print(f"--> Total execution time (including transition): {elapsed:.2f}s") - time.sleep(2) - - # Return to start - initialize_test_position() - - print("\n[2/3] Same circle with specified start + 40% SPEED") - print(f" Same transition and circle path") - - start_time = time.time() - result = robot_api.smooth_circle( - center=center, - radius=radius, - plane='XY', - start_pose=test_start, - speed_percentage=40, - clockwise=False, - wait_for_ack=True - ) - robot_api.wait_for_robot_stopped(timeout=12) - elapsed = time.time() - start_time - print(f"--> Total execution time (including transition): {elapsed:.2f}s") - - print("\n[3/3] Same circle in TRF with 40% SPEED") - print(f" Testing how TRF affects timing with transitions") - - # TRF coordinates (relative to tool) - trf_start = [20, -10, 0, 0, 0, 0] - trf_center = [20, 20, 0] # 30mm forward in tool Y from start - - start_time = time.time() - result = robot_api.smooth_circle( - center=trf_center, - radius=30.0, - plane='XY', # Tool's XY plane - frame='TRF', # NEW: Using tool reference frame - start_pose=trf_start, - speed_percentage=40, - clockwise=False, - wait_for_ack=True - ) - robot_api.wait_for_robot_stopped(timeout=12) - elapsed = time.time() - start_time - print(f"--> TRF execution time: {elapsed:.2f}s") - print(f" Note: TRF doesn't change timing, just coordinate interpretation") - - # Calculate expected times - circumference = 2 * np.pi * radius - transition_dist = np.sqrt((test_start[0] - start_pose[0])**2 + - (test_start[1] - start_pose[1])**2 + - (test_start[2] - start_pose[2])**2) - print(f"\nAnalysis:") - print(f" Transition distance: {transition_dist:.1f}mm") - print(f" Circle circumference: {circumference:.1f}mm") - print(f" At 40% speed (~40mm/s), circle should take ~{circumference/40:.1f}s") - print(f" Transition at ~30mm/s should take ~{transition_dist/30:.1f}s") - -if __name__ == "__main__": - print("="*70) - print("COMPREHENSIVE SMOOTH MOTION TEST SUITE") - print("Testing NEW features: Start Positions & Automatic Transitions") - print("="*70) - - - # Test 1: Basic tests with new start position feature - print("\n[TEST GROUP 1: BASIC COMMANDS WITH START POSITIONS]") - start_pose = initialize_test_position() - if start_pose: - test_smooth_circle_basic(start_pose) - - # Test 2: Arc commands with various start positions - print("\n[TEST GROUP 2: ARC COMMANDS WITH TRANSITIONS]") - start_pose = initialize_test_position() - if start_pose: - test_smooth_arc_with_start_positions(start_pose) - - # Test 3: Motion chaining - using end of one as start of next - print("\n[TEST GROUP 3: PRECISE MOTION CHAINING]") - start_pose = initialize_test_position() - if start_pose: - test_motion_chaining(start_pose) - - # Test 4: Spline with various start scenarios - print("\n[TEST GROUP 4: SPLINE WITH START POSITIONS]") - start_pose = initialize_test_position() - if start_pose: - test_smooth_spline_with_starts(start_pose) - - # Test 5: Helix with start positions - print("\n[TEST GROUP 5: HELIX WITH START POSITIONS]") - start_pose = initialize_test_position() - if start_pose: - test_smooth_helix_with_starts(start_pose) - - # Test 6: Blend with start positions - print("\n[TEST GROUP 6: BLEND WITH START POSITIONS]") - start_pose = initialize_test_position() - if start_pose: - test_smooth_blend_with_starts(start_pose) - - # Test 7: Transition distance testing - print("\n[TEST GROUP 7: TRANSITION DISTANCE BEHAVIOR]") - test_transition_distances() - - # Test 8: Timing comparison with transitions - print("\n[TEST GROUP 8: TIMING MODES WITH TRANSITIONS]") - test_timing_comparison_with_starts() - - print("\n" + "="*70) - print("COMPREHENSIVE TEST SUITE COMPLETE") - print("Tested features:") - print(" ✓ All commands with duration mode") - print(" ✓ All commands with speed percentage mode") - print(" ✓ Default start positions (current position)") - print(" ✓ Specified start positions with automatic transitions") - print(" ✓ Motion chaining with precise continuity") - print(" ✓ Transition behavior for various distances") - print(" ✓ Blend segments with overall timing control") - print("="*70) - - # Final return to safe position - print("\nReturning to safe position...") - initialize_test_position() - print("\n===== All Tests Finished =====") \ No newline at end of file diff --git a/tests/__init__.py b/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/conftest.py b/tests/conftest.py new file mode 100644 index 0000000..64a3fc0 --- /dev/null +++ b/tests/conftest.py @@ -0,0 +1,468 @@ +""" +Pytest configuration and shared fixtures for PAROL6 Python API tests. + +Provides command line options, fixtures for port management, server process management, +environment configuration, and test utilities used across the test suite. +""" + +import logging +import os +import signal +import sys +import time +from collections.abc import Generator +from dataclasses import dataclass + +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__))) + +# 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)) + 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 + ack_port: int = 5002 + + +# ============================================================================ +# 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", + ) + parser.addoption( + "--server-ip", + action="store", + default="127.0.0.1", + help="IP address for test server communication", + ) + parser.addoption( + "--server-port", + action="store", + type=int, + default=None, + help="Port for robot server communication (auto-detected if not specified)", + ) + parser.addoption( + "--ack-port", + action="store", + type=int, + default=None, + 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", + ) + + +# ============================================================================ +# 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) + + +# ============================================================================ +# ENVIRONMENT CONFIGURATION FIXTURE +# ============================================================================ + + +@pytest.fixture(scope="session") +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. + """ + # Store original environment values + original_env = {} + env_vars = { + "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: + # Restore original environment + for key, original_value in original_env.items(): + if original_value is None: + os.environ.pop(key, None) + else: + os.environ[key] = original_value + logger.debug("Restored original environment") + + +# ============================================================================ +# 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 + manager.start_controller( + no_autohome=True, + extra_env={ + "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"): + return True + 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") + manager.stop_controller() + else: + logger.info("Leaving test server running (--keep-server-running)") + + +# ============================================================================ +# 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: 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("HARDWARE TEST CONFIRMATION REQUIRED") + print(f"{'=' * 60}") + print(f"{message}") + 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"] + + 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 + + +# ============================================================================ +# 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(): + if original_value is None: + os.environ.pop(key, None) + else: + os.environ[key] = original_value + self.original.clear() + + temp = TempEnv() + try: + yield temp + finally: + temp.restore() + + +@pytest.fixture +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) + return mock + + +# ============================================================================ +# 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", + ) + config.addinivalue_line( + "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)", + ) + 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", + ) + + +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)" + ) + for item in items: + if item.get_closest_marker("hardware"): + item.add_marker(skip_hardware) + + +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"Server IP: {config.getoption('--server-ip')}") + + 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: + logger.info("Server ports: auto-detect") + + +# ============================================================================ +# 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, + ) + + +def pytest_sessionfinish(session, exitstatus): + """Called after whole test run finished.""" + logger.info( + f"PAROL6 Python API test session finished with exit status: {exitstatus}" + ) diff --git a/tests/hardware/__init__.py b/tests/hardware/__init__.py new file mode 100644 index 0000000..81d7dc6 --- /dev/null +++ b/tests/hardware/__init__.py @@ -0,0 +1,7 @@ +""" +Hardware tests package. + +Contains tests that require actual robot hardware and human confirmation. +These tests are marked with @pytest.mark.hardware and are only executed +when the --run-hardware flag is provided. +""" diff --git a/tests/hardware/test_manual_moves.py b/tests/hardware/test_manual_moves.py new file mode 100644 index 0000000..d4ff6fe --- /dev/null +++ b/tests/hardware/test_manual_moves.py @@ -0,0 +1,727 @@ +""" +Hardware tests for manual robot movements. + +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 +workspace is clear and E-stop is within reach before running. +""" + +import os +import sys +import time + +import pytest + +# Add the parent directory to Python path +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "..")) + +# Removed direct robot_api import - using client fixture from conftest.py + +# Define the safe starting joint configuration for all hardware tests +# This ensures consistency and repeatability for each test +# Angles: [J1, J2, J3, J4, J5, J6] in degrees +SAFE_SMOOTH_START_JOINTS = [42.697, -89.381, 144.831, -0.436, 31.528, 180.0] + + +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 + ) + print(f"Move command result: {result}") + + # Wait until robot stops + 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 + if start_pose: + print(f"Starting pose: {[round(p, 2) for p in start_pose]}") + return start_pose + else: + print("ERROR: Could not retrieve robot pose after moving.") + return None + else: + print("ERROR: Robot did not stop in time.") + return None + + +@pytest.mark.hardware +@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( + "Ready to test robot homing?\n" + "This will home all joints to their reference positions.\n" + "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"] + + # Wait for homing to complete - use client's built-in wait + 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 + ) + + assert isinstance(result, dict) + 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, + duration=1.0, + wait_for_ack=True, + ) + + 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-"] + + for axis in axes: + print(f"Testing Cartesian jog in {axis} direction...") + + result = client.jog_cartesian( + frame="WRF", + axis=axis, + speed_percentage=20, + duration=1.0, + wait_for_ack=True, + ) + + assert isinstance(result, dict) + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] + + time.sleep(2.0) + client.wait_until_stopped(timeout=10) + + print("All Cartesian movements completed successfully") + + +@pytest.mark.hardware +@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" + "Motion will be slow and controlled (5 second duration).\n" + "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", + duration=5.0, + clockwise=False, + wait_for_ack=True, + timeout=15, + ) + + assert isinstance(result, dict) + assert result.get("status") in ["COMPLETED", "QUEUED", "EXECUTING"] + + # Wait for completion + 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." + ): + 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, + ] + 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, + center=center, + duration=4.0, + clockwise=True, + wait_for_ack=True, + timeout=12, + ) + + assert isinstance(result, dict) + 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." + ): + 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], + ], + ] + + print(f"Executing spline through {len(waypoints)} waypoints") + result = client.smooth_spline( + waypoints=waypoints, + duration=6.0, + frame="WRF", + wait_for_ack=True, + timeout=20, + ) + + assert isinstance(result, dict) + 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." + ): + pytest.fail("User reported spline motion was not smooth") + else: + pytest.fail("Robot did not stop after spline motion timeout") + + +@pytest.mark.hardware +@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" + "Motion combines circular movement with vertical progression.\n" + "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, + radius=25.0, + pitch=13.0, # 40mm / ~3 revolutions + height=40.0, + duration=8.0, + clockwise=False, + wait_for_ack=True, + timeout=20, + ) + + assert isinstance(result, dict) + 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"): + 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", + wait_for_ack=True, + timeout=12, + ) + + assert isinstance(result_wrf, dict) + 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 + wait_for_ack=True, + timeout=12, + ) + + assert isinstance(result_trf, dict) + 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" + "TRF motion should follow tool orientation." + ): + pytest.fail("User reported motion patterns were not as expected") + + +@pytest.mark.hardware +@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( + "Ready to test joint limit safety?\n" + "This will attempt to move a joint near its limit to test safety systems.\n" + "Controller should prevent unsafe movements.\n" + "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 + + 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, + ) + + 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) + + +@pytest.mark.hardware +@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. + """ + if not human_prompt( + "Ready to execute the legacy safe test sequence?\n" + "This will reproduce the exact movements from test_script.py:\n" + "- Electric gripper calibration and moves (pos 100, then 200)\n" + "- Pneumatic gripper open/close sequence\n" + "- Joint moves: [90,-90,160,12,12,180] -> [50,-60,180,-12,32,0] -> back\n" + "- Pose move: [7,250,200,-100,0,-90]\n" + "- Cartesian move: [7,250,150,-100,0,-90]\n" + "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 + ) + if isinstance(result, dict): + 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, + current=200, + wait_for_ack=True, + timeout=10, + ) + if isinstance(result, dict): + 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, + current=200, + wait_for_ack=True, + timeout=10, + ) + if isinstance(result, dict): + 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() + pose = client.get_pose_rpy() + assert angles is not None + 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) + time.sleep(0.3) + client.control_pneumatic_gripper("close", 1) + time.sleep(0.3) + client.control_pneumatic_gripper("open", 1) + 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 + ) + if isinstance(result, dict): + 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 + ) + if isinstance(result, dict): + 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 + ) + if isinstance(result, dict): + 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 + ) + if isinstance(result, dict): + 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], + speed_percentage=50, + wait_for_ack=True, + timeout=15, + ) + if isinstance(result, dict): + 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") + + +@pytest.mark.hardware +@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( + "Ready to test pneumatic gripper?\n" + "Ensure gripper is connected and air pressure is available.\n" + "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 + ) + + assert isinstance(result, dict) + 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 + ) + + assert isinstance(result, dict) + 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( + "Ready to test electric gripper?\n" + "Ensure electric gripper is connected and powered.\n" + "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 + ) + + assert isinstance(result, dict) + 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 + ) + + assert isinstance(result, dict) + 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") + + +if __name__ == "__main__": + pytest.main([__file__]) diff --git a/tests/integration/__init__.py b/tests/integration/__init__.py new file mode 100644 index 0000000..242efae --- /dev/null +++ b/tests/integration/__init__.py @@ -0,0 +1,6 @@ +""" +Integration tests package. + +Contains tests that exercise component interactions using FAKE_SERIAL mode, +testing end-to-end UDP communication flows without requiring hardware. +""" diff --git a/tests/integration/test_ack_and_nonblocking.py b/tests/integration/test_ack_and_nonblocking.py new file mode 100644 index 0000000..711477d --- /dev/null +++ b/tests/integration/test_ack_and_nonblocking.py @@ -0,0 +1,114 @@ +""" +Integration tests for acknowledgment and non-blocking behavior with parol6. + +Tests wait_for_ack functionality, non-blocking command flows, status polling, +timeout handling, and command tracking with live server communication. +""" + +import os +import sys + +import pytest + +# 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 + result = client.move_joints( + [0, 0, 0, 0, 0, 0], + duration=1.0, + ) + # 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 + results = [] + for i in range(3): + result = client.move_joints( + [i, i, i, i, i, i], + duration=0.2, + ) + results.append(result) + # 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=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) + result = client.move_joints( + [0, -45, 180, 15, 20, 25], # Valid within limits for sim + duration=1.0, + ) + assert result is True + # Verify server remains responsive without fixed sleep + assert client.ping() is not None + + +@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 + result = client.move_cartesian( + pose=[100, 100, 100, 0, 0, 0], + speed_percentage=200, + ) + assert result is True + + +@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__": + pytest.main([__file__]) 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 new file mode 100644 index 0000000..1516cd8 --- /dev/null +++ b/tests/integration/test_smooth_commands_e2e.py @@ -0,0 +1,156 @@ +""" +Integration tests for smooth motion commands (minimal set). + +Tests one representative command per smooth motion family in FAKE_SERIAL mode. +Verifies command acceptance, completion status transitions, and basic functionality. +""" + +import os +import sys + +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", "") + ): + 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=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", + ) + + # 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=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 + ): + """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", + ) + + _check_if_fake_serial_xfail(result) + + assert result is True + + 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], + ] + + result = client.smooth_spline( + waypoints=waypoints, + duration=3.0, + frame="WRF", + ) + + _check_if_fake_serial_xfail(result) + + assert result is True + + 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( + center=[100, 100, 80], + radius=25, + pitch=20, + height=60, + duration=3.0, + frame="WRF", + ) + + _check_if_fake_serial_xfail(result) + + assert result is True + + 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": "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", + ) + + _check_if_fake_serial_xfail(result) + + assert result is True + + assert client.wait_until_stopped(timeout=10.0) + assert client.is_robot_stopped(threshold_speed=5.0) + + +if __name__ == "__main__": + pytest.main([__file__]) 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 new file mode 100644 index 0000000..b0a1881 --- /dev/null +++ b/tests/integration/test_udp_smoke.py @@ -0,0 +1,253 @@ +""" +Integration smoke tests for UDP communication using parol6. +Covers PING/PONG, GET_* endpoints, STOP semantics, and basic functionality. +""" + +import os +import sys + +import pytest + +# Add the parent directory to Python path +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "..")) + +from parol6 import RobotClient + + +@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 +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 + + +@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 + + +@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=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 + # Joint 2 range: [-145.0088, -3.375] + # Joint 3 range: [107.866, 287.8675] + result = client.move_joints( + [0, -45, 180, 15, 20, 25], # Valid angles within joint limits + duration=2.0, + ) + assert result is True + + # Wait for completion and verify robot stops + 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( + [100, 100, 100, 0, 0, 0], + speed_percentage=50, + ) + assert result is True + + # Wait for completion and verify robot stops + 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], + duration=2.0, + ) + assert result is True + + +@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 + ) + 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 + + +@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=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 + + +if __name__ == "__main__": + pytest.main([__file__]) diff --git a/tests/unit/__init__.py b/tests/unit/__init__.py new file mode 100644 index 0000000..c75290d --- /dev/null +++ b/tests/unit/__init__.py @@ -0,0 +1,6 @@ +""" +Unit tests package. + +Contains tests for individual components tested in isolation, +without requiring external processes or hardware. +""" 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 new file mode 100644 index 0000000..dcf2712 --- /dev/null +++ b/tests/unit/test_conversions.py @@ -0,0 +1,56 @@ +from unittest.mock import AsyncMock + +from parol6 import RobotClient + + +def _pose_payload_from_matrix(m): + # Flatten list of lists to comma string after prefix + flat = [] + for row in m: + flat.extend(row) + return "POSE|" + ",".join(str(x) for x in flat) + + +def test_get_pose_rpy_identity_translation(monkeypatch): + """ + Validate get_pose_rpy converts 4x4 pose matrix to [x,y,z,rx,ry,rz] (mm,deg). + Use identity rotation with translation (10,20,30) mm. + """ + client = RobotClient() + + # Identity rotation with translation in last column (row-major) + mat = [ + [1, 0, 0, 10], + [0, 1, 0, 20], + [0, 0, 1, 30], + [0, 0, 0, 1], + ] + payload = _pose_payload_from_matrix(mat) + + # Patch the async client's _request coroutine used under the hood + 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 + # Translations + assert pose_rpy[0:3] == [10, 20, 30] + # Identity rotation -> zero Euler angles (within tolerance) + rx, ry, rz = pose_rpy[3:6] + assert abs(rx) < 1e-6 + assert abs(ry) < 1e-6 + assert abs(rz) < 1e-6 + + +def test_get_pose_rpy_malformed_payload(monkeypatch): + """ + Malformed POSE payload (wrong length) should return None. + """ + client = RobotClient() + + # Not 16 elements + mock_request = AsyncMock(return_value="POSE|1,2,3") + monkeypatch.setattr(client.async_client, "_request", mock_request) + + pose_rpy = client.get_pose_rpy() + assert pose_rpy is None diff --git a/tests/unit/test_mock_transport.py b/tests/unit/test_mock_transport.py new file mode 100644 index 0000000..10fd837 --- /dev/null +++ b/tests/unit/test_mock_transport.py @@ -0,0 +1,398 @@ +""" +Unit tests for MockSerialTransport. + +Tests the mock serial transport implementation to ensure: +1. Mock transport can be created and connected +2. Mock transport simulates robot responses correctly +3. Transport factory correctly selects mock when PAROL6_FAKE_SERIAL is set +4. Mock transport is compatible with SerialTransport interface +""" + +import os +import threading +import time + +import numpy as np +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 +): + """ + Helper: wait for a latest frame publication and decode into numpy arrays. + Returns dict-like with arrays or None on timeout. + """ + start = time.time() + last_ver = -1 + # Ensure reader running + shutdown = threading.Event() + transport.start_reader(shutdown) + + pos = np.zeros(6, dtype=np.int32) + spd = np.zeros(6, dtype=np.int32) + homed = np.zeros(8, dtype=np.uint8) + io_bits = np.zeros(8, dtype=np.uint8) + temp = np.zeros(8, dtype=np.uint8) + poserr = np.zeros(8, dtype=np.uint8) + timing = np.zeros(1, dtype=np.int32) + grip = np.zeros(6, dtype=np.int32) + + while time.time() - start < timeout_s: + mv, ver, ts = transport.get_latest_frame_view() + if mv is not None and ver != last_ver: + ok = unpack_rx_frame_into( + mv, + pos_out=pos, + spd_out=spd, + homed_out=homed, + io_out=io_bits, + temp_out=temp, + poserr_out=poserr, + timing_out=timing, + grip_out=grip, + ) + if ok: + return { + "pos": pos.copy(), + "spd": spd.copy(), + "homed": homed.copy(), + "io": io_bits.copy(), + "temp": temp.copy(), + "poserr": poserr.copy(), + "timing": timing.copy(), + "grip": grip.copy(), + "ver": ver, + "ts": ts, + } + time.sleep(0.005) + return None + + +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] + command_out = CommandCode.JOG + affected_joint = [1, 1, 1, 1, 1, 1, 0, 0] + 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, + ) + assert success + + # Verify internal state updated + assert transport._state.command_out == command_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, + ) + 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,) + assert decoded["homed"].shape == (8,) + assert decoded["io"].shape == (8,) + assert decoded["temp"].shape == (8,) + 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 + ) + + # Wait for movement + moved = None + t0 = time.time() + while time.time() - t0 < 0.5: + decoded = _wait_for_latest_frame_and_decode(transport, timeout_s=0.1) + if decoded is None: + continue + if int(decoded["pos"][0]) != initial_pos: + 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 + ) + + # Poll until position moves toward target or timeout + final = None + t0 = time.time() + while time.time() - t0 < 1.0: + decoded = _wait_for_latest_frame_and_decode(transport, timeout_s=0.1) + if decoded is None: + continue + current_pos = int(decoded["pos"][0]) + if abs(current_pos - target_pos[0]) < 2000: + 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) + ] + 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 + ) + + 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: + continue + homed_bits = decoded["homed"].tolist() + if not all(h == 1 for h in homed_bits): + homing_started = True + 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) + ): + 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 + ) + + # 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 + ) + + # 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 not info["connected"] + assert info["mode"] == "MOCK_SERIAL" + + transport.connect() + info = transport.get_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"] + 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 + assert is_simulation_mode() + + # Test false values + 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"] + + 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"] + + 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" + transport = create_transport() + assert isinstance(transport, MockSerialTransport) + + # Clean up + 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") + 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") + assert isinstance(transport, SerialTransport) + + # Invalid type should raise + with pytest.raises(ValueError): + create_transport(transport_type="invalid") + + # Clean up + 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 new file mode 100644 index 0000000..21f647c --- /dev/null +++ b/tests/unit/test_smooth_motion_api.py @@ -0,0 +1,24 @@ +import importlib +import inspect + + +def test_smooth_motion_reexports_exist(): + sm = importlib.import_module("parol6.smooth_motion") + + # Required re-exports + for name in [ + "CircularMotion", + "SplineMotion", + "HelixMotion", + "WaypointTrajectoryPlanner", + ]: + assert hasattr(sm, name), f"parol6.smooth_motion missing {name}" + obj = getattr(sm, name) + assert inspect.isclass(obj), f"{name} should be a class" + + # Optional blenders (presence depends on legacy module) + # If present, they should be classes as well + for opt_name in ["MotionBlender", "AdvancedMotionBlender"]: + if hasattr(sm, opt_name): + opt = getattr(sm, opt_name) + assert inspect.isclass(opt), f"{opt_name} should be a class if present" 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 new file mode 100644 index 0000000..2f9daf1 --- /dev/null +++ b/tests/unit/test_trajectory.py @@ -0,0 +1,96 @@ +import math + +import numpy as np +import pytest +from parol6.config import CONTROL_RATE_HZ +from parol6.utils import trajectory as traj + + +def approx_equal(a, b, tol=1e-6): + return abs(a - b) <= tol + + +def test_plan_linear_quintic_endpoints_and_shape(): + start = [0.0, 0.0] + end = [10.0, 20.0] + duration = 1.0 # seconds + + path = traj.plan_linear_quintic(start, end, duration) + # Expected sample count: duration * rate + 1 + expected_n = int(round(duration * CONTROL_RATE_HZ)) + 1 + assert path.shape == (expected_n, 2) + + # Endpoints + assert np.allclose(path[0], start) + assert np.allclose(path[-1], end) + + # Monotonic progression on each axis + diffs = np.diff(path, axis=0) + assert np.all(diffs[:, 0] >= -1e-9) + assert np.all(diffs[:, 1] >= -1e-9) + + +def test_plan_linear_cubic_endpoints_and_shape(): + start = [5.0, -5.0, 2.5] + end = [6.0, 0.0, 4.5] + duration = 0.5 + + path = traj.plan_linear_cubic(start, end, duration) + expected_n = int(round(duration * CONTROL_RATE_HZ)) + 1 + assert path.shape == (expected_n, 3) + + assert np.allclose(path[0], start) + assert np.allclose(path[-1], end) + + diffs = np.diff(path, axis=0) + # Allow tiny numerical noise + assert np.all(diffs >= -1e-9) + + +def _timings(distance, v_max, a_max): + # Mirror of _trapezoid_timings logic for test-side expectation + if distance <= 0 or v_max <= 0 or a_max <= 0: + return 0.0, 0.0, 0.0, 0.0, True + t_a = v_max / a_max + s_a = 0.5 * a_max * t_a**2 + if 2 * s_a < distance: + s_c = distance - 2 * s_a + t_c = s_c / v_max + T = 2 * t_a + t_c + return T, t_a, t_c, v_max, False + else: + v_peak = math.sqrt(a_max * distance) + t_a = v_peak / a_max + T = 2 * t_a + return T, t_a, 0.0, v_peak, True + + +@pytest.mark.parametrize( + "start,end,v_max,a_max", + [ + (0.0, 1.0, 0.5, 1.0), # trapezoidal (2*s_a < distance) + (0.0, 0.02, 1.0, 10.0), # triangular (cannot reach cruise) + (10.0, 0.0, 0.5, 1.0), # reverse direction trapezoidal + ], +) +def test_plan_trapezoid_position_1d_shapes_and_endpoints(start, end, v_max, a_max): + positions = traj.plan_trapezoid_position_1d(start, end, v_max, a_max) + assert positions.ndim == 1 + assert approx_equal(positions[0], start, tol=1e-9) + assert approx_equal(positions[-1], end, tol=1e-9) + + # Monotonic in the proper direction + diffs = np.diff(positions) + if end >= start: + assert np.all(diffs >= -1e-9) + else: + assert np.all(diffs <= 1e-9) + + # Sample count should match expected duration discretization + dist = abs(end - start) + T, _ta, _tc, _vp, _tri = _timings(dist, v_max, a_max) + if T > 0: + expected_n = int(round(T * CONTROL_RATE_HZ)) + 1 + assert len(positions) == expected_n + else: + assert len(positions) == 2 diff --git a/tests/unit/test_wire.py b/tests/unit/test_wire.py new file mode 100644 index 0000000..e91dc98 --- /dev/null +++ b/tests/unit/test_wire.py @@ -0,0 +1,126 @@ +import pytest +from parol6.protocol import wire + + +def test_encode_move_joint_with_none(): + s = wire.encode_move_joint([1, 2, 3, 4, 5, 6], None, None) + assert s == "MOVEJOINT|1|2|3|4|5|6|NONE|NONE" + + +def test_encode_move_joint_with_values(): + s = wire.encode_move_joint([0, -10.5, 20, 30.25, -40, 50], 2.5, 75) + assert s == "MOVEJOINT|0|-10.5|20|30.25|-40|50|2.5|75" + + +def test_encode_move_pose(): + s = wire.encode_move_pose([1, 2, 3, 4, 5, 6], 1.0, None) + assert s == "MOVEPOSE|1|2|3|4|5|6|1.0|NONE" + + +def test_encode_move_cartesian(): + s = wire.encode_move_cartesian([10, 20, 30, 1, 2, 3], None, 50) + assert s == "MOVECART|10|20|30|1|2|3|NONE|50" + + +def test_encode_move_cartesian_rel_trf_variants(): + # Profile/tracking should be upper-cased and None becomes NONE + s = wire.encode_move_cartesian_rel_trf( + deltas=[1, 2, 3, 4, 5, 6], + duration=None, + speed=50, + accel=100, + profile="s_curve", + tracking="queued", + ) + assert s == "MOVECARTRELTRF|1|2|3|4|5|6|NONE|50|100|S_CURVE|QUEUED" + + s2 = wire.encode_move_cartesian_rel_trf( + deltas=[0, 0, 0, 0, 0, 0], + duration=2.0, + speed=None, + accel=None, + profile=None, + tracking=None, + ) + assert s2 == "MOVECARTRELTRF|0|0|0|0|0|0|2.0|NONE|NONE|NONE|NONE" + + +def test_encode_jog_joint(): + s = wire.encode_jog_joint(3, 80, 0.25, None) + assert s == "JOG|3|80|0.25|NONE" + + s2 = wire.encode_jog_joint(0, 10, None, 5.5) + assert s2 == "JOG|0|10|NONE|5.5" + + +def test_encode_cart_jog(): + s = wire.encode_cart_jog("WRF", "X+", 50, 0.5) + assert s == "CARTJOG|WRF|X+|50|0.5" + + +def test_encode_gcode(): + s = wire.encode_gcode("G0 X0 Y0 Z0") + assert s == "GCODE|G0 X0 Y0 Z0" + + +def test_encode_gcode_program_inline(): + s = wire.encode_gcode_program_inline(["G21", "G90", "G0 X0 Y0", "G1 X10 F1000"]) + assert s == "GCODE_PROGRAM|INLINE|G21;G90;G0 X0 Y0;G1 X10 F1000" + + +@pytest.mark.parametrize( + "resp,prefix,expected", + [ + ("ANGLES|0,1,2,3,4,5", "ANGLES", [0.0, 1.0, 2.0, 3.0, 4.0, 5.0]), + ("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)], + ), + ], +) +def test_decode_simple_success(resp, prefix, expected): + out = wire.decode_simple(resp, prefix) + assert out == expected + + +@pytest.mark.parametrize( + "resp,prefix", + [ + ("ANGLES|a,b,c", "ANGLES"), + ("IO|1,2,x", "IO"), + ("WRONG|1,2,3", "ANGLES"), + ("", "ANGLES"), + (None, "ANGLES"), + ], +) +def test_decode_simple_fail(resp, prefix): + out = wire.decode_simple(resp, prefix) + assert out is None + + +def test_decode_status_success(): + resp = ( + "STATUS|" + "POSE=1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16|" + "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 + assert isinstance(result, dict) + assert isinstance(result.get("pose"), list) + 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] + + +def test_decode_status_invalid_returns_none(): + assert wire.decode_status("STATUS|") is None + assert wire.decode_status("") is None + assert wire.decode_status("NOTSTATUS|whatever") is None 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 new file mode 100644 index 0000000..4b125e6 --- /dev/null +++ b/tests/unit/test_wire_pack.py @@ -0,0 +1,120 @@ +import numpy as np +from parol6.protocol import wire +from parol6.protocol.wire import CommandCode + + +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 + timeout_out = 7 + gripper_data_out = [123, 45, 67, 3, 0, 5] # pos, spd, cur, cmd, mode, id + + buf = bytearray(56) + wire.pack_tx_frame_into( + memoryview(buf), + position_out, + speed_out, + CommandCode.MOVE, + affected_joint_out, + inout_out, + timeout_out, + gripper_data_out, + ) + frame = bytes(buf) + + # Structure: 3 start + 1 len + 52 payload + 2 end = 58 bytes + assert isinstance(frame, (bytes, bytearray)) + assert len(frame) == 56 + assert frame[:3] == b"\xff\xff\xff" + assert frame[3] == 52 + assert frame[-2:] == b"\x01\x02" + + # Command byte position = 3 (start) + 1 (len) + 18 (pos) + 18 (spd) = 40 + assert frame[40] == int(CommandCode.MOVE) + + +def test_unpack_rx_frame_happy_path_and_signs(): + # Build a 52-byte payload per firmware layout + payload = bytearray(52) + + # Positions: 6 * 3 bytes (signed 24-bit two's complement) + positions = [-1, 0, 1, 1000, -1000, 123456] + off = 0 + for v in positions: + b0, b1, b2 = wire.split_to_3_bytes(v) + payload[off : off + 3] = bytes([b0, b1, b2]) + off += 3 + + # Speeds: 6 * 3 bytes + speeds = [0, 5, -5, 9999, -9999, 654321] + for v in speeds: + b0, b1, b2 = wire.split_to_3_bytes(v) + payload[off : off + 3] = bytes([b0, b1, b2]) + off += 3 + + # Homed / IO / errors (bytes 36..39) + payload[36] = 0xFF # all homed + payload[37] = 0xAA # 10101010 (MSB..LSB) + payload[38] = 0x00 # no temp errors + payload[39] = 0x00 # no position errors + + # Timing (bytes 40..41 => low 2 bytes in 24-bit value) + payload[40] = 0x12 + payload[41] = 0x34 + + # Bytes 42..43 unspecified/legacy (ignored by unpacker) + + # Device + gripper (bytes 44..51) + payload[44] = 7 # device id + payload[45] = 0x01 # pos hi + payload[46] = 0x02 # pos lo => 0x0102 = 258 + payload[47] = 0x00 # spd hi + payload[48] = 0x64 # spd lo => 100 + payload[49] = 0x00 # cur hi + payload[50] = 0x0A # cur lo => 10 + + # Status byte: bits[2]=1, bits[3]=1 => obj = (1<<1)|1 = 3 + payload[51] = 0b00001100 + + # Decode via zero-allocation into-variant + pos = np.zeros(6, dtype=np.int32) + spd = np.zeros(6, dtype=np.int32) + homed = np.zeros(8, dtype=np.uint8) + io_bits = np.zeros(8, dtype=np.uint8) + temp = np.zeros(8, dtype=np.uint8) + poserr = np.zeros(8, dtype=np.uint8) + timing = np.zeros(1, dtype=np.int32) + grip = np.zeros(6, dtype=np.int32) + + ok = wire.unpack_rx_frame_into( + memoryview(payload), + pos_out=pos, + spd_out=spd, + homed_out=homed, + io_out=io_bits, + temp_out=temp, + poserr_out=poserr, + timing_out=timing, + grip_out=grip, + ) + assert ok + + # Validate positions and speeds round trip + assert pos.tolist() == positions + assert spd.tolist() == speeds + + # Validate bitfields + assert homed.tolist() == [1] * 8 + assert io_bits.tolist() == [1, 0, 1, 0, 1, 0, 1, 0] # MSB..LSB of 0xAA + + # Errors empty + assert temp.tolist() == [0] * 8 + assert poserr.tolist() == [0] * 8 + + # Timing (0x1234 == 4660) + assert int(timing[0]) == 0x1234 + + # Gripper data + assert grip.tolist() == [7, 258, 100, 10, 0b00001100, 3] diff --git a/tests/utils/__init__.py b/tests/utils/__init__.py new file mode 100644 index 0000000..7cc9d38 --- /dev/null +++ b/tests/utils/__init__.py @@ -0,0 +1,13 @@ +""" +Test utilities package. + +Provides helper functions and classes for testing the PAROL6 Python API. +""" + +from .process import HeadlessCommanderProc, find_available_ports, wait_for_completion + +__all__ = [ + "HeadlessCommanderProc", + "wait_for_completion", + "find_available_ports", +] diff --git a/tests/utils/process.py b/tests/utils/process.py new file mode 100644 index 0000000..546636d --- /dev/null +++ b/tests/utils/process.py @@ -0,0 +1,329 @@ +""" +Process management utilities for testing. + +Provides classes and functions to manage the controller.py subprocess +during integration tests, including startup, readiness checks, and cleanup. +""" + +import logging +import os +import socket +import subprocess +import sys +import threading +import time +from typing import Any + +logger = logging.getLogger(__name__) + + +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, + ack_port: int = 5002, + 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 + ack_port: UDP port for acknowledgment sending + env: Additional environment variables to set + """ + self.ip = ip + self.port = port + self.ack_port = ack_port + self.env = env or {} + + 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(self.env) + + # Find the controller.py script + script_path = os.path.join( + 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"] + + 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}" + ) + + self.process = subprocess.Popen( + cmd, + env=process_env, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + bufsize=1, # Line buffered + 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})" + ) + 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, ""): + if line: + 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, + args=(self.process.stdout, self._output_lines), + daemon=True, + ) + self._error_thread = threading.Thread( + target=capture_output, + args=(self.process.stderr, self._error_lines), + 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": + return True + 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})" + ) + except subprocess.TimeoutExpired: + 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]: + """Get captured stdout lines.""" + return self._output_lines.copy() + + 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]: + """ + 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, + } + + +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: 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)) + 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: + """ + 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 + """ + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.bind((host, port)) + return True + except OSError: + return False diff --git a/tests/utils/udp.py b/tests/utils/udp.py new file mode 100644 index 0000000..a408a8b --- /dev/null +++ b/tests/utils/udp.py @@ -0,0 +1,379 @@ +""" +UDP utilities for testing. + +Provides helper functions for UDP communication during tests, +including acknowledgment listening and command sending utilities. +""" + +import logging +import queue +import socket +import threading +import time +from typing import Any + +logger = logging.getLogger(__name__) + + +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) -> 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)) + logger.debug(f"Sent command: {command}") + + # Wait for response (for GET commands) + try: + data, _ = sock.recvfrom(2048) + response = data.decode("utf-8") + logger.debug(f"Received response: {response}") + return response + 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)) + logger.debug(f"Sent command (no response): {command}") + return True + except Exception as e: + logger.error(f"Error sending command '{command}': {e}") + return False + + +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: socket.socket | None = None + self.thread: threading.Thread | None = None + self.running = False + + # Storage for received acknowledgments + 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.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") + + # Parse acknowledgment message: ACK|cmd_id|status|details + 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, + } + + # 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 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: 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: + return ack_info + else: + # Put back in queue if status doesn't match + self.ack_queue.put(ack_info) + 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}" + ) + return None + + 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]]: + """ + 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() + while not self.ack_queue.empty(): + try: + self.ack_queue.get_nowait() + except queue.Empty: + break + + +def send_command_with_ack( + command: str, + server_ip: str = "127.0.0.1", + server_port: int = 5001, + ack_port: int = 5002, + 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) + """ + # Set up acknowledgment listener + 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"): + # 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() + ): + 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: 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]: + """ + 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) + response_type = parts[0] + + parsed: dict[str, Any] = {"type": response_type, "raw": response} + + if len(parts) > 1: + data = parts[1] + + if response_type in ["POSE", "ANGLES", "SPEEDS"]: + # Numeric array data + try: + parsed["data"] = [float(x) for x in data.split(",")] + except ValueError: + parsed["data"] = data + elif response_type in ["IO", "GRIPPER"]: + # Integer array data + try: + parsed["data"] = [int(x) for x in data.split(",")] + except ValueError: + 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 + else: + parsed["data"] = data + else: + parsed["data"] = None + + return parsed