From a7349f315afc399c4df172e80106d89e4844122a Mon Sep 17 00:00:00 2001
From: urrsk <41109954+urrsk@users.noreply.github.com>
Date: Mon, 9 Dec 2024 15:17:09 +0100
Subject: [PATCH] Enable force mode compatibility with various move types

The joint positions used for checks and interpolations needs
to be without force mode modifies.
---
 resources/external_control.urscript | 19 +++++++++++--------
 1 file changed, 11 insertions(+), 8 deletions(-)

diff --git a/resources/external_control.urscript b/resources/external_control.urscript
index 3b3fd53a..12081e2a 100644
--- a/resources/external_control.urscript
+++ b/resources/external_control.urscript
@@ -68,8 +68,8 @@ JOINT_IGNORE_SPEED = 20.0
 global violation_popup_counter = 0
 global cmd_servo_state = SERVO_UNINITIALIZED
 global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-global cmd_servo_q = get_actual_joint_positions()
-global cmd_servo_q_last = get_actual_joint_positions()
+global cmd_servo_q = get_joint_positions()
+global cmd_servo_q_last = cmd_servo_q
 global cmd_twist = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 global extrapolate_count = 0
 global extrapolate_max_count = 0
@@ -78,6 +78,7 @@ global trajectory_points_left = 0
 global spline_qdd = [0, 0, 0, 0, 0, 0]
 global spline_qd = [0, 0, 0, 0, 0, 0]
 global tool_contact_running = False
+global force_mode_running = False
 global trajectory_result = 0
 
 # Global thread variables
@@ -158,7 +159,7 @@ end
 thread servoThread():
   textmsg("ExternalControl: Starting servo thread")
   state = SERVO_IDLE
-  local q_last = get_target_joint_positions()
+  local q_last = get_joint_positions()
   while control_mode == MODE_SERVOJ:
     enter_critical
     q = cmd_servo_q
@@ -221,13 +222,13 @@ def cubicSplineRun(end_q, end_qd, time, is_last_point=False, is_first_point=Fals
   local str = str_cat(end_q, str_cat(end_qd, time))
   textmsg("Cubic spline arg: ", str)
 
-  local start_q = get_target_joint_positions()
+  local start_q = get_joint_positions()
   # Zero time means infinite velocity to reach the target and is therefore impossible
   if time <= 0.0:
     if is_first_point and time == 0.0:
       # If users specify the current joint position with time 0 that may be fine, in that case just
       # ignore that point
-      local distance = norm(end_q - get_target_joint_positions())
+      local distance = norm(end_q - start_q)
       if distance < 0.01:
         local splineTimerTraveled = 0.0
         # USED_IN_TEST_SPLINE_INTERPOLATION write_output_float_register(1, splineTimerTraveled)
@@ -263,13 +264,13 @@ def quinticSplineRun(end_q, end_qd, end_qdd, time, is_last_point=False, is_first
   local str = str_cat(end_q, str_cat(end_qd, str_cat(end_qdd, time)))
   textmsg("Quintic spline arg: ", str)
 
-  local start_q = get_target_joint_positions()
+  local start_q = get_joint_positions()
   # Zero time means infinite velocity to reach the target and is therefore impossible
   if time <= 0.0:
     if is_first_point and time == 0.0:
       # If users specify the current joint position with time 0 that may be fine, in that case just
       # ignore that point
-      local distance = norm(end_q - get_target_joint_positions())
+      local distance = norm(end_q - start_q)
       if distance < 0.01:
         return False
       end
@@ -553,7 +554,7 @@ end
 thread servoThreadP():
   textmsg("Starting pose servo thread")
   state = SERVO_IDLE
-  local q_last = get_target_joint_positions()
+  local q_last = get_joint_positions()
   while control_mode == MODE_POSE:
     enter_critical
     q = cmd_servo_q
@@ -655,8 +656,10 @@ thread script_commands():
           force_mode_set_gain_scaling(raw_command[28] / MULT_jointstate)
         end
         force_mode(task_frame, selection_vector, wrench, force_type, force_limits)
+        force_mode_running = True
       elif command == END_FORCE_MODE:
         end_force_mode()
+        force_mode_running = False
       elif command == START_TOOL_CONTACT:
         tool_contact_running = True
       elif command == END_TOOL_CONTACT: