Gui support

README.md

@@ -628,18 +628,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**:
@@ -662,18 +675,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()`

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
+            return False # Command is still running

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.
@@ -74,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':
@@ -87,7 +104,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
@@ -397,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
+        return self.is_finished