feat: add torque control to urscript

Author: domire8

include/ur_client_library/comm/control_mode.h

@@ -52,6 +52,7 @@ enum class ControlMode : int32_t
   MODE_FREEDRIVE = 6,       ///< Set when freedrive mode is active.
   MODE_TOOL_IN_CONTACT =
       7,  ///< Used only internally in the script, when robot is in tool contact, clear by endToolContact()
+  MODE_TORQUE = 8,          ///< Set when torque control is active.
   END     ///< This is not an actual control mode, but used internally to get the number of control modes
 };
 
@@ -62,9 +63,11 @@ class ControlModeTypes
 {
 public:
   // Control modes that require realtime communication
-  static const inline std::vector<ControlMode> REALTIME_CONTROL_MODES = {
-    ControlMode::MODE_SERVOJ, ControlMode::MODE_SPEEDJ, ControlMode::MODE_SPEEDL, ControlMode::MODE_POSE
-  };
+  static const inline std::vector<ControlMode> REALTIME_CONTROL_MODES = { ControlMode::MODE_SERVOJ,
+                                                                          ControlMode::MODE_SPEEDJ,
+                                                                          ControlMode::MODE_SPEEDL,
+                                                                          ControlMode::MODE_POSE, 
+                                                                          ControlMode::MODE_TORQUE };
 
   // Control modes that doesn't require realtime communication
   static const inline std::vector<ControlMode> NON_REALTIME_CONTROL_MODES = { ControlMode::MODE_IDLE,

resources/external_control.urscript

@@ -25,6 +25,7 @@ MODE_SPEEDL = 4
 MODE_POSE = 5
 MODE_FREEDRIVE = 6
 MODE_TOOL_IN_CONTACT = 7
+MODE_TORQUE = 8
 # Data dimensions of the message received on the reverse interface
 REVERSE_INTERFACE_DATA_DIMENSION = 8
 
@@ -70,6 +71,7 @@ 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_tau = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 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]
@@ -225,6 +227,22 @@ thread speedThread():
   stopj(STOPJ_ACCELERATION)
 end
 
+# Helpers for torque control
+def set_torque(tau):
+  cmd_tau = tau
+  control_mode = MODE_TORQUE
+end
+
+thread torqueThread():
+  textmsg("ExternalControl: Starting torque thread")
+  while control_mode == MODE_TORQUE:
+    tau = cmd_tau
+    torque_command(tau, friction_comp=True)
+  end
+  textmsg("ExternalControl: torque thread ended")
+  stopj(STOPJ_ACCELERATION)
+end
+
 # Function return value (bool) determines whether the robot is moving after this spline segment or
 # not.
 def cubicSplineRun(end_q, end_qd, time, is_last_point=False, is_first_point=False):
@@ -748,6 +766,8 @@ while control_mode > MODE_STOPPED:
         thread_move = run servoThread()
       elif control_mode == MODE_SPEEDJ:
         thread_move = run speedThread()
+      elif control_mode == MODE_TORQUE:
+        thread_move = run torqueThread()
       elif control_mode == MODE_FORWARD:
         kill thread_move
         stopj(STOPJ_ACCELERATION)
@@ -766,6 +786,9 @@ while control_mode > MODE_STOPPED:
     elif control_mode == MODE_SPEEDJ:
       qd = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
       set_speed(qd)
+    elif control_mode == MODE_TORQUE:
+      tau = [params_mult[2]/ MULT_jointstate, params_mult[3]/ MULT_jointstate, params_mult[4]/ MULT_jointstate, params_mult[5]/ MULT_jointstate, params_mult[6]/ MULT_jointstate, params_mult[7]/ MULT_jointstate]
+      set_torque(tau)
     elif control_mode == MODE_FORWARD:
       if params_mult[2] == TRAJECTORY_MODE_RECEIVE:
         kill thread_trajectory