From df3e7fc5409934c009059060729e11b548b06d0c Mon Sep 17 00:00:00 2001 From: AlvarEhr <54718121+AlvarEhr@users.noreply.github.com> Date: Fri, 15 Aug 2025 08:22:17 +0100 Subject: [PATCH 01/16] Update README.md Removed the "untested gripper functions" part as they are now tested and confirmed working --- README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/README.md b/README.md index 209d118..1594ef6 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,6 @@ * **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 line 1556 (`command_queue.append(lambda: HomeCommand())`) 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. -* **Untested Gripper Functionality**: The `GripperCommand` for both pneumatic and electric grippers has been implemented based on the reference code but has not been tested on physical hardware. Please report any issues you encounter. ## 2. Safety Precautions & Disclaimer This control software includes several built-in safety features designed to prevent damage to the robot and ensure predictable operation: From 427dd1fd63a7ffd044a0cee4fada50bd0b1d729e Mon Sep 17 00:00:00 2001 From: AlvarEhr <54718121+AlvarEhr@users.noreply.github.com> Date: Sat, 23 Aug 2025 20:14:46 +0100 Subject: [PATCH 02/16] Update README.md Updated README to include all new functions and implementations --- README.md | 800 ++++++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 619 insertions(+), 181 deletions(-) diff --git a/README.md b/README.md index 1594ef6..a194f25 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ ## 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 line 1556 (`command_queue.append(lambda: HomeCommand())`) in `headless_commander.py`. +* **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. ## 2. Safety Precautions & Disclaimer @@ -13,290 +13,728 @@ This control software includes several built-in safety features designed to prev > **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. -## Instalation -Intructions are the same as for [commander software](https://github.com/PCrnjak/PAROL-commander-software) Follow the guide located [here](https://github.com/PCrnjak/PAROL-commander-software) +## 3. Installation -## 3. System Architecture -The system uses a server/client model to separate robot operation from command generation. -* **The Robot Controller (`headless_commander.py`)**: This script is the server and runs on the computer connected to the robot. Its high-frequency main loop handles a command queue that processes a sequence of object-oriented commands one at a time. A non-blocking UDP server listens for remote commands and adds them to the queue without interrupting the current operation. -* **The Remote Client (`robot_api.py`)**: The functions in this script act as the client, sending command messages to the robot controller via UDP. This decoupled design allows control from other programs or computers. +### 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) -## 4. Command Reference & API Usage -This section details each command, its parameters, and provides a Python example using the `robot_api.py` functions. +### Additional Dependencies for Headless Commander +After installing the base software, install these additional packages: -### Motion & Logic Commands +```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']}") +``` + +## 5. Command Reference & API Usage + +### Motion Commands #### `home_robot()` * **Purpose**: Initiates the robot's built-in homing sequence. -* **Parameters**: None. +* **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() + 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]`): A list of 6 target angles in degrees for joints 1-6. - * `duration` (`Optional[float]`): The total time for the movement in seconds. - * `speed_percentage` (`Optional[int]`): The desired speed as a percentage (0-100) of the robot's synchronized maximum. -* > *Note: You must provide either `duration` or `speed_percentage`, but not both. If both are given, `duration` will take precedence.* + * `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 - # Move by speed (75% of max) + # Simple move by speed move_robot_joints([90, -45, 90, 0, 45, 180], speed_percentage=75) - # Move by duration (in 5.5 seconds) - move_robot_joints([0, -90, 180, 0, 0, 180], duration=5.5) + # 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]`): A list of 6 values for the target pose `[x, y, z, Rx, Ry, Rz]`, with positions in millimeters and rotations in degrees. - * `duration` (`Optional[float]`): The total time for the movement in seconds. - * `speed_percentage` (`Optional[int]`): The desired speed as a percentage (0-100). -* > *Note: You must provide either `duration` or `speed_percentage`, but not both. If both are given, `duration` will take precedence.* + * `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 by speed (50% of max) move_robot_pose([250, 0, 200, 180, 0, 90], speed_percentage=50) - - # Move by duration (in 3 seconds) - move_robot_pose([150, 100, 250, 180, 0, 90], duration=3.0) ``` #### `move_robot_cartesian()` * **Purpose**: Moves the end-effector to a target pose in a guaranteed straight-line path. * **Parameters**: - * `pose` (`List[float]`): A list of 6 values for the target pose `[x, y, z, Rx, Ry, Rz]`. - * `duration` (`float`): The required total time for the movement in seconds. Must be a realistic value. + * `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 to pose in a straight line over 4 seconds move_robot_cartesian([200, -50, 180, 180, 0, 135], duration=4.0) ``` -#### `jog_cartesian()` -* **Purpose**: Jogs the end-effector continuously along an axis. -* **Parameters**: - * `frame` (`str`): The reference frame, either `'TRF'` (Tool) or `'WRF'` (World). - * `axis` (`str`): The axis and direction (e.g., `'X+'`, `'Y-'`, `'RZ+'`). - * `speed_percentage` (`int`): The jog speed as a percentage (0-100). - * `duration` (`float`): The time in seconds to jog for. -* **Python API Usage**: - ```python - from robot_api import jog_cartesian - - # Jog in the tool's Z+ direction for 1.5s at 50% speed - jog_cartesian(frame='TRF', axis='Z+', speed_percentage=50, duration=1.5) - ``` +### Jogging Commands #### `jog_robot_joint()` * **Purpose**: Jogs a single joint by time or angular distance. * **Parameters**: - * `joint_index` (`int`): The joint to move. 0-5 for positive direction (J1-J6), 6-11 for negative direction. - * `speed_percentage` (`int`): The jog speed as a percentage (0-100). - * `duration` (`Optional[float]`): The time in seconds to jog for. - * `distance_deg` (`Optional[float]`): The distance in degrees to jog. + * `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 (index 0) for 2 seconds + # 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 (index 2 -> negative is 2+6=8) + # Jog joint 3 backwards by 15 degrees jog_robot_joint(joint_index=8, speed_percentage=60, distance_deg=15) ``` #### `jog_multiple_joints()` -* **Purpose**: Allows you to jog multiple joints at the same time. +* **Purpose**: Jogs multiple joints simultaneously. * **Parameters**: - * `joints` (`List[int]`): A list of joint indices. Use 0-5 for positive direction and 6-11 for negative direction (e.g., 6 is J1-). - * `speeds` (`List[float]`): A list of corresponding speeds (1-100%). The number of speeds must match the number of joints. - * `duration (float)`: The duration of the jog in seconds. + * `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) + ``` - # Jog joints J1, J4 and J6 at a speed of 70%, 40% and 60% respectively for a duration of 0.6 seconds - jog_multiple_joints([0, 3, 5], [70, 40, 60], 0.6) +### Smooth Motion Commands - # Jog joints J1, J4 and J6 in the opposite direction at a speed of 70%, 40% and 60% respectively for a duration of 1.2 seconds - jog_multiple_joints([(0+6), (3+6), (5+6)], [70, 40, 60], 1.2) - # Equivalent to jog_multiple_joints([6, 9, 11], [70, 40, 60], 1.2) +These commands create smooth, curved trajectories with continuous velocity profiles: -#### `delay_robot()` -* **Purpose**: Pauses the command queue execution. +#### `smooth_circle()` +* **Purpose**: Execute a smooth circular motion. * **Parameters**: - * `duration` (`float`): The pause time in seconds. + * `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' + * `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 delay_robot + from robot_api import smooth_circle + # Draw a 50mm radius circle in XY plane + smooth_circle(center=[200, 0, 200], radius=50, plane='XY', duration=5.0) + ``` - # Pause for 2.5 seconds - delay_robot(2.5) +#### `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 + * `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 + smooth_arc_center(end_pose=[250, 50, 200, 0, 0, 90], + center=[200, 0, 200], + duration=3.0) + ``` + +#### `smooth_arc_parametric()` +* **Purpose**: Execute a smooth arc motion defined by radius and angle. +* **Parameters**: + * `end_pose` (List[float]): End pose [x, y, z, rx, ry, rz] in mm and degrees + * `radius` (float): Arc radius in mm + * `arc_angle` (float): Arc angle in degrees + * `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 + smooth_arc_parametric(end_pose=[250, 50, 200, 0, 0, 90], + radius=50, arc_angle=90, duration=3.0) + ``` + +#### `smooth_spline()` +* **Purpose**: Create smooth spline through waypoints. +* **Parameters**: + * `waypoints` (List[List[float]]): List of poses [x, y, z, rx, ry, rz] to pass through + * `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 + waypoints = [ + [200, 0, 100, 0, 0, 0], + [250, 50, 150, 0, 15, 45], + [200, 100, 200, 0, 30, 90] + ] + smooth_spline(waypoints, duration=8.0) + ``` + +#### `smooth_helix()` +* **Purpose**: Execute helical motion. +* **Parameters**: + * `center` (List[float]): Helix center point [x, y, z] in mm + * `radius` (float): Helix radius in mm + * `pitch` (float): Vertical distance per revolution in mm + * `height` (float): Total height of helix in mm + * `start_pose` (List[float], optional): Starting pose, or None for current position. Default: None + * `duration` (float, optional): Time to complete helix in seconds + * `speed_percentage` (float, optional): Speed as percentage (1-100) + * `clockwise` (bool, optional): Direction of motion. Default: False + * `wait_for_ack` (bool, optional): Enable command tracking. Default: False + * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 10.0 + * `non_blocking` (bool, optional): Return immediately with command ID. Default: False +* > *Note: You must provide either `duration` or `speed_percentage`, but not both.* +* **Python API Usage**: + ```python + from robot_api import smooth_helix + smooth_helix(center=[200, 0, 150], radius=30, pitch=20, + height=100, duration=10.0) ``` -#### `control_pneumatic_gripper()` / `control_electric_gripper()` -* **Purpose**: Controls the pneumatic or electric gripper. (Untested) -* **Parameters (Pneumatic)**: - * `action` (`str`): The action to perform, either `'open'` or `'close'`. - * `port` (`int`): The digital output port to use, either `1` or `2`. -* **Parameters (Electric)**: - * `action` (`str`): The action to perform, either `'move'` or `'calibrate'`. - * `position` (`Optional[int]`): Target position (0-255). - * `speed` (`Optional[int]`): Movement speed (0-255). - * `current` (`Optional[int]`): Max motor current (100-1000 mA). +#### `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 + * `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 control_pneumatic_gripper, control_electric_gripper + from robot_api import smooth_blend + segments = [ + {'type': 'LINE', 'end': [250, 0, 200, 0, 0, 0], 'duration': 2.0}, + {'type': 'CIRCLE', 'center': [250, 0, 200], 'radius': 50, + 'plane': 'XY', 'duration': 4.0, 'clockwise': False}, + {'type': 'LINE', 'end': [200, 0, 200, 0, 0, 0], 'duration': 2.0} + ] + smooth_blend(segments, blend_time=0.5, duration=10.0) + ``` - # Pneumatic +### 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) + ``` - # Electric Move +#### `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) + ``` - # Electric Calibrate - control_electric_gripper(action='calibrate') +### 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() ``` -### State Query Commands +### Query Commands + +These commands request current robot state without moving the robot: #### `get_robot_pose()` -* **Purpose**: Queries the robot for its current end-effector pose. -* **Parameters**: None. -* **Returns**: A list of 6 values `[x, y, z, Rx, Ry, Rz]` or `None` on failure. +* **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 - - current_pose = get_robot_pose() - if current_pose: - print(f"Current Pose (x,y,z,r,p,y): {current_pose}") + pose = get_robot_pose() + if pose: + print(f"Current pose: {pose}") ``` #### `get_robot_joint_angles()` -* **Purpose**: Queries the robot for its current joint angles. -* **Parameters**: None. -* **Returns**: A list of 6 angles in degrees `[j1, j2, j3, j4, j5, j6]` or `None` on failure. +* **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() - if angles: - print(f"Current Angles (deg): {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**: Requests the robot's current digital I/O status. -* **Parameters**: None. -* **Returns**: Returns a list [IN1, IN2, OUT1, OUT2, ESTOP] or None if it fails. +* **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' + * `wait_for_ack` (bool, optional): Enable command tracking. Default: True + * `timeout` (float, optional): Timeout for acknowledgment in seconds. Default: 30.0 * **Python API Usage**: ```python - from robot_api import get_robot_io - - data = get_robot_io() - print(data) + from robot_api import execute_trajectory + 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') ``` -#### `get_electric_gripper_status()` -* **Purpose**: Requests the electric gripper's current status. -* **Parameters**: None. -* **Returns**: Returns a list [ID, Position, Speed, Current, StatusByte, ObjectDetected] or None. +#### `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 + * `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 get_electric_gripper_status - - data = get_electric_gripper_status() - print(data) + from robot_api import chain_smooth_motions + 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) ``` -## 5. Setup & Operation +#### `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 -Before running the system, ensure you have the required Python packages installed. You can install them using pip: - -pip install roboticstoolbox-python numpy oclock pyserial keyboard - -File Structure -For the system to work correctly, the following script files must all be located in the -same folder: -● headless_commander.py (The main server/controller) -● robot_api.py (The client API for sending commands) -● PAROL6_ROBOT.py (The robot's specific configuration and kinematic model) - -> [!NOTE] -> com_port.txt is optional and it needs to have a single element and that is your USB com port of the robot. For example COM5 -> - -As long as these three files are kept together, their parent folder can be located
-anywhere on your computer.
-How to Operate the System
-1. Start Controller: In a terminal, navigate to your folder and run the main controller
-script:
-```python -python headless_commander.py + +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: + +```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 + +# Optional visualization (if using GUI components) +pip3 install swift-sim==1.0.1 +pip3 install customtkinter +pip3 install customtkinter --upgrade ``` -2. Send Commands: In a separate script or terminal, you can import and use the -functions from robot_api.py to send commands to the running controller. For -example: -```python -from robot_api import move_robot_joints -move_robot_joints([90, -90, 160, 12, 12, 180], duration=5.5) -delay_robot(0.2) -move_robot_joints([50, -60, 180, -12, 32, 0], duration=5.5) -delay_robot(0.2) +#### Client Dependencies (for `robot_api.py`) +The client API is designed to be lightweight with minimal dependencies: + +```bash +# Only needed for get_robot_pose() matrix conversion +pip3 install numpy==1.23.4 +pip3 install spatialmath + +# All other functionality uses only Python standard library: +# socket, threading, time, uuid, datetime, collections, typing ``` +**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. -Or use test_script.py
+### File Structure -* **Python API Usage**: +#### 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 - # your_script.py - 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 + 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 - 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()) + 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: + +```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) +``` + +## 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 + +For additional support, refer to the [PAROL commander software repository](https://github.com/PCrnjak/PAROL-commander-software). From 3cb0446d0180ac3895c835227b442a20d1cd97c7 Mon Sep 17 00:00:00 2001 From: AlvarEhr <54718121+AlvarEhr@users.noreply.github.com> Date: Sat, 23 Aug 2025 21:38:22 +0100 Subject: [PATCH 03/16] Update README.md Small changes to README --- README.md | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index a194f25..cf9eadf 100644 --- a/README.md +++ b/README.md @@ -595,11 +595,6 @@ pip3 install oclock # User input pip3 install keyboard - -# Optional visualization (if using GUI components) -pip3 install swift-sim==1.0.1 -pip3 install customtkinter -pip3 install customtkinter --upgrade ``` #### Client Dependencies (for `robot_api.py`) @@ -738,3 +733,5 @@ while True: * **Smooth Motion Errors**: Verify waypoints are reachable and properly formatted For additional support, refer to the [PAROL commander software repository](https://github.com/PCrnjak/PAROL-commander-software). + +Or you can head over to the [PAROL6 Discord channel](https://discord.com/invite/prjUvjmGpZ) for extra support From e28ffdb30bea779da242d3256805a3f01bd58166 Mon Sep 17 00:00:00 2001 From: AlvarEhr <54718121+AlvarEhr@users.noreply.github.com> Date: Sun, 24 Aug 2025 12:27:02 +0100 Subject: [PATCH 04/16] Update README.md --- README.md | 127 +++++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 125 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index cf9eadf..0dcaf78 100644 --- a/README.md +++ b/README.md @@ -233,7 +233,13 @@ if status and status['completed']: ### Smooth Motion Commands -These commands create smooth, curved trajectories with continuous velocity profiles: +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. @@ -241,6 +247,7 @@ These commands create smooth, curved trajectories with continuous velocity profi * `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) @@ -252,8 +259,12 @@ These commands create smooth, curved trajectories with continuous velocity profi * **Python API Usage**: ```python from robot_api import smooth_circle - # Draw a 50mm radius circle in XY plane + + # 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()` @@ -261,6 +272,7 @@ These commands create smooth, curved trajectories with continuous velocity profi * **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) @@ -272,9 +284,17 @@ These commands create smooth, curved trajectories with continuous velocity profi * **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()` @@ -283,6 +303,7 @@ These commands create smooth, curved trajectories with continuous velocity profi * `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) @@ -294,14 +315,23 @@ These commands create smooth, curved trajectories with continuous velocity profi * **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) @@ -312,12 +342,22 @@ These commands create smooth, curved trajectories with continuous velocity profi * **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()` @@ -327,6 +367,7 @@ These commands create smooth, curved trajectories with continuous velocity profi * `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) @@ -335,11 +376,18 @@ These commands create smooth, curved trajectories with continuous velocity profi * `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()` @@ -347,6 +395,7 @@ These commands create smooth, curved trajectories with continuous velocity profi * **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) @@ -356,6 +405,8 @@ These commands create smooth, curved trajectories with continuous velocity profi * **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, @@ -363,6 +414,78 @@ These commands create smooth, curved trajectories with continuous velocity profi {'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) + ``` + +### Updated Helper Functions + +#### `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) + ``` + +#### `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') ``` ### Gripper Commands From 6615879018b315b4227f0593ee7a299c1bdea179 Mon Sep 17 00:00:00 2001 From: AlvarEhr <54718121+AlvarEhr@users.noreply.github.com> Date: Sun, 24 Aug 2025 12:31:22 +0100 Subject: [PATCH 05/16] Update README.md Removed duplicate section --- README.md | 88 +++++++++++++++---------------------------------------- 1 file changed, 24 insertions(+), 64 deletions(-) diff --git a/README.md b/README.md index 0dcaf78..0786d66 100644 --- a/README.md +++ b/README.md @@ -424,70 +424,6 @@ These commands create smooth, curved trajectories with continuous velocity profi ] smooth_blend(tool_segments, frame='TRF', blend_time=0.5, duration=10.0) ``` - -### Updated Helper Functions - -#### `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) - ``` - -#### `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') - ``` - ### Gripper Commands #### `control_pneumatic_gripper()` @@ -621,18 +557,31 @@ These commands request current robot state without moving the robot: * `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**: @@ -655,18 +604,29 @@ These commands request current robot state without moving the robot: * **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()` From 7368dcace8ee691c3e7f305d96fcd73da51602a4 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Wed, 27 Aug 2025 21:07:40 -0400 Subject: [PATCH 06/16] proposed changes to support webgui --- headless_commander.py | 182 ++++++++++++++++++++++++++++++++++++------ 1 file changed, 159 insertions(+), 23 deletions(-) diff --git a/headless_commander.py b/headless_commander.py index ab3498b..18c3f5d 100644 --- a/headless_commander.py +++ b/headless_commander.py @@ -25,11 +25,23 @@ import re import logging import struct -import keyboard +try: + import keyboard # type: ignore +except Exception: + keyboard = None from typing import Optional, Tuple from spatialmath.base import trinterp from collections import namedtuple, deque -import GUI.files.PAROL6_ROBOT as PAROL6_ROBOT +try: + import PAROL6_ROBOT as PAROL6_ROBOT +except ModuleNotFoundError: + import os, sys + legacy = os.path.join(os.path.dirname(os.path.dirname(__file__)), 'PAROL-commander-software', 'GUI', 'files') + if os.path.exists(os.path.join(legacy, 'PAROL6_ROBOT.py')): + sys.path.append(legacy) + import PAROL6_ROBOT as PAROL6_ROBOT + else: + raise from smooth_motion import CircularMotion, SplineMotion, MotionBlender # Set interval @@ -42,6 +54,17 @@ ) logging.disable(logging.DEBUG) +# ========================= +# Runtime flags and globals +# ========================= +enabled = True +soft_error = False +disabled_reason = "" + +# Ensure serial globals exist on all platforms +ser: Optional[serial.Serial] = None +com_port_str: Optional[str] = None + my_os = platform.system() if my_os == "Windows": @@ -54,8 +77,8 @@ except (FileNotFoundError, serial.SerialException): # If the file doesn't exist or the port is invalid, ask the user while True: + com_port = input("Enter the COM port (e.g., COM9): ") 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 @@ -97,7 +120,7 @@ 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_buffer = [b'']*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 ####################################################################################### ####################################################################################### @@ -557,6 +580,7 @@ def Pack_data(Position_out,Speed_out,Command_out,Affected_joint_out,InOut_out,Ti 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 ser global start_cond1_byte global start_cond2_byte @@ -575,8 +599,12 @@ def Get_data(Position_in,Speed_in,Homed_in,InOut_in,Temperature_error_in,Positio global data_buffer global data_counter - while (ser.inWaiting() > 0): - input_byte = ser.read() + # Ensure serial is available before reading + if ser is None or not ser.is_open: + return + + while ser.in_waiting > 0: + input_byte = ser.read(1) #UNCOMMENT THIS TO GET ALL DATA FROM THE ROBOT PRINTED #print(input_byte) @@ -1019,7 +1047,7 @@ class CartesianJogCommand: 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): + def __init__(self, frame, axis, speed_percentage=50.0, duration=1.5, **kwargs): """ Initializes and validates the Cartesian jog command. """ @@ -3179,8 +3207,11 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: # --- Test 1: Homing and Initial Setup # -------------------------------------------------------------------------- -# 1. Start with the mandatory Home command. -command_queue.append(HomeCommand()) +# 1. Optionally start with the Home command (can be bypassed via PAROL6_NOAUTOHOME) +if not str(os.getenv("PAROL6_NOAUTOHOME", "0")).lower() in ("1", "true", "yes", "on"): + command_queue.append(HomeCommand()) +else: + print("PAROL6_NOAUTOHOME is set; skipping auto-home on startup.") # --- State variable for the currently running command --- active_command = None @@ -3209,9 +3240,21 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: 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}") + # Load port from com_port.txt if not already set + if not com_port_str: + try: + with open("com_port.txt", "r") as f: + com_port_str = f.read().strip() + except FileNotFoundError: + com_port_str = None + + if com_port_str: + ser = serial.Serial(port=com_port_str, baudrate=3000000, timeout=0) + if ser.is_open: + print(f"Successfully reconnected to {com_port_str}") + else: + # No port configured yet; wait and retry + time.sleep(1) except serial.SerialException as e: ser = None time.sleep(1) @@ -3243,11 +3286,9 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: active_command_id = None # Clear queue and notify about cancelled commands - for queued_cmd in command_id_map.keys(): + for queued_cmd, (qid, qaddr) in list(command_id_map.items()): 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) + send_acknowledgment(qid, "CANCELLED", "Queue cleared by STOP", qaddr) command_queue.clear() command_id_map.clear() @@ -3305,6 +3346,89 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: if cmd_id: send_acknowledgment(cmd_id, "COMPLETED", "Speed data sent", addr) + elif command_name == 'PING': + # Respond with PONG and ACK if cmd_id present + sock.sendto("PONG".encode('utf-8'), addr) + if cmd_id: + send_acknowledgment(cmd_id, "COMPLETED", "PONG", addr) + + elif command_name == 'GET_STATUS': + # Aggregate POSE, ANGLES, IO, GRIPPER into one frame + try: + 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)) + except Exception: + pose_str = ",".join(["0"] * 16) + + 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)) + + io_status_str = ",".join(map(str, InOut_in[:5])) + gripper_status_str = ",".join(map(str, Gripper_data_in)) + + response_message = f"STATUS|POSE={pose_str}|ANGLES={angles_str}|IO={io_status_str}|GRIPPER={gripper_status_str}" + sock.sendto(response_message.encode('utf-8'), addr) + if cmd_id: + send_acknowledgment(cmd_id, "COMPLETED", "Status data sent", addr) + + elif command_name == 'ENABLE': + enabled = True + disabled_reason = "" + if cmd_id: + send_acknowledgment(cmd_id, "COMPLETED", "Controller enabled", addr) + + elif command_name == 'DISABLE': + enabled = False + disabled_reason = "Disabled by user" + # Cancel active command + if active_command and active_command_id: + send_acknowledgment(active_command_id, "CANCELLED", "Disabled by user", addr) + active_command = None + active_command_id = None + # Cancel queued commands + for queued_cmd, (qid, qaddr) in list(command_id_map.items()): + send_acknowledgment(qid, "CANCELLED", "Controller disabled", qaddr) + command_queue.clear() + command_id_map.clear() + # Stop robot motion + Command_out.value = 255 + Speed_out[:] = [0] * 6 + if cmd_id: + send_acknowledgment(cmd_id, "COMPLETED", "Controller disabled", addr) + + elif command_name == 'CLEAR_ERROR': + soft_error = False + if cmd_id: + send_acknowledgment(cmd_id, "COMPLETED", "Errors cleared", addr) + + elif command_name == 'SET_PORT' and len(parts) >= 2: + new_port = parts[1].strip() + if new_port: + # Persist and trigger reconnection + try: + with open("com_port.txt", "w") as f: + f.write(new_port) + except Exception as e: + if cmd_id: + send_acknowledgment(cmd_id, "FAILED", f"Could not write com_port.txt: {e}", addr) + # Do not fall through to queuing + continue + try: + if ser and ser.is_open: + ser.close() + except Exception: + pass + com_port_str = new_port + ser = None # main loop will reconnect + if cmd_id: + send_acknowledgment(cmd_id, "COMPLETED", f"Port set to {new_port}; reconnecting...", addr) + else: + if cmd_id: + send_acknowledgment(cmd_id, "FAILED", "No port specified", addr) + else: # Queue command for processing incoming_command_buffer.append((raw_message, addr)) @@ -3327,6 +3451,15 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: parts = message.split('|') command_name = parts[0].upper() + # Gate motion commands when controller is disabled + if not enabled and command_name in {'MOVEPOSE','MOVEJOINT','MOVECART','JOG','MULTIJOG','CARTJOG', + 'SMOOTH_CIRCLE','SMOOTH_ARC_CENTER','SMOOTH_ARC_PARAM', + 'SMOOTH_SPLINE','SMOOTH_HELIX','SMOOTH_BLEND','HOME'}: + if cmd_id: + send_acknowledgment(cmd_id, "FAILED", f"Controller disabled{(': ' + disabled_reason) if disabled_reason else ''}", addr) + # Skip processing this command + continue + # Variable to track if command was successfully queued command_queued = False command_obj = None @@ -3470,7 +3603,7 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: elif e_stop_active: # Waiting for re-enable - if keyboard.is_pressed('e'): + if keyboard and keyboard.is_pressed('e'): print("Re-enabling robot...") Command_out.value = 101 e_stop_active = False @@ -3589,11 +3722,14 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: # --- 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) + if ser is not None and ser.is_open: + 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) + else: + # Serial not available; skip IO this cycle + pass except serial.SerialException as e: print(f"Serial communication error: {e}") @@ -3608,4 +3744,4 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: active_command = None active_command_id = None - timer.checkpt() \ No newline at end of file + timer.checkpt() From 289bdac53d43a6c263e4395dcbae84c9e7a59e7b Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Wed, 27 Aug 2025 22:07:10 -0400 Subject: [PATCH 07/16] set speeds to old gui levels --- headless_commander.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/headless_commander.py b/headless_commander.py index 18c3f5d..b178fc7 100644 --- a/headless_commander.py +++ b/headless_commander.py @@ -874,7 +874,9 @@ def prepare_for_execution(self, current_position_in): 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])) + # Map jog speed to dedicated jog caps (old GUI behavior), not 2x global max + max_joint_speed = PAROL6_ROBOT.Joint_max_jog_speed[self.joint_index] + speed_steps_per_sec = int(np.interp(abs(self.speed_percentage), [0, 100], [0, max_joint_speed])) self.speed_out = speed_steps_per_sec * self.direction self.command_len = int(self.duration / INTERVAL_S) if self.duration else float('inf') @@ -985,8 +987,9 @@ def prepare_for_execution(self, current_position_in): 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]])) + # Calculate speed in steps/sec (use jog caps like old GUI) + max_joint_speed = PAROL6_ROBOT.Joint_max_jog_speed[joint_index] + speed_steps_per_sec = int(np.interp(speed_percentage, [0, 100], [0, max_joint_speed])) self.speeds_out[joint_index] = speed_steps_per_sec * direction print(" -> MultiJog command is ready.") From 66f0090f6a253f3d5c5c869197b45aca1324c0fb Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Wed, 27 Aug 2025 22:13:58 -0400 Subject: [PATCH 08/16] ok maybe a bit faster --- headless_commander.py | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/headless_commander.py b/headless_commander.py index b178fc7..c4fd40b 100644 --- a/headless_commander.py +++ b/headless_commander.py @@ -48,6 +48,9 @@ INTERVAL_S = 0.01 prev_time = 0 +# Jogging speed scale to slightly increase jog caps while respecting absolute limits +JOG_SPEED_SCALE = 1.2 + logging.basicConfig(level = logging.DEBUG, format='%(asctime)s.%(msecs)03d %(levelname)s:\t%(message)s', datefmt='%H:%M:%S' @@ -874,8 +877,10 @@ def prepare_for_execution(self, current_position_in): print("Error: 'speed_percentage' must be provided if not calculating automatically.") self.is_valid = False return - # Map jog speed to dedicated jog caps (old GUI behavior), not 2x global max - max_joint_speed = PAROL6_ROBOT.Joint_max_jog_speed[self.joint_index] + # Map jog speed to jog caps scaled up slightly, clamped to absolute joint max + max_cap = int(PAROL6_ROBOT.Joint_max_jog_speed[self.joint_index] * JOG_SPEED_SCALE) + abs_max = PAROL6_ROBOT.Joint_max_speed[self.joint_index] + max_joint_speed = min(abs_max, max_cap) speed_steps_per_sec = int(np.interp(abs(self.speed_percentage), [0, 100], [0, max_joint_speed])) self.speed_out = speed_steps_per_sec * self.direction @@ -987,8 +992,10 @@ def prepare_for_execution(self, current_position_in): self.is_valid = False return - # Calculate speed in steps/sec (use jog caps like old GUI) - max_joint_speed = PAROL6_ROBOT.Joint_max_jog_speed[joint_index] + # Calculate speed in steps/sec using scaled jog caps, clamped to absolute joint max + max_cap = int(PAROL6_ROBOT.Joint_max_jog_speed[joint_index] * JOG_SPEED_SCALE) + abs_max = PAROL6_ROBOT.Joint_max_speed[joint_index] + max_joint_speed = min(abs_max, max_cap) speed_steps_per_sec = int(np.interp(speed_percentage, [0, 100], [0, max_joint_speed])) self.speeds_out[joint_index] = speed_steps_per_sec * direction From b3f7b5fa2ca2924589488cf43b16db9e518412b1 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Wed, 27 Aug 2025 22:29:09 -0400 Subject: [PATCH 09/16] change connection to non-blocking --- headless_commander.py | 45 ++++++++++++++++++++++--------------------- 1 file changed, 23 insertions(+), 22 deletions(-) diff --git a/headless_commander.py b/headless_commander.py index c4fd40b..b3552b6 100644 --- a/headless_commander.py +++ b/headless_commander.py @@ -67,6 +67,8 @@ # Ensure serial globals exist on all platforms ser: Optional[serial.Serial] = None com_port_str: Optional[str] = None +# Non-blocking serial reconnect throttle +last_reconnect_attempt = 0.0 my_os = platform.system() @@ -3246,29 +3248,28 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: while timer.elapsed_time < 1100000: - # --- Connection Handling --- + # --- Connection Handling (non-blocking) --- if ser is None or not ser.is_open: - print("Serial port not open. Attempting to reconnect...") - try: - # Load port from com_port.txt if not already set - if not com_port_str: - try: - with open("com_port.txt", "r") as f: - com_port_str = f.read().strip() - except FileNotFoundError: - com_port_str = None - - if com_port_str: - ser = serial.Serial(port=com_port_str, baudrate=3000000, timeout=0) - if ser.is_open: - print(f"Successfully reconnected to {com_port_str}") - else: - # No port configured yet; wait and retry - time.sleep(1) - except serial.SerialException as e: - ser = None - time.sleep(1) - continue + now = time.time() + if now - last_reconnect_attempt > 1.0: + print("Serial port not open. Attempting to reconnect...") + last_reconnect_attempt = now + try: + # Load port from com_port.txt if not already set + if not com_port_str: + try: + with open("com_port.txt", "r") as f: + com_port_str = f.read().strip() + except FileNotFoundError: + com_port_str = None + + if com_port_str: + 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: + ser = None + # Do not block or continue; proceed to UDP handling every tick # ======================================================================= # === NETWORK COMMAND RECEPTION WITH ID PARSING === From 850656d5abb9010768087bf5cc2d47c03d5812e1 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Thu, 28 Aug 2025 22:57:07 -0400 Subject: [PATCH 10/16] switch prints to logging for easily configurable logging levels --- headless_commander.py | 309 +++++++++++++++++++++--------------------- 1 file changed, 157 insertions(+), 152 deletions(-) diff --git a/headless_commander.py b/headless_commander.py index b3552b6..04aa242 100644 --- a/headless_commander.py +++ b/headless_commander.py @@ -51,11 +51,19 @@ # Jogging speed scale to slightly increase jog caps while respecting absolute limits JOG_SPEED_SCALE = 1.2 -logging.basicConfig(level = logging.DEBUG, - format='%(asctime)s.%(msecs)03d %(levelname)s:\t%(message)s', - datefmt='%H:%M:%S' -) -logging.disable(logging.DEBUG) +# Configure unified logging (PAROL_LOG_LEVEL) and simple console formatter +PAROL_LOG_LEVEL = os.getenv("PAROL_LOG_LEVEL", "WARNING").upper() +_level = getattr(logging, PAROL_LOG_LEVEL, logging.WARNING) +root = logging.getLogger() +root.setLevel(_level) +for h in list(root.handlers): + if isinstance(h, logging.StreamHandler): + root.removeHandler(h) +console = logging.StreamHandler() +console.setLevel(_level) +console.setFormatter(logging.Formatter("[%(levelname)s] %(message)s")) +root.addHandler(console) +logger = logging.getLogger("parol6.server") # ========================= # Runtime flags and globals @@ -78,20 +86,20 @@ 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}") + logging.info(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: com_port = input("Enter the COM port (e.g., COM9): ") try: ser = serial.Serial(port=com_port, baudrate=3000000, timeout=0) - print(f"Successfully connected to {com_port}") + logging.info(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.") + logging.warning(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 @@ -249,7 +257,7 @@ def calculate_adaptive_tolerance(robot, q, strict_tol=1e-10, loose_tol=1e-7): # 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})") + logging.debug(f"Adaptive IK tolerance: {adaptive_tol:.2e} ({tol_category}) - Manipulability: {manip:.8f} (threshold: {singularity_threshold})") _prev_tolerance = adaptive_tol return adaptive_tol @@ -353,9 +361,9 @@ def _solve(Ta: SE3, Tb: SE3, q_seed, depth, tol): 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}") + # logging.debug(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}") + logging.warning(f"Target reach limit exceeded: {target_reach:.3f} > {max_reach_threshold:.3f}") return [], False, depth, 0 else: damping = 0.0000001 # Normal low damping @@ -404,7 +412,7 @@ def _solve(Ta: SE3, Tb: SE3, q_seed, depth, tol): # Create a UDP socket sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.bind((ip, port)) -print(f'Start listening to {ip}:{port}') +logging.info(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): @@ -512,7 +520,7 @@ def Pack_data(Position_out,Speed_out,Command_out,Affected_joint_out,InOut_out,Ti test_list = [] - #print(test_list) + #logging.debug(test_list) #x = bytes(start_bytes) test_list.append((start_bytes)) @@ -579,7 +587,7 @@ def Pack_data(Position_out,Speed_out,Command_out,Affected_joint_out,InOut_out,Ti # END bytes test_list.append((end_bytes)) - #print(test_list) + #logging.debug(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, @@ -670,9 +678,9 @@ def Get_data(Position_in,Speed_in,Homed_in,InOut_in,Temperature_error_in,Positio # ako je dobar paket je dobar i spremi ga u nove variable! # Print every byte - #print("podaci u data bufferu su:") + #logging.debug("podaci u data bufferu su:") #for i in range(data_len): - #print(data_buffer[i]) + #logging.debug(data_buffer[i]) good_start = 0 start_cond1 = 0 @@ -753,7 +761,7 @@ def __init__(self): 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...") + logging.info("Initializing Home command...") def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): """ @@ -765,7 +773,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): # --- 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}") + logging.info(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: @@ -780,12 +788,12 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): 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.") + logging.info(" -> 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.") + logging.error(" -> ERROR: Timeout waiting for robot to start homing sequence.") self.is_finished = True return self.is_finished @@ -795,7 +803,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): 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.") + logging.info("Homing sequence complete. All joints reported home.") self.is_finished = True Speed_out[:] = [0] * 6 # Ensure robot is stopped @@ -818,15 +826,15 @@ def __init__(self, joint, speed_percentage=None, duration=None, distance_deg=Non # --- 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.") + logging.info(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.") + logging.info(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.") + logging.info(f"Initializing Jog: Joint {joint}, Speed {speed_percentage}%, Distance {distance_deg} deg.") else: - print("Error: JogCommand requires either 'duration', 'distance_deg', or both.") + logging.error("Error: JogCommand requires either 'duration', 'distance_deg', or both.") return # --- 2. Store parameters for deferred calculation --- @@ -847,7 +855,7 @@ def __init__(self, joint, speed_percentage=None, duration=None, distance_deg=Non def prepare_for_execution(self, current_position_in): """Pre-computes speeds and target positions using live data.""" - print(f" -> Preparing for Jog command...") + logging.info(f" -> Preparing for Jog command...") # Determine direction and joint index self.direction = 1 if 0 <= self.joint <= 5 else -1 @@ -861,7 +869,7 @@ def prepare_for_execution(self, current_position_in): 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}).") + logging.error(f" -> VALIDATION FAILED: Target position {self.target_position} is out of joint limits ({min_limit}, {max_limit}).") self.is_valid = False return @@ -871,12 +879,12 @@ def prepare_for_execution(self, current_position_in): 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).") + logging.error(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.") + logging.error("Error: 'speed_percentage' must be provided if not calculating automatically.") self.is_valid = False return # Map jog speed to jog caps scaled up slightly, clamped to absolute joint max @@ -887,7 +895,7 @@ def prepare_for_execution(self, current_position_in): 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.") + logging.info(" -> Jog command is ready.") def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): @@ -912,7 +920,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): stop_reason = f"Limit reached on joint {self.joint_index + 1}." if stop_reason: - print(stop_reason) + logging.info(stop_reason) self.is_finished = True Speed_out[:] = [0] * 6 Command_out.value = 255 @@ -939,15 +947,15 @@ def __init__(self, joints, speed_percentages, duration): # --- 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.") + logging.error("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.") + logging.error("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'.") + logging.error("Error: MultiJogCommand requires a positive 'duration'.") return # ========================================================== @@ -959,13 +967,13 @@ def __init__(self, joints, speed_percentages, duration): 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-).") + logging.error(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.") + logging.info(f"Initializing MultiJog for joints {joints} with speeds {speed_percentages}% for {duration}s.") # --- 2. Store parameters --- self.joints = joints @@ -980,7 +988,7 @@ def __init__(self, joints, speed_percentages, duration): def prepare_for_execution(self, current_position_in): """Pre-computes the speeds for each joint.""" - print(f" -> Preparing for MultiJog command...") + logging.info(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) @@ -990,7 +998,7 @@ def prepare_for_execution(self, current_position_in): # Check for joint index validity if not (0 <= joint_index < 6): - print(f" -> VALIDATION FAILED: Invalid joint index {joint_index}.") + logging.error(f" -> VALIDATION FAILED: Invalid joint index {joint_index}.") self.is_valid = False return @@ -1001,7 +1009,7 @@ def prepare_for_execution(self, current_position_in): speed_steps_per_sec = int(np.interp(speed_percentage, [0, 100], [0, max_joint_speed])) self.speeds_out[joint_index] = speed_steps_per_sec * direction - print(" -> MultiJog command is ready.") + logging.info(" -> MultiJog command is ready.") def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): @@ -1011,7 +1019,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): # Stop if the duration has elapsed if self.command_step >= self.command_len: - print("Timed multi-jog finished.") + logging.info("Timed multi-jog finished.") self.is_finished = True Speed_out[:] = [0] * 6 Command_out.value = 255 @@ -1023,14 +1031,14 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): 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.") + logging.info(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.") + logging.info(f"Limit reached on joint {i + 1}. Stopping jog.") self.is_finished = True Speed_out[:] = [0] * 6 Command_out.value = 255 @@ -1065,10 +1073,10 @@ def __init__(self, frame, axis, speed_percentage=50.0, duration=1.5, **kwargs): """ self.is_valid = False self.is_finished = False - print(f"Initializing CartesianJog: Frame {frame}, Axis {axis}...") + logging.info(f"Initializing CartesianJog: Frame {frame}, Axis {axis}...") if axis not in AXIS_MAP: - print(f" -> VALIDATION FAILED: Invalid axis '{axis}'.") + logging.error(f" -> VALIDATION FAILED: Invalid axis '{axis}'.") return # Store all necessary parameters for use in execute_step @@ -1080,7 +1088,7 @@ def __init__(self, frame, axis, speed_percentage=50.0, duration=1.5, **kwargs): self.end_time = time.time() + self.duration self.is_valid = True - print(" -> Command is valid and ready.") + logging.info(" -> 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: @@ -1088,7 +1096,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): # --- A. Check for completion --- if time.time() >= self.end_time: - print("Cartesian jog finished.") + logging.info("Cartesian jog finished.") self.is_finished = True Speed_out[:] = [0] * 6 Command_out.value = 255 @@ -1131,7 +1139,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): 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.") + logging.warning("IK Warning: Could not find solution for jog step. Stopping.") self.is_finished = True Speed_out[:] = [0] * 6 Command_out.value = 255 @@ -1162,7 +1170,7 @@ def __init__(self, pose, duration=None, velocity_percent=None, accel_percent=50, self.command_step = 0 self.trajectory_steps = [] - print(f"Initializing MovePose to {pose}...") + logging.info(f"Initializing MovePose to {pose}...") # --- MODIFICATION: Store parameters for deferred planning --- self.pose = pose @@ -1185,7 +1193,7 @@ def __init__(self, pose, duration=None, velocity_percent=None, accel_percent=50, 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}...") + logging.info(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') @@ -1194,7 +1202,7 @@ def prepare_for_execution(self, current_position_in): PAROL6_ROBOT.robot, target_pose, initial_pos_rad, ilimit=100) if not ik_solution.success: - print(" -> VALIDATION FAILED: Inverse kinematics failed at execution time.") + logging.error(" -> VALIDATION FAILED: Inverse kinematics failed at execution time.") self.is_valid = False return @@ -1202,7 +1210,7 @@ def prepare_for_execution(self, current_position_in): if self.duration and self.duration > 0: if self.velocity_percent is not None: - print(" -> INFO: Both duration and velocity were provided. Using duration.") + logging.info(" -> 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) @@ -1261,15 +1269,15 @@ def prepare_for_execution(self, current_position_in): 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).") + logging.info(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}") + logging.error(f" -> VALIDATION FAILED: Could not calculate velocity-based trajectory. Error: {e}") self.is_valid = False return else: - print(" -> Using conservative values for MovePose.") + logging.info(" -> 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)): @@ -1277,10 +1285,10 @@ def prepare_for_execution(self, current_position_in): self.trajectory_steps.append((pos_step, None)) if not self.trajectory_steps: - print(" -> Trajectory calculation resulted in no steps. Command is invalid.") + logging.error(" -> Trajectory calculation resulted in no steps. Command is invalid.") self.is_valid = False else: - print(f" -> Trajectory prepared with {len(self.trajectory_steps)} steps.") + logging.info(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. @@ -1288,7 +1296,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): return True if self.command_step >= len(self.trajectory_steps): - print(f"{type(self).__name__} finished.") + logging.info(f"{type(self).__name__} finished.") self.is_finished = True Position_out[:] = Position_in[:] Speed_out[:] = [0] * 6 @@ -1313,7 +1321,7 @@ def __init__(self, target_angles, duration=None, velocity_percent=None, accel_pe self.command_step = 0 self.trajectory_steps = [] - print(f"Initializing MoveJoint to {target_angles}...") + logging.info(f"Initializing MoveJoint to {target_angles}...") # --- MODIFICATION: Store parameters for deferred planning --- self.target_angles = target_angles @@ -1327,21 +1335,21 @@ def __init__(self, target_angles, duration=None, velocity_percent=None, accel_pe 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.") + logging.error(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}...") + logging.info(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.") + logging.info(" -> 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) @@ -1399,16 +1407,16 @@ def prepare_for_execution(self, current_position_in): 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).") + logging.info(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.") + logging.error(f" -> VALIDATION FAILED: Could not calculate velocity-based trajectory. Error: {e}") + logging.error(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.") + logging.info(" -> 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)): @@ -1416,10 +1424,10 @@ def prepare_for_execution(self, current_position_in): self.trajectory_steps.append((pos_step, None)) if not self.trajectory_steps: - print(" -> Trajectory calculation resulted in no steps. Command is invalid.") + logging.error(" -> Trajectory calculation resulted in no steps. Command is invalid.") self.is_valid = False else: - print(f" -> Trajectory prepared with {len(self.trajectory_steps)} steps.") + logging.info(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. @@ -1427,7 +1435,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): return True if self.command_step >= len(self.trajectory_steps): - print(f"{type(self).__name__} finished.") + logging.info(f"{type(self).__name__} finished.") self.is_finished = True Position_out[:] = Position_in[:] Speed_out[:] = [0] * 6 @@ -1457,10 +1465,10 @@ def __init__(self, pose, duration=None, velocity_percent=None): # --- 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'.") + logging.error(" -> 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.") + logging.info(" -> INFO: Both duration and velocity_percent provided. Using duration.") self.velocity_percent = None # Prioritize duration else: self.velocity_percent = velocity_percent @@ -1475,28 +1483,28 @@ def __init__(self, pose, duration=None, velocity_percent=None): 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}...") + logging.info(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...") + logging.info(" -> 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.") + logging.error(" -> VALIDATION FAILED: The final target pose is unreachable.") if ik_check.violations: - print(f" Reason: Solution violates joint limits: {ik_check.violations}") + logging.error(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...") + logging.info(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) @@ -1514,15 +1522,15 @@ def prepare_for_execution(self, current_position_in): calculated_duration = max(time_linear, time_angular) if calculated_duration <= 0: - print(" -> INFO: MoveCart has zero duration. Marking as finished.") + logging.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 - print(f" -> Calculated MoveCart duration: {self.duration:.2f}s") + logging.info(f" -> Calculated MoveCart duration: {self.duration:.2f}s") - print(" -> Command is valid and ready for execution.") + logging.info(" -> 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: @@ -1543,9 +1551,9 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): ) if not ik_solution.success: - print(" -> ERROR: MoveCart failed. An intermediate point on the path is unreachable.") + logging.error(" -> 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}") + logging.error(f" Reason: Path violates joint limits: {ik_solution.violations}") self.is_finished = True Speed_out[:] = [0] * 6 Command_out.value = 255 @@ -1561,7 +1569,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): # --- END MODIFIED BLOCK --- if s >= 1.0: - print(f"MoveCart finished in ~{elapsed_time:.2f}s.") + logging.info(f"MoveCart finished in ~{elapsed_time:.2f}s.") self.is_finished = True # The main loop will handle holding the final position. @@ -1607,7 +1615,7 @@ def __init__(self, gripper_type, action=None, position=100, speed=100, current=5 self.is_valid = False if not self.is_valid: - print(f" -> VALIDATION FAILED for GripperCommand with action: '{self.action}'") + logging.error(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: @@ -1615,14 +1623,14 @@ def execute_step(self, Gripper_data_out, InOut_out, Gripper_data_in, InOut_in, * self.timeout_counter -= 1 if self.timeout_counter <= 0: - print(f" -> ERROR: Gripper command timed out in state {self.state}.") + logging.error(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.") + logging.info(" -> Pneumatic gripper command sent.") self.is_finished = True return True @@ -1637,7 +1645,7 @@ def execute_step(self, Gripper_data_out, InOut_out, Gripper_data_in, InOut_in, * # --- Calibrate Logic (Timed Delay) --- if self.state == "SEND_CALIBRATE": - print(" -> Sending one-shot calibrate command...") + logging.info(" -> Sending one-shot calibrate command...") Gripper_data_out[4] = 1 # Set mode to calibrate self.state = "WAITING_CALIBRATION" return False @@ -1645,7 +1653,7 @@ def execute_step(self, Gripper_data_out, InOut_out, Gripper_data_in, InOut_in, * if self.state == "WAITING_CALIBRATION": self.wait_counter -= 1 if self.wait_counter <= 0: - print(" -> Calibration delay finished.") + logging.info(" -> Calibration delay finished.") Gripper_data_out[4] = 0 # Reset to operation mode self.is_finished = True return True @@ -1663,7 +1671,7 @@ def execute_step(self, Gripper_data_out, InOut_out, Gripper_data_in, InOut_in, * # Check for completion current_position = Gripper_data_in[1] if abs(current_position - self.target_position) <= 5: - print(f" -> Gripper move complete.") + logging.info(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] @@ -1692,10 +1700,10 @@ def __init__(self, duration): # --- 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}.") + logging.error(f" -> VALIDATION FAILED: Delay duration must be a positive number, but got {duration}.") return - print(f"Initializing Delay for {duration} seconds...") + logging.info(f"Initializing Delay for {duration} seconds...") self.duration = duration self.end_time = None # Will be set in prepare_for_execution @@ -1704,7 +1712,7 @@ def __init__(self, duration): 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...") + logging.info(f" -> Delay starting for {self.duration} seconds...") def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): """ @@ -1720,7 +1728,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): # --- B. Check for completion --- if self.end_time and time.time() >= self.end_time: - print(f"Delay finished after {self.duration} seconds.") + logging.info(f"Delay finished after {self.duration} seconds.") self.is_finished = True return self.is_finished @@ -1756,10 +1764,10 @@ def create_transition_command(self, current_pose, target_pose): # 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)") + logging.info(f" -> Already near start position (error: {pos_error:.1f}mm)") return None - print(f" -> Creating smooth transition to start ({pos_error:.1f}mm away)") + logging.info(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 @@ -1791,7 +1799,7 @@ def get_current_pose_from_position(self, position_in): def prepare_for_execution(self, current_position_in): """Minimal preparation - just check if we need a transition.""" - print(f" -> Preparing {self.description}...") + logging.info(f" -> Preparing {self.description}...") # If there's a specified start pose, prepare transition if self.specified_start_pose: @@ -1803,7 +1811,7 @@ def prepare_for_execution(self, current_position_in): 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") + logging.error(f" -> ERROR: Cannot reach specified start position") self.is_valid = False self.error_state = True self.error_message = "Cannot reach specified start position" @@ -1814,7 +1822,7 @@ def prepare_for_execution(self, current_position_in): # 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)") + logging.info(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.""" @@ -1832,7 +1840,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): ) if is_done: - print(f" -> Transition complete") + logging.info(f" -> Transition complete") self.transition_complete = True return False @@ -1840,7 +1848,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): 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]]}") + logging.info(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) @@ -1860,7 +1868,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): ) if not ik_result.success: - print(f" -> ERROR: Cannot reach first trajectory point") + logging.error(f" -> ERROR: Cannot reach first trajectory point") self.is_finished = True self.error_state = True self.error_message = "Cannot reach trajectory start" @@ -1874,7 +1882,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): # 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!") + logging.warning(f" -> WARNING: First trajectory point {distance:.1f}mm from current!") # Execute main trajectory if self.trajectory_command and self.trajectory_prepared: @@ -1908,7 +1916,7 @@ def __init__(self, trajectory, description="smooth motion"): self.error_state = False self.error_message = "" - print(f"Initializing smooth {description} with {len(trajectory)} points") + logging.info(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""" @@ -1926,7 +1934,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, Position_o Position_out = kwargs.get('Position_out', Position_in) if self.trajectory_index >= len(self.trajectory): - print(f"Smooth {self.description} finished.") + logging.info(f"Smooth {self.description} finished.") self.is_finished = True Speed_out[:] = [0] * 6 Command_out.value = 255 @@ -1949,7 +1957,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, Position_o ) if not ik_result.success: - print(f" -> IK failed at trajectory point {self.trajectory_index}") + logging.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)}" @@ -1969,8 +1977,8 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, Position_o # 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}") + #logging.debug(f" -> WARNING: Joint {i+1} velocity limit exceeded at point {self.trajectory_index}") + #logging.debug(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 @@ -2018,7 +2026,7 @@ def prepare_for_execution(self, current_position_in): self.center = transformed['center'] self.normal_vector = transformed.get('normal_vector') - print(f" -> TRF Circle: center {self.center[:3]} (WRF), normal {self.normal_vector}") + logging.info(f" -> TRF Circle: center {self.center[:3]} (WRF), normal {self.normal_vector}") # Also transform start_pose if specified if self.specified_start_pose: @@ -2039,15 +2047,15 @@ def generate_main_trajectory(self, effective_start_pose): 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)}") + logging.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])) # Convert to numpy array - print(f" Using WRF plane {self.plane} with normal: {normal}") + logging.info(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]}") + logging.info(f" Generating circle from position: {[round(p, 1) for p in effective_start_pose[:3]]}") + logging.info(f" Circle center: {[round(c, 1) for c in self.center]}") # Add geometry validation center_np = np.array(self.center) @@ -2059,8 +2067,8 @@ def generate_main_trajectory(self, effective_start_pose): 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") + logging.warning(f" WARNING: Robot is {distance_to_center:.1f}mm from center, but radius is {self.radius:.1f}mm") + logging.info(f" Circle geometry will be auto-corrected") # Generate circle that starts at effective_start_pose trajectory = motion_gen.generate_circle_3d( @@ -2154,7 +2162,7 @@ def prepare_for_execution(self, current_position_in): self.end_pose = transformed['end_pose'] self.normal_vector = transformed.get('normal_vector') - print(f" -> TRF Parametric Arc: end {self.end_pose[:3]} (WRF)") + logging.info(f" -> TRF Parametric Arc: end {self.end_pose[:3]} (WRF)") # Also transform start_pose if specified if self.specified_start_pose: @@ -2184,7 +2192,7 @@ def generate_main_trajectory(self, effective_start_pose): 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") + logging.warning(f" -> Warning: Points too far apart ({chord_length:.1f}mm) for radius {self.radius}mm") self.radius = chord_length / 2 + 1 # Calculate center position using the normal vector @@ -2226,7 +2234,7 @@ def generate_main_trajectory(self, effective_start_pose): 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") + logging.warning(f" -> Warning: Points too far apart ({d:.1f}mm) for radius {self.radius}mm") self.radius = d / 2 + 1 # Height of arc center from midpoint @@ -2371,7 +2379,7 @@ def prepare_for_execution(self, current_position_in): # Update with transformed values self.waypoints = transformed['waypoints'] - print(f" -> TRF Spline: transformed {len(self.waypoints)} waypoints to WRF") + logging.info(f" -> TRF Spline: transformed {len(self.waypoints)} waypoints to WRF") # Also transform start_pose if specified if self.specified_start_pose: @@ -2395,18 +2403,18 @@ def generate_main_trajectory(self, effective_start_pose): 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)") + logging.info(f" Added start position as first waypoint (distance: {first_wp_error:.1f}mm)") else: # Replace first waypoint with actual start to ensure continuity modified_waypoints = [effective_start_pose] + self.waypoints[1:] - print(f" Replaced first waypoint with actual start position") + logging.info(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") + logging.info(f" Generated spline with {len(trajectory)} points") return trajectory @@ -2433,7 +2441,7 @@ def prepare_for_execution(self, current_position_in): # Update with transformed values self.segment_definitions = transformed['segments'] - print(f" -> TRF Blend: transformed {len(self.segment_definitions)} segments to WRF") + logging.info(f" -> TRF Blend: transformed {len(self.segment_definitions)} segments to WRF") # Also transform start_pose if specified if self.specified_start_pose: @@ -2562,7 +2570,7 @@ def generate_main_trajectory(self, effective_start_pose): 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") + logging.info(f" Blended {len(trajectories)} segments into {len(blended)} points") return blended elif trajectories: return trajectories[0] @@ -2616,7 +2624,7 @@ def parse_start_pose(start_str): try: return list(map(float, start_str.split(','))) except: - print(f"Warning: Invalid start pose format: {start_str}") + logging.warning(f"Warning: Invalid start pose format: {start_str}") return None # Helper function for calculating duration from speed @@ -2652,7 +2660,7 @@ def calculate_duration_from_speed(trajectory_length: float, speed_percentage: fl 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") + logging.info(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) @@ -2678,7 +2686,7 @@ def calculate_duration_from_speed(trajectory_length: float, speed_percentage: fl 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") + logging.info(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) @@ -2702,7 +2710,7 @@ def calculate_duration_from_speed(trajectory_length: float, speed_percentage: fl 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") + logging.info(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) @@ -2737,7 +2745,7 @@ def calculate_duration_from_speed(trajectory_length: float, speed_percentage: fl duration = calculate_duration_from_speed(total_dist, timing_value) - print(f" -> Parsed spline: {num_waypoints} points, frame={frame}, duration={duration:.2f}s") + logging.info(f" -> Parsed spline: {num_waypoints} points, frame={frame}, duration={duration:.2f}s") # Return command object with frame return SmoothSplineCommand(waypoints, duration, frame, start_pose) @@ -2764,7 +2772,7 @@ def calculate_duration_from_speed(trajectory_length: float, speed_percentage: fl 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") + logging.info(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) @@ -2904,7 +2912,7 @@ def calculate_duration_from_speed(trajectory_length: float, speed_percentage: fl 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") + logging.info(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 @@ -2913,23 +2921,21 @@ def calculate_duration_from_speed(trajectory_length: float, speed_percentage: fl 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") + logging.info(f" -> Calculated blend duration from speed: {overall_duration:.2f}s") else: - print(f" -> Using original segment durations (total: {total_original_duration:.2f}s)") + logging.info(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") + logging.info(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() + logging.exception(f"Error parsing smooth motion command: {e}") + logging.error(f"Command parts: {parts}") return None - print(f"Warning: Unknown smooth motion command type: {command_type}") + logging.warning(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: @@ -2965,7 +2971,7 @@ def transform_command_params_to_wrf(command_type: str, params: dict, frame: str, 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") + logging.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': @@ -3174,7 +3180,7 @@ def send_acknowledgment(cmd_id: str, status: str, details: str = "", addr=None): 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}") + logging.error(f"Failed to send ack to {addr}: {e}") # Also broadcast to localhost in case the client is local try: @@ -3223,7 +3229,7 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: if not str(os.getenv("PAROL6_NOAUTOHOME", "0")).lower() in ("1", "true", "yes", "on"): command_queue.append(HomeCommand()) else: - print("PAROL6_NOAUTOHOME is set; skipping auto-home on startup.") + logging.info("PAROL6_NOAUTOHOME is set; skipping auto-home on startup.") # --- State variable for the currently running command --- active_command = None @@ -3252,7 +3258,7 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: if ser is None or not ser.is_open: now = time.time() if now - last_reconnect_attempt > 1.0: - print("Serial port not open. Attempting to reconnect...") + logging.warning("Serial port not open. Attempting to reconnect...") last_reconnect_attempt = now try: # Load port from com_port.txt if not already set @@ -3266,7 +3272,7 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: if com_port_str: ser = serial.Serial(port=com_port_str, baudrate=3000000, timeout=0) if ser.is_open: - print(f"Successfully reconnected to {com_port_str}") + logging.info(f"Successfully reconnected to {com_port_str}") except serial.SerialException: ser = None # Do not block or continue; proceed to UDP handling every tick @@ -3287,7 +3293,7 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: # Handle immediate response commands if command_name == 'STOP': - print("Received STOP command. Halting all motion and clearing queue.") + logging.warning("Received STOP command. Halting all motion and clearing queue.") # Cancel active command if active_command and active_command_id: @@ -3445,7 +3451,7 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: incoming_command_buffer.append((raw_message, addr)) except Exception as e: - print(f"Network receive error: {e}") + logging.error(f"Network receive error: {e}") # ======================================================================= # === PROCESS COMMANDS FROM BUFFER WITH ACKNOWLEDGMENTS === @@ -3457,7 +3463,7 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: # 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]}...") + logging.info(f"Processing command{' (ID: ' + cmd_id + ')' if cmd_id else ''}: {message[:50]}...") parts = message.split('|') command_name = parts[0].upper() @@ -3571,7 +3577,7 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: # Command was not queued if cmd_id: send_acknowledgment(cmd_id, "INVALID", error_details, addr) - print(f"Warning: {error_details}") + logging.warning(f"Warning: {error_details}") # ======================================================================= # === MAIN EXECUTION LOGIC WITH ACKNOWLEDGMENTS === @@ -3599,8 +3605,8 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: 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.") + logging.warning(f"E-STOP TRIGGERED! Active command '{cancelled_command_info}' cancelled.") + logging.warning("Release E-Stop and press 'e' to re-enable.") e_stop_active = True Command_out.value = 102 @@ -3615,7 +3621,7 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: elif e_stop_active: # Waiting for re-enable if keyboard and keyboard.is_pressed('e'): - print("Re-enabling robot...") + logging.info("Re-enabling robot...") Command_out.value = 101 e_stop_active = False else: @@ -3649,7 +3655,7 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: try: new_command.prepare_for_execution(current_position_in=Position_in) except Exception as e: - print(f"Command preparation failed: {e}") + logging.error(f"Command preparation failed: {e}") if hasattr(new_command, 'is_valid'): new_command.is_valid = False if hasattr(new_command, 'error_message'): @@ -3711,8 +3717,7 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: active_command_id = None except Exception as e: - # Command execution error - print(f"Command execution error: {e}") + logging.error(f"Command execution error: {e}") if active_command_id: send_acknowledgment(active_command_id, "FAILED", f"Execution error: {str(e)}") @@ -3743,7 +3748,7 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: pass except serial.SerialException as e: - print(f"Serial communication error: {e}") + logging.error(f"Serial communication error: {e}") # Send failure acknowledgments for active command if active_command_id: From a5f3d3f5da9b632e93b22fe7e4743acabb9afa2f Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Mon, 1 Sep 2025 18:35:26 -0600 Subject: [PATCH 11/16] simulate position feedback so testing without hardware is possible --- headless_commander.py | 72 +++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 70 insertions(+), 2 deletions(-) diff --git a/headless_commander.py b/headless_commander.py index 04aa242..e4f9e3d 100644 --- a/headless_commander.py +++ b/headless_commander.py @@ -65,6 +65,9 @@ root.addHandler(console) logger = logging.getLogger("parol6.server") +# Enable optional fake-serial simulation for hardware-free tests +FAKE_SERIAL = str(os.getenv("PAROL6_FAKE_SERIAL", "0")).lower() in ("1", "true", "yes", "on") + # ========================= # Runtime flags and globals # ========================= @@ -743,6 +746,68 @@ def quintic_scaling(s: float) -> float: """ return 6 * (s**5) - 15 * (s**4) + 10 * (s**3) + +def simulate_robot_step(dt: float) -> None: + """ + Simulate firmware feedback at 100 Hz when no serial is present. + Updates Position_in/Speed_in based on Command_out and Speed_out/Position_out. + """ + # Mark robot as homed and ensure E-Stop released bit + for i in range(6): + Homed_in[i] = 1 + if len(InOut_in) > 4: + InOut_in[4] = 1 # 1 = not pressed; main loop treats 0 as E-Stop + + # Integrate motion according to current command type + if Command_out.value == 123: + # Jog/speed control: integrate Speed_out (steps/sec) over dt + for i in range(6): + v = int(Speed_out[i]) + max_v = int(PAROL6_ROBOT.Joint_max_speed[i]) + if v > max_v: + v = max_v + elif v < -max_v: + v = -max_v + new_pos = int(Position_in[i] + v * dt) + # Clamp to limits + jmin, jmax = PAROL6_ROBOT.Joint_limits_steps[i] + if new_pos < jmin: + new_pos = jmin + v = 0 + elif new_pos > jmax: + new_pos = jmax + v = 0 + Speed_in[i] = v + Position_in[i] = new_pos + + elif Command_out.value == 156: + # Position control: move toward Position_out with capped delta per tick + for i in range(6): + err = int(Position_out[i] - Position_in[i]) + if err == 0: + Speed_in[i] = 0 + continue + max_step = int(PAROL6_ROBOT.Joint_max_speed[i] * dt) + if max_step < 1: + max_step = 1 + step = max(-max_step, min(max_step, err)) + new_pos = Position_in[i] + step + # Clamp to limits + jmin, jmax = PAROL6_ROBOT.Joint_limits_steps[i] + if new_pos < jmin: + new_pos = jmin + step = 0 + elif new_pos > jmax: + new_pos = jmax + step = 0 + Position_in[i] = int(new_pos) + Speed_in[i] = int(step / dt) if dt > 0 else 0 + + else: + # Idle/other: hold position + for i in range(6): + Speed_in[i] = 0 + ######################################################################### # Robot Commands Start Here ######################################################################### @@ -3744,8 +3809,11 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: 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) else: - # Serial not available; skip IO this cycle - pass + # Serial not available; optionally simulate plant + if FAKE_SERIAL: + simulate_robot_step(INTERVAL_S) + else: + pass except serial.SerialException as e: logging.error(f"Serial communication error: {e}") From 7e500a0dd3251aff86165be4d7e8f1d4db5bc428 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Tue, 2 Sep 2025 08:57:57 -0400 Subject: [PATCH 12/16] add minimum one tick jog and anti-backlog --- headless_commander.py | 28 +++++++++++++++++++++++----- 1 file changed, 23 insertions(+), 5 deletions(-) diff --git a/headless_commander.py b/headless_commander.py index e4f9e3d..1ddcdc4 100644 --- a/headless_commander.py +++ b/headless_commander.py @@ -959,7 +959,10 @@ def prepare_for_execution(self, current_position_in): speed_steps_per_sec = int(np.interp(abs(self.speed_percentage), [0, 100], [0, max_joint_speed])) self.speed_out = speed_steps_per_sec * self.direction - self.command_len = int(self.duration / INTERVAL_S) if self.duration else float('inf') + if self.duration: + self.command_len = max(1, int(self.duration / INTERVAL_S)) + else: + self.command_len = float('inf') logging.info(" -> Jog command is ready.") @@ -1044,7 +1047,7 @@ def __init__(self, joints, speed_percentages, duration): self.joints = joints self.speed_percentages = speed_percentages self.duration = duration - self.command_len = int(self.duration / INTERVAL_S) + self.command_len = max(1, int(self.duration / INTERVAL_S)) # --- This will be calculated in the prepare step --- self.speeds_out = [0] * 6 @@ -3305,7 +3308,7 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: # 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 +COMMAND_COOLDOWN_S = 0.01 # 10ms # Set interval timer = Timer(interval=INTERVAL_S, warnings=False, precise=True) @@ -3512,7 +3515,17 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: send_acknowledgment(cmd_id, "FAILED", "No port specified", addr) else: - # Queue command for processing + # Queue command for processing (coalesce jog-type commands to avoid backlog) + cmd_upper = parts[0].upper() + if cmd_upper in {'JOG', 'CARTJOG', 'MULTIJOG'}: + filtered = [] + for m, a in incoming_command_buffer: + _, m2 = parse_command_with_id(m) + c2 = m2.split('|', 1)[0].upper() + if c2 not in {'JOG', 'CARTJOG', 'MULTIJOG'}: + filtered.append((m, a)) + incoming_command_buffer.clear() + incoming_command_buffer.extend(filtered) incoming_command_buffer.append((raw_message, addr)) except Exception as e: @@ -3632,7 +3645,12 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: send_acknowledgment(cmd_id, "INVALID", "Command failed validation", addr) else: - # Add to queue + # Add to queue (purge existing jogs first to avoid backlog) + if isinstance(command_obj, (JogCommand, CartesianJogCommand, MultiJogCommand)): + filtered = deque(c for c in command_queue if not isinstance(c, (JogCommand, CartesianJogCommand, MultiJogCommand))) + command_queue.clear() + command_queue.extend(filtered) + command_queue.append(command_obj) if cmd_id: command_id_map[command_obj] = (cmd_id, addr) From f565ba0e91a74c044f66e4c53875e8e624631f85 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Wed, 3 Sep 2025 19:12:36 -0400 Subject: [PATCH 13/16] added streaming command to support command streaming for jogging --- headless_commander.py | 195 +++++++++++++++++++++++++++++++++++------- 1 file changed, 166 insertions(+), 29 deletions(-) diff --git a/headless_commander.py b/headless_commander.py index 1ddcdc4..39268a7 100644 --- a/headless_commander.py +++ b/headless_commander.py @@ -25,6 +25,7 @@ import re import logging import struct +from enum import Enum try: import keyboard # type: ignore except Exception: @@ -49,7 +50,7 @@ prev_time = 0 # Jogging speed scale to slightly increase jog caps while respecting absolute limits -JOG_SPEED_SCALE = 1.2 +JOG_SPEED_SCALE = 2 # Configure unified logging (PAROL_LOG_LEVEL) and simple console formatter PAROL_LOG_LEVEL = os.getenv("PAROL_LOG_LEVEL", "WARNING").upper() @@ -68,6 +69,20 @@ # Enable optional fake-serial simulation for hardware-free tests FAKE_SERIAL = str(os.getenv("PAROL6_FAKE_SERIAL", "0")).lower() in ("1", "true", "yes", "on") +# Streaming toggle: STREAM|ON enables zero-queue latest-wins for JOG/CARTJOG; STREAM|OFF disables +stream_mode = False + +class JogFrame(str, Enum): + WRF = "WRF" + TRF = "TRF" + +# Jacobian damping +JACOBIAN_DAMPING_LAMBDA = float(os.getenv("PAROL6_JAC_LAMBDA", "1e-6")) + +# Simplified streaming state - just track when to stop current streaming motion +streaming_timeout_time = 0.0 +streaming_active = False + # ========================= # Runtime flags and globals # ========================= @@ -808,6 +823,64 @@ def simulate_robot_step(dt: float) -> None: for i in range(6): Speed_in[i] = 0 +# ========================= +# Jog helpers +# ========================= +def compute_twist(frame: str, axis: str, speed_percent: float, current_T: SE3): + """ + Compute tool twist (vx,vy,vz,wx,wy,wx) in BASE/WRF frame. + Linear units m/s, angular units rad/s. + """ + # Map speed% to linear/rotational speeds using existing caps + linear_speed_ms = float(np.interp(speed_percent, [0, 100], [PAROL6_ROBOT.Cartesian_linear_velocity_min_JOG, PAROL6_ROBOT.Cartesian_linear_velocity_max_JOG])) + angular_speed_rads = np.deg2rad(float(np.interp(speed_percent, [0, 100], [PAROL6_ROBOT.Cartesian_angular_velocity_min, PAROL6_ROBOT.Cartesian_angular_velocity_max]))) + + lin_axis, rot_axis = AXIS_MAP.get(axis, ([0,0,0],[0,0,0])) + + v_lin = np.array(lin_axis, dtype=float) * linear_speed_ms # m/s + v_rot = np.array(rot_axis, dtype=float) * angular_speed_rads # rad/s + + if frame.upper() == JogFrame.TRF.value: + # Transform tool-frame twist directions to base + R = current_T.R + v_lin = R @ v_lin + v_rot = R @ v_rot + + return v_lin, v_rot + + +def jacobian_qdot(robot: DHRobot, q: np.ndarray, v_lin: np.ndarray, v_rot: np.ndarray, lambda_: float) -> np.ndarray: + """ + Damped least-squares inverse: qdot = J^T (J J^T + λ^2 I)^{-1} v + v is 6x1 [vx vy vz wx wy wz]^T with units [m/s, rad/s] + """ + J = robot.jacob0(q) # 6x6 + v = np.hstack([v_lin, v_rot]).reshape(6, ) + JJt = J @ J.T + damp = (lambda_ ** 2) * np.eye(6) + qdot = J.T @ np.linalg.solve(JJt + damp, v) + return qdot + + +def apply_streaming_velocities(qdot: np.ndarray, Speed_out: list) -> None: + """ + Simple velocity scaling: convert qdot to steps/s and scale proportionally if any exceed limits. + """ + # Convert rad/s to steps/s + speeds_steps = np.array([PAROL6_ROBOT.SPEED_RAD2STEP(qdot[i], i) for i in range(6)]) + + # Find max ratio of speed to limit across all joints + max_ratios = np.array([abs(speeds_steps[i]) / PAROL6_ROBOT.Joint_max_speed[i] for i in range(6)]) + max_ratio = np.max(max_ratios) + + # Scale all velocities proportionally if any exceed limits + if max_ratio > 1.0: + speeds_steps = speeds_steps / max_ratio + + # Convert to int and apply + for i in range(6): + Speed_out[i] = int(speeds_steps[i]) + ######################################################################### # Robot Commands Start Here ######################################################################### @@ -1199,31 +1272,10 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): # 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: - logging.warning("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) + # --- C. Solve IK / Qdot and Calculate Velocities --- + v_lin, v_rot = compute_twist(self.frame, [k for k in AXIS_MAP.keys() if AXIS_MAP[k] == self.axis_vectors][0] if self.axis_vectors in AXIS_MAP.values() else 'X+', self.speed_percentage, T_current) + qdot = jacobian_qdot(PAROL6_ROBOT.robot, q_current, v_lin, v_rot, JACOBIAN_DAMPING_LAMBDA) + apply_streaming_velocities(qdot, Speed_out) return False # Command is still running @@ -3514,6 +3566,83 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: if cmd_id: send_acknowledgment(cmd_id, "FAILED", "No port specified", addr) + elif command_name == 'STREAM' and len(parts) >= 2: + arg = parts[1].strip().upper() + if arg == 'ON': + stream_mode = True + if cmd_id: + send_acknowledgment(cmd_id, "COMPLETED", "Stream mode ON", addr) + elif arg == 'OFF': + stream_mode = False + streaming_active = False + streaming_timeout_time = 0.0 + if cmd_id: + send_acknowledgment(cmd_id, "COMPLETED", "Stream mode OFF", addr) + else: + if cmd_id: + send_acknowledgment(cmd_id, "FAILED", "Expected ON or OFF", addr) + + elif command_name == 'JOG' and stream_mode and len(parts) == 5: + # Direct streaming: compute velocities immediately for joint jog + try: + joint_index = int(parts[1]) + speed_percent = float(parts[2]) + timeout_s = float(parts[3]) if parts[3].upper() != 'NONE' else 0.2 + + # Immediate joint velocity computation + direction = 1 if 0 <= joint_index <= 5 else -1 + j = joint_index if direction == 1 else joint_index - 6 + if 0 <= j < 6: + max_cap = int(PAROL6_ROBOT.Joint_max_jog_speed[j] * JOG_SPEED_SCALE) + abs_max = int(PAROL6_ROBOT.Joint_max_speed[j]) + max_joint_speed = min(abs_max, max_cap) + speed_steps_per_sec = int(np.interp(speed_percent, [0, 100], [0, max_joint_speed])) * direction + + Speed_out[:] = [0] * 6 + Speed_out[j] = speed_steps_per_sec + Command_out.value = 123 + streaming_timeout_time = time.time() + timeout_s + streaming_active = True + + if cmd_id: + send_acknowledgment(cmd_id, "EXECUTING", "", addr) + else: + if cmd_id: + send_acknowledgment(cmd_id, "FAILED", "Invalid joint index", addr) + except Exception as e: + logging.error(e) + if cmd_id: + send_acknowledgment(cmd_id, "FAILED", "Malformed JOG (stream)", addr) + + elif command_name == 'CARTJOG' and stream_mode and len(parts) == 5: + # Direct streaming: compute Jacobian velocities immediately + try: + frame = parts[1].upper() + axis = parts[2] + speed_percent = float(parts[3]) + timeout_s = float(parts[4]) if parts[4].upper() != 'NONE' else 0.2 + + # Immediate Jacobian computation + 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 isinstance(T_current, SE3) and axis in AXIS_MAP: + v_lin, v_rot = compute_twist(frame, axis, speed_percent, T_current) + qdot = jacobian_qdot(PAROL6_ROBOT.robot, q_current, v_lin, v_rot, JACOBIAN_DAMPING_LAMBDA) + apply_streaming_velocities(qdot, Speed_out) + Command_out.value = 123 + streaming_timeout_time = time.time() + timeout_s + streaming_active = True + + if cmd_id: + send_acknowledgment(cmd_id, "EXECUTING", "", addr) + else: + if cmd_id: + send_acknowledgment(cmd_id, "FAILED", "Invalid frame/axis or no pose", addr) + except Exception as e: + logging.error(e) + if cmd_id: + send_acknowledgment(cmd_id, "FAILED", "Malformed CARTJOG (stream)", addr) + else: # Queue command for processing (coalesce jog-type commands to avoid backlog) cmd_upper = parts[0].upper() @@ -3813,9 +3942,17 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: active_command_id = None else: - # No active command - idle - Command_out.value = 255 - Speed_out[:] = [0] * 6 + # No active command - check streaming timeout, else idle + if streaming_active and time.time() > streaming_timeout_time: + # Streaming timeout: stop motion + streaming_active = False + Speed_out[:] = [0] * 6 + Speed_in[:] = [0] * 6 # Also zero feedback speeds for test consistency + Command_out.value = 255 + elif not streaming_active: + # No streaming, no command: idle + Command_out.value = 255 + Speed_out[:] = [0] * 6 Position_out[:] = Position_in[:] # --- Communication with Robot --- From 2adfff3d0d5d7cc26dfc2b15a2fe38bad8c52fdb Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Sun, 7 Sep 2025 11:36:51 -0400 Subject: [PATCH 14/16] remove .pyc files --- commands/__pycache__/__init__.cpython-311.pyc | Bin 1835 -> 0 bytes .../__pycache__/basic_commands.cpython-311.pyc | Bin 15105 -> 0 bytes .../cartesian_commands.cpython-311.pyc | Bin 23328 -> 0 bytes .../gripper_commands.cpython-311.pyc | Bin 6919 -> 0 bytes commands/__pycache__/ik_helpers.cpython-311.pyc | Bin 10992 -> 0 bytes .../__pycache__/joint_commands.cpython-311.pyc | Bin 11007 -> 0 bytes .../__pycache__/smooth_commands.cpython-311.pyc | Bin 60844 -> 0 bytes .../utility_commands.cpython-311.pyc | Bin 3010 -> 0 bytes gcode/__pycache__/__init__.cpython-311.pyc | Bin 1109 -> 0 bytes gcode/__pycache__/commands.cpython-311.pyc | Bin 25901 -> 0 bytes gcode/__pycache__/coordinates.cpython-311.pyc | Bin 13686 -> 0 bytes gcode/__pycache__/interpreter.cpython-311.pyc | Bin 14739 -> 0 bytes gcode/__pycache__/parser.cpython-311.pyc | Bin 12198 -> 0 bytes gcode/__pycache__/state.cpython-311.pyc | Bin 14224 -> 0 bytes gcode/__pycache__/utils.cpython-311.pyc | Bin 12222 -> 0 bytes 15 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 commands/__pycache__/__init__.cpython-311.pyc delete mode 100644 commands/__pycache__/basic_commands.cpython-311.pyc delete mode 100644 commands/__pycache__/cartesian_commands.cpython-311.pyc delete mode 100644 commands/__pycache__/gripper_commands.cpython-311.pyc delete mode 100644 commands/__pycache__/ik_helpers.cpython-311.pyc delete mode 100644 commands/__pycache__/joint_commands.cpython-311.pyc delete mode 100644 commands/__pycache__/smooth_commands.cpython-311.pyc delete mode 100644 commands/__pycache__/utility_commands.cpython-311.pyc delete mode 100644 gcode/__pycache__/__init__.cpython-311.pyc delete mode 100644 gcode/__pycache__/commands.cpython-311.pyc delete mode 100644 gcode/__pycache__/coordinates.cpython-311.pyc delete mode 100644 gcode/__pycache__/interpreter.cpython-311.pyc delete mode 100644 gcode/__pycache__/parser.cpython-311.pyc delete mode 100644 gcode/__pycache__/state.cpython-311.pyc delete mode 100644 gcode/__pycache__/utils.cpython-311.pyc diff --git a/commands/__pycache__/__init__.cpython-311.pyc b/commands/__pycache__/__init__.cpython-311.pyc deleted file mode 100644 index 59de7fac96b5f76d0ff3ad6014b6260d7dce51ac..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1835 zcmchXzi%Wp6vt8^a@u#?@qKkChRrt-1T^-U1QoxM#k>~5@_nx1h=jV1i@Z9sm_rITi z*YLbQ;G=vh#o+m`Zt$ZQd&rCZxWau=K^0L&RZ&AVv4+;fI$9TXR2L1@@WH0aH$;E} z(L_zrLaj`z@wVuoj_9JU=%HSwt?^r;kNVkooo|XQw3UtPd|M3AAR9OMj@U)JVh`)I{rH{mkvx?q9LvNg$vIWdHZg=HIwYKjQNA#YIMJHwY@y0lMEQzPrc@Eb zWTJyVeOIAT-L{HwpA){Iwwp*L2xreICdr&r+g>D>ilq6Mw!?_yZZU|Dz?`L+i(52edurFA}3 zEOiB1DXj;TldJMOu!qL&Q@isJILd2{Q=*8_t_Jv0o!OoHMAH)?r8%ESsB^K_?&RWU ziaeo_k*aiRH}i=xQxRV#Jjlf(6^&_PmKoXYr9OqQrSpC+A5+f0DYe4yN$Tpm^jjF- z=QN2+!y?uf#B3g-TCN$k(uqB673o+D(=$XGPV7v zY@C(J%h}t`6R5{(8g^rQ^Snn^GVIUx7KU+Vu2wSa*miZ+3M=)-C4l|AtBvt5-gD>C zd-2uy-PvR9iZCPmf~eU8s-GH}&ZbI2XN4ayPW3GNUEC{+q^K-Rb=WxE%ig diff --git a/commands/__pycache__/basic_commands.cpython-311.pyc b/commands/__pycache__/basic_commands.cpython-311.pyc deleted file mode 100644 index 4ebfe523b4ea17a7eff76e94fdf242efd86c1a0c..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 15105 zcmc&*du$uWnO{;Q#m7pdELjruTuZVg+7d0VN)e{`et^`cM4OABPrsnuW+a z6iZ#ASQ~3k*e=<~ul-UP`E^`!uw|?x;aqfGa@i<5#o4Zx>y2>kk8Bk63H+(gB@gSo z})6C__E&74Wibu&q!xru)&Zk-7JS&R)~NK)UIv~)CYeh1%p_8=g?MsKA0i{pM@2^D5(xpK9K(}o ziYaD^<7ZR+A`=${CY}^IJ_!??Pc33yfxEHHC1YHeG(0^Y7nqwIFT_(xW?A3_Cdvq6 zROFb&Xly>7$2`P8|+%3 za{2i5$%6Cz^x)L=6Ko?c!~T?-Mv`$c9!(t94ark<3i*nWU~by#Af5k zxG>ML1t+0cp$dL@F%ny3BeB#nlD*&;(eV?EmT;P@UPH7mXv z<+&@Tf&5TFxT0_1D>JzHp=)*9hnH>_+>rzh^` zcPsLZ%&J3i*Kh2^Pw&RAcKmGFXeGZ*kKL7T`rh!#H3xF;gNpm0tiMm85h~`yPtpHf+k-~(5cLtnKQX?oW9N5S=5O#&qc}v^3TqwM;g#mGlKHyz8peE$6qv6OH|A^Y z*Q3`q|FZrq{aVg6v}+VEp;o0vt?3eK;r`PeHt;79oddp!3|Qlj+KD4VV=)EiEyaVZN%6=`mJ*z(@>*mu zCCpZ3o7n0!k82|JFig);<~)~Vu{5$)Mu^WLH}C4}?F|7}0;vY9hE2Vi9AKWXP>07u z!sF>FRKGGfGCeYWDspmSD)Q3!>4~$$Lnoj75wf;$=7W&=X`h+NaJRVF zvdBdQky{EM0Z|rB2uH(4;lntBJrMjC{JTTRJ9^>GMA!N0l{IwEf40nZZPYxt;>uKmfVE zl;V+0xV^$5ACFv`r!nwDP;j!`%<>$MoUGsk>9@=k%A(2Jd;m*Sl2nf8Q#_<55+N5~ zg-O22lmH}5B|+X4+_()`jCsy4aS#KESn!=t0?a}ER5guB40?@*g=iVi5g#!brRsn1u}t@V?Xy91fJbsr=9n8);vH^<%>lY`Mm zHOfc?e&Nm0GYUN;(=&OdeRWi!oAY!)rrQ*{Eem;_YBcY!R{Xo~RjvE?%Kp6@%?H*T zZ+r9A^=r>@U2WsE(z2%dePUZrql)x!Da4O&2i>+_qFc^hqvV%E#w;CnGXwTkQ_jk$uF04zx zMa#|K$ps=xAR-4M`S#8=hZ105_DXfTOt)(kkb~XpzHZsq{Y{2fR?%q;?_4f0r39wr zz?9JivnwNH&%OKVnkTah|NW5OKq|G$?pF1LhFqw0BtgrLL<-(WWHH4qCooP&A~%+! z2`#5Q5@A!ZNQAG3rQ~}sAZG&iG1&?{ZVeLPz7vos-!cAe+RL2oO$r0I$JxJ089{Bg zli8%U24rxgxfrx$JVfb$FT#jQ6m=IU={8W+9Yj@kLg*r9MtQ*tMuE01rOcP=>nS*N z$o0w5Biscfz!DdW&w`c0F7r_$8-VGsao{$FmOditDn07i2}X#{a^h_wUx+#yhmY7i z7h4dRfR5e0p1yanxdgN|V8|7*AHP1~VW z-!t1j^Jk1(ZJQ>euh?eXO_ovO&fW5`m8G-A&6GyU8(Ll(ts;Y#X|#$AO)QO8kzxI? ze)vMZO&(%DApIbs38{%365vUE{a6|!8m=622XK_%!T=d4{~`ne9#C8oRpb%|EL9iDVi`3tybrIa zzw*p=EmEJ%(J6&a$>hyf?A)Z>p6Yyki=}}CHju#g_>>m8Hm^nM6FGWOp%-QH<|~4m zloMO94ne>c1bndt61fHql1ROfqeX=lW%7P%XbeR)f!?ut5@uQ3h*oi)>BVU`=j*{( z2AjF5OQ%8u=2zw$!FUEEIv;GXe6|wXx@ki-wO1=vD~MQ%8H^MtZZPUKSf_U=5t8&L zYp`fN1%FxFR>lJ6i_vDxe#@>IwII>7m@{d&QAs=NNZJU%f}(*uP}7LB4$W)>Q@Ge( z64-O`i*-qG5`c|3^cGnA3b3*3h8S40@#SW-STxuVWimuX)C07~T-!P~$T9k{LV(tC zFalt##j-{78(C~|hG_Ol(5soXiUzZU@fj?3`_`@il~>AE9LTD#8_d~E@w7vwxbXd? z%|=<~ZZK?fU2{Ly8Ll{=QQHECl+;qPZ2Lwn(ZI3*B!u-YB7s$%_W9R@0~< z!D?Ce0qkz*%d(V2oRe}HDT%s%-V z^o)ixdG54Z`Y@dWy#!Eu$5*#%rivIU*1c{(EF~w*7=g8I*3MSX*;)HHp&46)=V;FU z5pa%A4DMlV7gikC+{h(>3l*u=M`~VczR5frbB-9+%fq?dy4sauUooHEX&nLVLSvaN zG~b@9)>>=BO2WHI$uVqHjE1FM z1bKk^OHp128$j+FT2wepbfhQ9I*Fd~R=Jz^nmMMwi9ZB19oAaXZZ8-g$yzu22Txy3|0ZYoaI4p13H& zp$1hUYlp(BN^}%s$1p&Z2pbR!G(p;*k4#M*pO`LqhK5faI6pl+c^+_%8G%2J<#rGa zHKLM{XnF!b99ZZ86fcwmsF8$`s>&e(Ttw|E_|xS)5AU*f-w zU3f>vr-!Ftp(E!D&e=pND)NJn9`dRP|-)hpMCcsF=;pC9;_z_ zA|I$-bCD&H-HkxSyDwzC>%KPG*OnDOSb?AX$k~5umM>mfAGsutT>7v*o4han<4SJN zP_AuQX&aW|y%v?}*2gV-m6m}A{mQpSm6oy9iM+op8(H`F%l>|ZowONUv&(0$;=rzv z;gN9IK3)7UGH}FEGU6!aLfkUqaw>peU7$wscgX&313X*E1rkaiAqNuX;*Z;Qe>n3& z{O4u3FeAC%GfMB7N9W|p^N&u;uUz=urCH_DP5IT=fIhcur?Jz^wh`=g%zoAm1G#KF zi(jwU#__9j0!u&$@%BlKLkJYu2zK8SfAZa2@I|t1j~@TUqTG1({ev0ty_KIzxz-nQ zjRQ*KfZRCnyK^tguUyHUyQ-YKO7<{%-^elP%efCI?gO&^s%C_ta>Kzw29L_?)3<;_ zTD@-qs`G}>6NoPk)Fc;CnUSB8$B>oO3YIqnH<%El3||eYL?y%G5I1^XATjXqIaXZB z7c6z1THP?D8Yl_)pwt$9Y3#{BL)3PJT1#2o?^aP#mdpqGbi+Jc!mOo^-u|w1xfoDq zhvD@iwqi19it}b&#ke+>ZcDrRSoVLzc0Hzc)p1v{UTE2%Js4FmK18$LFT?rp&xtj~ zac2#qq?iUPu+{Hv@y@Xxlb;I|Jk`&|R)F`?FhtPXvwb-jonZ(NW5*aP;tJq@G__LV zZ*@sf)9i0${h&w%zIefm84X_;KJgOKYgF8b0R)s^h8r6IZo=&Y{x*|HrIwi3?O1{f zPml9(QH7GK(R;%ZgjILVhlv)Wy2OUDOs#<=T?b98SPaMiMco+cL#QMGn95>bMAk81 z*uq5X;_IQ*lh+_P8LCv>Bn0*$iUSevFF+oV&pgp6j^pPD24fhYg`=WC9T@vI1R=Ky z%yeM_>goJm4EABr4MD*tz!iW93`s!j$0Y3q1O_dVm%!#yoe8=z`$#^5;=-G-zwS^Q zDE3qj2}slzi>{K5+J;qMzIIo(Luosp)E->*K~doCu619F>}w&O2PQkDbo8q%I^S|| z&G(0VMb+JlnZ4^39dboSK2Y;^Wp>wkpj{5M-}9lU99{1kmAgjOoVN8qhaBj**Z9Dp z93EQ_jme?0e9QA|z72l^Ao!k1TS*H)Pb-Ij?_E}2nNtqU%kk^Vp@sFX1-WaXq`_a( z%Hdb!3s;m2^UC44e0@PVoLCPf)pwN_~Nm+w&TgZ8E6K))7!37)R1gvappgK#ilNkINk1jKw;%+W+mRN2z5m$*5 zPPwb#*Dq0>P@!V!Zwb8otou!>zhn_FUV-CWJ&s^y#W5wCgqt-&;`X!LD5~4zFX1|g zIgB}*067|D;5YG{h=wEs?rbxA2&{k$ztd9E{_K)M)7QBFRJO%%g~xXyU!9Al|FWtsoll6B%-v_=zm?xCpR`cpX4{Kx!Mno5<*?+d(G) zju;eX?Vu@?YXFS_6A_P-(FD1<7?6&W!;C`7aOQ05D8u6~ zl(37UkEWA|W}%aKqP74CkPi120Y6B9+eQeFOQ*EJQQ(f=O(H+Q5y}*+d*1JOFEGBvEe+ z5u|`23XHUq>Egn3VYrdnw~tKwOkYn3U?hXqSX(C;;fgAa7^#AJ6AOTgyWe?_+p?9fF;6xtf4L~6*OEj8-OYI9TL6-Qm z1l84a8jABMo``x>@T&L$Uj7J`t13_uGgBC#4q5QQ?L|%n8w3K<6uhPsvVDz`s zx&2eh{wb1TTpyXw(Q$>2%XB=C&Y(|ym8&;@@U6SwdcR$6*`I6Z1-pIS*DL#ajT$pK zI;PMunU0Yf8~!@dV&-u6#*bdiyl9j=m!qc?dP=6JNV(s8sk(+Aa&L2>n(YLw&F$$T z!RqivumyCJ;OdzTe=u_yE`wF+sQw0$u&H_XTPIGW|LZ^hR?YL#VyEg0pNf-aR1%) z<#!!G^Rz{Chdpj=DY0dV@e0L3d1}wv-tB}+{x5ZcA%8{5^Yn9A!avOkCK~oAu}+B` zgBD911RyznMw~eI6_8TtGPCPuduJKQ8@LjpS*%ba3CxZt zi#F<40Q==&i91wjalI&^oOq`1{=I0PnXW&Y(m(>!Tl5B)L8wCNv;ggRu|Q=Zgq5Mi zrfP>_Y0oyWy3QAL<_Z@liJakwp!1@6$TRRMbw7(sk0i;J2hA!Z9(ab2LKQ&E5q*|g zNzKyEOu%zR5qK_mI1n@(T={|P`A}J9eaKZ+eoPqVBz~U5fLM_e7+ZosHyfuRUa-YW zL2TmTzYKZX7>Q^}NW1kh>Wp$*MD~BNg$}|s4Mg^6&V5XAACuk3KCK5k@q>N2`mjtUI00v@rmHq$8Abiun$Lb&%ve$_M$Ba~aOJX(N2eRF}U zO5my-xN2o{z|q?GAf6}pe*0nH!`Q>FN9TSwc}1B_$cwj>$=mXnl-u)~vgfs2TUu#L z12mW4cl2Q}x9<$xJebIg=IAb&?lK7Re9nDHaUYWPcPr+Grx10#r=J75@YR@`8S#O* z!J#z#>^ZX?@C>d{qOHT}x9F>E8EA6q*(_t7tW(2$KszpS48(lQ769w8o=4!2{|y?o z$yJ{=;bOoeRSV(YK|?jA6U-0+3-XwApa!w7_=|v=8QLYFafVK9DP3OVLmFLPw9sXH zof@_Ja!srEY{N6ZF_nZ+(-F~tX5o96L4*|Zc?k-0^*OkT9Vh6T%8Rc-Ey*8d&I9(fgjy|_3SoW* zQcpsgxW+1vM0Hh#+pN%9!O2IH@ZW5ZRhsZOWUe~r?82;V05^UaGgt_=;hhM;NKK`@ z(~Pq1gas6SL0IN9Q8wUectyD>>rfbzjz}&>9n8`F3f(W${a+V?0=8D$nCbiBg|{zk zQh|!yBv`WnJoWamY@bq%7nZB{%GG=GZ9Vs*AB5LNH|iSSem#3bsSDj3T(9eq>w50* z`%RNFcukJZD1$MX<&?qM^#il=f!W8w*1vA~QH$Jl;o+`_(O>O`i2CM&7nR^eIe0PO z-J7XVf}wn9@4d2n(T~eB!E7J?d$%dy)0=5jf_w8!N7j*bYy_L%YrR*c1oz#Gt_Sza z!Tt9~e{)1RF(b!Vu;Xwkge~Ar(v~D@ib%(=R6&X zr{fO#3@dls@oqYf*x+9F*5ETt`@EQuw%xMXfL7awpON&>&ADRP4N_tI@G}b2KPTbl JmXMJD{{U{}m>&QD diff --git a/commands/__pycache__/cartesian_commands.cpython-311.pyc b/commands/__pycache__/cartesian_commands.cpython-311.pyc deleted file mode 100644 index 7e37ed06ce510899070ca40dc2ee0bfdc6f53314..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 23328 zcmd6PdrTZ>wrBMV`UTCSF%8Y5cp2kB+t`j1V>>oB24e`uhB!8{({8%T(75Tw)r}vW zCgWVqtVm>O$gHnIX2=eA;L?3CqL?En7_vt^|EFk zzWNnByu)zJ7{i%3bJR3uqQB-b3;nf@S?RBB%m%-fXu)*hSfNRYYaerPR?Zf6PP@ij z)9x|%bkSImi7_+0X{uN)i1&P8VwjKMU%AFgxPq}#Gt9~ zZ5DZZA|AcY2P0R5HzLW&V2BINBqQLFj7Nis*>Ns%J(7sTW3uPkY$TS9goBB2C>n`P z$oB5@{UgB_yN4%U^Zx6;|C##!k+9M)imP_hS1i20!|)6Y7Bgn%OfXo5ob~%A7%bcO z85pVp=n%VHuC)%DknM=n|8TN!b+fT}tYbVH4_}3X)jna9ads*`!6qkpR)~+sldTDs zk8vIRl`DKW3EdTm<+_EXJr|`jx zj`MsB%Dc&PN;SUN10C}lrL%r_By3Zjs9OlG3;w_Q6$}zgcvG4QYQgkPo1#f|aX|R8 zMT?|+;LK^$PZ-XUHiK(@&t{zBCMIo8+H=vbn*`R7r@mfVG|onID|pY6pBq;=!lbdj z1!`n{?rC$C>dinLY$v^vrRoy{{!^MiXV1kqmISFCYMQhK-YEG%pR^U;u=nVd%w@n# zxmDi};Nc_n1xwW8r@$A|7pZ}q1K!jZ#?Uhw+0cUF0Z0LiO^$>B`GFp3bESIebiwJ| z53>U1P$b1C==!`4&s+#-0i8J7GN6+?yZ)6KgNb`n4O)7fpjvcTHC=445Sr%MRAD>Y zeG_K*?=68qAhn%kJ6>SVbr1IUbdUC*9%6gD`v*_-bU{UE(QF9gwx-Ojfgu5>(`X6` zf+X2hh-)Pq5vYm4hqzmTHjiwNB!bjicEJ-Yq)1|t=j4KDd}4wZWNRb_Gz&*kF1!&E zVn{Mis}TSBp0Gz%e~q$^1-NzCv(7lf4+2C5jE;AJ~EtE@8Fnv6gW zc39{^`efsHSYT}hvNgd+uL$K>o)z;KD&bM^;d{YVsK7)bfm0IukY#_*AMd*OQUaFx z#ZdHmNVwR;C$1*rGZ%-2_!QKO?TtkF#Kqz6v!@3;vfaQ79f|mrBxJ>3JPDlu5fT@( z;{y*WH4my@ftg#fJs5<3Nd|+d8vQ_Ja}21VI7btogL9kNurpP4^VT~>89zI3C3fGM zz4(W&w_Kw4^g|msaIbL6FSwF@nAnF!`|!HU^OrY8S3TTo?y?^ay)`8I2UmK)QEtio zB5}Vcx?jw=%jSDG3YcwvD8#u|)4V*oG=v3Jt@-N~lcaw4gHlqzSMu*8{(ZCp8Fn}R z)@FQAp*uxuzM3V+yRIeIGIwuU^6f>xbzcn)f~vaTa*GwuN%rT7{drNnzk;SQ1`3B1 zhf=t|fI)c&S0F~ASE3N!H2Ri+9b<$^>Zs6^y0b8wRhTg!xrv!)53G)XqS|4d6mRDQ*A6&lHSBCN{)LMN@`Hr+b*{;TA7##mO zr=cH^ywEG{$d8%V7MCHNHXES-kUG(ev*flZW9qb%mhFCuJe*I=!$tEbO4h37!Tix8 z8+@zP80jKoJ*KqTlg?vl=Zc{v+HB%nSLyl`3ev^tLRzjTW=j{Qi9hUxZY1ra5y`7c;l5rGY6oPNlcacl;yFhs=j& z*-iP5{lT-RkDVTsogGISQ}LCUs_j8E1mB} zN!_y#LJv!&x`U+dpcX1LKFAqZs?!0n|D_7AWxE1Vw)sPWk- z2O?4uMJ|w|;?Y@@ny6fhQXH)pI|4k|45SVG26BaK<4D_f0rye?nQZ|lt$-v-z+Fhd z?MJo&4?4@s=GcsE3kgE#mh9kTv(qS*@d7qTwo&O2m`{)%dD#i#Yz*bOFfU*if`2p~ zO3DQw-SWZ=WgF!NtqX%7>+&H1xK-%0Nb**2IuZ+>I^Fk;Q9?Iql)79lq1eQ1G%p+Z z8^c34WoQ4;=!vtyxCck%LXMx-%xKi14 z4H^wa^0Eb5B^QqjpE%JIgc-C4=aPV1GqBy`3E6Wh4iSRWP+`h*w^ay8$z>Oy2Oy-x zW5B-3_LK2xV8`LfF+v&h_-s;k9aoTvAiM}jmu1JbV0cy#fMb^(quQe<+kXm6Kb?@v z)UHr`pjipo&GFG>DBIO?sq&`AgGS4aBs7PL400ie2`D?rP9@q5;2g3m2@F1;4AK`1 zF!ZwfnmS|=J|T2q1xsi%VdC>aMcS8bh-L5zB>b?FHeYmY$!`7HNf{)rY$q62*KWuy zaT%u8Z6@RL5SM?0F*$t8`|cg451I1Hg?+y*VB95l4!n8r&cVgzrH)lstLSP)#(m(e z0nyhfx%UwF9?`ugQ{Ob-L)Y zdB_xId8aDS9^99uc_$g*UBzc`MSJyn<@S%OrOLhY?zNh_rBh<-fnS`GT6?9MK2p;s+N+?t z6_s~SE{>Ag!2N5awo~%%A>KU?E=t~RXnC3U?v6!}ZrbmkA^u%b*=|y{Tin|tmG#UQ zu2uN%o_p{3Qr~i)RMSrI4m@y36Pdr0U)C1M>qA zX=h>fKB+vFe@{8w!Vq{t@j!DHaQXHGNth&#aa|BbGmtX>Aw1&A(fB`4_{ht_IQ7U}$y^ZFY}|HhcNN{WJFtiF=Mo zO~<7AMCl4#N14;^5sRJZTik3P+iZqQCP#D=Mdk{NDs5eWA zQu7pMdNN{)m@&nf=%$DmY6w70J{-9c3Fk-~i-c4~OLIUo8%ZL{f`Cv6cqG_5Ius-t zo?!2cM=5@zq#L5cB3r@tRv0-X^}?+Rs3Y^$3OJjXDUARn@M1bB5*R}!m=n?vOvLe`SK0<|INRvOF~B$$42||u1~FA-z~$6& z!5$V6O>A>1JPdL(vKu%xMEWT^CYQkOhNx3qxJ5k@I`#(o4`3<^W0hz;NER8Bi$bv9 zM^&Iq_UQ6S-kRat3g~VwOhJ(}3*jn0MbViCM+b;0)1C#-luxa?y1A(+6+)v4Oa~jA z0{EyH6>LuwLdvj>6e@bGVpazpSVivv<(BMS#NH*^yMR_!Q~_S($RbJ_z*M^vQ&(r| z1Nd7b(wi=sur>*xcpMx$f%ZVZzQZKpRlSzAsiXSfFUnOp)Cf>;^X4g$)G%j|{;Xp!j(^a|FTs%{RJ;*^H62}J+KS+i2(e+6HZ6zKIvwV@WJs=xl1D>F)oCMK!L zz;ukX5|i6=={8H3fYt5Dg->b7!qhg^cUi-aR3H2WQe*?<&M*>lg~`rboaElzbGk6s zAI5qc-vH}4HfQI`^X9THb^3{VmV8Wd4)h^Y8cMXRp+dP@K`R_w>6J~m>Yd{Y))=oJ zkp;^e|IUFkUqg@HK0!&=h`|RSCQB1JpEUG`;0v|=bm8ZiuC=@ zIgBxSm<7|NwmG{V*Gjwf{h4;@-w>%X<^cFvG0a#>;e>7JqOJQhAHOm&cMcm`tYKsN z9tM?n5#X}=m5Xozz4Wy*#9*@#7=4Ul6tN@X2-lJWiP{$=gOZnvgvYTE+&{!cb-YUNGm{{ zd@L5RGOeW!&Ifc7`B!>TI_c#K4fHA39_QweIj^CO8r|ZoX|Fz)JgQVN0Swt(<=nP! zULBYI_aCx0v~@9)-nR9+Fw`C49(oz5WQDPRjqA|}!Wmfv?(Upq45%E2#kJ7#;?GsOKA*9jtADe3Xf0D1LH3Z!KlJ0qHTD{Gf z!_+P{#WI2=o33PY&dZq6FxncImaa6$`iJNLO|t=3nYQme1t(DjYqa_&R?eSKZ%j<= ze>E}5@mw358DZC4l_AePxzOAbSCg*#3G?2joz=)GU0}G;LH74D7Soe0VAjm)+E}n>gI37@r0MZPV4km^K+j)P|v5ELNcKx!wo z4jC5snUH`sD7utrrl!h|n&|tdQkE#F4Q^NhQcQzJL_ivoI-u4?m1SpWr2znr3&C8A zw$O*Xiy0zK27O4=v~vHFTM0cXeSeTNAAH#T@S4=zO`5y4PzBpTHtsj@&oD=Sc$Dfz zzCf=pP+%31laVa|dCQiYw^E?#v7&{TY>NvVF9=8rL4Q&ZiAJU)$yCobnC*b!3eTcC z1MJWu;RJgn1okQ%3$|xkqgWigK<`jCpZ=lV(_QSbcyf|eZD|zaqpV(oy}`pE&j|7B z5e_WW*q5mJn;JD>7(x`lslMwxJY@A>%NRr7c^lzMifZyLyOQ4 z=@mVWLseja9!$W^Nj6Oh0r>r+4*bWE;a4bd!NB_?%Kgh`0KLz=cl>=fVfR0{_OL-> z4-xi|RCAcr9M)nf696USf1=(JkkM>Y`bJTCSU`F5N9~=TK?#X{O7FDa66r0y^osb> zIr01}ACLU3hIIBlvOGE?b)F`jr=^Zz(lM;W0+)7Bszo(BqY9v16vE2dP-eA(fF`o3 z@&NlHkRc#H2u6Z3>N+17O+B}@VcHILqOzdSQ79Y7UJ>Hch;8y*7h1yYxstLZ+5)Me zC#|cTic-r2tU(pEU&nYT0Shcg=>qHtL5K@oY^t5+r>vJS*n#rGby$>uvQot)tW6zB z41DqEpLlu*X&gaZm!R%aP`^KrQvP4aOL!d}6!U=GfaTz*254#ng34ZjPs~P>(2ro6 z8;et&Zz7;KGmIz}iGfsYUM^X>&9P{t3ZOH z0@aXGIwCxe0f^~SMW}+x_Tej{tO`Vm;E+Ojy5UJS3!q$1#^ce1?95F8**1kQ!byx` ziSjWaf=>mZnHexoRAANt%;*q$tEyH|AJaK045DvQRsjpXun@FY(8t27a0;V1&!K{w z&NeV|ye5F=7ZjOPD-LBTYF;bu;Ib{n3-JUfLxs=`pNfM~X;d~}<76vp`DIHgG9#Nt zAx#3oVcBuwW|#+CZ?NsQ!R`|m+Nu@hZ+TXYi`rC83G6{TMzC5%&miid$FOcLFdJ7@ z`Eqei35zjgmkM%2*FYe2Rd9lj;Zlza3VE_)W~lk6oh6s*%Rui0K;xQXSAoW5PbeA< zQr!bJ08PlnV3Iuv=ITKx0hX)mx=!`kG>Ys9(HB}B(iecrJ>}QDmjA%dexVmO(hr*S78WGQ5m4>2ZC<-1@(=-9MI!3Bvpp4yC z1*+$^g;`F{WU0$M}F-VUj_~GC28BMWZSEsg*XzL z1a&5f2x1~B&R!Q2*Gc3CXl%b{>Y?gi^BMdZH=RYDE&hD1y;Yty&$}LPJGAEZ5qI6P z>wa)$1hkfwr%Ycm=zdMzc^7C0O8msrxWQN)<(e&PNOF%8_qgaD&(t)`_uNtJSl6qX zmulyW!T9wKDA!M!97h&MmoD8uvsxPvYXcciIq@`$p51FD+um{g$R*Z{OC@1a5*ACs zYt^-j+|u;NQ&QdYQuP5+ePDiIt+syYJgMFNdF}qy+Wil{_o!GrAC_u4Qp?Q`J+7#J zZ|~ykq<)v=-%b3xrHW2c(Ye8xi;hrd!IYug!l!NG=r|eWME(jHosdpVl2enwAxQoR z@kbWynG!E4Y5BaQeYK?h{^Uce*l}1Yd4ZI?AkzCw8|G(cipxk*qf`{wFh5sRnc3e( z_Ma5@k1ksYyGMk3?BYVvCfV?!DN^*4dM)W(L97xW5#qE--b-XIA5C1pdYvk18Zy}SQ~Ua zY~~g(S+X-d}3(ocO+U3$89a%V%X?ihoKU)8Oa?-t8C#1?=QrY{cVYPBVtQ^SrYJckfvHShA z_g?;ZzCH7k%e5zPg_l{kZ7;@q3dWCmy^Y z?K(zw9h0^nC)Q15$XN7rS;yc;=U6rA+qnJcxp_1hDw!fBQzE^8_qeebf3&r*M19;0aJ*71R}Fe=^`3- z1WjvD%~#)g6-i@Jm=uM@qHw0Raenm9rFr;!451g_x+qp(`bE=2j=V4;b&Zm)QFvEw zspwTw^s1=d2^My#|I*Og`jVgdZ_d6F`1#E5JmmV*@B^(+iB);hY^nG@1mObf{rekq3~tav5dM z?7@KxNu82%SqEyWsQ~sw9x;2R^He%hT3G%N2BfV`d*#cI3Is?y140{tDWTo|H5XiC$ zm(g<{IBgEavH_JO!Vo&A(HTbP6Le4;Cj3iqWK%GKqoML*S(yioeo{60te3*`DMqrt zgBS@LOt0I_x~me-)nxn)KkfQ)*K!P~zW*5UADbTpUd3IqwS^z>B*azozGbAk2Nz56@6FTS zW(&ApT}ObOvy$E2r25#K1M?>rLYcblPnUtaM0D4JRpD}}7%8-QMRTxJnj1G&=-=MRVZ559J=*+@@0+Zg>{x-}Ow5>wNUS!*~!5E!7 zr_H_L3z=*|FcUNU^jfyrsy3J>I0`a@Wn%rbOQjZH?EtSlv#SCy{lFutR%&!rNR6-* zqbs{&bcMFPSyVBEcHQ5}?rMvpsv!_*g-~Q-GRdOJEX{873D$Nz{W=rnlN5KIR7|r_ zoQ7k>{7pE+s-b%UC(WWi$=-+ySK;{StuE@(8DIgq?Vu<30H;9s7P5hb3a45XVM}NH zwI=|F1U1vHY+Ml9_$?0Cq8A(vj8fECu>so~U`OC|F%|-#4+Sj;bf6^wBYY@42??PH zFfon*AWa2(M2`7bViple5bl)IhhTi2wJZx;wINh2fsn|5RRV*s$(#&NRDi#7rOmhN z(w5sbhEtr{2BF5n-z`psfw5QG{6=%y@6C>P=mE65xg8PdANW^x%v)U^FZb zAPXdmdkVKnxE1S0jjuLJG`F3u)=4+=ht6{<#O9-#O}9>fZQg-4-3_9&?Wc8UVHnPfKJ7CFK z*9nbJk%LE{;)$AUjj^V-EXt$V5r+O4Gl2E2T;9yF=ZTClc2RTjOL zz3P+xSKo^{OFr<`=3&lv3+pbR$+?)zn*HCCN-*~EoXueIpxF%STL|`6Ua;}fMjBd# z#FAbHUu{urvbQSu2bH2}zYV1z6i=^Br4$-UsZryl3-j7+hx4DR1t*-dhSQxOB7!ZT z6>Jafx)591u7x-a_IHkS-UnIcTxpkPF}2e$hcvr6wLbXE<=G5@6~s1JubS-~eKXEe z7lgBF9)=ROo~4yAhFW~i2bLmLTg+26L->6HU8pwp9@G?d{QlI*MT>v7Dams-w>g;38{lLfx_*nyd(Xz z4eFh=#Gtng3G_of@X;F$02oa*8HxiXpq7PNY!@rM0bM4W5~+X^Sve)K#Ro`e4K)=_ zpsnFs^nf_ZisJ78KK%!DkXsVIj}Gdx6a`@~KH=seJC%+D^;kkt5ne^#0+7u@(Fxfa ziwo1TDJd7iF+~p4Yl@Pu7Uv=TsMA*GM&)k`29@3sEc_@s$Iw|o2Z@XVkRVZSb1U2} zVkmNv%KToV>NY#*;=+@B_^Pa)xx{+mIak@Mo~2ZMXoB*rZcTqeQ}$*()f7}$Df_na zRg`>aW}=?lbkmn?Qn_4HqjN!Zh!eO?(TSRM6o&k7z*o_Ped|Krjf>U`W2e)Eop_M^ z&)PGU(Un2fXpIr`<%M;>cluTX|g@l-#6@^(cd3^)EZDu--+KvJ9F#d#0XHe;Z zKVNHaHD}C})7Nx-MNgmh zPG>eIxnCjfS48(K8BZnEsF{kcm=M-3{!y`5&po$F()igHWti^P3VbYFa2$1X*F8vAh!4|g6W9Y?Nt=C8_Ql zsXI4+I@8ojn)ZF(bZE8d(8E!3_(iE{h%^m}ZWaa#lvnwRqnp2FnAc2w*gdbActufk zg(`|BsG?}XEV}Da>-F7FltP(nt!?*G;+|fqwU4y+iSA}-Zjb3@lXzil_4t^0d~9)- z*nUK+J|e<>IW#|z8M-1yrdC0p1-E~fxaXAQKPAFFb8UVQTd;lk>Vrd4<5ALh6e_mv zL2Z<$RrIuK8i)&0$rvda6HCURFHkwbX4qy;k+5E~3pJZsUynRY{(MfX8qsdaJxbi8 zqI*=E9w)T94$~cOMJ&`YxAfZt6k&fImdu|PVZZIuW9+#@R3E_N{s{h+3)mziy!3jy z_&MYm*=slsgYC&)fMqjNT6>Wl1n33!=iFEN!2Db%&cFczpdTN>zjD#zESliCtg-ZJ zIQ~Kj8pM)zop$Fw z$&+|hZ$`bW2|4);AOn_=Ii z&2WG=LlcdtHp4xo_3K;_=gjNPyicd(_ohL=v2}?qeTg7eShAX*?S?jMf>f@>FvHyG zLaqoH_TtS9JA7;;RWCcvQXNW2%MuokQoxw&BXE2V?AK?ok-)ct3$p&W2Jtl(n=warW%#vt2oEPq9@Eu-!2u@H>O&iCM4`Lp++w zBl#R|n}Nd}rpWNt+$}wktFYrZg$;d;tDW}sd3gAfTCY-j%qN*9f9Fy7XE+L-P-l=< zsyXHUrFW%trTe|T@1;q-@{N}IgQWhTT|!b78mQemH;#iyL%9YWjr=5{6MM{(W%gipw3 zQ*E9=t?!W2Z7Y5Xy&im!BO00kb4-c>3;z+)$#$?K19lU>Kd4!+Wm#6#R>(!FNgHLA zg@1ubJgOfWp`o2Dequ(?nyP3qLh6dHP|uv|b%i*;5}$$d4Z1?A5ZvlpAZWdGM07Q- z)io|f-o3hX6_|k2k1PFmZ@vB6-Pe{!?**mGeekUdw`aknAXs)3fs?tO3xc+5)VF)N*5mjyhgVwY9u@M%0sL~>6N_mt?K%6O`Y zrv*P+0X$}D#z}s!@Z^LXyohCYEUR&)3V|3a(ZGE84)VWto9X57@d4i*qiUq>QdcyD{ z7vdK;L422O@ZF=CTLN8)xMCHvc~|zEwfMKAjFi~Z$(gntD`Mv#+@&%3;gujP)&^1gV z04|vQnM|PSHCaK`3yza9d2ScuUxq0Y)jPwuZqvUEQ!c7^hVhHWJHyn7>YZUOh)=%P z9o6%d#8JKYyyR#mj^^71Fv!Jax1Af-%O+FB2D7#MCH4PWAKGKNU^aoed29De>i@Mq X^t$z^*#u*{wfiOY|N04`bin^V7faU5 diff --git a/commands/__pycache__/gripper_commands.cpython-311.pyc b/commands/__pycache__/gripper_commands.cpython-311.pyc deleted file mode 100644 index 87a90474ec0cbad11ff4c29961d8e0ce7541fe65..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6919 zcmd5=T}&HUmag*uAF+wuK){5eF%Uy=Lz3=J=qwBg`5_@dfEki(RyD>|fP*QMsv<6l$YYXoE!2^0??-& zGVmF&a+;5UXN-~3!g7W`&TI(jr8LhSXN0UUu$alFR-wU!%*a{D>f=ggG8W3E4Iw3` zvx1Z$+$LluEy%nmK%a+EZ7cSuCi;7tM0MG=JVeuq1H zExQ)CX^!kVFKEtGCM)q`Bc{PidY7=k8z^_0VFpH+pNvgTjE~JvOiwWv$0jB(j1y~F zZ@ifMX?eu}Jk0d|K=)3?Lz*WovG)J~r}-hblory`3eRcIO!hu6Y5~Ktvhu@qUUTWA z)tp2{S`Zta?1s#yvKv@=S}VuPV6VVLu^hh$l_}n83(84xnU`79hyjyz9t62l8(`!F zSqor$W_1q`5v-hLC0^Ei>se7|(*nmo&|F{*mqD157sad?cWVwXnM>jkteaLyG><7l z?7(#luJZ%TbwG;wNLYh-uHgA0b$KPmFJFY`$bI!L!oFLm2gm|2lnf2es_zz&0BbItWrk}wch5xyLW)0H^i}+Q$ls?1!%u$pe5n$QpmV&PmUuLD4&Dk$wucQ`YVn7sIgy-^l& zqtbA+*k5TFKn()~J9_sd3gTDQJE`6AFaNo8vC=(^x`&H&k$ypMjw^w7wJTBTtaP13 zT_>L>pC`8)H^-k$Z%*$-J0Wvz2h{LrkD|RC{W5U)my@4Nq9coF{)RHo;tYAqfs_(R zA@kj{waTjN8a;VUk~61X^*?)8i#(co93MR%rbVrmW$_H zb5yM(lH9e1U9#!z59m8pXYS55gLZ9I_YM@*R*&ImkG};rFHl;?0&pU}1Dwb~XsyEo zXFA&xz^QsL0Gd12?4N~ct;1p+5^Cq03%r${s*(+dU9gG2t{nw;$Z)!?P-{7V&UDls zE(F@Q#td$_v9?^t$#mbx>d;#bz7GB2edyhJCl|(J=OVQduv4y+5;5>z$Wt)O)&YSt zXW6O&Dl5;Kvs{nuV9hq|!v^cuz_xF8_u*^Q@fB$C&w${NIcmsT2NTGL%=(laczf`; z;p;ZO%|s7Xscz1l_i$nCne-<>ac&4U$Zy&6$R~H1EfjUPPa(FZY^lAkSZn zR7=%d{qdS>so%?h@(4yJ_g5*vHh{zV#yYrjt*hiPEPC>8uC#^;ZzG5K$8wAvZh0WMZ|I!TLdN-v<&tueg+rm{ zoe-wsV2HE9y2iAidGxllkQ=K%w=p;XNaM(%js%#H$iiaDtS80f8XSs6X`US1$=N^m z9yzmLn4O)T9X2^sFAYdEk6EP!BNq9APKem_x-#Z*u}e33u7J=Q3C4^n5@HyLckxg}Y{W$DxpGaI z$j~DLu@i;>HzzqibIgbhhM8O>ew@iJk|VhuWbho+wh@6fwD`_sZ9M}TPvkD|Z(PY{ z$Oz2{f#EiIEUS()#(jT<7Z@CL0hs>-PB@6eoZ>B-9fBY+h6mq`6FB2gA~s`U6en;A zN9m!=yEupagXTyJaP>t(8z+QS8_QxRZaa#TYJ?LYG0F$*?DVNl{Eb)a(%GYW5X~r0f}95Zkc`dvZ~V9};_T-alhpGeOLvc`j$+ZVBhS zb2wZh`^0}$$f24VkEI`~bR3KZA7&!IXyzM3KL%_z?A;-kwHh z=ga`X$|=f2`cC2zT9SZNvFoKTO(zPR)G z9TXqlo-QAqRE|z=POHuB&vKvTihWxv<>n8S<_|Y7Lx2@xP_TFF4C+7oGSu`e@L2%0 z|Fj%RDxo9_CDkqx)-Y;wTS4CO2Ni!Cjv-o#r@s6rl2OCWg;P780P;j3ZY$DdPp6($ zap#WWP{kiZ{+Jb}-1zR)ABNG98~R%g+*ATLQQ#&H{2=a$5_VS9=dbwtk-uM!4=8aM zQ-OP$EeCoqudAP2MJ*%cz^D=!MS;UOS9Us2;SAPB#5Nwx1wzXLGc$ zL1duz6P4fz6g;7ZA`k~cG+4Y|4)y3+7?~1|ZPAr*9EIaABdyP_es&cx=gN`uO5{9> zoQIefMPfh^=~a9C3*$;8rnYrFKc!;+7P7%gaIhR4+?knIW;nETM_F1&;yp$DR}{Kt z0z=*Jmy+e~(`fk0cSGgjYqgl8?a%KPSGMkcxmtX$G*p7fa(it1E@~SqcwV-|l$Me0 z2j!N_O3USf^JPzbtNZuy-^IV~|F*v*mV1Vko?%nQpU<4#X3J-;DQB*snd_A^*U_2l z#nIA`()B)qci{$fG}O08QAcNOB~JO^ik{U|w6KJhSCoZxZ6)M?WSg|AAJ4voyIkros=sYFhe?v^8`_3WOVYHoW&f7oWR zlR9XC8*W0)y<0J*Z=~EjQVx$Q;n5dEmGC$Uk6ZIKQjUx&kx>*GH799@=`0Ry9V-oe z+h5FZ3W$Fsc?!D}QEN0E5Rp$v|rOAD331vI!|Ay_R(GD;+aA{i3_StkE`LSy|Z!4Bl< z5b;4f?wlf5e0qYzF>(lqJEq`?yYRh`0LKHC)qE_wmgP1wI3HlyyBo=j(c)%VE}LRm zu@93^K=L(3m=migVsA_m>`JAZkUX}2dp3vDvq#~?*X6{w7W>jkn)>h~_0;J-=TB)T zvq$|;sV)@@jJw5a@X_4(Dh~XbIE+hP@NARhZce-cW!=dUd{elH6+$ro;5n<0bws=l zb@*g0$&kR+3Qh0X?6hN#!ikTfgO3m4pGt+1`R;gIH=7l2>+{oPZ%pyV9=rA&S8X(4 QKREp>ssH{BtpwG716A;wxBvhE diff --git a/commands/__pycache__/ik_helpers.cpython-311.pyc b/commands/__pycache__/ik_helpers.cpython-311.pyc deleted file mode 100644 index c456bc21b47b0908484572b01b312ec90ea7c38f..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 10992 zcmbtadu$u`eZS-TDe6r<{5VUtEZUZ0*>Mspj$PN5V)d*U4_N)##G9c_y{ zW*QJIwC5pmX2VMAVqBM|3Q}PDqXk^71-$+h6c|7n#KPTXOu%4t{*PT+py�pWp9} z6lu$8iu}m?-S7GRy}s|?{m|=mF$gdI?@y=h^f1hSkxV(%8Hq1`#xl%ZMqtJnffY((fZHu{P-Q#YWw#PiP-f=HYn__k2 zbtXnMPy5P)iuE63v`_KqVZ1>w@r@?tbw+T!!wAlgO{m3V)HX#J!S#I>vHSaM*o3#I zCg-im)z##DHM#nl9H8ltDNH>F$W=K&uF3&&RSuA=a)2B*2@Ur=$VEbLd#Pt@(VkKws+X^HlZD@+MSI46MBWsqEpx+x`nNHOes6V2%Oj+l9JUBO?^wpS2&OS65J;t5Bd53%Vr)*7xZz|xq(l>Ond9REcTtJP zqDoYhU1L+cBnn)(Qk|5Mom}8%lQAVa7ZbTqVs@7F3B`C>7G*A+khp04iYUn0akOx`B_m=l7NJqXQ>;y`i$lPUAMj@p}n69*(v5+FEO9hA9xJAm!FeyL7r2hzv zRlwL7f}5G4<4WCsWE7m)^G7K|Sp^hevnloJ*@QI9$D%0_q#gkQD+%t~eRI)0++1{m zlTb#Bra;rE)SFzD__?`gJo1EWHmJZ9pVLfpQ9>BBflM}rqsXKA;$`?jS|@d)ZaW_N z+lUsKHJ9&B*XX3?RF;z>cXdjXM6Pe(<%7RirIKmjl>w4MAsvC6 zOe!33Q{*Ks%u70G31r6vtePd100XsBl-A5KF)p>CLL$e5TGxEAJZH^0A2epEx#xpP zy1qG4(AcPE5u)L+Ot63~-N13L-b6=mS=xcxJ$U3hh!&ZWi)q??_l5Ug$TxNu8oL)y zJ@z)<*}s&!b>#MuytljH?JhAc>&rCCuxq~d=^4rLCl zZQi-U-kVtY)=wIL-15n#{N|T3rF1yS+kdM6 z{@Slj{Nv9){GZQLheH(v#@dNoq}+3n5}Z=umkhJcJ5^Sx>hmMER@VW6Fxe^s1VkgK zmRf>KTLn|vRx?Hgu4+|nDa=MN3zm-o?b8axv|Tk5)F#BJ+9??Ue%hf>omU^~FS`{Y z2aTZC)RBI*^Qi7g$Fv!w{F>?z>_*xoIO0y^ot3;R&I<0dQz>iI7%l54k6HZ|Jm}~B zu0^O*o!?~ypK2BAk!w(G$Tfb?Svwn+Q60Qhb%11>_(AY%N^Qps?F$TD;Gtt(Aw1qV zV{VcsIvq%NPD)&h&PJ65LbfvaJrp*H>VZ08oh4*eQ$EPW5+HP`3L4S>L|%@fFq9yA zU_=BW=>U#i*%H$WlKZQKypsBn-cLZC<}99;T&jtcc%G9BUkVDk0ba`4G zERw3;kOZ;loaPGxpRSNi)|s1T<|k#%7E44TqJ*W2lgUWH1%W|uM`PzT*ZOg5&dLdC z_W7VO8XZP!Stp5wU}bumj}!+h+K`}m%CtwOpm`7lL%bqJ64HW9xDPoyJ?NU$rn1c> z2CIAOlb4R8$1yx|7r5*q^V_|Qui@s@()sq;#=U%%1LVoK= zzG-Cf^lxp9!=JNn`9XI!_LE?~=XkzrsL(a^FTQ-&o4N6chg}o7u8Cq-|7!EYt^>KQ z14~zzu70*D_twP2k%`>M#QnkhgSpU5#`DNg_nzm5r^GO=Zd>iPP2D zO8RYf%v8w$lAiuG!#LkG{f5RULAuz{lku&&{W*(Y*AA-$5<`Rq5er|8Q(+~rX|`IJ zJgw~sCWCc5ppKi?bv&?fbFI#(nrc~c+FUL0rYoXIun=*PwrpJMW{i5PLX0XSKzLc6 z6+sY#wQZ~$w6QBhJeT3vRQpqc*Rk9n^R)ETmp@e;}!1UUQQ4rk|@f1s9Gt8!?41Mz|)69D>yWgA>E4s)sf0PkRf32 z?ce*N0hvzs%S8D3Hhs-n9Z?yA7&1sVRdY-q&~;(Wi&PEEBPM3>;NBn(MXRSeelRXZ$|W8l zDl0*v5uik?U(e}ns!I$QK6-k@jtJ_CeSJl135NNQl8`hs0a9UVIA+pNYpNi@`kblH z#9+78On3J>)2$KzSb(wB>oBB?*qaz0`Im^G_-|oaI~LDA-n2Dq`{2~QQ!Cp(Jhyx< zb7hIYxsX{{ZCX6>sJT0PBH!FsXzp7)^Qfui_JQSrTL%|UKWgi^XI;9IWpAZ&mgYxI ztxM^wn)mO|H@#SBdJ*;hj^)>KyI;+BzMA(RD)^M>L81|B$a}rd*1}RgT4e}Z)q-LLn zScL!#!6#V>M8Bmh_piVa-eumkylZ{e2gBUF*m9l8R+?2Oo@J(jNg-|sBM9c|T-cKF zWvm%XhJmls@{Z?*Pqj&A!J4+m>#I%q>o9MhVt;lMbKM8_%Bm)?GyBJ2XP@HH!(|Xo z+L3mqU1_&cwIbC~%jyN&3}x?LV`_us+sh31vab^PwiYTV|{6F8v4GpN%cw{ z)z*mc6lB$dVc_c7cApH^H2V@ypSr@%h1UBJcvGZshr1l*Y5q0g#>U%@~aqyYN3 zuVEq>5WcpJK;evpxwuFyTwYq(1C9@Ll_3Pc(4NaL>MyYD@JC)Z2RHVg_9b7op;%A9*e2c|Q?KMJYJ??Oyw7(>7yU|AKwfxWN}=|Y~T z8hegEWb-!v+ zoj-a?M5bM;Yr5jVl6n-f??ylkF>q;kM)0~-cZ7vlb4|mZFxt4LJ>@i+k!rr)Hbc)< zs;4vklC}!bUyYz_E~n^eRxXrPcKvc0*T)dq>kh)FPTB*tpcS5T4{;g_t`q>m5sA4x zr5r$9r+w*qwNAFt8T!GWC>E?8c>Qx)*h)t?bZKkOf z0u@(XFY7FU>m}j<&MO&Q%OL(>IL1fzY)Gqte(I)4Z8li4ZWHO0C0+}`o)O_!fh%Tz zIA!JzLue7oz&)aAey=o+q-G|*3)9uJ7o)Yzke4rzj9|BOT@NEw_r zP(CG|q`hUSqT41OLoVq#cT|R95K0hnt~FIGm@2o8UEpWQU6*FY*u>(ABoYD`O9(C? zF%0z=`>k0LGr`lE1^2S2(GHPyJ~$f{G?x^OOjWfRrENw@n^Db1E*vm0*+=OTi3SnL z9f$~TeKss2P*;%jHrk|KqTPeIA10bYjZY|8C71dS#S$=L#|?&a`&#SP%b`Gv=bZ?&hl{#^n44`>3TWyZ@sDD?Ry^fkMkbX1Lhg zvGmIF1r#1$-T&i5xuzpI&k@`ZE{!~NZ_c?l7n@p^lFOlc@s-ezV!5WloM-T{xBgDs za@W1>S<^?BY$)HbtI)73@7-PS?#`IjdiG?)E2FE8t8d)D_+a3tZ$1d+drlU5PUaom z8RydR(bt4Kac8d%TgV>EHSNrKcCOX8FL&McW~{{ujEnb&9&G#R>kr2AJtKvl z5dy=d!yvx8)@4`T(^d4;C(-zoj6YY;+&A(h2el5UQeR9 z>LhZUR1-U1^U!Y&UJ8z$`{ua|S|e3egH)7)UBE<>r5-d3)Ke^!-XvX|Wzw0K-Xd@) zl2UXfcem0vNgE3#08?T{GXn;^r9=(Dw;8%|IIq=}72zN}$%G`bN0@nf79JRfB(<3FpK*N{yu`nFIZi7 z_LmsEzITkizH%f0sISDBt-~zfW;_|sWB>luq5A+nRPYaFJjKp!%P(d3th(=C`i%YT zP`>l6Lg!m~OWWe{Ojjnfbl`F4mSyFGgZB=uupb^?KAcgOzIpRn=Guw^SHQNu!nSWb zX#ea)e%r;uwu^bomc_G6CyRC`8SoUFiR~-qrGZ

x0XtWj@>Rf$)Jl@9xe{t{h#N|K#}J&*a?$Im-au@PJ98 z#Mz6^0n1s5OhS4C(Qsg^W)B91LW~NpQjshX6ri8`XpWW+mvG(^?#A6Tcp1t)0M0J z7n#mn?Z3!$=W72Owb_wlYW_tA!x{fIXGg|gaCR)el6P(|IJYm_zdO{wi?$S>oycF&hd*Q~vTV_gBgt!|fjhz5@g^h=0Qw^)VF^&o4vb zEsN~nn> zOVLr5iSSfRZWK;T#u;j=(3G=3!@!3oe_r|b$Nx3`?vY5LGx?=<{k#pbcL|07#E4M? zO#)(8+WamFh*{nxM$NR1E`gjy9-v)vb=@ zz6SFHvrA@(koH;5Y4b)K;G0S;Vv>LyZYe)JhEsUv&1$OnO&c@hCotppje2e%i45*j z8&d}NqVZ0~2ssUQ#soQ=3N>SfoXoelg!icB1QBzpX_!73XH0|MM|=jKzED3C`mtm! ze69A0pe_2ZeZ<8}*%H2Py+*7|O=n8Bs!3uDXq2^PZ5eCEQd}M}TsK^ismWS0HYLUF zO^_s^qn=v>RFt4gG9|P%4JANBTQNq(J-|2neY1YlBtgQO9U<<7Xa~?sgNe)h&Y@#0 zp3!tozsfY2Wi`geocXOW5C}vxE`Zv2ek=%2Wlp$QohA9AIx#JsA@$?=rQwce zg!r8J+#neeDXD~JCOg?MjkAG0l38B393B%zeH-rW8avA|EH@U8&xF~rK8Cx(CsSj? zY!Y)kbu1cZxUu2xGp7dI3u};J+qvWfe=W>1V<(s}4HdXCRZfh_!Xcz)1F0L|+7HH~ z91kU_!|8Sn?*gzuD1;YNC=@sdt2rF!4nsn%-ij&E1ko)*XgaHerK$&=3tfwh*t{1t z@70Q*LoMPNX2P_eqS_Br=eh^``?`bur-rCw-Ti|{`?{!Ln4M&JS&}Jo?-?dx+Ca6X zt0^FopNeu+a+;@-6BG+nVgf;bxnzxUp&3}~wB&%)1dNuu%+QjvfMST_nUv&Smk@FR zCt2dj$w`KlOwq(dQgZQf-ywzel9evV9?3n!0NzplMkvLw5fC?$D;$Y1aV=X`1ew19 zmGHuYNo3jZ4aqDIQ*z4lvoCb!RL`lPR3Rf5ibtoSJQsqAkA@SHnPJ%^>$giLj)_mO zumcfnJ&qc1MB%6rBB``6&{R@xDv?pCWGb4#U?35`juRY~2q1+E4_l5KTGfe6Z++<; zS3yOt8Hj$1$a{UWraP{DEj4RGR&TxzvdFqMPc`AU@2R!)eaCHwP}%#?2ays)Yag=q z3D&+f3sJul*Q?2Up*?`1_Q=ihz8y+&Ljb?){%j#?>h5_JE?I6tJwZr{O=l6rThF1b z=VnV+%d2x&f9d_uyGEF79yyxrUTxTk8jdWJsG&!6*3BNzy_v79ox8ahMh$y}$^#Hq z8c(=(8hNO-mwmAqlEb$8E<$ARM160u4*It9w@Lct?w!2Avj~5CF+DT3;#?Y}qaH%m znQ;Qg%6!jOXB;o4DQM}iioeiSGSKso4r7hqiYZK+Y3HPY53G+Z=2g?hqteyTSfRMa zq+tVBx?a*dDD0d1tWo>a^`mKen_!pnyVVx3dNh46Q<|~pZIx_U#s##Kslr(@WonwO z<`1kxpj{aYU0TrH$~H>5zalfPOc`FoS$D<>X?Gg*t1EJI)~CY~Z;A2)_2d@%ImxDRc#|%m$vzZpm!XFtyR;pjA?8ZQ3$I zz*o0UKCRfAw7q1K5 z*qc-;=%i$Yp1%6^sOKkrO|Xoren79yKcrVZWPJ|S@`pO>sYQjTA8ST0S1LBlNcGET zl&OB%KJju4p$*orRP^7uc_8k2rN<$;Im7G{TA5TSb32j#`{Q>T-MMPtxI z2OGe*nOkum{`&O(UcK*Y-SDJh(;QEYGvEURm)LbCGL8LaP%sHD!a#bbY;X^=Oe)M` z-$qeG>^0QPLG0>aG56|R9oS~aO6K#!5EWbj|7hnk93~t?i=wNS?oq)u`H6v6o)l$FkWTDmBrJQ?$QRS`X#^_28jY$jas_3uZn3`Jdv<{D>V3@ zyL)8ZS0t#~&FVofbPzp^j5x`!H%BR%$`4Pzbx1p3xAcTl*8T%MN?| z8G5(uQDYQuzwG{#os<-dEvj@yfOGZByBo z(oPF#YJyEpDcyBZX=9*sB5maSf%MSJoRPiB={OC?82maAZPB!khdG8WlA{6YD0bfg z-f$xJij)02vO z7CU-bZ16}nCNVvQ`Ijm3*IfgTA zGp=roGYN^j%3_;cvV>D9CPA~5{3SdNj%yH%44YD9W0}T83S&uXfy!x;YrO`rn3jd_ zI9ob0eDr8vNR_aX{TyaNnVG>8eU;@UBQPzCT^2AD#hY!Kn2yIKbDCk3oaBIUr zOtUPw*h9)XPbT2#K`JYp<>5l?+-BQwRYycQ_Y7fLm15e3CYc1Df+VY`5msCuPrO39 z@?tPDKfsxQ;`3K4a#C419uLLv>1l|MPBENRnhNumLwqs>J;07BIc7q54a-#|TUgFu z&$~lODsW49Oj@XIUk3$~o#exD8LU*Q=_^X!jJv%Gw8Af1YX(S3B*S6a7d-i!UW#kK z>V>*cdfN}wnE*a#0WI$qfxY_9+fJcsba^l8JqvFoh}H|pdO@&WcnaS7HbqbRDk_}6 zA`ZpTP#g-x<|zbmO#;;Aov3LvIdvHn%D<~3CF+U&rpui@T#OfT;YlOjdM2>P~Uf$-v@(aOn z6r=@a0tF|R?B2EF-nDpn$t1KN65WT9`>-H~Z_F5; zl_=eUTuq`Yux8lr^5pk*p?xQWeZhGXqB;c#qhmSOYRk5T6GHpz1#Q#axZ2#h@T#&$ zejU0jM6ZdV>nL;`O2l0^5X8-C)SMRF)RU%`91yjB?&?S0dvFj^QNI5h9Ob%KTiXQx zYfFvDe^59wESx@rj*N)@5wSIhT7$Wvyr*@se8sav@a)KYw?FmDe z2b|74ZE9KYFZQ8b-OJs}-14hm!Gy+yD-ch8o&5Qq2qHxe8rBHn4a1N@&Yi-!A#&L8 z4M8-Y#$^!a23D!21>0i#QsdIdlK1z6sPEF(Z^zNwX`%P$B9%c@CfC2(6hKXfmZFc! z#ijw&G>{v3;%S->uXwf#p6!bVmKs-f9uRgOSgi+?qJWAp5)@Adaas;yeGJvd1aIs~ zy?=2)tlytIj?r^GoD|y*t#}RzoQuZXSP%g4pmfjN7=viWi4j+M$Ci(O*nK2*6+sN4sMr#@=FLG*PXUq{aVq{25} z{=t#lk^J_Zxjt0clCP}E9g|BRjQ+ZQrDBgzu_xcygfqF5PaB&Twk~!o#(uj)Y&?n@ zkLF6BP)+kaxtW}~X!=UZf3`+I1k>lK5o&iXcB9&W(9yHpfjW+gwMRwIG2}V+sBy(J zAb1AgB<@q^$Ib_57T)}Fnu0jyqEON7g zo7G@Wi|!fZo)O$Ld8##M`p{K4;lrnVIm=(*S+fB^c>lSSa8kei7YDdacGEKg<)R0eF;lTR|L5tf-92uHO&U^T%3j9Q}{Y|drYXg z_}k_s8XX=HyMm}I2ze!lu1m;uNl*hv!G7@j>SNnS+{C{eIqrtnAKms~v+<8LhlBM- zRBwi~?*0rme6U^n{GY*OzDs1xx2iLic_`TYz-^ihCSVqBu&m#d;PB~_W-xLFe9Z_- zupuKqyms>ieDXvL_bfkpL-LAT&c4NC4XyCZ7x_iiGVq*gr*{~h9!gB>3%oMM%|1W* zE|BIV$iRgun)n-oc60-DP)e{}Nt-4~+Kf*%GRC4(OUAT*d#8Y_hEp-#>riELyCiGP zl>B$$XNc?MIwk3nG@OJ+4D0uoXe)S`Y(sEaLAJ6$`+}<$!L*%HuT%tN#{w%`arXcz z**rVucSuIKwIQpBvRcRD!LrA2gf&=MZ)UMFESY3g5H2Dl!c$BrB$;J<$8S@#KZn*Y zXnocb$ti}$`k!qCn}~QUlA~bh;S9@Th?Xm$LQZn*IL?qE4o{h;Bq_>~Q&F}USr<(E zHt9sgpZ^w`b7q*+Tf`HGM{qRcYa2i9`nYR80sBU65322%9fZ3LPB(JYt`VfoJHK}U z)EG$QD?GWqpw%Ep-2>x%A8L6`q`DB*wc_X!99^r6y~4+tcP}wJI9HB;@126{7jv)7mw&W>wm%ofzxM|7RP*cr za{82>YwA(W?uRC{ryJGuyf-j=A{WlrZ~I{YoNmGC14#4bqO(a!<}19ppUGyH-!wGD zVxDBNAJ9(@`AgXAkdo}7&{UG1j^l}RhC)}T!|}qjmV`odG7<{0*tyNJIJ$)+?EF(4 z0eIE1CN;vhy|^)mZfU6DFLib_~)#j~h#7iQjW8A0T z$^JFGq!N4q2&@mu48~jPhJ0_QU{uL>6QdCh+iBy;>ue9MZv!qLl!IOR@Ol)^cy%{r>y?R&K7oQV(m^WVdlKES_xoGie*Ib((6MJJ08 z&+K<`7Jo5k#jnj@eA>*}U(GyOVlwP7aJjDd?Gwwpb1;E6NkiHAMO$CCoT z`@Iar!oL!$lszlNv!W%>%Gfg(o)s^7=4H=H@XWpBSvh;=!L!mO&nnonGCcDxc~;4u zmE&2(l4m~7bjZ+DId#^0Xnb-qc-FUfGB`Rp;p?3oA0M9J0@mKiiQw?)M8G#ZHs%Y+ z?*#ebbN-RwB!AU+#y{cbhgp;nC5rF#B<~yME)P$P_&Faxd1^A~8=t)FANNlL1J;kJ zf(Dz6@r*Eo z$~W}yc#aKUy)ZdC5j?Esa{t)y1hV-tbqRdAJ}^0U**`RTe(1%~;Mt*JZumlQ6gPsC zV?%*Ur?}C}qXE$s2LbHBH8&AWipl4p%#_$!A!9@nKl!Hqo734L1DBzcZ423+GKYDT!(cJRm5+!$5z_v04t;LEj@j$&lwM>sgv}F&(Te$UD~65y#6PNb zEMV>Uos>RVkZhT>eNy{8U7yrGe?p(se)(K(qJ7XZ+B6EeqF**?^5o)*@095KHtW@P z^lv*01*N4i9bQRl`HM`@3{)LE`0xtVH$G)SCQ;9eTL`0Jzc1swL^tz)`&uG>AFzJ zeZIAzkbBzszXhS?LSEG4%DUPatzO>!-&p%%u(5LaV{ox-XKH%YMSo>y7*i%}eR%vU(G5@I@ zzUY-#X$P!O)Z_J3nfnP;nf`gY9FlXi2p}j0AB+Q88SRq`Yd=D)5d9&t`5~$ zA@ErB^x^BK8)b(K?;GAX#qImM5ANHuZs_2?o_&Yo)H)`RJ%ByA%M2tVnFqAb&rR{T7 za|guI^-}42?Mrg_5t1;(a}Vv^x9{-Iq27Umy?grO*&}E}KhG2M`s1IVT6kh&eoU9~ ztP5bU``^!C4`o0P6E`T#?&5|TR&K;i$4}t@WZZP(WIT5Q9c_4QNbVC;#Xeta7wcam zkP(deee5&7sY0JxIS??&eJ`G`WVicZ?@(|5;Nkv*aWjMlL+FP7kEs<0;~AX)Og!Vt z)hV=o=B0_zU_5J*C$WN46BkX`y_avKV%e43JNx$x9FJSilI&q<_{wM?Zn<;--lU@vLE6B{_5#Uw0{L7a!!B%V1kdEqKg<&7JI@ob1R z&^zMBgYoRK$unnA*38j~)06RxgZod!Z6lKtBS>cg|M3j|!qs@j89yQoU$}s5HI?(k zvgC=r+*ER`!T> zy;5E8!wi}kK4J62Uowbr{&pk6fY$~-a(X3a^*v|fqO&pT5uIx!=bCE+4+`90Yks5o z_12rM*LFY5HkgaTuPo-&3pw>MTVeRJU@OOY-{YM<8rdn8w?>Dh@>O$1V)@!wSyiM_ zDr=5*OJ!}*pjftM>77rqOr>l8(f}~C3u%|uL{5sHl{33ybt`W_JJ%=HZMavvZLxNn zSlc7j_RQ>_HH-GD2eyLnX~E{j`JlKmJQ^O2>3$yOWLD)pGyn+p@}F4^rQTa^p?b~S z_WAW6?vtwfL{Gou>BmPr)wjwbQ=+FG;pVmXnl~>tZx)-oq~^1qNPQ&wEX>5iEFl4uvOx`U+kH!kCaNK zEztu~>B_lGv9u%Rsf<`94>g5{n!>YY=^Zph2{lE@T6Ck5(wk>yPe)El6|3g*=L+Tu zq>7IDwRf|G;z6-^P%0k$BqyU}pK)f#L$jg87ikcSn`U<0uW!6P5p$N@w9NL2P9H*F zv%O)9xT5~BO>}lh&W?v!8SC;sF=RA2{*u67WgDEuuU)==<;InVIfh*KtW~tvJj^rL z3lr^&+AZ;Yku98Pzve|tvuJ6K+5U0W<7~ZP^WltDRL^V=ufM(z6SXeI= z*3Xz@h2EQ+B5tv;3E|q7Z|!?`pIEzAs$GlB?-9*ah*VH?(>>cne`0>ElwUiO9m_Ah zSvGrF%&$YZrs-Q--re$zt+%%#%5Kr@!zq`ya?#Q#SQ=xtVzv5f>jisNtk^sIj8xq8 z5VQ6c2ErzqI1fgQw~jSV6|thyn>%NNukS

!qUg7>Dfz7>Ct`7>Ct`7>7lrZ>)QL!_5sChxwHl)cN)IoGpva zmgw`MbF<{!EU+`yuo*L>O|;j_^Z3fMcr=9-SP>8 z;fz>y-M1RwZT!ZH+bhBj)KyVQ_%d3dezCAoC~SQ=^ELDS@2yxk@V(Z%nPTUjyCcHEV?R76oH+TzapBn)#K9ND>S3vR_(>x|;qME)2@z_C?l&UkSf*zpfG0dHhcK+(oHr<9w~y)U}YYu=5AG zcZa2I`-KBX#BE1}V<&}YMxmXq_^dTn*EUxw)@?#)?bZ*U z#XPch=e>^oiyiyLj)PLi!C6PtEV|p#7u=PRn()ivm(eznrbtsv_k-!FwG`7+YpLLF z!t{iN=D5(d6?xh51H06=SF9V9>IT{8Za*{EDb{U4cvJtqO?wtM?GZN(N}C4pC5PzV z@HY=!Zq`ma7hNrat0h)a`HL;;gOZA0NJ0O4NkOY4BSJ|N&e)2USqH{qBTnoY9LtE~V>h|2PZMeN7+7rDP<)i$a?ene$zfikJtlcBk z?!h3d-)o%R{s2v1EY_``-G0Ay<(=nazWUqdXva4k2z<->u5~VJ{z$|s`g$c_FUHuG z3JkHfN(`~KN(?by{acscymE`sg7RieN98T|ysH`zn+=L>z2}N>u(2O-El@N+l zLMT$nnx%JuBDn}fauJGDRC06u?7B#cRJL-?IA@wONo8y1>lQ8vg?q)qy;9*`ph&Jk z3FmO;A;lI0h*SElh8ba zrlpn3c-06^%gk2|C$qRLz-%rXFo(+lG;?M^3ugfb$QCc@84mbmwLn?LPGM|J?PH7{ zdg>b)8xDXGL>hld{lIZqhcLn0@R6FG?*c!0!O#1^Lcq(QaYjI|o{^b7pXMrxg>uA^ zL;R9GE~uD!YeWsd3pA#bgIZ@l;hf&V(yf9ge-Js=Tph zIchw5GpS5QSE?!*mw_u{Xt0CDgIb$%Wk3~X4j$efH#FJfHqIXy;YXonI5`owqVS6P zWV|35PEdR7-h|9MQ}3BU>s}rn8|C6Qgieo6j0Vnv@+=Bm@Q;k19`$oWNp?ZJkWr8_ zg-+b0F>veHP35aY7kEFINr-P_twC||My`A@4mQ>B89$?d6A=_GPe0cL&Pl*OcA5{M zm;tJ5pHC)R%|qY@j0T5>rhLnEo3_Loe?@Um16(u2%Iaq_C9^wLS}~J(1M?BPtWtn9XsxrxLl~C zs;X~bE@y-?N%=EX&3ZaZ%U%cLHtUW<&$@-eDKVy$b1 zCCM>B96~laRF-txAm4^+`ms_~+oqZrr?Gns33FF{6aHZ&L+m#-hOdzgj!xgF8Ex&S zKg}2)Z<<=cBC*V1CVSj>8>h_UMXc5Bu8^5Yi$Tq3uXxIT$0Fx!YorJm~k$UgD)s-EqOBVjIbt>xn$}@ zL<;3o?aR#$%SOq=ErfpJbR^R$8&GSckD{y263iw$5xNzRsT4_6VM13nu{7 zQ?wtK?8gQB@mOI=xZ@*p-dF9f*#*~$g(d*?6wN0k^GU&c5-U4W55g>2zD>d zSZ?ub!E4XYJg?q)z#e^6Q59(t+PcJQpwMDPpH$H&n9Dx$G{*A1F;^p&{Zd-?OOacE znLOmySs|FqZg+@=#iT~lOB1B{wPgV=QN8d zlXYP_icn`+a&Q@3=AEpR0iUItHr1&rlo`tUJQ7Px9@$gvuxGgJ$H-R3Lvn;;mY}iW zd*({Z)t0Fi{z?mfwS}L0?u!ya@h}K5@WjaF-2e$m6B|H0@rtK*EjLtX=xWFF5DDUF zFteeSPBc_cV@MSZRdkbRNQFPDs*B`Dx*-~>Y?dn4&JEA!ixr!sicQ*=N1l5s9$!0C>powt)=~|A9p92?p`F^gx!<&UZj;!wRchJ_ z_B!enEOkJz)a9_Nhr)m}mX9EPD{H-1wq~(x&D?-k)+Lp731;_$;(DpLG1?&(ubi>P zitFzcuUITzLFAE8yh1GQm5O_3Z1)}cH(s87DY{#z*dRJKN{)^99NQKh+eAl?Ro znFj8h9!tS$uSroq2>`=ZClS1*hWTUKtMrpfY7UH`7Lq9>#p{7~Ee4ogwUk?8JOSKF zOBrP8XaN|<3<1lL4=(4BVFCn}wk#~RYP!L$R4hrC8@2utsnIWIMoF}-WLeru^wlQo z)W*F)%OO++{Y6N>Y^v4jJ9jC7IqrxA;Q{#|wu?f|nq(PRviVhUn+=U&Cutt;@|i zX}{lwRu5PJFxe#VkIj*w)UaM~ZI(~byhSo^5zJe_HnTe=TkS)G1EP!S`g_&=i`D(Y z_7h_DNvZmzV6TkTueev=w^-j7bJj!-OLbj9h#K;M5H;k@^hpr5m<`S%$x;8%kds>$ zL(ti9&$(sMxkczYAUY39&Vz#UV63DJJQ1k``T|+`W%t2O-t)#Dq3VcObW|!jDij?B z(G~8yxi{9(9PX2fYGY0N>9-z?=kWHweo((c{#3T1ZiSA9yPqiEh1tW9915-r@+q1x zO6H3~;#@kl(GfNyld2U|bJzJRg{ z7XJi|i^u*%R~~`kB6TMk778ugHnsXtGTt~1qfsKUNw%-1F*FX7Ny2`WNH8TV9Gnae zBwqA${lo@oGRX^Ik^l5*KNMnMgP0`ab=0*xk-OBCr&cZ3v6hQ5Kz)mpw$Zops)XvJ zV%{++@0egdCfBi+kR(Gi(36pWK>hB3ve-{dk_j#PXzYz+Ja zOOT33CQ_&0)TLrN=PACA1>l^@48l?gi{w`gAx!j~SxeNXV%c9eyq#imNefd%Q{H6G zLOJ?HRJ*JMld5l2x9W1E&DD~{G`YwSim4puR4Xyg0*aw3aeryOq?SdWJ6K+7^1)e? z8HCkjjV4u#_8Dhm)}R*7o_t@umz#P|ZyBde^;|*X3n2?<=5n-cz~ym{J5GIEMv+*9 z4T)G_?ldOD=Lj=rN5~p7bNO7xor3o;3O-QB2U%^NQ!OzAy}BM#l8CMS0RpY(45-jof_z8Xbl!FQaWPDneX%{Pb<%9hPU~-7Fx9?)b)pHS<2W9i0pB;065FK-?tK$ab;VO7YL`Em9Nw_pN~aVQYp4H0I^uVjR=h5x zNqUinBgS5n{s%J4KRN>W<+H-`L*kJaq$4lrpDR~vd6YSa=_`Of@pn^9eKJqrb5{bb z)kzLOHB$~%;g9N?A_t?-YQjZ1(mD5B!^io$U2_1PRI(K)fCb* z1U_Dkdr-8@9vvGUhpJ`HIp`mPikMotEd3z&=_J;K9tSrLQ{_6^LGzg5N*@zR`0?KY z{22c@indMFu}>W9J~%iqxT6!gxinTZ5m~xE{+ICfloOm;rpK%x@Kp@sZ}Rb1@KmM) zh*03C0OEFSS7QVQzn31qOyKJTNG`(vIsu~M;w7^94C751g1xen4E_?m(o3L^KtF*D z0)qr-El)B~;yHm!BO?&Yq);UBEQo|I`58^ZzeZ7h1t6XSjpGT_bUf$ma9}tX3!flm$IAGkCYjGH+qo{|fY08PO%gGPZei|e;=d#d(n#`Iq5x-+Pq zYla6lhh!^5Pqerr9k-!Bg8*aGDpXK4WY`u@v_op{WuaJcXPA?UTczU8n6qs5c;xC_ z^TP~N2{Z!%V%#s>+aYvH_DeNd`C2%1Y#8pPD$={He+z)Yi{3N_P^aBdp6Uom#m(`_d9 zLsx%kFq}8`Bqpm*Me0l^qsCj$fuB{ra;|i~`@{12pxEBKa8lTR;0MnMhmMK;$3)L@ z$#Y!RX|hJ!qpPECKbp-0 z9YO$*v7Wq{0no5T-t7)L-gUC?%t?Q%f>X@%; zHV`@R=8Iv=Y!&^zZjaSAgl&?u3KVZR_@J?c2-iB0we(wyytqqmUj5q3Z@e5i2c5{O zxq{mh-{n8NB5XS(96l->ISMFkJ0`9_F0DT<)*ly3PDmvuW_HFLC9^fJy*%?WWiIcH zJfYIRa0Ng;MdxYBd0KFuPIhG{dO3)G%)$tLrml>}be4pJpxEbfMMsz9=%RkT+qlpn z^`4Y^Ps>-y%4m}6HcNH8Sd~0*6r&U5HYT1vC@h^Bg#3ca?`4BcUJ0>!U%(h{vc@6% zoqz+Jp`o~SXlQ(ryEH~&`_Rxua7L6TIYXF4Muvv?YltH=VAzEDYjnvbOqvkn1+aob zgrD-03Gf#P1ZaA@rv5!NnX=Gb37G9!D;^s3z+0YG@z9`xE^`*sa7n@ zWgW*I4KxoJv)m62>7l$7QWi(>uO0~!_z_VPDe4zFI`s=P2u33mFtUK=S@?g2*QdIa z8CauOA+I^afCI0nH9^*M8iAv{wv74%ZLCE`Xdy`)MDEh{qyGk=XOkq!_f+y1h-#t~ za9M;aHARM?W+oC((W4-J2$e&dqDMi`RMMOAJ5|y-Sd>T)a%`21P2K~)Gb>~Qft{@* zc~h7?c%JiimY(4=0S$A9Ny0JNOh`c0E!Bu(59H8N>${=Q`xeb%BMLp+7ZC2 z*^9U3^*Kl(THE6!B30IMES{YIii{%=wvbpB2@loq4M{G`zfI3G;KnM!JXBNEGYhZb zy(!-^Sc9rT@pW1eU&5=`4ELef-Ut=k(l)8IjprJbSm^wnSHAa(d<7_)k4xs`LgHk-KfS6BwI@8i5jO@Q`Cxv=GNyJMIzM{B z$5dGotmtHg`oNbPR)p}$elBBYvU!+FgdBzoF;?>m4~;Qnbu>$j*{ zorniQ0c%b<(>*cg&1B0H-iX?_@d}aH)K?!pz-V|1)>ziH;!t*aof7B~rI@KrLz<9e zo(0vB0j^MN_n0`tK{ znl6m_UpW+_R;=r)TD9Q`NA- zayYwQFNe$J@}OCQ`7G(Ik1>MG+PNjkRN$Ooh-nxfsWash7-E)SrCRNVpf4FdM;uso zgv?;IfXpSyYnh6Y}sNY-qNbVk)d z6$6BVkrp>hS-JH66KuZ5Lo!m|lo8JWH%GQvwZP7LHM6(|Iz(fMCmwd(cu|iG(Y!mg z;q%u1J*xd~B*ALmy#aNpfFHTNv&LDfdXM+*JrXp^+vkQw?*_@cL3D4F+#8dxvJk8P zGcOhW=8iv^dUflWHSBn__2a+A@$oHup{Y#6E;@i4{QpVdhXfd+$8bQ#Dk3D3|1JTl zQJLM7#54Qp9-$RU?hpSVUEUxNA}~$hj{v|l9iD)qZpQGbK-{s5cx8|dUXc~|`2U1A znw+wrdY+>E4uRh#@NWqGA%X7^_$q;aOW-hp1%S9c!PLP(56F!JOIH7dA#U{a>3A;t zjCz?F7(Fw=@FpFb2NzmS&(k#Hx2ehg9Np^)oE|q!9AQWLPUl;lk%_tUVp)$=)-$t* zaff{0?2TsLafr1Wq}mOOwhe-9gKELq6{&@_gW&FxPtm?rvTqgaTcM^WQ@7so*)@^; zH`mV$goo+x`kt7tW@fi!FHdlpO3H3dMmVW{lT^HU#s+o(On%BD2X0lscF5BbJwIP{ zXJY=a*w($U>F&iJbO{HKioHig*D=X;3>F7Bcg*gN8l$GD>DIovy7@~&QNLK!FBSEJ z*HH{UiuGr(6DYaKw2KmR!2QPdxn{AkOKR*Q?V@c=yJ#EJE@EL9(=-B{u~6(y_w8=U zUJXaE z;4ry2fWhS6Fk>gpN@zCtBJGQ|TESL(-&H!>{`&Eo#~&JOxd$+r5)2#fdn+UM=z-gg zx#s!d58FiFHqqNHdAq}w`@R;*w|V}6;cBQ@ zR<$N$FZ#9#zHPJZv+WC~!hNxNI7+xZF&F&)$U>{wwO{JmFRtG&*6$DRiR8b&@2`;VSyOZjX5@XKa$$gEuX?(BuR`7ReXQ)pGEGqiE)m zJiGrp47(=dAoO+rJ&nR25cqup{|A9<1Q@F%9hc-^(NhAmG=82z;F|gu%iJks?tGYO z%{qY^TLRsgSq|7#C4jxpnB{tCcpQ|oOJ5tozk1MYM#rNLfUjUJNtm5j3>G+QoH3j> zg4JU?F!Svc zJQAXMB5SoPpoTjFMFx$P2^3W|Rwi)xqT!B&NJC7e&O)RcrgBF>5ooMT;5bQRWddQe zMzajqo?%F>17^oU+;(=btGwhDYB+0f%Br~1XymtinmIn+;``Kue`8aVBc2tw1Opwp ztN9QQ{;Y?&(Mtio1{eHwx?>rE*OButj=~X&gKl1>o8|;}W zKY%Ofk6WQ5!(Sy1p`UN2_=f-#=>+ei8y^!;zsLU>U9vCrQ0S)w{+z&nA@E-T;+CNy zro=Nev_#NwO=|$Nx3%CVQ`z zjxyac!I6$i&x7lhL3S;mcr!6}@Gl?C21Vln4m~iLUPgKiPcDTohd3%PC8+jM@GI0Y zkoX$?au_SgJg2H9V3cBnB=tJetJfqIb@;U$#!&Kll-vhBRjHRLn^lkVqHJnvTCPOO z3`9uYk|UM7fH7a?lQ(o>!X=cW4GYxfyI8smRM0uDht9k#A)5|FZcb%zWF_3%Yf7}KysQU;GRy`^%rlvc+b(Y=x7mIp$OhB zIl2XN_tL3?x+I$_-op_4is37dT^^@R*ELhemv(9QB1p0dwD0JLz;wn_7?wfnZyCM5 z6jkeVjI4Z#JDQK}reo}i|ZQe#=^+hb{| zQsO}eG?YbbICv1v(WQZV@&TkNL*H7V48{iuQAatm#^Q;=C>=P9-OM(&uEx2z1=U z<-cmyGOAOJ9a4KnN^Lz&ZVQ&lZBhDsg;EyPY3pZc&-7_Nsn*lSO3g_c*04pVF-v_% zW$veW+r}BE?I9ais4#wijQ5GgVaIf?_KGV~TCV}`yOj51N@tAfNj`jM)? zn$8QEBE^X~+TZKO5RELgK0#pRah06!PStxLo<2|sDAcI^OFdcvK*+A|rSJpJW(0oa zQU^auMs39S9Q#G!ZUKgzwa+l+%GXy@{v1 zE#$!J057J$oS`OVn09E#vaTc!eVyp9AR;N5c4CxZKQ8Th6LN%{$<*bin~{YZ(98W& zvo8JR>fEgxTdFuMWDhw~Ejg#*-5+}U4xqk`QyUa*RQQ>YJwP&*Ka$!rp)gr$$mTaG z7DsJ^Qx$|u9tqI9pigC2n&6bn=y9iy8G!S(C(d5WT+)vaN>~328C+)A_@!Yg*sl`IPwLQc&(TV z+TmhQ=KwK%*)z7EX?4&BB*&alfP&cfchYkz%o%8Kd{g#EyAK318 z{~%ASA5iU@Di=Jjk}2gC2>j z-MdlM?o~X<`uWjua$bzj3||UPw!-=8DY#NN<72ETjgoLYv-`;5ePjcv8|PEyOWyP~ zCRAN9t-MOv`3k^~@sDF*TRi9DC5UE5N0d7!w!NQ|v|z?u5zmz;k0F`-W<0QXwmj3w zY&Sc6=bVP1Pu_J7d(eT622ZQZ2P~QZ3PW`C6LU>d0@WITgWHN+R2^~=@WEvU?~5A) zU^M#>Rpz*vKsd*33EMNg$G)rKxBW3*ZK~j@6Um&rUJCUQNYeUIx@1g%8VdOU;B$8} znAAdx zpHbrb0GbM9{sRq*xK+088$wDvtAJdcKUy$5**G79>s_;4H57vTWp)847)oe}MT-|C zdI5Q8mZQdV6ULz|rFe<@3at}E;I_)0G45m~(-;iKH70@bGsZy8lfwx|9I|YYiB27- zFO7|95(p4V`N9jVV`zZP5&d;Mn`-$iOGX`y^!sT3zs6Ai$WLgd)j1`;-64Ai> zYtu)NTOFU};4D;hwM(w{neD3BZP$G9zxDpE7b{8S_uxKt7f#U*q1tYNok*m%HL@|X zac+SA7EaKg;N6FFX77EL?D)Nkm5Xpinfal0et`bOitSRxcG0y%a_!K3=|D7iXG(A% zzih)bi>?Hv39dmyLtYySlbU5wKIA5 z%PVi4c<1@Io{tWS<(s7PO@g@;QNhHr-E%iCx*J9J3dy}?eo=A-4@Z?C3(A0p9zN53&Mft1@{X$Mf|~C=ART`Jv$|L}ECG5?N#0YM)W?PA&kOExoT7a~vQG%?ME#&R zJ0s7D-ZheU&CFihsPM_<`*zQrox<9IyVZBw?^a7|_lhlpQp?~U9XKQ%I3b)oBOW*_ z9XKntoE6JQrSehK0z5^;wfOP zp;d6!K5*1TE-yM-1xKr5I)JC;Jtm>8M-C{L!&kJI+AyWu%Ti|6NeH8As_P<$zFiX? z5!URwdstj^KwNQ9T5(XUIwVyc3g@9XwQcumdKYVYW4?}9^~xC7o0}jOkCivas#e6R z>K{4`wS5p5n`^;I1Q49QhsB1XTA^^|cYB2mJH?Ixsbk=7_hQFhp<^%Ebnkv+x8Mu? z{u*J&F|qf!)O#FPbe_4S1lXlmG%XcP3q{kA&=&jdxi&AlHpdEm5Ru>96f5+^+$}Me z=jL-UuTS!>f*SqJSCErpcmOpQ!?O(lCXOOZ95xOzsd zzy*)3;oH5!s%>IRx75h@tp>?-du}7-dGqW#NSS{GA9~4qt zhMT}E2@^T;e}Y+ZkpI5{8Itk-63Of}dc=gXG@-_G6-m^HA2aa~wBs3Dno^CP`%!AlrBCF^rU6=a!k^XZ+0LUu_y6ki_&)EIOESn6TQL41Y}5*N#eW77yJ z&7W32UP#%-{{>P7J)i8YEx4h%q%JK>7%j4JG5O{=Jz{hPdx?#OpU^!v_dY@>t?*Dm za8qZawzTrYH>rOMNDBR1CpCQF_1)SF_msDq)5r`p!n&l)5I9a_)Jo`0-wnj9d#wEM zfuj(1gv$v78=}Q?E@8v&Ckq2VauhwSD3Im=HtoKPz~i)J;IYXRQ9CtUmPqF|h@Oealjl@6yZ#i#VbdvWuPD!Pk4jVGGxFZ>34$`& z8Ce%(6LU%;nMu-;x6_w`NK;|EBs3v^9k-@dE;H4tUr%u!Ge=$X7-FVUUW9sWTy$-W z*?p1P+pTj&AC>_HlM3r%#ichVmY`s{hT_go3^{~?0RROnFgP)_df3$Zex?rV@lXNE z00Q_=;4kHLlfVti5lyxl{Ne7a#q<0G|_Kku}U962@{vdS;nS&n)Y0i+*AF`_?mK zs0@{t9qh`Uyuzr=9-Jy*J5u?B{OAZ9n9wP^L<5`D%Kqd0mP<6K{@t7o*~v{sc0R^C zaSM$%=rWJ_p91fZmBwru>hfc4s#hFr!uJ+P(s<{8g5Qir0okT2K0N> z>A!{-mtsI?qEHe$gL58^MCtc#zx*p4;go>qjA%Y9na>L5vneQ%q967-Fr%kd5BuLy zHw;74S>$2r#eMlu@ulCuT@+NgYvMAPQkXhjLm4Y`S*5U_BQB&fcG7%3ku&6j@S|i- z&74Imv%)e86mK!6Z@vINlhN0K!ZBjDye99Wy{)KGRkGsD0Q)= zE(PUE@_3&X`D#u-n9|~2e@G+iuaI-Xy=vB_)!Ztt0v6^ePlJP1N47`%g*Dq#^tQA( z*yHrJ4$NnMFL&Y8!o`JC(uM(qN0G*J=r%d1re!#hibdlyp^;gSSH4%N(+*`dv2c zw*1dD_fX-}bT1S#vEM7pe#yS%OKhgs>)1h`1X>U&_i~}HkgI9`yw6m#h@D0o$N>$G zzgxd{Jaxaum(urZU0pqW-%X_Vw0+m1j+W!PFRJ|jdP$l+c;WR;b47J$GvE!2hT~$_ zUeOy%v`L&H=Vxjqr+&n-e(PMW-@?nFCCwML{nir7L%(&Z{WgEXthqzI7WyvQQs7Ec zYT#NL%^vhg%~i4|p2?WD1y!A)CAx_PIx}lqup%)#rD|pIrjTu-Fl383lW!zLddy|I zFjTP2PR@GfK%sUG`I0G=2{UJULmk@)6N8u?2&=df!bOSucXCr$14W@C5FvR}8+EJ! z#$ENnU#o1n!rb67x?(BQK-mL=uf`{)YGsFEvfr#u-+ozCj_pigV2hn?K<5N~jj~38 z16x0^{ce;`#7*Pl$z5%K{6F6RWO47Q{aa;)qFN-*SEGM7xirp17=}fdnnp0rXc&fO zF(G(n;36NCrQlg3{3Kj|{TaPaYfaob3auu{py699o=MxgGH2?14L*}1_{LWmi}+oN z2K!CgP?r3s=g@Ad2*_e@%g9-5gG>9_VwVUOgqxBx!Lty6v#W9JI%8C?z+Z{yLUMfp z-cJXFG0f z!QNL@(E_QgMJ#BQ3R-7!NTftUZqe2xOQW?y?uT}R#}{c5-OV!t_shy}ZG5Net*&U3 zSk@txb-*OmQ8qga?o9KWlS%m?bN_UCVN9sJtej`gOJ{iXQgps3IbRfwagi@Wg7fmL8j@Q9bR-)3XaPA*dAlY+r1GZe4}leGk&9M z?%9Q^?>xUSEOzV=Yj%j0f)hM|dXSO3HH}?8q`+ZEqot6t*Ce9Ae9=!Psdj?VZX|b?2Y7`5aW_pnq z?15_`KU=exvAJC3D$&_4IooHtA2^)h_Hgj#ORsNTbW{tDYSkUr`GsMj^1O0B`-1fB z8TC?hj!Dij!8sPIsEky->5mM*IZCY|e_Zh!64%%%G!o|{)9>!MY+H%70_8}Ga%)^rM0o92%$bbs%}UAJ)X z&=1SREr$i)5wYZ`RB|+&2`TE^r*4hDGx63$^e{SuRIyqtL6-<;e#Fwk{tc0Av7|vN zfqyglQqhf)cK5&! zPKxXH35O1gYY$_q+nE6rw752*^YKwlYjpJcMPf~tRMRCCAEljH7Y6S29bW7^EcP9h z`i{;}KTYLMmc`1er1F(Q`KIu?@VbSf&%vbRX%hxMEpM`(o)B1|!F~~wh>kaDCxPm6 z!0Tna$z|Z?VIwgopA2OdCFNlRpH~lJ7}D{1S(F9xWVe%cA5X)d1Tml=WpZtey0h`L11i6-*s1GK9h0Uvl?cDBbJiCSe2+#OGCGcYaY!dJj z3ST4OBS6bAwzD4%j+~8WpY@NPIjg9`vE?_ND0Ur1TMq!sIJVgO=muNP`>0R$6Sz+I zl1cQ?r)~_{ z85A$Nw=tN8CcKeP^t=B9eLAhW%Wj5Lmh~ z=B$WhN3*c;y^b>oBT$V+XGPOHlW$E%$Fxd?(-uTYYTSL)j=9}nGEbo)b`|x1! z@$y;?FY*b-o1Z~mOEUwT)eF#t$gLh{8Z(T;YI)o=onbV<&ACb6)L^=47;;~t&ZiE5 z>8uIr_As5qj(fCTD2GN;O9miWT4wrt=ntBGe6p8`r|IG;F>;oPk;>#Vhs;{zoTQX^ zna-Os5E!v;hT&iiu>z(oA&c(2YH8t7!JIPa%_@PeGiG~Y4pP`<(>5*xUdD)qQ4aFZ zkTgF`!vKOYmi7%UJIUA#8~GzajWJPrLqrHdz&3#~Qkp%0w1hur)tUuC_>s%msmAm* zp}(5WL+{E+>0J((bmb~+!EE@WVB0rLJ14%ZE4P``STN}O1I!~??eIDebIBfv{^>|% z(oMK@ZK+PQc-NWwF0oW}Zzo$m|1m93u~d-_n5wGSX_?-auP>F%DgsN=sclczuTa|Q z0hz(mU93b?{MMJWbhvf*2e(%mS< zqA$6Ek4XvGU@>w*eXS&ovR!6GW#c4sgZyab1zm}jVfB#r6V2uuhNM?O+%HL_%rsEc zv2n-!9!C5J_?M6A!r+=j7~6Vi%apPOhrSJ&30x5bD_MHhZ>TU-lx#csDYc$r0;f%X zx!QN@dSq!LcVH zjLd;kO&(w>^GlTes|3DG;41|F2Z0u>H)sCfhjje8!bq#5`_%;gCjrvG;Qtn2s-CjJ zXrrZeISGCc>|c0t4Z&vvKyA`Tu|FoDV(9VA;VYwo6!saPLy-t=Zz_u`TU5h&4Q&XO ztQ;9L>Sd`B@DX^8z^@Q6QxZR-5c?qG@a?5b+TDvW_;@RYeu#VVvP6!DSZ(T6kkq`U z=8`hVGjp1K2sITad49=U#$_6a*}Ck+_Y&Mdm>=Cczm@*tR5XRCqUH1d5SIYO|$>hQX&uk6F7a zG;f<)H!12-(=3eXoOWeF0TLoJ^>8o#@0b2K< z&>P+u-Z;|-KF6}`LGadeh8=hpOfql`UvInFHj@)8@Vwn~Yv7&1w+5r@W%CBHU=8jU zdS|bSg^gMxk7Hu(7O8g2LXKFwL-g!`kq12Qd)}#dt0I~wde%#x_1OAVv15Rh16>=bA zyU!MWb0k^=3x~T!;>x|S%aiJ1Rc#$#G(eNs6i-d`2ABqID7Xh z{fXO8NZU_@jjn5NRRf9* zhv0qr_YVmJC&d1fQvXR@(K-5}5@462^A*YYir{<&Stzc&=jvQ^bt+~MWD8MHd~=0- zneR^3$0I8V($jnBG8Aw6#9$-6JOH==^cubp^N0J(n?HPnp1fgp7i^KF!UnrDglNANY%3(Q1T%yFXqkawV0(^la z+5nO;>2iy?qEk!4T+ykBFJq{ws7Hc@W)83l@il&GQ4C3SYJFA>AB|Qrej|bF1k&-$ z4&ebdGH2|vlsD6{#wc&fa-n65DK6b~O{Mb1{t(r&jJ?8fI0&_TWAOH%;5m*HTkNvm zg-Q7rP&A*A%qIl%iG=euMLF;BKHQ$xS=$@vvyA)oWNo~gj2C7?`Ps2VN9}2OVQLCm z0w=xZ4VM{I8K?|-SL5nYW%S52ZsUQXtNjXmFLKA(-T~2Y$w#Te-0XUIT{T+ z)+slj&NaGpazE=Nj9K+2F_rIoarmlYSq&|8IE+}TP{$Hutg$5a#|Ft&d_cBqcnbcQ z;tEaQ+tV`0eneeA!5~YgPWPa=3If6|!L?dplr7`>tfDDe1_3Fty;L`6&woE-zVCa3 z;)dPQhTVc27Nm5F_94kWB-n=tX`qdfmNRJ37)<>97-Td*uz9#1P_HeQ!n!a8)!q!E zN)1OS_L4*A@;W)MYKSo{ivv#AbzZvkqh*=2z{-1EtTgN*m_%@y3PIl-GM*t#3U!tz zjSzT1vnPuZQAbGf9(d?kNF^I&JVclJd>RcCt*NO(%LN-gc+>=*`YTIR+I6*%Tydxj z>WZUdBBkt;s^7MOZIYofNT8|4+LL#~Xhl;4YPJobOwM%2mcro5p4f}8CsVO2sk9&! z%{-`0MVniMRj*$>F>KT;Xu0Ix$xF#`_Czt#r6qC60nNRv)V%3hGObPy=g`$xigg+0 zXy?)v4w19n{HZpDc!xKCd2MIE<{%)nPG{zBz^i z%J!6rxl!jKwN;i5d&=$AMkQnlv=@6|_l1S}Rs@`gKIj zVG3>i2oq!A7E&bBqpH`Rzxh1K6Of5snOePX&%3cReE8<`k#5OVAK|0M2rs!-i1ud5 z-u&%Tcg_myci-)~d*E)5w0@u1wqI)7FWUD*98(Hf2LM#A*UBcZ zdXRc;V%1K;UZK*oXN3bpg8Qs`iuO^-J}R&iD^AUNCEDHmy9IX(q|QBJ)gEp9q&7D> zsdy%mAL+Zj_q%I^^#kIXUDBFeqHDM0+O2xCsElk}w6zGf78s`A^K4r5Y>GLnqrrz6 zhSJUOq+UwiK}+*ycFpX<$MXuL+^Wcl#oRU_w~a;UT=c-5Xm#}Po#$e0tM5z-Wjml& zv~_mtLdU}_Ln&yz3`zokC1Ev`*531MT=Z-dI``Z?Cwfjuo)a^>)D4+-eYa-r+;>)f zxKeQKl26sR_JPBbu-|%&YQ!Px5d{7twdOhW3P{cpe-BM&qPLasCSE{~JP2TYICQK>BT!xmMNO_hSBK-!HFk+01#G+%+ zlRSNfaK>|;Y|o=?ayG0b>ZYMhz?l@106zI9jG?U8szYWjL%DYs?`wSFzvjcW#taf> zI#e>{z&>wGy08B}T-&PT2Z++_#M@x~Sm1mXxlZm>H*G_mzl*4f2YC+!6SDOd`g8_%*f48Vg18itEY-2^ben~NVotoZt-*ncEKSkTvl(spSrfoEPyiGe(+X~mYNir8& z1UV5RQI;HB9_L6Q)e4|0rfJ1wlkeb5Lf2<3+?cbxB`bw5yB7%jO>4WyRUXrR)C zjW>>>eECZG{yXxUTAl(;{tK7Qe<4?tlK-M7)diOq%DD`wCEm-^e4}8Z78)2X=7PPL z!4f!Pcf%1oWZ3CzFH|JQNXY@!V0yCM3F&*m^=c00Ls~Wj>eKTcM)(KnC`Wzf!zHdA zZ#uYAol9K1b`)q$Wm0Qe->aBDKry~;;T*be+e5|4T9BV^Mx#R?;Hg^_`7BMQ5m6Zj z$-cA1kS8l_x^WCWpiJoj_t6^OWE#^YsjV2)^xtzT+abNi$ugs^gwwEoQrkdZTj^6+ z+H(3>X_#E@kee%4X11K`)nIa!Fs74dB2T`;9w55*ke}+luZ}XBM|Cak(SJX6hQ(F^ z`Pw!}ghQTCD)Z9UCm-7}y1%!g;Bk))bZaaHQGDm*iB)zf9r@+#$O?$qjQX4(r?TTSavmm?ij zw~ebi&#H#c;_9a>ILCD5gokUGuF$P`+V|6~eU;jA$>lt*M)dOqMrJjPg1zb8hKI^w zs;k*qHdGd>3{~7|OqnmrZ`1@yt3^4|JMQM19+xZN&?|J$%k_1L=gpxqIOlr z>i{2f6r$OJkX`klskIRQkNN<9HA8Vw?1h73AXv$XhcjHypkYkPZmF(|6AEx}REz@4 zj;yIK!XFPM5I3HabCHejLdiy^T4fijidUy=I;X{3Wj$dX8oeC*DxIV2 zTxVM;<30j^MxYsB5PN}|h?BtXP_N{-5%`!u4S_}iwFK%2)Dvg`AZ0a*$l{RXi|;0| zk-&H8xsfMCQ`Q|jL2v#EF5}h!+q#x**G?K`U?*4L#yfhnb zqt8xq@mx0OhUE7Ga9sm0(nEpKDgTt6n60c|Vo1Vqbb8Gj( zCzAtr)E`K?YeoclXg8J09+znt^8ExDDf*u%)Jxzbm4&H(9i&U@)=Z!ncc?>xT(Sw{Oa6Z1 zN;lOc+1A)cG%+>d{dj)TsncRZ6m$PXaf@eD+dXHW8@q?G8Rg0POox>&MLFOs{@Qi(`M;tDye)` z*hAmNyS#;LGY4gtLVbUcLoogiL8o{{+`^AQH z!?_RgJ+ns^^Q(pY>SWcxP}nVbR)n*Yl@#;VM0UeVZkY5EORL@~d#fxmG{=daO_FC* zIQv0KO73mY5P1qky za5k~byv{Ond@;XH$ghhf^N*Rs6B==yf;?9;$C^%VtHHi(%dNhiRIg*@@3Y( zvME+nI=elxJ?3kTo{C#D4~NUwD;fUZ=%a?Utsnlnk&Yr=iA`PhKBd$wn`=k?v;-D)#xYV3ip zHgfQ``CE>69kLE$I9FePYI`NC2w8x)ZD#mQ7TC~&j9k3+Y_vMs9)*Es=FMR3@o&x7fZ%YTqLk?SVAROZ&We3&Lh}IoL6eywMfziq)?{Ke#H^Z3_>E z2Or2+3;7EL3k4rei*ANHIZQ{BCY2AQWzDp|K6}HBxa=R8wy5?WLJ0+H! zluAxwj3qj)rnF>|U40wf^>0uHyJIFK+Psp@J9}}_<`Znb2lh>Z{#2NQHIb`}j#YwV zRjjfxY!BOG`7SBHF6vp#UoGUX*552y%wHws_2#a z=#{So|3x_<+JllkDA22XD};{fO}HPg{;0q!ly{2-JyJoBVDFLf zpyhQehiS(#=6}P@FGxX?b_s@34&X0HL6hu)k~@r&wak!OX0}(b zEl~qp7HT+qn6~z@`jlgUgnYwez*o~&?8xN!`0xbRroOKRy7@DKPW6gXQ&FQ%-yT>B zV80gi7U_cc1G=|m#EG1yjCT4O2y1TeP4Yf+{6R6fd z#n(Ux36@A|Ovq#?;>REfZVCm#;D`7geQb zlTeWr{T8LfY)5Q;kx#>iDfT2f=s)e}{Xk5i%7Yua>?QUN>GbWpK-n7}gWM|!n0TPR zJ!Ml{7|w5w&~)^t=vvHjeh}8~KcS0j1eg*GQCo4FrkN9RWVUmOMQx3^U0bhlCtYc4 zMiw74X)@7s@Q|jfA5yczks6om}EXCn2$YBe0!GFlTSl;LI$ne z^)}L7Pt3Q9e#_M@p>^9w&*aYJviVo%`|8JMsah**>;? zbBfj>`IXY^_;H!=`>ESY6*_~NX*Pq-3Sml5GBa?Mle}VV*=(@H6H7%RBkVV{ge7)k zKiGB}%l)U&RyL<-E30MIGtMwQr!$Y$X!vRCNgo%Qg)G0klRSndH*M$axFV{M9h?cQ zJ7W4M$9wqwAaP+>Mii3b^A6K>?o`F$go3d;D+PR`iQS%MN9wX3UXux~Dv0)t+hi)2 z%{Ou9p}qU|9o{)~toy|NeFKAs5ApwkhR9a|XrrjsF4{HPr*{4ndAQ1^P^972D8u%t z^6sE-%n!QUw_1}c-jB(N|NmOMwwN}qG&}<~*w}!BxrGFr!3JVVxi<<4O+caDq)BOk zZlfY4t&<^yltMaoZz&|%mlY3v;AJJRRIP}Xm3YWXRP9Qvlt`9#w@M!>opDF%$WkQl zL*FXZNPgO<{r)o-#>OP7RCDxu=A7emX3lWV{OAAv%bbik%I6*zxNxAHiz7&KWJH1H z3&baMx=Mtv9Dl))Wdtxoxz~sghvA3}eS=Pa0g2N<(_050(+GeXu%Z~*cVk(Ll|8L<%tSufMW9SBPD;l{Pi6NsZTwKKgl{iu^2MzrQ} zGg|Wj-ap%4+`6nuFYWkJuWKg}uw{C^Mh|rS80bj{dSFKt7}Wx!jHhhtAn8}zs%}>w z5A7r#-BnLdYNsdn>sK3Wdg6<_s;^D+wQU?q``VeWT|X+JmDhD?j0dIG{dH@v{t6ER zG`4&>IR(jq)5Cx$6og&1r||{`a{tGPx9=$>V`5cUb)DEcmK>K8Q!AWw2ed`L37+w* z5?B%`OuJ1ijIVX{a4FXpzU^6D;6kRvO}F2XsF{1=%9BLk7CI! z!=0vajB>yAba-w13^jaaZNGIaSL3LLa>?riRF*5{s)w~j2pZXM+Y75~1t2b$1FK$+ zMA;*Ou?B032@Yj+lpzc;d^UJWBcb4Iy*Bb)w&I4K1N^6>Ct7&;$9XK*nJs(hdiP#P z^s9%#A{2-mjoQl9O3rx(x7hqYUHPfNhP=q@gIvXJ|9$X_VAcQL*^22Q0hao9*xkp8 z4(qzPZi?N%+(6R)!nv?oT`(K$my9ev^Qt^F8&;}|o=y#zg4&fDumbbYfCxOl1@q-M zSa8KfZZe@O5m-4y{lU*c)r!N<0bJkIS1 z&f=Y)yKSW@0D!`aFXOd7Z(Ltw3`XhLCS}UOIvXA)IloIt1!sDVdSmy-jDxkW$1)yb zrfq)$`W4w85ix50A$3LK8h_Wgo8xg1~<+5YR*z~M*3%TVO~YY<-V5nE5Jex{b_0O8q7ZJ z4UCP)4|K$lkV38_TE7QG@ZnXiQWt-qHUnI^@7$~y;(mLVqWp=fRZ`awGGyw^14Gcy zx0PJUJ`*rwy}oO0pECaV^%xMO8!%gdihkA7+$Zmv{xp5di!!`s$_v+lg?N=Y14EKC zz^>ih;4(cb!c1uW(j_jAR42PH=zvx2T@iV{rjz*dTr#w_rr#L0CKXC4v4zz zwdjZ%9$~Q24jjvdyR&NuAaPsm-V)L#FyR=DevmP2U8&6H8rc=|}iSugfdGrzr zYoR_8E5ddBRbCTlh>u4}JW8Uue!aU_>ps19F=%T-DjSFH`IK`sM0Vr__f}1 zYmftySp)OGknJ6^&a&;t*L>@Ubww8G!5&ukg6?n8o5Ol@Yp&82tb+VH2tx-D_z9wv)10HAwV~OzW3$5?FKb;f`$%{NlMp-S#21@`Q<9>JFAAyLfi|P z=1@A=$%36fNfCG=xUxN?cAQkD5ltFdf9+{I{GD#a{t?`s{VMcLC^euCOr&n9?NeI& z)cQsJSo`_~J=C2ODjWJZNdh!#`&)2h_CaVfv^}6o!|5Z#?8q?5dCxuYY|G{#x`Hx9^Y|)bpb9YQ!P)%*_%_}y`qgzv)|9EWp350&AnQ4FXQQ{6y9uxpH^vDlZMy1xdo5%`S#=MsqTL+KU!w>ZyU+p&g*;T z7C@Yd4V=UV=3lh@{F}&sh2GnCleq zfa5(m_Yt?06AmJ$8o7nW0knUHJFk2`HDtVWIZ=t+R2sCA<;0 z1CabScABxp4`PX%3hfDug`PMi!;FgfYs2>l?JW4(3$MlRlUEY}w{gXX+>CdAE^!H6 zz?6v(;U&tfyAB`4bMXZ%_@*c_t`J9>DTk;nAnb7A< zIoE8t*t88?mA-cp#`lwR#y4B|z&I#D??B~QY|gu^}gvrn@{p z?=1be>~fh17ZIf?YZ=((MR{ih#Wg6x*l0gtt;4&No!=|FDNdGA<PVQQlc$rTn}|{gkHsKBy?~tngOZXJsOxF9(0|&UkUYu-2#Ui~PPy PQ$7$>lov`@;B)-{8zna- diff --git a/commands/__pycache__/utility_commands.cpython-311.pyc b/commands/__pycache__/utility_commands.cpython-311.pyc deleted file mode 100644 index 3318d608edaae5fddb0e98b34e9527d62bd4f78d..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3010 zcmZuz&2Jk;6rWwMo%KhWwrN$Tv~-{W>Ow6FNI(ThQB0`YkQNdUgrL=CcWiHC@0yu$ zORRu$0A#9)mQ!0%q#U3W@u7maaN|#KkPp#HNR<$WUXW4XQVGGES?|VCGP`~|UvK8U zdB67?e>FImLQuZ{_vgw{9iiVvBP?34vOWWqtH?&P$X0B%s>~|#tInz_BFec~s6e!v z3PRt&FDSEd6}^P)*ayhgZmKW>zuri~R;N)e-h4Xs8h5G=U&P~Ht!BD5OO1OjHyxMZ zMsLLGjo_-YKyZOn&BfGRvCfp$TTXt$ZP!Pkc@+@^W};cuR%Ty% zr{oYD+YM@pfxKL*(4b-oULn{PWrlckvcI+y_^NW}f*Y4@b zWz2e=dUfiUoP_4EU;`^UQSdnqKM{-d3>*c-OIbn&G=m~r;SnJ&=Jv{x!V_T~Ar2@A zOG(`vHJVt0_%Fy+DCQD5!Q7R;6+&$UkzZ0igV=u_+l&hZiR0g-Kd_;-QL|w`&>z^U zza)H%EVbj%N``v2zR7P9=w$OFd5|K!Mc3huS#_Ea0>B$S4|N+(HVBBsLwLyfRHQXV z%j`td66=JK#z#t3&*ZricqR-p=1z@Z$Av&z9zZ&Xl~9_XB4Eu^7$189zdkluER4+* z&rIQovEt;5g%bg|Ht~w!ppm+*#_K zBNoRKPL;5^vtzHGnLO6ZKthi(uf#8yl+2wZCZq#nb7A_=_4vRDJM#5KUpEYJf*VG2 zZ+|N1!|q+epie+7qwWqga(_#^G`MyjZfPAo`%ePx+<)!Jm($-pv@*Fmbi6Zkyo=Q2 z@a;E#+4bYwx8MFPv-|So#mV-_JMEGyq zTnKd!Ty$7W*juNh4p=c@T_fV;=7XD%uEb@Ws1@TGfU0jrNmB?G6SbNx>cS4Du}8x%Isf7}eNrD-_w zC2b>QcC4(}S`*e=irH~H{-G)i1TDeovd{@0u^hl63hNeFL>ddyI(l>y*KW_%3D_xO z;q6&>^NrH)9l%6yUXJ*2yh5x6hMm&J)we-v6_dd|${5_1f`1nXsrO%o;u72-%$bVA zaEYfHBa6iE8Vhj!iP|UE%Xok0Wr% zd)O>-LUA)0v1CrC;)JQVTKqULWrGCZ+$VY?gi6%r5&=xpXC*gFc>4NDuSRSDs-K*$ z6R3F&?x)9te=xFfl0ZZR22fHP>Lw6`VeTf(#qX+QxiW{wo_IL=0*t_XNY(}FQd*+*OW}NSfiy?wf{QY_KqK?pS=ghxaWn8=k z<15Nggt8#3@j%p~ny5$h!1;CF5Y4FR?FMg&cGUKElP`!))B(E{g(H-;zXee@Ti8Xt z4(R}NxIWxkdxbYd&Z(f%63djthf3q^mpfbUp5YyJpllfK9WjH2$|juRT&rW2QG?6e zoM^-32osrM`h}(w50R3b95Bu-gFuI8kLVmzJIq12VX@Uj8cr<47z+SIXBBFygn$Es znY0S~45(j1S672D+$2n52+frQ@@6IMV@HSTL4LAQAz3x1lAV^FIg%A&h=rP#`p`|4 z5nlR^R62GYIof59SA9X!oSDxXmRztPdG1rR5?6IA+VcdTt8~VsYuKlMPReOMB~{mRN$bWH1j68X3A+y#vnTj)B2zcAas#Q7#}szud^d-b z@N@#hcL(900G?N`R&>`>mC$^E^ zE)Sj#mU~SNPaElmQ%$E@PPIY3@7*cdNs_5FNs3mv!0X_m>DT0Qitde;JwVaD9-r}O zMK?*RJ^>DJ3Fk@j16@ERbop_(67RzUF)`uCM8|8?jIGM!ZLL1h)Z*8SQxp5g->()& zb>FB%`I_hAJ diff --git a/gcode/__pycache__/commands.cpython-311.pyc b/gcode/__pycache__/commands.cpython-311.pyc deleted file mode 100644 index 57cf7167e945a3c063406d4469775b94e987eb23..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 25901 zcmeHvd2Ae4nqPHy^~omL>gL@%N+(IlIw(=rXTcj@fqUGsrv5HblO*UQKv?XqH z?cF%po`HeT1MgahJy`B$;HBb8mPuxJxr@OHeBAs?BtTZ>8o_iy(Z4fBkv{Fl9np1T{$W3JJEMWI zy5YL9`r-PqhT#T_U=>aXvgUWsr9?QP;H&PHn0lix7E|TOID9VMiYJt@Zktvexqc&*h=oQ_IYrfcYFLg; z#tVGBZ-t`~8A(E6b!3G192_RT-rt3fj|4>+wqVv{wh!B6>wChmDBA$-vIyvq?TT~M zDm&2IF4>9Rc4=N#L8Gsy+FlID6=@_Ij_ciB+_xiqG8zTbgfI%0ElDASPyLat_~Nl; z0bgcCL6lIiZE)x~8t#h3L#%096amNm=ceNuPS#eniyB2qjAbm zdiZMJ!OJhj6*Yc29DOUSUhY@oHxjXl%jeY?4W@KF5>?`t8KP`A247LP$77?3x5KJ( z`J@t-5g~s0Dviu#jvf=YG*>8u8YDuYsrtfR*@CM_9YR)>#0SD-^1zljB`W=sCJm*zq0+r) zPv|uap=Kdov#AY5{Zuhd8m&1HDU)aA1aAh&HZ)&>YhSeN($G_ zc!W9DsW@-JG1f(j6dPA0Or;4aHp-^0F{nHqcU)8wlj?X$-wmmgiWC{;ogV@28<@xw z4s~19b*M(SL+`Mv^ZAa}Tqk2=ii|GT+?OU4xMGtDt*TF_NbJLL1GslJ9;e}_;WE3` zt9PXmqS{Y27xiXg&=#bN(?GCJY&DRsHjt_9NV_^%hiVR$HLhrqR>r%Vyjgv*@wXH; zktdpV7Hs*@OXGk357IIC^Qz6C!^%{hC**nhNDJ^GZd0zBcYJm zK$Qs+*Z`o}PQ8v+If$&@hiq>^Y3 z8=^d0(3v~)2f9GrNq{D)R!bEkL1rvG zaxF5hXgOl#1;P?naQl8O~Huld8X7Odjo3>0_r)>$22NmHx!Sau=uV(~uCSow8#Z(qYmfyFM0WchYLSZN^(P-ge{dFy5|@p+hB|9}7S7 zO}mqBP!!M91jmyCPBQ?!2m9o>zY4S9Nikt2<4U|>){qb{b@lApwONW2!|mpARU!y0 z)I}>aP4C#4M;@%m@YTdYNDNc%fpae(JAdxdvE)PBjyHRLvz%s$r76G^ar$m0RF9)sn}` z0<34+&IN?W{{{ez-X{baGycu9;W^9fNXEbQ`_7al)sqT;-qSljl-YZ7vFB8#=hV+H zEcFbg>))6e%G7twt()ta3uNlIr|}zlL;ro>ld`5>oUsB;TOlqqWdqGA*Rp^7b~x=H z*MGBl=Um=7e}zX_P`UHxR_;t+c+GHMj_Qt-Gh5q`s$mF!$E$XtC2ALeZUS2g(BP=s z05tn(G!{;%L^V~C5OoSdU8GeT^Lj*yMnj=H7Ii;-naFTN@5t>GpTI#1_!j_}&*ooN z>`IGWIni%_)iNi}U7FuKH=OC(pA*RAZj1g-zaG;aYrrm_y^)wTUXKCvsZJFe!9RZp z8R_|U*mjS}w%4EN0oy(cLK{|feRd)iWoT(~?h*o8nMMB})bux;4B$QVLmjj}+DW_BVugd(_?NfJ&TO-9rJr1ojeObkRLZ+70NOqV9v|DlFH;M$U4t zQrrZ4P#*-IH!SA`?n)D>ySIP40x@s-9U`|&RrRIWbvWjAt#+BXEUfBbQ zpb8R!7ZQQ~wi$2Hc-xJ)!+3ik5qObf^~WM)g6d>7WP+Nh-{Ui@L?-A9LlZTa=bAD> zk1i7!(~+6(IH_aiJ8q6!#8jUL&_#^CqZfyg>I>v~1OUQD&uF)s%_#LK zJT$Lv)eK!c_u{!B^(Bh-LhoYmg}WOTgZ(7nlze{$!KeL`16i-{{lRQ?Te@Rs+Pf=TT{B}# zo&3@%wt5~207&?iha&DvZ`?C~VZpW-Je&z0hWD~AFsURkK@OrH2~38Q1SZ2t0+Zn+ zfywxhz+`@rz{K1nFd3eOGLw3S24Rr;k(6LV*f~$01Y`=B1^^{B|FYt?w74xN*4w*_ zWF35ysDl@ibns>$6~T^HUB5?FeZW5p`rT`bZf_^T3oIg4H!V7SAiJ zMP^B&ql`z40{)WaArUeO)8J4C7$frZy|P*2Q^fj=qOu2R&A}jJs`V|1-U%|g84q{_ z4~H2A9HPKpWJPJDy0GUbwog%u&+;`9LY(W2?~4f2SXt;7-dH#7NIFW+W?iPy*u$H-hyM+*U%Pz+40htHfEXG?Ap zT62(U1F4JFStm57W=`v+8-cq-u?(>kB3KmNzvpwT)&w`%=ljcZNZhlYFWdi z*s=+lNo~1hv13!Fz4u=Gfu;5Xi|vOp?T48C5Xy|Vc4lk3sx>DB+%;dcY|Z-WGQN#j zUq{y0a(mB*wb_>TY(wzF8@Vb+u;xnv-~oYD|NCcBXL5caP@n3{Hg3rY*6N)M%-FJl zhTB`yflWEVvTkR#qkC@SC-2Pk-L`#j@)w`CbeaVO^lY6XWeWfyNzeAH8o}3kyFb&q zbH3y5roY?v^U446+l#fYXKG(Z{_gXZZ0mZIK9F61F4dp$wxbE{Tko|USZX`4uqktJ zV6km5(>6%0Ysm&X?*;cQ1@|q~E(T9#f+xwhDU0as2bbCpF7*EF&|>?EO#2D5FIv0q z1)p0AKDQueoL+^+V3;3RLJz1Lw{SoeUkl*ca+MHaK+bqRAlF)TMXpx-@4V z#2HGghfkzZt3yCQ^%VlI5@32U*MpgctXXzxA}zo_^q18?slJ9(cdFF4De+MPHwnB& zV3+_`a@E)A^D+Tal+`x??zHL#d^~!4=S<`6 z&d-nZ-R;lx4=o;fDRbnd^lQUQM~2g#k=aD1bKinxVf}(F(|I_J-)}|qAI}kl{7KCM zUj*B}fl&YjHw4rEkp7#k=gzs8xO1U_JMWHiXZjM107g3K{!LQ4zF+^%-sH~tYuuUg zWEHgl#_rfRJSVWyO7IabHe6Tw=Pw zH+EFbZk(N*-I{6Jp0mMYxoJbrjyD=1_2!)95>^9AOP6lXz$+)L33Ph~RdNLX{4rJ3 zU!FZ`?dtwjSGh&mpA8>n~Fh7P}LoJqSzcXlHFXC^mL4$U^8e=L`_7=j=f2^99Q{oU8f=Sgc)w*8gT@|6Xr(ipL@#3vgbR7x@&BJ;LzOWS$5jPT zy+$BPU<_bT9Vh24fcUDax=B#Es_w;D{1d9`9p9G&?ggf)RaP*m;+{`q6hg8Mt*RZ0NZ)VIFpqVu0@^Nfh8 zj}t&R>qAPZ9S2i)?>FauE8Lj;`nfJT3w90X*~}3vIzb?S9OKc9{QopRi*<{>n)}= zC7KQf@>{3&RYHC`3264*D!8wSC(jKW(~p?_5pRZ6V6zVvG@}&RZ&P!~;-e#bb0L0X zak~Y{!?`JA&*WhFI#;?VQ?JFon=^qb;;$CV$cfvxi!`Leu#8!+LWgh#8mKEj{$@ zkOn5>3F(R=_0TSs-7L)@4q)+FSg>K^4zj{28^+$qy()X-1H)(LkAZ6UFC)#WGjqAB z@%CHks?EgRw)1TTuPn5t*T16w*balN1pb!swx!u`S$d}6Yn5ru30VuuFXeJrYc+YC z^no_Q|q2furvh!R-Xpxvp+@H86~#m8cJb2Oj?{+P-oF`$Sl z^rpq$oanZ1&j|ptm)L7{(BRa$9$s|nbq$~o92Nepkcqt}^XnYgTZv4Jj!Ipz31!@z zNIH#R!}e=O8a5P?hP`HsB-T6wgVoQ_(Z|()3#k4(0z@YU)juQWJV2btQHj7hVD=%O z(LV#4sQ;eQ&}Kh%BS2;GVXaxG{zoL8YABmNROO`l=al(sbB9-|bng5o!r^8l)#uI& zbG?~uhtvM&@hhA<>R(Xqe@UQJ{`+h4BtUz;%BMq5TI~6jrvnvUG#$Q~5a=G@2fo)0 z(1CB

Y*^4X`UPHc@%xUPcX`1_vxr^XTdq+f+*-qV8g~Fomh1BCSfRRh2g_6)W@zk4Oj@1mbZn$OVy$WRTLqPoi!Ks4;2;Kq23V(LE)$|V z<(Lwujg&I#Ig*Ip!j8%blCHzXg&fAz<4H~>I^)tcY{iV?yvmhZ>?RB2^at%RYBfmI7rS0Qo=>9k4Nnnw>tV|{hro~`R6z$kXSOIwL z`V|LwfqSn7^iHO-o+@^m2UVt82IyR*c}I^lu_l_!sLHp1<|Me;kTrYQeMh!p!3`3Z zagk6d_KOAL*IlG%H|Cg;S4Drb2RQnFL1Wp2^sAyjZLq)|RXYA2#3?##$ixvC{Jz#i z_-7nj(eqDvB1l(oNs&yFyxDy0po&evB9oxIns~`;&lBD|rSs%}p_kOZMK9@%t~fpZ zj)DpNeZ}dqDJ^cwi4MB=qXO_f!StZAil&DM4|=|-%4AwrruGkz5wMcVL^@MA3Mzzq zt0K6z!c-Z;BXs8d*I77PU&wQMdBgh9;%_1wnSIJ9-Y*EHo`!4`Zb42Q$-lNz!cz{$ z63RgdBc8Z55g7?bi9sphak?C>bW=iHSE9kUaLEhCdk4?tI26RN*=f&!W#RH9q>svZ zW+z8RM-{r9O45;qbV}%!ljFEGgfvb?&Sz)bSf%qyy7i6trHN~a*ww31T=s+#3oud7 zG@pg~Q_P4*i->$|N~gUI;*_agH9SJSdW4MlpQ&^LUlCO(Q<+{&i!Xj18PTh&OnBvl zHGxj7sPG~(@y%pJ&%mAnA#qQkl!(2n|I|Vv>VsOCGqprWtihdf2CEMt)+$UgkZ&NL zto4Klj}#KK?HhWm7&^8^f#6}Xd1-9=y{z=#dNd`J|h@B9A- z=M_tjYusSdV8){Uxa8Oa^QSC|H^WWalDI)pPo-{4T4oDNqUh|w1~HQhm!=mBPuM3# zI%ipYwh@Lr8xB{n@}IDTYO@|VBf44%qESNVN!q|pc-nEoGssSO7PJ;;N_~Z`uXyUS zHEEBpORQPT!owSAhn@8m`Za%cll7J3*Ptc*>?IqK)mq{-TJoaoD(pk_>AJ#i*+upW z{gK^MI6^n=9B;;Pk>YWvDg2f_`80@am95jxqO-rwVmxO(TV>RzCQ)yehxq^}nT7O+ z#q*+uRvGs=%vi=Zpj0zZkZbfDFu$svV1Csk$Y5kV70NYdNloD?;tKIh4B1~&2R=t< z3vkga569|VvaR^`-{L-+b|u>Kv6HT!)E0-czA?x2>6cBbSH`nS**;_Zo_^Wu&60u7 zmD|0J;|>vI<0&1R@v0(h@tb%=xYQk(`rnLc^{EPO>5#%EJ2$T9l}6Rr7<~2n>JqUV z$~cS8K=G123p^;X%Oi&Q>wXc28?9|2k^T{%F2Wp;>M!vtOj7}P8SOjce2#}roD0dTRg<+%Fa z;LNVT!Vi`YLs^sHs($Ys zY^QLxeTnOp?RB5m`e*iL0-F|VH)m=$r^Mx|+L@jo9LNd#-A&8&E%)ksm+E`xU&`z~ zwpf2WQ-3^l{PRHT?U!d?nmfA~IFJb(NP7<~SFfA7@PpTK!k#BcvtzY1%gw>m>1^XR zI_P_t0sXY^=1dC?{BETKzlXD1wq>?FJ0H(%IW%+ncKm~LzgP}npNO^k`7au!*@n6L zdGBK5bD75H(!S@uVD%oJuV3&k299L{$I{+oyyhLh61DK578nmD)37C7i(}6A>W*yN z2Amq28_Bfoo_~3PoNcf;?Ijhk}>e+Gjj8O2dnRu1ugS?d_s= zUi`r@%s1|)&jW2A9RBcd+S>u^SdBkje=1#l3VnvM1C2AiU)tfyxdcz``yIFYmfRg* z3T=--lLohhqCPnE;i0s*op)!TqOZ>_eGzDwnanhH-fP^p)VOV~Z+_ik38pHd{hEH@Hbx*qW*|hsvbe+9! zxi*kWVD6-Pnc~{LncWYl*+Eh!Zm|=cnANaNj9=C73?w?J5}i^VG_`HGiBz-3;+W&% z#5K)7HNB8NL!{A-EEF6nBE-zF($0xF|7*7r!Hw-94z;ao2V6cEmn7Ts5q~to`hpxl=#l+96{}-X8B*q6pYH~J<#geny zEh4ZOphmC+(t=1ov(_Iw z|Frf`YUiy#b^c}Tf2h6dTHJLiv+LBNbUGuQUi6*G_|7Z}XEVatv~YG=XiFFWvcmRs z@h>YJNw5CPIW}0Z0#FXVB)F#J_p<;Z#_t0ZZrLdpTfhVO%X(Ee(QSq^4eza-xW`5>vDVwMBS6TT1f zLA%N$v4j<3*3e*-1BwaX2l=24K8jfmC?j zGt?qQgF+j)8zo3#b=TV^g1})Kw-t&O6}tVQ?CuZ$D`^aBVgLgM+C^dX4}+wDk^bp9 zcgW!kA5MZ~f9!}pbLY;zXYS*C=bU@b^(QWuoq{X&^*_HE7^0~E#E0U=6cS&~LE;w0 zQS%f>bB0BFo+fw0yn)<}^98Qq-W;*aTO!tZYs5BhqbUP*n&OyuD9&`pKvAE; zKjoUYbB0;UXHNW+{gs$-m7R#i1TGv6#(8%3h8X7~?DLVOMLxnuCtD|6=V=9F%Ss@kqt&UHZBCCBGio#E+YE|A7A15sJ*O? zIK)PRp*O=(Uc(47k%bU@BNo?YkJuk$C#HRj%uI$uahW+67UQyYW(ivwT$D|7%h2{v zp{wNPC_H?53Wneo#ZxeZ6b|3Kk?Y}%Jj0vX)ba}k7&6nmnKSVw&J6!7yoIy!)&(nP zdxx2~K@F3>2E4WNCf>4O&~RZmJ8*FjE?Ar498lZIIe8c7LXMna!OYb`ULA0B0!LS= zHgL2+4L5Lb+bA2w^>PiB->7IuBX6zR7Aw~ToazglJl~5`GjM9)TB`8aQ6;b1$e6iS z=yfC4#y9a6U7xFsVmp-ea2-{7>*~XJbwb%@u8-@2dkfrm!hJ`z`GT=$xNhi4D~xRq z_iaW*nJ*UVv=Q)lWe;Mf1RVuAiGxLelnFWe6ktrtj!-PR5WXB(2!rUVI0cz>!apPY z`!Y(S=qBl<@wh6g6muPGid&d zx6PU-1NTEbGq=s~J+t~f6K4q1oR*qz8=-_nEn$HtwJ)3%o@~W;Nh(D&Zt=NzzXOTS ziVqY`aW>G8_QXv)TezN&hU4MjVmQIq(0FP|H8Cz+7X9iI^3hDPpM5bHf0K>J*t4@U z(=5(A(XvatAc8&v&C81Mm;kC)<&bDbkqVS_tvGT;j79&u*z8ZuGX1JdUzF*2nI4zv zlQKQwGs{NM)JR5=97ggCk|Ri-72HtUXB1F)9^-sJ#+1;61XI&Khir(6GP4A&m6FiifZ90Y(00N8u66$j-%B zkSmt-8DvJ}7Z(UO5x>r|U%xitzx0v_gK{ajcr7Sgn&ic+@z~O(7lqgrJ``t9!N7@^ zUL3zLb8a{ki$uU?@WQYdTZpd&1^&_*KFC1<@zUi`jN>n9$8PBcb|4J%8VDr1s$qGg z___hwBD!FB-lp<(ROjwIW$3j3UGLr9|1k8oLl2#GH&4Ge^X|+q&t#qblCwXv``8yo z!=U{u3dkBGSzGf?s=X_9Fg?2|Za3XKm}#1UU#_k551EyBSz7IR3dkCB)0uCf8a(T> zYl&QaljfGUn7W`45Nu~#zMk6IhpG0wndlIf{V+%Y!>+VvJedwJsB*InuqAKzkmelvm|u|^T~CM7QiUaQb~h0 zH%XU_gjxa+U#h1qku)3yUVw8|dli_ITLsS14>ATvp|FaxSw3EYaYdL3LW8Wh#D~HQ z;Sh_RRja9qYPbjgsxU-;7DY|t(G&~=J5cbbd@3VU8Tbi5I}VgZ%&`Nn4zU-9*!dxL ze26_c#7^jolY#fMGX#k&d1Av+HWJY>RUa;p2`bZ4bHXyuh8I|IITQluEG#b$v8RHI zBG1O+Z}P%QSS;zJQ*)mh1Qv!?c8Wm&c>(l3M(Br3!G~lIl0hIM3IcXY*aZpM4$7iX zA<R6i0!8 zw0Nk-otcJ%DUZ~1aLo);eKYyM-F@HPoie8lS@)pi9(>^5ci+7)>pmd44`i$dDkTAJ z5s?HG*n7u?UC z@+r6{vk7Sx?ghiGK)eAFth4`#T{V%eD>si*h)l^80^kqbKALf$<+ZiudWP2~CFjmu z(~k8+Ik#tRnM|4NAbv-5Ir0WC$ks$bT0&aHVVcY&I>r?(Dc;L&LIn>+iDe@?&1je+4*6#BL)6t z27$PM(=dfSFeJiWAm9&@>3kM5{6It;GGM4osv9SjG(c%@!dr7nHKoGH=LH}zq4kt= z#|9_u+?#RiMNPdgH@p{Tv}@yP*10D)a+r7{8-tQ{S4zxU2P;*!;)GNpRn^(o;7d5a ze`ZM1(H}x@qYkLJV6kJ=>vz!IOyXbFX+YhLgYd)6a1{+rzMy^*XI_1%s-~YN4bwy% z1l%zJG3bMvA{Pgg3{@^<2D?=z7^s0YQ(i+b%1Yr(iN}Btq<8PhxL;U%YwfMAL(+3E ztTCXlB{%!P?Y-~zrsmQIvhESdJ@UYP;J*97=D{sv)_qiRAI(^g5?Pp&tt+@jT7o4@ zLvU$n@rJfNI!DwDUnH6!4+)SI!;)S~m|8AmlF=`YS50AR)y&b8)aw^kElEp~zD5b> zz#?e&Hj<_q{(*%v=;bE?tx1}b7NU17obf7=JHeYY3cZ+r-EdvA%LHx2K2%(Ke$J;orp!R@j`{$E8_aJ%GR7JUTUIJLlx%%7P>JJ*3Pht2#zS` zYJA_4_l#1jT}jk*lfS`1shD(N^d^sfEB1xFRRCG0Yrh#a+HO2;Oa$DrApjy!`n2Hi z9L8Kkk#HP{&#I`{6PQ7~JKVcRcGkuDr>5`ky>x4fa-PnOx%KJw=?|w<+=E^I`@8(vT}P!|N3))znEn1V zpmfKYn}8JdvzG+{HZh4Mc7&rLL5U_U>b!9i^oZEtcOZe@S_L*+}df=?Q|V z&_@>ZGcj_W5c}Alo~76RLi>s^d4!uc+T*g9~vm zikh#eeN&l~q#u?C2Vs2GRAUXOm~KtE zUjrlYH6S4MtyI&_2aUb=8+)^jUa8T$W+5_eu+4x`LaLQ=+BHMx*uhWSAGtS~%}|#0 zORRtEwR;|EY&y$MuUj^l_gx^aEo*g(xY|gV5xbz<0Ycd%@+m{&76pEuA+C|MB(3)Z zIr=L09X#eF1ZqoimV{LT@#knxBYlu-D&(4VWr^?i)bCg|-_dN20!Lecqg}_*QOI@b za$O`BH$dwW|EJD*(G^qzuM4hs22&Uic#Lx`Gy?I_vJqDB#bxs}WXVnva8QCZqJEjwtc(O00BxDX zo>#yeOsIkMSrSJdsap2m;S1tVfe>@h+P>!ey`ygJ#!rvkJht(p`;MJ?%4i$TvA%TA z$Ii8L8%_xqTB@kIzOSW!^3-(OFCf+sQfu4M(_6CCS7W~t5!Q5G@kJ=AW zr4_?<<3(zPrm2e*4S)Cs0M<4O5vgJT0{t~L(hg^boalTP!&S-_S1&Yz?0C(ALbvj; zVG03;UAM!w_7K#;1Kw622&_C3HZ2*%}F6W3Z2MqClZ_pnTaj&QHY>J z#9n5&<;YUOT);yx9+c??onVuF*hEiNlb{W>L2oG$y%UgLGu72|`&_P*l{$xWT|H9Q z-dsnw)G?U1nd&jfTMt3r|E;5Jj+>6ZI{D%0TQeWbq$VZrp{!?A@{DdX8S7NmIwe`B zAo0-EunulkZ)zgz8jxHAxJ&Cxk3V$Q-+X?Z+Zay;GtPmmb3k$qKq7C1x1+}C=t&iI=;=47MgH_bHA>VRnhK-{Wz>O}<$FjOxsaP&eh z{FjZ=f=FAk{6q{0H62$48P4UX&?qDT;vvME_flghQ`pIvcY34;)_7u zrhe~m-F)V~WA7f@@Jj7_vW`IsBkFeB6rDHNTvPPJ<~H&sJ&|qRCpGT_I5rGyjz8?^ zzWr?KM}K|v_R;l8fPMpNhXMiUHw*yuwRxqtrIdpD%2H;E_5SMm&#$NJwwQl&X1k}P z?x}U=LubxA@X48v&ZK|5HHT|B?~LS~S!bmBo?O50lUF}_H67bp&h}49{ZLw}@6Gk? z{^ZCnEl9$xO8$wEPKl z8meQWiHsH*Gx0f?fIH0--PEs}$2*Om*E=Ekc_%Z@T0UounC_!7?Hzv>9)8ua2(jlZK!kk+j{5!h$}zx*2@=rRi(Wk+RT5dsc#>CbRm7r#&b-e;trJ*^=csy0&^`wK5xMyZB&tNUl)bY~vUi4Ysuqwc?Vg0z zyEJNc3zR*wE|80=MzlgMaEViX`k}a8R@^pL*Z)D0$OW$W(JT9|8wcoR^sM`jj<`r@FBr?j-lm8+Gv;Z~~hT;)d99LiC$r$8*C+wmj1CF(Q&L zOZ5;@MY()baUx0+UiZ;(Heh-VHUQy2D8PBpc04tSN6`WS*&YZ)V%+i~rk#PnpDqU% z3vVoe02d3v$x!UKY!u^yup3jR1+YbM=I0Fv9t-;*M?h&6NO0^3DGJ6;z>O&qCKFC! z>I@P*(<7jG3oj$Nhy)KS5W6QtFckyxDMdg*qGJMTQ9|kxVfW-P;uzl@pS4k4$Vzc-9 zx96o3FOuhS((W=fr$c!PZkrsrm6M$gQ)im+?cD0e>Ydw@ zZcmY%a?(j1_UHDG=1uLUjywe<&A=(u{`9;waD+VP`iIk@k6+Il;oYM>Bc$LX|FJD` zdul5o`OlVWlHv;Sm$-teY&W&04{aJZU)r=wgAf9QXARfZ0@v1Zu1ZZ(Tp>pqO*_(a zn|+(hn|q{T=oUQZcI`A(yqgKBW65zeA~GF(zYE=50DH}OhX3RVHD$=Q+MA& zuAN))?TLHM+h?WYGvv9P9BDGOLGg*L=FPLxUO*K*Z%^K9{@n{CSx)viOt9$DlXj;G zeT@p**^HtGB;C5%cejfqx4q<6PWDm$V|imemItD14?O35`w0UqkUM#*RFIS|BMLka z89s*~B0z#j$l}!mhlt=SK2B757W?ReYcj(O*_bKW)Wns-mT=RMP&`I_k(QIH+7Gwz+QovxktP5W5h z6|bAGpRQ+Vcf4WVKka8}PrPw{$MlZ*z;r+qY{G;f*L+Eky&u>F;bZ(WpXs1%n-U_m z>G!;dDMejS6+me=sY<7ho_XQaA?Zx=wPeccJ*_5Z6irjK0``KM zyr{n4Mk09QtFa2FQt-_N<{cPKaXNe%<8w19!}G!dmB!+R^IQT6!+CabA+FpL zfg7^qjm=kG2Pa*T^HFV=0K@qU3!n92mv8r9NU2!PRR*o+_tod22 z?U$U>F4?6xWw+uk$3>avOO9!eT%*(|&WbWyS)J8%UeI|Tg02>A)B@Kh`&j#wuS32L zeW{(b$@NIr%aYuHcLUyjy!~>cQY*V<8*q0Z-w0gG7(o}n_Z@N&b%yY-NeQs}JaRL> z2jv!wB~adfR#z*^LcnVReH+rvXs;db7CB7x45tJ6*0T0Hk#A$Wx!drw6)M}t>8HxJ z_gvZbPF;$nBLRCdByNV8LLF2DlLQIWc91G z2!_w>v)MJHuAobz@wtSep>R=El|+hV4ey*5RTmS9xx_`I2I+;^qNd1(Ps4|}5`8V6 zoVjAu73KBoi^`&6xD-`Qs#?TuI5Z_btJ0FGI{-9dGR*YnuZ)deI0q%6U5Lfsh^ZHj zE83M*a^V8B++}4ZC5>Z}wF^uG4i)NF)FCZ7o4OiPl?%@(F&PNj1vYaR%GP=B!gU(> z9JWFNPCM#LF&!l0=y;sVu#S&^VRuV^UIE#wOcO1*R&;wHRr@nTHD`m{$}{= z;cvfo`{IYmZzccuQm+3gz5l7~(WzYPS-th_^0WEc5Ft-L2>In)>v_HP{PMFu4RvL_ zxzHXxvRxsg zn^A*HLWV|H^;mU)DBOItyoL;yY~_TNeMYN}%fxoNc#@*ZZmOfS)3KHZl|X0Yei z)b6lqGqnS>Tu-S@54$r~#G6+t`t7W!`7CNC*})$5`3h`T1$G43e21t#YuPUpC~VwSlSlT6`V(2^u8m=j=%iwfU?)AB^M;w;IW zQE7_l1&LLfoHd)^q@-ne<_wcg6*`!F4GDA>2L@VLMYS3yHN=UC>)s*E)x8k{hY1`Z zFiPMt0HYxlQ!gs1=t5G1SpZk1p0tx*-$0{y0Fi%^)gKu8w+n@EdF z%WQa=mNivj{TnbYWO7fKN22x+f-APLfZEhH)@HH(qQ>-T&}zk2jmE_VMqcLC)G*bb zy1t+ocBnC?A!zN8wr>erL7^?Y zTo0wUqv=-rM&qva#$Bu4T;l<~@xZccGt|B%L~4e9Mqv5GW>5d^JsUlbulGED=UlF5 zOz#=X`MQ>$S((cRfat7i&HID5YB&5n>;9h1k(_@>_YY_y*Q}1FNB&?-AYiNY?iVzWYNr-^}!`H|&H> zP~PwIy! zk;gCRe^K|pnDxK-3#j;gYtFUQ_tSTW^|7Q()b@mz314^Cu*6C8aw7dWT~4sHqF#-TgA^wHBS1-gpa!8Cmm3cveO zwtplSI;@8dZwVeyyre&ImZg5$-1+X+)w*2sA-x%myFx>EM)b!fS!%Pr>-|x^{Q&yZ zx<4P5Ho|+?!+Ue#eR_CbHoWhbnBLZojh5Z(ExU6qgL=zgwq@{_n++XVPlwu%O=7qU zyk{|Y$dw~?_vz;V?+E3t;gXnQN*o1vDg<}al9(wgHE{^a6|T_@Tf(*^-mt-tvNQLD zE7v94apBdYWFXx~WmcF)H=IlM3|HEv7NH28mFf?Qgtw#CBj+mOU*;x|W)p>kc=Onj z9ZJBK{yQ!_i%I|-iqK7}=qweckE8KKwY)E&j#@)eQjFqDB&a1sui^zk&U^}97qLFi zJWv6|;&H1IC`td%MQ_AIj>%jCJ`H>_!;W_0;4H-FQcNl{xdJ!j8eBG0G^?Z{A}&*^ zJ%_a6WIZ!{Xs9Sb;YiU(!&4Amnnv@=#FfP46-i0Nwu(|R-FY9$RhXdHL9M+F04E~| zzhdR$H?&-BuU^}`CD?1i`C$7y&%gbArq2{F%O@bVg6#k>>V5g97P9O&gRNkX8ZbM+ z^6`9g`#W#G{bnYWYaY~_2bZ78`&u@9UF*KCj3?)d=)OqS7a>U$y!ksMYnn2{w@3Bf zCs;b~53M*q+iYmhdfFM!SXhPZV8$vxgb(l=uuA*GvI@L>zG)NgC9ym&!wZQLv$BUV z5@!XL;~U%(rzm5CN(&RrH^V#)8@v_aGPm+G+=?%@M(-uNmBqle4#vKSO_b5{=!S*1 zf<0NQF4)#CO#(F^M}~U`<8$i#rLuHShkry35$tr za2`9AjLA~0f>9~D%p<)Vl_&mh8UC!gs7Q0O6b__-wiHK?6Lx$af%IfbaqIcGtkFLmwEu zb2Qiaq~7@?44#^2L=%2x5>}9$54GNUWh1m_J+x=_crLVG5A9zb&$o6#Fx3$z!4+q| zfA{Sx8~snO_dk6%l` zP$@g6gkB-V<&_}x3F-iglW_UAy_%QpOOEY4`}2ZqyW#kPLv~CRQdh;Rg4wr-GyP|r zI~cE$tSaKk>)2L<7a5w!9>~pxs7|JXCn#3P@W^r? zqETr);xZgb_*I4j=Rvai3RcXhIdN@9VeXm9(ZsS5r^&4s2tk}xnPwLijf5`aN>gpu zFrK5j({0=HBb9vzMA|l49TS&Y^+K3lv zVdOaO_MB2_JmbU^5en@4GRwef2~b%{gGM=5$#&>DNBRqVCr-iJN8mt}OkbXBlLx4| z2X5XX1Hr&lQ}Bh+$ zvZlD+s5UJv18i4pwox3Jhm&_kkn`%E zo_s@KIb|Azd;w822|KpfgGk`eoD4Rk%!ih#wA@iH7z+#2e43&Od~WzEoE@Kx*em8j zokf*pGog+lzpZIq7NGbCq9McREn%}2(Q<^xckV8XC+q3K?*Rk)EzngBh=#|9&vs#H zP$(-oT=%n63qzNt5KgHcc_G4vV9q!*U?$;=lr{#lQ~2&syV1d_X`%R5y4sv5k1+ic zP_%sjn3amNgTjddpQNI(koC0TXDVQ~l8C()S7eL+<=cRI3!bOBK+1f|W{ghbZ0WCZ zjy%EYB9@frG^$oA2Atp}Vul8u-yh+~JW;Kc3Vj-tm|nq=ocY08+n_Wai(|u~xX>D} zaWY~v`R*9H{E^Mn7c>HmMpiW1Sd^{UK^759_f%;Lio{uP{)MWcI3eT0K6b35d!#J; zRc}BCFd0YvHk!7IPqnRlS~Gkjf`UJl;&X}wFb#q|s=IuXsxWL=Ikm~2RGIq$OfE{T zZp0OatNwjC2GYEik}razYvGz$Dv|AJ#3g5JCH8>9#G?^~POw+iaS{wR^<}iek?wOS zQ+4Q3PNR;L=N@hbnQO=$LUx4Y<(()?e1LSxaD*uc^@G;(*#I{QEO!@7ivY5|g65beG zxkOT$PpS&Pf5dAuB_H$M;INUDGwch=g-Stq9tEZnrSPx%A_0j2X$I5yBVibEn9R(4 zvvX8*5m53MDIDWUVmgu zXhHag)kXy)df+f(J3fEb*P;75GL1Q3kC{XTEn#cKXS1PsOYqh8Zc-4rXTu*^_eWL- za{dE4g30~^pa{00ms^6nu{Yn@qj&Dx=p0_}9A0}d*Eynhj;u_uO7-Re^fNS+ZwXgK zRX5vvRwjXC#*l1vVT7wU{N3yRZuow>zc=geMHp%&wYC2NgWEbJ)YNTwTGu_TIZwOp zp_o(2&M;3l9IsvwqwFdkj=g4~ zVK@p;RO`g@>l>pI-K8t9r*PwBVB+BL(8R$bB`r%_L32JT9R-vIULKUr4@%R6($PWb z*q}6K#boO;D!ssZC9#}@%Zl^!ykk@!&ul9xioReen#1wo;@Qzd0tX2kA}~VWFo7fK z_LB*AtW-TTIEWv)4Dn?{Ja358hIrHvj~U`vy1qzZ4ifUA2g_p`;p{BU%QQF}P*Hf$ zca;LQstQmcJQ@g}-NiB2CM{LTjtYz&SY++Xs0pH@P4Km3yG~|nPga}e&)vC@?RpNs zobQzGJC*gFVkSD+4pN}`V73;gE%4hc=KP^GN4Dz_emUQW?i#HY0<8Q%I%gSO*{#iAOXn)1b6MMy-S4~DEfe75&am#WMQ z*#pL{Nq>*;*(q>s;dYc%oJ>0QNGocRc*8uVtzvAGA$cu4QO($uD>W*OnU}GoLP@FF zS2*{cLFx<``ONgIyy{Yr^d4D=_h^Oh(UROF;i{$q$P+tH8O9m`+Q`M5UokC8LjpoC zA^a|Y;lY5TGA;pYR2Mj(=~wkD$fWzWWs8d1b`xuS2k2mp9m0;DOgF+R`NnR``;&n^ z*}=zi15fA!PuvyP2aaYNpUD<*vWwMpkp;&gE4#r(9wOBfO<9ORjd%7Y{ zx5L%z($zWKpEQY49}XPmV^qZH@BP=We)X$K#7dac%Y-nU&>8iGz)RttAq(IewW(w( z7B{aaGLatfn)fZdWVxBGn7g>ndQqan!DhRioeMC%FpCDu;|{-%{5F>nN^LwrJ6Hi0 zvkRNrZcT55cCUwa=R$*eXb^|idU!|=KbG(6eg93EUhgN&PoK1RzMIm!_UGCk)!QG< z@80{}J=vxwR^(e(GCi5zOt0S5pKa>D^XOKC$B&I^f*No2=A*5{HeUh>tN*yYJ!IW{ z_YYxL;#p6!*+pAhy621n8DZHSaJT8EB`)2vwN@sfRcS#DC&F+_N!$nodR2 zO;uupkxkMx(?u0TF+9GT-VIO7x~C=QX~WqDVikFhuhOtI&p+rEq^a?|xUf5eE3EiG z4;EcVoW&+^90}_gXDOIP<|a-BfLAi$z)75MOQ)EHh)-D*67w^wAY4dCOi~j6Cqtc+ z;a&0G@cqvd)jFVr4dbi23Ta^9(4+hIm25R8>hJv+Bf|6>w#0(rn2FCVm1~=@YFv5&}cwKFNuXeEU5Phe^_wN(n1j{;q^vBs%!fspxI8TjJDO3ZH~I9LQKm> zC;Wa9ru$b$F(nB-HKkV^Da&m7Ntc?`<^}0(e9lUgS7|OuHbJ)33mtvt1>l}SjJ7N5 zpb5!)oSKLFgxNITtjrt99(sNs!9G2}(u%8nYoX!P~PSiDf;jz;C=3~qum<?*B7o{jJYB*j?CgbeHMV0B}MFL|47&p&TY5~A-&LZTUQol$a$O=#$1W1Ug z#NF!G2)qaIk-++G+6#1I<_UydrdRncj0hQ{>Nv{_cm}}qx10E@jIdKKer_ydG|PbRf7Hor}teH;Z+HG z95f?V;Bz~lT0KEScxp{%uS&4Pp7Kt_~Gp+J+vDbPn(Gx`6=VE>ImWiclNJq4Bi z`wH9D>bEG>PoNLrbI;=@OFpT7fGkNgZ4&?|hoZP;bBY)jfqFsozEwkid0|Jk_{$3i wvk&*n3wyHL{qjOb*80l}Ls{!DFFcXm?ziPS>l889+ryu+??1m!DVu@+0by1eApigX diff --git a/gcode/__pycache__/parser.cpython-311.pyc b/gcode/__pycache__/parser.cpython-311.pyc deleted file mode 100644 index eba6e71a46ed1b0f78a5c93bf2e9c920322ed0da..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 12198 zcmb_iYit`=cAnu&;+qsHih6n^*^(&RvK(1*EIW2(z5J3aOLA;0$}%)(Br~Q+Wrp&D zOKr8N7i9u%SJ+KqC2<<=Vw;tl1*#uGV4w>$-L~5;utkUJ0AdFSV4x_lLV<#Tvw&gr zN6)!Kjz~)0#_0|(&)mnk=broi?z!_btJOro^JCLzH*U33)PGGSq(@tsUbW^%H{gi&rFlCr4n<}G`M;9)iGfo+4iZg1cVT#q?qgcbo z8jAV^{>t+)ygtbXs3{ZSD+9jrV!kOetC^sD#`sT6!~GWr2AQz{FK|3F6Xlt)zVV9} zdYJL(^=QmwnvCA$BB3}ZFo6im%p;q?D0#x62>3&hSdOKncOX+RyugMcv&`_pV3g&A1I$R?lPf1M$Hh2a=rCn9iH!cVT#Jp$Wx?p&9LSK(*hT&v3niEHf!ltJ3esw&D9QkSY@H`3 zAof0$px#syKfsX6%hDg{KGdYt!KSi-ht+%xe4nVi&*9P*b1l*!Ki@M{N>?Z`?=K$G z1Jvt1)1bkkW>K4fDUr~Cj#{Fb_BmyKBhBJgD1%-;3i{1z zUm8Z^x{kx0vohWJjP7_wGjT0*obefWGgOAR0FezmOqI|)58^3VABu5v0&fGqPbcdH zE<6JaoUjm%$=aLTU0Hi85Eigi7aq zNbJbPRYBpxXF!&yjNAL}sOUVN8j$uLfw$r|ou@aPr$6sXJBKCb@TPNg!#VnEA?=)y zoD)ey##{HmzUe)<;XRo49+JF=vJ`DRO)K>6Gs*rHSH@BydVH(RNCY5?+w^p9cskcz zX-~K0>E865*zlZqG?Di7NuIv6r7t;@9LkvO%O_CMej=iMb>P7T(bEntZE1&Odu7u4 z1T?37Ho4sIkHGZs`(=~gKNn>e!sxg7{ckMUb{jJ$-#&^bZH%vsc&2wPF`IJF26478o{M?jhV)M zOXpX-tIhA$i@JT_(z<;aw`b{0GN=m6>I`~MmfA@aO;O1p{MDx{=X)ZPB_Y39;!>%h zINkP?D4q6GhLUtxTHnk|qe}Lm@)BO%@8N|Vuc%!8@8LCtU|(d*enhk7tP!Y*H3PM< zR-iW44%ERqfmX0Cpl-GjsE4fr>Se2e*08le>)3jr4eTDE4BH5_iEReDm)!@ng>41e z#`@Uy2b#a6VL#o^9$*i$9qb{tlRf;vFlAw1B8z+%XJe0WcFwWADa`2D?t8i^C)>k0 zAgzLPac<65kY2Fq0Pj)Ytz=*3Je&>WJcnOiR0k$?TwdxYqV{e-S3Ng z+FlzTnS9-cp|(@K(;XOl$`@~Ld+qAAslL5q@y|d^Akv=@!Xi?GHzTH*bdojtTI= zEu69O%KqpAavls8rQzBt>j!Rg;qVqt;CRK=Ys`E&5aAeDjbTxRBDX4(gsB|~E|PF< zXM!6OOlKp(8_Yr^6ce^8F~Ttt3Wq}k*5smbiHO_AIY3SkXlmw0bdKBV!AR$oD1Va) zMx#8SBxrHw@Yc~?<6qi(dDr-^tz)~!AKB{NHNJc6_^$Ciad+Q!AsSu)9SS-jUIn$| zIYb$OaAAT^vo!^Rwq8SN7TB6*0ha&@jLyfcdCaB+D<|ed5!jIq4a0^w58yKzVfwGc zJ-bCiylY4N%uL*zFMx}%u+L)m^X*6uAUOzR3zz9FT#$=E4|3wbc+vy}P&fb)Klqeq z7GRHGg)%G!Get8>g!?qIiD0TA4ky{b@qCmQ9@4xQatfG*A?rSFI3ehH*i_vGGYocB zCH5?`<0ye`wWcHz4drKYgF9Z!NTfphwVo*zu;Q#h@5 zJZuT~y!*MFr7*Er7Zs2d76DL@F5X`j;Z(I~fLb8HB~MD-iSjY{-cgIPL>YfFhRZ?j zNtAuP8kMw$AwfeM61W5W3L>b+<(QnA`?q0h}%(yt}$Wloq_{Y^6F_yY+uYq&|qEZ+k9BSt1q z3K7?ihb;phHTDBI?wb+|{;%8A9ql#hHa$x%T0jeRjDPJ>G4=wZ z!ukgo6Sjn9_a0AJ6SkR>qaF0`rG$n5e!>Fd_ib`av@Y5bR@R#9@s|@e)^>~HkFfT8 zRH8hEJNu5Oc$N*6u95Z_Sak9aYLT!fbO}R3`*X*~u+n@2f8|+pL=Knovd(W>?(@n> zXr9wPj?(^LekT%y<7+w8zbj!8igd$xR=)55pXTJ0}yJ$(fvpfr{W*LCv6g`uJI)R}G;o%-qY22W}Twxr!Ln zfP&jfvZHm1l&uoWt86!V~2MDk?A3VIG*V*U~S@+ zz0BDN!Qadb2WB=NKw&Q<(-ZOP?G!;lRE#hWEWJ#;ZaWE#KpYg)^v2r^i7$*B_POv9v}$nndt-z=031v z%%6<67h))KJRxSRjuQap!9sGfW1IKSK!$J%$Pxv%fqg968dUm_T??!`GcB#+{_A4P z^{f_?QkT|>n3N0zgcX`@h{;Ad`QsKu&igOeN_1f>@gpb&+kn3cq^R=zG{n3}tv^Hw zYOVLqVue)taB}U^I-S|qB6e`%J`QRPNvXcnTZK%DUxA)w{TD|@21h4-ZvKxzu&gIG zE7_il_K)>VP7aQb$_4>WV8I)*ewL3e%!A!z;kxX)aCUUiKYDrO^x(M4sGkeOf;adU z6s85JF6+TO3|31k7_0h5RJQUNI1qqMjOX_OC*Ozch5&Fv4%4eRa0d}@%N8_Q=X3FB zq9P_1pOe6Fet_gtX1r{T@}XHcLWRkQA^j&?bM{fcq872?*l^yXsWcY}Ae!Pilw^Sc z7nt*hxfp0Bo0SlmMv4hm@Xx?ufGsfcS?8j7aP$*aLDnm(;ARV>yZBK~3uwI+IHF

4t8pp*vmEBh~aI&x71m z+ordD!`q(r9so5(??G^qtAogPxPK6TC;oO~Ik9PP+^{#M?ah+CIjMtU;pAk>R8~us zt(%pd81ev4OKRv^F+X@>-DwR z&;J-+=q|sZc+&2m?3pc?SyvNPhbCX||lIs)nw_Hf`CgJhV}H2>Q9QTdM5NQd;9N zLZC&x^&gqvH>djEvpujS&wf#}C#C zwXELrB?aUwBrAg}gTHfA^{omg6rir2>rF!~vdl(9U6es{F6ae(GD$-t# zB7uB`WMwd0K{>1LTYqR><5vxl-dSI&vn2B?v-G4*Zo`oi6isj1J@ zwWNdx7c;f>8SkDS9Lzc>tLq0N?~JVOOIsMp!idB{gW4*y zl+oBoL^WsX>R(vQV>Fx#46HZ*sOxhLJ2b!Y9tQePCOuL|pZ<(j|LZCmD8B$qMWzW@ zpK@juIL7@ceV=-}$TX3I;lF}d)f@rG5Y^0Zi{>q?hSe6P;~mQk0KURFRtF%~7b}C( za!-<;fxDS|<*cC;uouH%!3y%}LRoOED*I52B}6_ejAs!jU)oWI5y~)qgEGuY8LS1$ zz*Zv9ElSwKT1#R)tgR$wH=XS{ou`X*cH~;v%{q%@(7c(gD2dUqE~xVfh|Q}*vF=>{ z+Cr(&L#oLo*F%+s>G>Xtc#33sp#A6w_GeXLe4d;20*J4Uf2JZ}*x-qA8F0K}hbG&m znyh+uLUzK~%aDz860b?omI^yG5vS(EjaLoqirvXgGJ^NEkl-iz4=}{S=bwXnv%oCp zQ}grZq0xrom6s!ea?=fvT)D~yJ$#yNNL>UG5#V;g|4BQLKc~-AG_l{kZbh5V+nVi; zxJA86-KXI^YeVbfLwXcUjEG1cYWZvM;?cxPW?v3AE-9GVcw&}|aCheUlktOvFe2X; ze$r9}usP5EuOLrA@c;C;)NcV|h&9*1tzTL_oYG57r&!ym#1zszLLNn3E+QF2atX;e zk_jO3Muz9!S_r|dCIik=5BGXe+4N%&M_9FBo(Xh9&BZkZnA(XQ;%48@NRA7NO$)!3FW#-qY+l{ zVg3oGO$`#Til1D_^hoizR=&*WWk%qrhzB>Bo+JhF-$Er_mm@bL(c2NI2+J|}WkN6G z(@c&Qo}Fap#O1NEi{q1n1O8z@qW1||8;IOhuE7fuN0dYu5@=9YMmX*c%o_?&wiS=$ zc+K-i`QJh|;kU5OJSB+P;#@wp8r(29W~n;3fzH_7lAT%8No^-Z`$D*OIVSG*l^$ay6!j!lSHrll*h;@GSb zL$nO!gayPV;%Wn;JMi?e(}U^-Hk_@1TtU5z%d^T!4IQGZ12extuXjD-q?4m!*QgTC z)cVC*|I^3Tie*25H!FZQ;|n%7pwM&JG&a|=Uu1ekYj2j)gN(-(2gJIo#hSi#{ik+# zp>y?#mcGaK>SR23cAZzQoQhzH3C04hCQtqc#>0Iorh*T&{>N1K0rw%`|3nS*9WpAQNvWG^o)_PV5}WlR z+^ImGD|nNUQf-SxIZ7u+TMscfoeC-BIPwO(dWS)K# zgu9JK1+r2Hz^whE3+R8X>+q=hP6g(tjoA-?~nX&B(*o~Zk61v$-(5{V|Oi( z$|^7uD7UP+JJ-iDcBh_M(e9Im@q!t^ZuSSce=Vms_s}ZWa>e*-g%gi zmBCDV$7cI08||+=;?nH{Qu_duv~p%Ol&M7xDi7mFPiDoEA@0Ale&o|r@B;UFNO`Bd zfAe;)eH5ct8R9hW7?WpRgt1O8Jy>c2{k`mnjxh~?D++W!Ae+jr=6m*$sUrv794 zU%v8E{}KJ)AJK!q2*l#ziIegP&hO&y1!02GuqSf>^m3yedk!ZA>3+dSD}(Gb%|Jsf>L9t}qEK!f`uH|ow1Vq;G9Qh7%9zKfW z-@q8t+2PTPMx@^CF)MWepc1Ea{b*m#l|Dbmu^cJGc|ix zn;x{Mrq(MytxwmSl4^jnD&wkOdFkC=vEk^WieJ>HUBi-VcI4A{pH{n%}rS&227Z6G;2n` zg)*AKo{+U-z?KWxG2qAroEWHp02tC-=m(P|WNFA&V!%TJb=fKmcu8PSwi*L9xj-!j z>T-d43^e2ddoaM{0*x4G$_1J+u$QtnXDs`&tr!3>pb$}4neCt~H5oIwri{5cV`egz zYOtCh0}y1W(CEC{EyFD0jGjlOnuAmw^+ z78R|1E+HeI3@x9940rxVFvMg7pl_IL$~TH+{368Qap>|mRAsaJZ8RtF7yX58f&c&j diff --git a/gcode/__pycache__/state.cpython-311.pyc b/gcode/__pycache__/state.cpython-311.pyc deleted file mode 100644 index 6fdd0cce9345bdec6cafa785d1d54dab8d94347f..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 14224 zcmd5@Yit`=cAg=J6rUm~Qt#L3Wr?yRTM{M9vg25hY~|s|ab%+j*K01#8QGF4Q9DD$ zHdl7kB5j3*(W*hv2&vPsLEtD4V1=ST+@ffe?vK?^(ikAbphks0un<}lOp^k^qCa}h z9r6xINr}DP1v(m@xsP)nbLZUeoO|xo$4;k>fotR6|N8p90fzY(jN~sc z6J!KdFhtoPOP+?H0iMRFah?lu^QNF_-W)Wu$iqd;<}E?XyftW@w*_r1BU%j1BqNyK zVg&O81H*gJV>4>nwb@&f@%)S0x(DD&_TeQ zi+uel4DY*)$iRnUf`&`*NrFbf5adLYXcmkoflc6oWh7=IF>}yDVr3XJFrrnkyv2NO zeZa!UfuD8-ZK55x@aLE@)aH|!!Z6ftqjlK#sl&OqJd|x-g2cQ*4~h9mtTI?ZVpSwo9rTiNH6&IW^of;Ft}e$}ML6q8tRYw})<9lk zE>=roO}SVdi8Yg$H&{=~@gz1Zw2BR)sc@Wxwzo{dM&X#y@fH_s6598^*Nj0p1bNLu zr)UzoM2FBWw9J?VKcw+O_g?;e>)?dLz~3VDJTZSS@V5$mPt1P=_}he|`|(46IiVl& z+u=P2;MoDsL3kd5X8@j^*k^dpAqaN~Eim0jRNG_(rWl#*34hUq)}}C-*7;dY=d)^< zJgVB~V>nwvI7d|@Oi|Uns88K|@=!o^-Hb^$LJ>Oai>8hPq}d3+udf3EtV4v${~7L^ zX_z42Zy;{GZ-y7=rX&QyPs}R&n##VevO$#{Q`zTKcKj2w)ur*#%a>$Pk}rp&Z-k}G6QX=09$UQpiWHj@BXR!hY*dsl6WHvH#OCJ# zphT%xj?Khxh9&Xx^I{mFN|rBQ!>@3e40hk*?O$0=M#J-0h486_SN}eJ`NRfjS^hmB zD@>-cYW+g;!p4Qo3lA@(T+hP&TTaS4%g(drEQ54~k$C8apH*FgI1^rq#zQmVNIWLp zRvqM<$7W_^F)kN<@G>YtKDZA8dq4Oz^NCUWI8p^Lkg?VXkI@z+tw8o2nM08Abt4cM z8E5(0h5XoLjfUs3(@1ljR^5>$NdmR;6%1C5HItI=5G zhU$#v^kYZ{Ay=KG53x5yNtzYJDb+^gH#7q>A2DeCCzMX{{{oGMUwoMf(QLs0F{l)7xk~GYJnlR4@@5(%lmTbn24DmW#L8$nE5s}`W`mfO z#_SLyW+0{r9K`H24>@t<$R#)+<}8Yp7sU{=$R%KV?tE++$J(REq@j3wKE^O}-h4QT zlX1_r%of`rQq3}xtlYx}2@l|>8V=B8|7aK$FKA882{q*`6*HXv0WDdrDws~ z1XGqU)8Z12xrBpDfS=424SUdnkjSj<#n2HX{YVCZoSpJlN%(;z{1OrlxrAdYbs_0S z(u1TA$x$R=<}*?N$qauFj+U#z^`(Or6$97;IlE)+>XzN zo!YvjXT!7UdFa{lq`c#BXMVd$pi>%cpi&xb*nwK;T4&crcr*MkycOOKr}$Uk*1CA8 zbN}?px>fF;EmPlwY&Dr$c-|||G?Evr%j9z#wA@zmzRmgf%`~)-W;2bg_^iu6vu36q zIPbYK_R1{7TASW)z2Esk-+O%;u8dLc@YXp9huM-XR z&5{^jk`{^p5&?*)#Qz}xxy;T%f_4e)4FZSKTAacZ!vF^b$>J$`$FOV=3~w-!17Ozh zXUzKuue794G0i}}u_T|Pt!epOu1&XKg3>z7Az11#r}36iUW7fJ7h$NB*H+T54w713 z_FP?70(UxSYPn9DTMl&><&yU*95q*tx2kZYmYF%)qXhI6YSEz>+bFETovWd|xP}6_ zE-m57m8gHRF9ogZU=MZSJNUhcA5&bqvost6W$~hjX|YkKGU#C@&4{ zMMlCn$`d>CA{OFfSJ5`5MO1du&k@wyMLoQWlC_H><2UZ2>exj)Y!_9*5MBZQJupSxT3Y3Kg zm7XOsz#-H#A;OhtD6qEo~1|4;+Za>n7g_w0J)k-a6$bXtcWd#l!#l{iUkv~?C#k?~Zn69z{C!=VBOQb($`KjrBMcm<};{*6E~kxU>@ zdy3oz?HGZhs~7V+Qskx$u%$TwaLzu!HXvrE+?aFkvnUG zT(IP=!vs$wD6;trB^N*O*N1?-3-epYQ3z8`-7x?&j!m>qig7--7~pZ)!s|;_UbBq- z22Hi2uzkW3!IH7MoFuufiSbauf+tZ|AV(#6??EvcC({bE<6CCYa{_!+tq@zUu=RW| z_S$+ZfLHG4HzYoyz%mWvy8n0*gDebS25(B)X<#pgA>SxJ29#e2&x055wI2TU9zNK^ zkM;1+_3-04 zenf!&4g&8ovkX`ZOcKI-uLqE%8}xUK0z1L{aQqIp%q_DZRL_9$ar!)3HYVxpDy(an z6AZ;(=LBSfKypOfIKi0HnU$D!%W$i}xmhTT39!y1#)tt+-J4iOPc@hLNBT9#V+*hd z-6L!=E&_6k?1DM{L=smpgrht@mMHVrUtpH~R?9&leCg?Uw;VHb!|*2J>zNLxR#Lib$z?zYD`%giIhtV(69e@(rx>-q`P__>Ha4wNqOnsM_f?A z@tZxZbQjA@setd(5bX>K5{*28j33y$5&@T@@Hz8Bjh~iCwiLqc50Qu`RPd z)|MbP><|KCkKQpZ8wp|u0I`h)h+UxSaV-SM&dgDDFBo#TYzDkHER&6^d!0pQL}6WA z4%Icj69k`)VH2nXDk|gQXk;mh>zR010ubZ#URR#3Z1O4jeLo&|^Yqc7Rnfp+J3;3z zm1(pfG)q9%97|+-gI4vlVqNkQGMX3PBkJg20;>bPL%14QysdeL$}u?y<}&7)wYm$8 zj}ip01y8UxmLZLmI*=3X1$O6wTwg*f^6vs6s9sT}cn*E(IsC|TIPK|GJiR$gFSAa< zWI(D-hD6E*OC7UwlFifrNGmm68v*6;iFD10HPeo_TJd&moO$GhWrooMENxwDXV(LN za&GP1PFttaHuBJ?w4K_jOt+oY(qRfzt~#|D;8+lE(pM;&11OJV4y;4qN#;PQ!;%G) zHc3rdsDxgnoM;K@^1uHKTQ(H?T5!i5ED=NECsdAdYxewVA}`W3p9c2IZs915@>AkX zeTsaSaG@njWXh~9SfxdHN8*0Wt$?LlkN}%?`es9kHJe-L=KETNq$PR(302820Rhn| zXRJ=}8h&YMe`IM-UQSyE6w5%$GO$xs_hnW0qpI$Wlj*7vrD_D=*5c4Pe~`BHE0+FL z{w6|Dw2VWy5c%xiK;Y^23&5$OgBUum5Jt%*pz{i6k`{Y%k3DkgZX)!`gObnHf-&8R zgDs##m|TLj;LlF>84v*u%0n2?@*E&i&P4UABCI5G&L7b7x&1Y{)LDHP3Q7BTr-?u3 zEK#ykepjiVT6dr5PLl&2)1%k~ikRlqjzFkz@oSQ>rwMlf^pKQaA>Ri;V4+*zta)Bb zJk8WG`#h`NMBWrXfcSP!&s_{ z>y`mCNB5G@mHfG}cmc;U@fKQDk&8v(iaCdvJH~jM&O3*GL~@}iBA7lmKY&sG3`d$= z0H}p%c&PynELuGGDlTZZG%y7C0_K>FhUb6Y}Yt}A3gB>^p zL@g6$C3I@2mih1v5hAef6I&D)RBld=0Tq_!7XjnPkT{kwquz?-O8b7 zGRSL?VXI$0jq8NJYu6W13{`^Kfw0%Gyve5A18Mhw;vRqix*i7!3n5=2^EXKJv;#xme*5i#d0K`a#{&mNMsfXz zrkZi1@#jWvysYf!HWp|RQc#D=KZ6t(BgGmk{cD_}1I$nwPEv`72IvLzn}&;LDAe?Y zm5EbSD4xv%TRdN2*0FJl6wt1YI3>HecVKqoUvYVsS+MUv+m;PUideL(5Wqcq9lwfe z1C6L7+d<`V=|SIaPkEYiVEdrwb)4K&UPsV_+BcVCPN7ymuvOUZnnM^N*Pd2s&Vl7& zgG@*5J>@B$A6Oo2k2#9dx1w-+n6|FcyH+oHvD3^j^ixMREvr!_&RkkC!`CZp*aEe-UHH&J^IHsN!6h8$3!nnq=%seQEp zIWh443P*YwdMHgHL3=JycSe%n$Zh^KgtiTc$00^}ICy|AYh8&*&tmlVk^BJ=mBUif zJjUCR^dNZ}?MK$8s`bpRh)9n7z&Ro~*Bj62A?c@B-zpH56-#u_X-F;7J^HDIPSic! zfP*>R^A%HV9eH26fAfR8@7>)HK98lFPbtl(NOIOtGn%X`z72EYk~c_53am-Wn*&l;$H>Rji;5y(_}-gv;X6}AKpz3Pj35^7k-%T zyrOhoA-ijt?!eFbKJ7~#e|cL@cTX$b)0o@|TY(>+`ta0d@0L#)yO3^sRcU(_Gx$u` zz|YD*El(YLVLPC_^rLiFSm_F5PRBt_w;cYs>BFW?`@@TWdnMg+PH8y@Ju1(@8(jJD z%I5j4i^`ebNq4-abi9Vuv}U{=8+9o!s!7jFY^G-zno@dRT$@mQ{_n~I>!9c8=c5lR zA00lGYMV?3l7X#p(q(NU^V`SX2B_0HqLat&%2d_xWMvwo`DkqK17hL67hw+PA2ZlSiF@MJlM%!e!@fXgD z$qwT$I!q8QQr8HosC>WP0aC240n9);wF|;jSwonrYY3yAMJ@H!Oo@zAYnM7ukU8ynSM-Pl6gs7j7f3uz4;yc zt`>C@Szjn3&}xf9a*XLXME%aEE4{S03Mm3DTt@}rOV(KLTj;ZJG>i&D@E9@L6v zYn^U5s-zLCjW{p->=bMek3;iQem|bTLaRI!Qf(nT1ib{uCqt?u6#DT}IGW2T3x$MO zBorcJr5fe9q`E?(@WMh2J_hVG$&iH_T*GX%x-=X|QH1|P07#7&tjGzv_aW1MAL)mf zLC>10+*LRPT6*FP#aem}Nf?QMWEKgIsU#t}iDVhcT_o3$oJR6C5o5BT$;!F4_BDBZBI!+v$vUN>Cso~}RP;g#QyC$^ z3LziUePja;bs0{RQ^6YThI$O$P!AC7hp;}FG_8*+Rfm8Pivs};2-*Ix#Ve{;&aA>9 zRKcC*sxq}W5S1ye0q!){fJ4)`^1|v8O@WhbD=)7NrMM=z(_9m9l+=M`YPI34NpUr4 z4$k$pLG|mV6bH|?G}i_!jjb@87wG`QK{*^TT5YDT8JY>CxMsN1Tyv(L*P6`@f0k=E z!Lc?V$&romkDejHQgV*%GrH94MT;DKKm z9uM2eqmvlp9@99gE4Z%Vb-JZo%U~ zqD@PjjDYNTa)g+O#CC+ehTHJpQr9HJ6^VcVDmB$k|LIATC0svB1cEFWB2&9~y2HOK zXgGIDdLLL&kIEkd0maU;S%a13vJ8?chIOpi@h`(zSIA$6=}H}ZXPBd@gKrRLgFS1w a%dznC`_r#T`Zt7rWd!!OYWEf(1N6TtD4du8 diff --git a/gcode/__pycache__/utils.cpython-311.pyc b/gcode/__pycache__/utils.cpython-311.pyc deleted file mode 100644 index 1f80e07a8025525e41b525f40a0fc25258fed67f..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 12222 zcmcgyeQX;?cHbp;DN-aw%A~9h%hvisvJ}a(=x^)n#IkJpJBcIv>ge1P8171@%@1Xl zl5K{%Drs8Cfr~JQ6k#qo98p|z3VbMtq6GxBMV$i*q<>Wm2pqeGQ5Wsukbeptask`` z1^RolDs_JWDMTR;1R55EUoHsBOX)ZJ20?+9&KhCku*07V)zwjwzem@TM@) z=-?cj{0u8?M2)jlYnNSbiWAOKtsAAawN0pX$sR>iP&Z|1FLTGq-gU~1{+iKW6ME}k zx1DdDc7DYTUhDdpvdb;#-@~{YSeM#*T(n{oZHiakh^k9&2i4{cIb4rsM?H5X`rfHV zc?;-m0=+#?%_GguR`5s?Ym~R4y-)69Qm`rRij=1WS%NhE@}Rr}Ep{p`XyL}3?m~;r z=yC5;_t=B>Hf0?tSM_F>cY|&#>x(!O;Yc`fSDKoQ zg%aU-Op~VKs&r=bg;S@c88seKG%XyPZftx}jn9Q;MUz7D*qowjK&`YHkV3&oXf_gL z6b7VVOqOmaks07skqbv>B1%+=B^cUZkSbgINVkxHn+IVB>6V^TC~qMAb*md4C}8Oq*hIA)@;%u8&{RsEbQGlvG6 zf{BqehNzM#en~@zd{2DL^$BVNvZ@_GZ!)`_j7BFl$hSmA6Hra;e|2?q`1(~1lXpEB znG34dPbu21M11Btbl|2EN=Rejh@xG8apa{JF764%qftz^qVCb+Q;FL_Rk?mv3Ccjw zu1|;JvT}VE8m0}-+|^quJtyLm~F?E_fgRyT9@=Jd!L-&^H)vAO8+F21^I z<2H9?{rQ&eh3Ap?wq*wMp5BGCkDb1S7rsEN@?gJv!Wgym(-XP_@|)CVAXH?7cZ_U^ z6r}vvkYNflh0J?gf+>qpdDF~8S@@LYBj7zS+s@bGno4NkPD9;f%k*^b0Si-^y(K?w{7$yU?DrSoE>-ENz!LW2G91BNhqmt2ssbW&EtV{)GBMB+6Z%8^W z?Sppi)%sYMqztC$Kp}YN>nZF%G(;p04XvTUh}CKDIL%GfP^I3XJ%OPiqid$ZpUaqy zkFB~THlqunNO(rKg<}atoqE`{^nBL(k~g>zrpxVDaW?`GSVUS4*r44iy;`BUq|v-#Fb zh1N?eg8#8|%Zji?Z3Y3g6X3T|HBy|XRW(D&NI5rBwdDUW!I$;SnD~l>$R?3gn5|=& z4Akn`T1gE{<6k#zOk>LHZbO}8{XCs=nW!vNr$b|9{tdHg+DxWCYAek}8vyi$rlg`T zOnug>$l9@zrkHA@c7eb!e%iADOhup#w^zFM=9+V(`PL(a)+5jYC1^)q!@iY67xUgr1@EPWbB~>VATcG;MYuIJp<9)hT;s49nT;1h*p zo(X)kf3Gp0y;sfJla=(=qvJa9N`|#V{Rj#oaQU=(FnlHJyK1=O;cVyWMBe zg=jEwLl?AH)dciJLZP~y0J%(EfX&m^D9WaCu%k?tJ-tdX$7TGq>j2D&d7Br`7QJ1o zoJBm$z@kuedeR3MkHfJ!4ri`p$1~qy*`mvz@nx=j*q*(*bl}I|$+r*XF6Y}0E0YB5X5s$9g7kSht36-qOXHNHC_<7#jgBFa_F%5h4)cvM8v3 zK0?t0vlz++zjcfGGWC)S#e#zEKwDESOjKlKt1Ns3+y@MwYAC#_C}NUItg@BNsVsg2 zE!#S@#5!6wq80l~aM|#C;nMyMTsqd_(q6}kDI#8(e9T;@>9maM9V6 z-nlqz*ols=ANBp9FFU%_obTu_bo4KUayx%|Bj0goaV)+6-i6-;_es&wlc{dMx#*}QOxS;Z^Z76#?;tfo6ck$C9V?Jzb)@xnBj<)SQ>^BbAelqlP3&EkB+hPbvmTrBQvnb(?h9lDMUaqH@3%mAQw`4ZdS~^8#EA6q?(MBsMJOe^PZGP zZb*>#EIrshLCqhSjwFdWxg}7g+LCaTvA~@@W%+=U9VyQT+z*A?HgE7X6g{=IFt&`0 z>W8$eXWS%S>+Ht44Mqp&!bXP?kfcgugk);_oA;)goLtJoq}iNumvWn6_H89jtSPUP z1D;hY=bnnQd0)zz@}`9 zZ7$hi>54jQG(59!KpG21w9?ytv&(fHgIUKizlHt(*kYLfT@be#04Nu98+levd1G_Y zgIv;CHRfa+++|$SqQOXn4Z9p1O4EP$4?q9ce^33}@jjb+kO=H1&tYksvbaTpT8;$Jwmug2{8X6Qa~EeS+Y;J+zIHGyZGHf{w{veJNVtzEizL1uFlIk ze?zy-;BDrvdI(fB+B4Ivt$mDrve%fvDqr4TZ=c}Hi#(YbIEjp~bBZ@0eHnr_MPc}GXV(Lq>s zMWE(%FRJ%;r!U{z3XDcq<%N23ExvNEm0CKD%p<$&z9YRO9ek%dvm+CHzx$m%dAn4w zOA8}Fw>uUCf2O4m-Fxl+Yxkb}tf7O>o-Xo_n%Y*{&u8ZV@c-~tBh5EmC^TJIv0uPm z&+c3dygv8loFNci>@0fxZztbM-b>w2t=PBxg7w;y+5S#n=GAQT`&#z$`#18Ndh+(& z1^aH+HAdlEe6`GL@ZRQylf+O{ntvlnTzHD^rp1$se9`Hzeg#f-$WZie&NOFUdVljf z7Z%1A559h>uAt&3nHq$c{l&D>LuV;$!*Iaxi@23A{h#3%g?SNv5dpnfZumtiFZ)I0 zH`rftS<`>8{z?FTc8#}8S)LY??QIQPy%F^~U%VmpI?M|7b^M!cgHiHN%ln3W)XAg5 z2fY3IGOXi$L+W*y73%AZvetv!jQF6|3(9uG*TDxll0{~AVaUQV9i5Z^nF*@FsDh2d z@5%2N!L;FbjJ)Xsesll8d`@_ZF&@@va}t&rIkg|^s(yx^W2>c*ZusK)SQj`8N z4(lSZ^6+G1d~5HnAWlew9jLLZA;e4)ii|TkS{dp%fbL`+8YlVes7|w?*jP2jn(-?o zIZhhFnC}Zr*dW;5oWtKWZG!*{5(jHiIL@P?bt>#2izwV<>%(o95V#f0kk_Y7%j_}lHfV-}%z|{h1JTU2KEpRGTn>>>Y z)V~&!=kAZ^x&&8ZOxd5jLw=H+9FLSqfo_m?&@fYjc$t7sIO3yCbieN#} z?3Nx`ND0{Jt>KE=%hq|}dqT`^q__FooIyKiDl|km~N_xzR{r4i_WWG@!iiSJ6J3) zskjKGBkT&@VX4oi3-K8xM#ujuZnPSi<=1Y9aRzM*&Y%YOm8b876n6iO<6RX8>wTj7 z2auXBvaWP~N*5@n^NL2hU1C%!#qV&snlj05UMFdUssaRjP{L_V08n^8=k_d|UbWc8 zp-1lKbZ=%f@0JQ~X_XTlyR#$3w)Ry{aPMXyEj(`B@?P@$$#+ukrqV*u*NPald3RR$ ziRUjoIcr`zP>>F=qN2b3y^G(!h)Y_3Pr=`_;_rE~>L4;!P-UbU_!V{XYXX(B^>R<1 zJXyb(%zFz3!b27C$nIWg8e9qh;6HEQTd?n4srv`XC#zO8q}0Z_nino=&m*w|2o@}>%a(vTCzx8K+5&6$)_2?pe^DMqt zkd+Y3c0{YW%rPCy6j13bUglFg4mm78w0WL~A zL8*+6XYsX&iOMo{E63m|fm=sXf2<8sd@w{`N8m#g6eJZjZczD(A5>q#Fm-;YkH^^_ zTQGfAbsA;r4XSWpP#7Y}X*3n)D)1TQrDb3UK#dUwErbzd*cFYxeZ{*wa|FQr#}%Eo z{pVikGq05O<-OenZ}-o=1D|;Z;1axnf;X^m4m+{(9o{cN3D2$*`X+Wyu1wSCEt3%? zHVyk8mtp8&_$o+7BDeTT=h-E?o|g630m}}=cd^UwV`07RwruT z<^^~jvF6Z?$2N>-8vuBX2d3wM-?=bvOWBM7XTBlK&D+^E=644jcKJ_nKPTbeP9j_7uA>x!<1>*FUyP*=zZSC%Tvtq2t!%7fiv%XYlos6p2G| zSnPp&U>I_C1_SNXj0rK+iQ(7kIYNc zBnjRmp?(jD0dTAR7c#gl@8G;UvfWF*yk}3rvj?BMo* zD+4$uak%_RbadUpZQ- z3HfL#u_aF*HyNg%6rW=K>8R1yy1}?#%v(a!Qf&i7a%fFmr&(RYqHT{@p7EhzO zFL`;F(Tq;%qEYn?RE(qc3&LLD9y`711NV-t2(2t2{YIW(GHQ9M;)ED8`cfA_objW+ zj$92l_>K&ml<@upwb#In`p49OK8Haxzlrib%JXVJE4pH&{1{5eHzJ8s(xEp@PRjAn zZ4<^`-fql)xzSq#L@EOKdg09?j z;gxtiqEh6~_RwrIzyc(7j>^bGFwJEmRcR!~h-nl|*}t+Mi^k>Ii1Ms@A7wNp+Fyem z1QtBMY7uw=aT9=x<6R3*`YUpqR?7cHZgAzB{#R{&-k&|a$|1=qEO{uhWGxut4h|O& z46llQzLV-Ym#(s;I1pI1;`K37s(bw0sR|CmFhEs%L{xshCC7SgDYb7YwQni4H*grD z0S3K%S4Ph6UlOy!g>8eY9Nu$RmUk}CExjQ#e7&^h;i)~vNac!M z(I%KVmGxznY)7GUz$nNaShg--U2ZJwA2kY!o!eKfgjyWjXJj64*=CS<+`l(x&0Wnk z7W%(cP1I;uf@dwjh@9X#w1Kt{-nXS*{Kg|=SyzLdy~F1O^)7xq5K-ivLW6$ENk z3>;vsizBBiR2ZrOF7n*55q{O$%HtXsAS-0Ym%6hP1qm@Q-gAlN(T6R|=L?6=u=nE5 zUWQye04brqc=~*$F~cyxffl|wqpfmCvWcb9AKzu!wP2`;$IJs{W?7Q6vSb;;E7w%z zc5&3HIk5@cRkvi%7rOQu6Du$8e<&^w7Y>aX6Wc#zOzg1}5B(2ApB{X8voLbGaO|ZD eiK^K>#asBc4AugYEc6P=TG0577-lPGnf?#%+e~Bt From f03f4e974273ef2abbc1372a2de238a194a4bfe3 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Sun, 7 Sep 2025 13:40:03 -0400 Subject: [PATCH 15/16] reimplemented streaming and cartesian commands on latest dev --- commands/cartesian_commands.py | 5 +- headless_commander.py | 201 +++++++++++---------------------- 2 files changed, 73 insertions(+), 133 deletions(-) diff --git a/commands/cartesian_commands.py b/commands/cartesian_commands.py index bc90aa8..a4487aa 100644 --- a/commands/cartesian_commands.py +++ b/commands/cartesian_commands.py @@ -16,6 +16,9 @@ # Set interval - used for timing calculations INTERVAL_S = 0.01 +# Jogging uses a smaller IK iteration limit for more responsive performance +JOG_IK_ILIMIT = 20 + class CartesianJogCommand: """ A non-blocking command to jog the robot's end-effector in Cartesian space. @@ -87,7 +90,7 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): 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) + var = solve_ik_with_adaptive_tol_subdivision(PAROL6_ROBOT.robot, target_pose, q_current, ilimit=JOG_IK_ILIMIT, jogging=True) if var.success: q_velocities = (var.q - q_current) / INTERVAL_S diff --git a/headless_commander.py b/headless_commander.py index aad4362..541e67e 100644 --- a/headless_commander.py +++ b/headless_commander.py @@ -21,7 +21,6 @@ import re import logging import struct -from enum import Enum import keyboard import argparse import sys @@ -64,17 +63,6 @@ # Streaming toggle: STREAM|ON enables zero-queue latest-wins for JOG/CARTJOG; STREAM|OFF disables stream_mode = False -class JogFrame(str, Enum): - WRF = "WRF" - TRF = "TRF" - -# Jacobian damping -JACOBIAN_DAMPING_LAMBDA = float(os.getenv("PAROL6_JAC_LAMBDA", "1e-6")) - -# Simplified streaming state - just track when to stop current streaming motion -streaming_timeout_time = 0.0 -streaming_active = False - # Global verbosity level - can be changed programmatically GLOBAL_LOG_LEVEL = logging.INFO @@ -202,8 +190,8 @@ def parse_arguments(): except (FileNotFoundError, serial.SerialException): # Fallback to user input for COM port while True: - com_port = input("Enter the COM port (e.g., COM9): ") try: + com_port = input("Enter the COM port (e.g., COM9): ") ser = serial.Serial(port=com_port, baudrate=3000000, timeout=0) logger.info(f"Successfully connected to {com_port}") # Cache successful port for future runs @@ -686,66 +674,6 @@ def simulate_robot_step(dt: float) -> None: # Idle/other: hold position for i in range(6): Speed_in[i] = 0 - -# ========================= -# Jog helpers -# ========================= -def compute_twist(frame: str, axis: str, speed_percent: float, current_T: SE3): - """ - Compute tool twist (vx,vy,vz,wx,wy,wx) in BASE/WRF frame. - Linear units m/s, angular units rad/s. - """ - # Map speed% to linear/rotational speeds using existing caps - linear_speed_ms = float(np.interp(speed_percent, [0, 100], [PAROL6_ROBOT.Cartesian_linear_velocity_min_JOG, PAROL6_ROBOT.Cartesian_linear_velocity_max_JOG])) - angular_speed_rads = np.deg2rad(float(np.interp(speed_percent, [0, 100], [PAROL6_ROBOT.Cartesian_angular_velocity_min, PAROL6_ROBOT.Cartesian_angular_velocity_max]))) - - lin_axis, rot_axis = AXIS_MAP.get(axis, ([0,0,0],[0,0,0])) - - v_lin = np.array(lin_axis, dtype=float) * linear_speed_ms # m/s - v_rot = np.array(rot_axis, dtype=float) * angular_speed_rads # rad/s - - if frame.upper() == JogFrame.TRF.value: - # Transform tool-frame twist directions to base - R = current_T.R - v_lin = R @ v_lin - v_rot = R @ v_rot - - return v_lin, v_rot - - -def jacobian_qdot(robot: DHRobot, q: np.ndarray, v_lin: np.ndarray, v_rot: np.ndarray, lambda_: float) -> np.ndarray: - """ - Damped least-squares inverse: qdot = J^T (J J^T + λ^2 I)^{-1} v - v is 6x1 [vx vy vz wx wy wz]^T with units [m/s, rad/s] - """ - J = robot.jacob0(q) # 6x6 - v = np.hstack([v_lin, v_rot]).reshape(6, ) - JJt = J @ J.T - damp = (lambda_ ** 2) * np.eye(6) - qdot = J.T @ np.linalg.solve(JJt + damp, v) - return qdot - - -def apply_streaming_velocities(qdot: np.ndarray, Speed_out: list) -> None: - """ - Simple velocity scaling: convert qdot to steps/s and scale proportionally if any exceed limits. - """ - # Convert rad/s to steps/s - speeds_steps = np.array([PAROL6_ROBOT.SPEED_RAD2STEP(qdot[i], i) for i in range(6)]) - - # Find max ratio of speed to limit across all joints - max_ratios = np.array([abs(speeds_steps[i]) / PAROL6_ROBOT.Joint_max_speed[i] for i in range(6)]) - max_ratio = np.max(max_ratios) - - # Scale all velocities proportionally if any exceed limits - if max_ratio > 1.0: - speeds_steps = speeds_steps / max_ratio - - # Convert to int and apply - for i in range(6): - Speed_out[i] = int(speeds_steps[i]) - - def calculate_duration_from_speed(trajectory_length: float, speed_percentage: float) -> float: """ @@ -1340,6 +1268,7 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: com_port_str = None if com_port_str: + logging.info(f"Trying: {com_port_str}") ser = serial.Serial(port=com_port_str, baudrate=3000000, timeout=0) if ser.is_open: logging.info(f"Successfully reconnected to {com_port_str}") @@ -1558,8 +1487,6 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: send_acknowledgment(cmd_id, "COMPLETED", "Stream mode ON", addr) elif arg == 'OFF': stream_mode = False - streaming_active = False - streaming_timeout_time = 0.0 if cmd_id: send_acknowledgment(cmd_id, "COMPLETED", "Stream mode OFF", addr) else: @@ -1567,61 +1494,84 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: send_acknowledgment(cmd_id, "FAILED", "Expected ON or OFF", addr) elif command_name == 'JOG' and stream_mode and len(parts) == 5: - # Direct streaming: compute velocities immediately for joint jog + # Streaming JOG: create JogCommand instance with latest-wins semantics try: joint_index = int(parts[1]) speed_percent = float(parts[2]) - timeout_s = float(parts[3]) if parts[3].upper() != 'NONE' else 0.2 + duration = None if parts[3].upper() == 'NONE' else float(parts[3]) + distance = None if parts[4].upper() == 'NONE' else float(parts[4]) + + # Use default duration if none specified + if duration is None: + duration = 0.2 + + # Create command instance + command_obj = JogCommand(joint=joint_index, speed_percentage=speed_percent, + duration=duration, distance_deg=distance) + command_obj.prepare_for_execution(current_position_in=Position_in) + # Cancel any active jog-type command (latest-wins) + if active_command and isinstance(active_command, (JogCommand, CartesianJogCommand, MultiJogCommand)): + if active_command_id: + send_acknowledgment(active_command_id, "CANCELLED", "Replaced by new jog command", addr) + if active_command in command_id_map: + del command_id_map[active_command] + + # Purge queued jog commands + for queued_cmd in list(command_queue): + if isinstance(queued_cmd, (JogCommand, CartesianJogCommand, MultiJogCommand)): + command_queue.remove(queued_cmd) + if queued_cmd in command_id_map: + qid, qaddr = command_id_map[queued_cmd] + send_acknowledgment(qid, "CANCELLED", "Replaced by streaming jog", qaddr) + del command_id_map[queued_cmd] + + # Activate command immediately + active_command = command_obj + active_command_id = cmd_id + if cmd_id: + command_id_map[command_obj] = (cmd_id, addr) + send_acknowledgment(cmd_id, "EXECUTING", "Streaming jog started", addr) - # Immediate joint velocity computation - direction = 1 if 0 <= joint_index <= 5 else -1 - j = joint_index if direction == 1 else joint_index - 6 - if 0 <= j < 6: - max_cap = int(PAROL6_ROBOT.Joint_max_jog_speed[j] * JOG_SPEED_SCALE) - abs_max = int(PAROL6_ROBOT.Joint_max_speed[j]) - max_joint_speed = min(abs_max, max_cap) - speed_steps_per_sec = int(np.interp(speed_percent, [0, 100], [0, max_joint_speed])) * direction - - Speed_out[:] = [0] * 6 - Speed_out[j] = speed_steps_per_sec - Command_out.value = 123 - streaming_timeout_time = time.time() + timeout_s - streaming_active = True - - if cmd_id: - send_acknowledgment(cmd_id, "EXECUTING", "", addr) - else: - if cmd_id: - send_acknowledgment(cmd_id, "FAILED", "Invalid joint index", addr) except Exception as e: logging.error(e) if cmd_id: send_acknowledgment(cmd_id, "FAILED", "Malformed JOG (stream)", addr) elif command_name == 'CARTJOG' and stream_mode and len(parts) == 5: - # Direct streaming: compute Jacobian velocities immediately + # Streaming CARTJOG: create CartesianJogCommand instance with latest-wins semantics try: frame = parts[1].upper() axis = parts[2] speed_percent = float(parts[3]) timeout_s = float(parts[4]) if parts[4].upper() != 'NONE' else 0.2 - # Immediate Jacobian computation - 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 isinstance(T_current, SE3) and axis in AXIS_MAP: - v_lin, v_rot = compute_twist(frame, axis, speed_percent, T_current) - qdot = jacobian_qdot(PAROL6_ROBOT.robot, q_current, v_lin, v_rot, JACOBIAN_DAMPING_LAMBDA) - apply_streaming_velocities(qdot, Speed_out) - Command_out.value = 123 - streaming_timeout_time = time.time() + timeout_s - streaming_active = True - - if cmd_id: - send_acknowledgment(cmd_id, "EXECUTING", "", addr) - else: - if cmd_id: - send_acknowledgment(cmd_id, "FAILED", "Invalid frame/axis or no pose", addr) + # Create command instance + command_obj = CartesianJogCommand(frame=frame, axis=axis, + speed_percentage=speed_percent, duration=timeout_s) + + # Cancel any active jog-type command (latest-wins) + if active_command and isinstance(active_command, (JogCommand, CartesianJogCommand, MultiJogCommand)): + if active_command_id: + send_acknowledgment(active_command_id, "CANCELLED", "Replaced by new cartesian jog", addr) + if active_command in command_id_map: + del command_id_map[active_command] + + # Purge queued jog commands + for queued_cmd in list(command_queue): + if isinstance(queued_cmd, (JogCommand, CartesianJogCommand, MultiJogCommand)): + command_queue.remove(queued_cmd) + if queued_cmd in command_id_map: + qid, qaddr = command_id_map[queued_cmd] + send_acknowledgment(qid, "CANCELLED", "Replaced by streaming cartesian jog", qaddr) + del command_id_map[queued_cmd] + + # Activate command immediately + active_command = command_obj + active_command_id = cmd_id + if cmd_id: + command_id_map[command_obj] = (cmd_id, addr) + send_acknowledgment(cmd_id, "EXECUTING", "Streaming cartesian jog started", addr) + except Exception as e: logging.error(e) if cmd_id: @@ -1869,12 +1819,7 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: send_acknowledgment(cmd_id, "INVALID", "Command failed validation", addr) else: - # Add to queue (purge existing jogs first to avoid backlog) - if isinstance(command_obj, (JogCommand, CartesianJogCommand, MultiJogCommand)): - filtered = deque(c for c in command_queue if not isinstance(c, (JogCommand, CartesianJogCommand, MultiJogCommand))) - command_queue.clear() - command_queue.extend(filtered) - + # Add to queue command_queue.append(command_obj) if cmd_id: command_id_map[command_obj] = (cmd_id, addr) @@ -2076,17 +2021,9 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: active_command_id = None else: - # No active command - check streaming timeout, else idle - if streaming_active and time.time() > streaming_timeout_time: - # Streaming timeout: stop motion - streaming_active = False - Speed_out[:] = [0] * 6 - Speed_in[:] = [0] * 6 # Also zero feedback speeds for test consistency - Command_out.value = 255 - elif not streaming_active: - # No streaming, no command: idle - Command_out.value = 255 - Speed_out[:] = [0] * 6 + # No active command - idle + Command_out.value = 255 + Speed_out[:] = [0] * 6 Position_out[:] = Position_in[:] # --- Communication with Robot --- @@ -2117,4 +2054,4 @@ def parse_command_with_id(message: str) -> Tuple[Optional[str], str]: active_command = None active_command_id = None - timer.checkpt() + timer.checkpt() \ No newline at end of file From b11c3faa7f29a9902312a2094f8a6bf9a630b62a Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Sun, 7 Sep 2025 14:15:05 -0400 Subject: [PATCH 16/16] restore jogging to original gui implementation --- commands/basic_commands.py | 25 +++++++++++++++++++------ commands/cartesian_commands.py | 18 ++++++++++++++++-- 2 files changed, 35 insertions(+), 8 deletions(-) diff --git a/commands/basic_commands.py b/commands/basic_commands.py index adaeba8..30a5ed9 100644 --- a/commands/basic_commands.py +++ b/commands/basic_commands.py @@ -146,17 +146,25 @@ def prepare_for_execution(self, current_position_in): 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: - logger.warning(f" -> VALIDATION FAILED: Required speed ({speed_steps_per_sec} steps/s) exceeds joint's max speed ({max_joint_speed} steps/s).") + max_joint_jog_speed = PAROL6_ROBOT.Joint_max_jog_speed[self.joint_index] + if speed_steps_per_sec > max_joint_jog_speed: + logger.warning(f" -> VALIDATION FAILED: Required speed ({speed_steps_per_sec} steps/s) exceeds joint's max jog speed ({max_joint_jog_speed} steps/s).") self.is_valid = False return + # 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, PAROL6_ROBOT.Joint_min_jog_speed[self.joint_index]) else: if self.speed_percentage is None: logger.error("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])) + speed_steps_per_sec = int(np.interp( + abs(self.speed_percentage), + [0, 100], + [PAROL6_ROBOT.Joint_min_jog_speed[self.joint_index], + PAROL6_ROBOT.Joint_max_jog_speed[self.joint_index]] + )) self.speed_out = speed_steps_per_sec * self.direction self.command_len = int(self.duration / INTERVAL_S) if self.duration else float('inf') @@ -268,7 +276,12 @@ def prepare_for_execution(self, current_position_in): 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]])) + speed_steps_per_sec = int(np.interp( + speed_percentage, + [0, 100], + [PAROL6_ROBOT.Joint_min_jog_speed[joint_index], + PAROL6_ROBOT.Joint_max_jog_speed[joint_index]] + )) self.speeds_out[joint_index] = speed_steps_per_sec * direction logger.debug(" -> MultiJog command is ready.") @@ -310,4 +323,4 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): Speed_out[:] = self.speeds_out Command_out.value = 123 # Jog command self.command_step += 1 - return False # Command is still running \ No newline at end of file + return False # Command is still running diff --git a/commands/cartesian_commands.py b/commands/cartesian_commands.py index a4487aa..e56d90d 100644 --- a/commands/cartesian_commands.py +++ b/commands/cartesian_commands.py @@ -77,9 +77,23 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, **kwargs): delta_angular_rad = np.deg2rad(angular_speed_degs * INTERVAL_S) # Create the small incremental transformation (delta_pose) + # Use explicit per-axis rotations to match original GUI behavior trans_vec = np.array(self.axis_vectors[0]) * delta_linear rot_vec = np.array(self.axis_vectors[1]) * delta_angular_rad - delta_pose = SE3.Rt(SE3.Eul(rot_vec).R, trans_vec) + + # Build delta transformation using explicit rotation matrices + if np.any(rot_vec != 0): + # Find which axis has rotation (should be only one for single-axis jog) + if rot_vec[0] != 0: # RX rotation + delta_pose = SE3.Rx(rot_vec[0]) * SE3(trans_vec) + elif rot_vec[1] != 0: # RY rotation + delta_pose = SE3.Ry(rot_vec[1]) * SE3(trans_vec) + elif rot_vec[2] != 0: # RZ rotation + delta_pose = SE3.Rz(rot_vec[2]) * SE3(trans_vec) + else: + delta_pose = SE3(trans_vec) + else: + delta_pose = SE3(trans_vec) # Apply the transformation in the correct reference frame if self.frame == 'WRF': @@ -400,4 +414,4 @@ def execute_step(self, Position_in, Homed_in, Speed_out, Command_out, Position_o self.is_finished = True # The main loop will handle holding the final position. - return self.is_finished \ No newline at end of file + return self.is_finished