external_control.urscript

# HEADER_BEGIN

{{BEGIN_REPLACE}}

steptime = get_steptime()

textmsg("ExternalControl: steptime=", steptime)
MULT_jointstate = {{JOINT_STATE_REPLACE}}
MULT_time = {{TIME_REPLACE}}

STOPJ_ACCELERATION = 4.0

#Constants
SERVO_UNINITIALIZED = -1
SERVO_IDLE = 0
SERVO_RUNNING = 1

MODE_STOPPED = -2
MODE_UNINITIALIZED = -1
MODE_IDLE = 0
MODE_SERVOJ = 1
MODE_SPEEDJ = 2
MODE_FORWARD = 3
MODE_SPEEDL = 4
MODE_POSE = 5
MODE_FREEDRIVE = 6
MODE_TOOL_IN_CONTACT = 7
# Data dimensions of the message received on the reverse interface
REVERSE_INTERFACE_DATA_DIMENSION = 8

TRAJECTORY_MODE_RECEIVE = 1
TRAJECTORY_MODE_CANCEL = -1

TRAJECTORY_POINT_JOINT = 0
TRAJECTORY_POINT_CARTESIAN = 1
TRAJECTORY_POINT_JOINT_SPLINE = 2
TRAJECTORY_DATA_DIMENSION = 3*6 + 1

TRAJECTORY_RESULT_SUCCESS = 0
TRAJECTORY_RESULT_CANCELED = 1
TRAJECTORY_RESULT_FAILURE = 2

ZERO_FTSENSOR = 0
SET_PAYLOAD = 1
SET_TOOL_VOLTAGE = 2
START_FORCE_MODE = 3
END_FORCE_MODE = 4
START_TOOL_CONTACT = 5
END_TOOL_CONTACT = 6
SCRIPT_COMMAND_DATA_DIMENSION = 26

FREEDRIVE_MODE_START = 1
FREEDRIVE_MODE_STOP = -1

UNTIL_TOOL_CONTACT_RESULT_SUCCESS = 0
UNTIL_TOOL_CONTACT_RESULT_CANCELED = 1

SPLINE_CUBIC = 1
SPLINE_QUINTIC = 2

#Global variables are also showed in the Teach pendants variable list
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_twist = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
global extrapolate_count = 0
global extrapolate_max_count = 0
global control_mode = MODE_UNINITIALIZED
global trajectory_points_left = 0
global last_spline_qdd = [0, 0, 0, 0, 0, 0]
global tool_contact_running = False

# Global thread variables
thread_move = 0
thread_trajectory = 0
thread_script_commands = 0

def set_servo_setpoint(q):
  cmd_servo_state = SERVO_RUNNING
  cmd_servo_q_last = cmd_servo_q
  cmd_servo_q = q
end

def extrapolate():
  diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]]
  cmd_servo_q_last = cmd_servo_q
  cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]]

  return cmd_servo_q
end

thread servoThread():
  textmsg("ExternalControl: Starting servo thread")
  state = SERVO_IDLE
  while control_mode == MODE_SERVOJ:
    enter_critical
    q = cmd_servo_q
    do_extrapolate = False
    if (cmd_servo_state == SERVO_IDLE):
      do_extrapolate = True
    end
    state = cmd_servo_state
    if cmd_servo_state > SERVO_UNINITIALIZED:
      cmd_servo_state = SERVO_IDLE
    end

    if do_extrapolate:
      extrapolate_count = extrapolate_count + 1
      if extrapolate_count > extrapolate_max_count:
        extrapolate_max_count = extrapolate_count
      end

      q = extrapolate()
      servoj(q, t=steptime, {{SERVO_J_REPLACE}})

    elif state == SERVO_RUNNING:
      extrapolate_count = 0
      servoj(q, t=steptime, {{SERVO_J_REPLACE}})
    else:
      extrapolate_count = 0
      sync()
    end
    exit_critical
  end
  textmsg("ExternalControl: servo thread ended")
  stopj(STOPJ_ACCELERATION)
end

# Helpers for speed control
def set_speed(qd):
  cmd_servo_qd = qd
  control_mode = MODE_SPEEDJ
end

thread speedThread():
  textmsg("ExternalControl: Starting speed thread")
  while control_mode == MODE_SPEEDJ:
    qd = cmd_servo_qd
    speedj(qd, 40.0, steptime)
  end
  textmsg("ExternalControl: speedj thread ended")
  stopj(STOPJ_ACCELERATION)
end

def cubicSplineRun(end_q, end_qd, time, last_point=False):
  local str = str_cat(end_q, str_cat(end_qd, time))
  textmsg("Cubic spline arg: ", str)

  # Zero time means zero length and therefore no motion to execute
  if time > 0.0:
    local start_q = get_target_joint_positions()
    local start_qd = get_target_joint_speeds()

    # Coefficients0 is not included, since we do not need to calculate the position
    local coefficients1 = start_qd
    local coefficients2 = (-3 * start_q + end_q * 3 - start_qd * 2 * time - end_qd * time) / pow(time, 2)
    local coefficients3 = (2 * start_q - 2 * end_q + start_qd * time + end_qd * time) / pow(time, 3)
    local coefficients4 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    local coefficients5 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, time, last_point)
  end
end

def quinticSplineRun(end_q, end_qd, end_qdd, time, last_point=False):
  local str = str_cat(end_q, str_cat(end_qd, str_cat(end_qdd, time)))
  textmsg("Quintic spline arg: ", str)

  # Zero time means zero length and therefore no motion to execute
  if time > 0.0:
    local start_q = get_target_joint_positions()
    local start_qd = get_target_joint_speeds()
    local start_qdd = last_spline_qdd

    # Pre-calculate coefficients
    local TIME2 = pow(time, 2)
    # Coefficients0 is not included, since we do not need to calculate the position
    local coefficients1 = start_qd
    local coefficients2 = 0.5 * start_qdd
    local coefficients3 = (-20.0 * start_q + 20.0 * end_q - 3.0 * start_qdd * TIME2 + end_qdd * TIME2 - 12.0 * start_qd * time - 8.0 * end_qd * time) / (2.0 * pow(time, 3))
    local coefficients4 = (30.0 * start_q - 30.0 * end_q + 3.0 * start_qdd * TIME2 - 2.0 * end_qdd * TIME2 + 16.0 * start_qd * time + 14.0 * end_qd * time) / (2.0 * pow(time, 4))
    local coefficients5 = (-12.0 * start_q + 12.0 * end_q - start_qdd * TIME2 + end_qdd * TIME2 - 6.0 * start_qd * time - 6.0 * end_qd * time) / (2.0 * pow(time, 5))
    jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, time, last_point)
  end
end

def jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTotalTravelTime, last_point):
  # Initialize variables
  local splineTimerTraveled = 0.0
  local scaled_step_time = get_steptime()
  local scaling_factor = 1.0
  local slowing_down = False
  local slowing_down_time = 0.0

  # Interpolate the spline in whole time steps
  while (splineTotalTravelTime - splineTimerTraveled) > get_steptime():
    local time_left = splineTotalTravelTime - splineTimerTraveled

    # Maximum deceleration in rad/s^2
    local max_deceleration = 15
    # Maximum allowable joint speed in rad/s
    local max_speed = 6.283185
    # The time needed to decelerate with 15 rad/s^2 to reach zero velocity when we move at maximum velocity.
    local deceleration_time = max_speed / max_deceleration

    # If the velocity is too large close to the end of the trajectory we scale the
    # trajectory, such that we follow the positional part of the trajectory, but end with zero velocity.
    if (time_left <= deceleration_time) and (last_point == True):
      if slowing_down == False:

        # Peek what the joint velocities will be if we take a full time step
        local qd = jointSplinePeek(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled + get_steptime())
        # Compute the time left to decelerate if we take a full time step
        local x = deceleration_time - (time_left - get_steptime())
        slowing_down = checkSlowDownRequired(x, qd, max_speed, max_deceleration)

        if slowing_down == True:
          # This will ensure that we scale the trajectory right away
          slowing_down_time = time_left + get_steptime()
          textmsg("Velocity is too fast towards the end of the trajectory. The robot will be slowing down, while following the positional part of the trajectory.")
        end
      end

      if slowing_down == True:
        # Compute scaling factor based on time left and total slowing down time
        scaling_factor = time_left / slowing_down_time
        scaled_step_time = get_steptime() * scaling_factor
      end
    end

    splineTimerTraveled = splineTimerTraveled + scaled_step_time
    # USED_IN_TEST_SPLINE_INTERPOLATION write_output_float_register(1, splineTimerTraveled)
    jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled, get_steptime(), scaling_factor)
  end

  # Make sure that we approach zero velocity slowly, when slowing down
  if slowing_down == True:
    local qd = get_actual_joint_speeds()
    while norm(qd) > 0.00001:
      local time_left = splineTotalTravelTime - splineTimerTraveled

      # Compute scaling factor based on time left and total slowing down time
      scaling_factor = time_left / slowing_down_time
      scaled_step_time = get_steptime() * scaling_factor

      splineTimerTraveled = splineTimerTraveled + scaled_step_time
      # USED_IN_TEST_SPLINE_INTERPOLATION write_output_float_register(1, splineTimerTraveled)
      jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled, get_steptime(), scaling_factor)

      qd = get_actual_joint_speeds()
    end
    scaling_factor = 0.0
  end

  # Last part of the spline which uses less than one time step
  local timeLeftToTravel = splineTotalTravelTime - splineTimerTraveled

  # To round off the float to the steptime step when it is very close to that number (1e-15)
  if timeLeftToTravel == get_steptime():
    timeLeftToTravel = get_steptime()
  end

  jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTotalTravelTime, timeLeftToTravel, scaling_factor)
end

def jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled, timestep, scaling_factor):
  local qd = coefficients1 + 2.0 * splineTimerTraveled * coefficients2 + 3.0 * pow(splineTimerTraveled, 2) * coefficients3 + 4.0 * pow(splineTimerTraveled, 3) * coefficients4 + 5.0 * pow(splineTimerTraveled, 4) * coefficients5
  last_spline_qdd = 2.0 * coefficients2 + 6.0 * splineTimerTraveled * coefficients3 + 12.0 * pow(splineTimerTraveled, 2) * coefficients4 + 20.0 * pow(splineTimerTraveled, 3) * coefficients5
  qd = qd * scaling_factor
  speedj(qd, 1000, timestep)
end

# Helper function to see what the velocity will be if we take a full step
def jointSplinePeek(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled):
  local qd = coefficients1 + 2.0 * splineTimerTraveled * coefficients2 + 3.0 * pow(splineTimerTraveled, 2) * coefficients3 + 4.0 * pow(splineTimerTraveled, 3) * coefficients4 + 5.0 * pow(splineTimerTraveled, 4) * coefficients5
  return qd
end

# \brief Helper function to figure out if we need to slown down velocity at the last part of the last spline segment.
#
# The max_speed and max_deceleration is used to compute a linear velocity profile as function of the time left to decelerate,
# then we can use the time left to decelerate to find the maximum allowed speed given the time we have left. If we are above
# the maximum allowed speed, we start slowing down.
#
# \param 'x' is the time left to decelerate.
# \param 'qd' is the velocity if we take a full step ahead in the spline.
# \param 'max_speed' maximum allowed joint speed 6.283185 rad/s
# \param 'max_deceleration' is the maximum allowed deceleration 15 rad/s^2.
def checkSlowDownRequired(x, qd, max_speed, max_deceleration):
  local max_allowable_speed = max_speed - x * max_deceleration
  local idx = 0
  while idx < 6:
    if norm(qd[idx]) > max_allowable_speed:
      return True
    end
    idx = idx + 1
  end
  return False
end

thread trajectoryThread():
  textmsg("Executing trajectory. Number of points: ", trajectory_points_left)
  local INDEX_TIME = TRAJECTORY_DATA_DIMENSION
  local INDEX_BLEND = INDEX_TIME + 1
  # same index as blend parameter, depending on point type
  local INDEX_SPLINE_TYPE = INDEX_BLEND
  local INDEX_POINT_TYPE = INDEX_BLEND + 1
  last_spline_qdd = [0, 0, 0, 0, 0, 0]
  enter_critical
  while trajectory_points_left > 0:
    #reading trajectory point + blend radius + type of point (cartesian/joint based)
    local raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+1+1, "trajectory_socket", get_steptime())
    trajectory_points_left = trajectory_points_left - 1

    if raw_point[0] > 0:
      local q = [ raw_point[1]/ MULT_jointstate, raw_point[2]/ MULT_jointstate, raw_point[3]/ MULT_jointstate, raw_point[4]/ MULT_jointstate, raw_point[5]/ MULT_jointstate, raw_point[6]/ MULT_jointstate]
      local tmptime = raw_point[INDEX_TIME] / MULT_time
      local blend_radius = raw_point[INDEX_BLEND] / MULT_time
      local last_point = False
      if trajectory_points_left == 0:
        blend_radius = 0.0
        last_point = True
      end
      # MoveJ point
      if raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT:
        movej(q, t=tmptime, r=blend_radius)

        # reset old acceleration
        last_spline_qdd = [0, 0, 0, 0, 0, 0]

      # Movel point
      elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_CARTESIAN:
        movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t=tmptime, r=blend_radius)

        # reset old acceleration
        last_spline_qdd = [0, 0, 0, 0, 0, 0]

      # Joint spline point
      elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT_SPLINE:

        # Cubic spline
        if raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC:
          qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate]
          cubicSplineRun(q, qd, tmptime, last_point)
          # reset old acceleration
          last_spline_qdd = [0, 0, 0, 0, 0, 0]

        # Quintic spline
        elif raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUINTIC:
          qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate]
          qdd = [ raw_point[13]/ MULT_jointstate, raw_point[14]/ MULT_jointstate, raw_point[15]/ MULT_jointstate, raw_point[16]/ MULT_jointstate, raw_point[17]/ MULT_jointstate, raw_point[18]/ MULT_jointstate]
          quinticSplineRun(q, qd, qdd, tmptime, last_point)
        else:
          textmsg("Unknown spline type given:", raw_point[INDEX_POINT_TYPE])
          clear_remaining_trajectory_points()
          socket_send_int(TRAJECTORY_RESULT_FAILURE, "trajectory_socket")
        end
      end
    end
  end
  exit_critical
  socket_send_int(TRAJECTORY_RESULT_SUCCESS, "trajectory_socket")
  textmsg("Trajectory finished")
end

def clear_remaining_trajectory_points():
  while trajectory_points_left > 0:
    raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+2, "trajectory_socket")
    trajectory_points_left = trajectory_points_left - 1
  end
end

# Helpers for speed control
def set_speedl(twist):
  cmd_twist = twist
  control_mode = MODE_SPEEDL
end

thread speedlThread():
  textmsg("Starting speedl thread")
  while control_mode == MODE_SPEEDL:
    twist = cmd_twist
    speedl(twist, 40.0, steptime)
  end
  textmsg("speedl thread ended")
  stopj(STOPJ_ACCELERATION)
end

thread servoThreadP():
  textmsg("Starting pose servo thread")
  state = SERVO_IDLE
  while control_mode == MODE_POSE:
    enter_critical
    q = cmd_servo_q
    do_extrapolate = False
    if (cmd_servo_state == SERVO_IDLE):
      do_extrapolate = True
    end
    state = cmd_servo_state
    if cmd_servo_state > SERVO_UNINITIALIZED:
      cmd_servo_state = SERVO_IDLE
    end

    if do_extrapolate:
      extrapolate_count = extrapolate_count + 1
      if extrapolate_count > extrapolate_max_count:
        extrapolate_max_count = extrapolate_count
      end

      q = extrapolate()
      servoj(q, t=steptime, {{SERVO_J_REPLACE}})

    elif state == SERVO_RUNNING:
      extrapolate_count = 0
      servoj(q, t=steptime, {{SERVO_J_REPLACE}})
    else:
      extrapolate_count = 0
      sync()
    end
    exit_critical
  end
  textmsg("pose servo thread ended")
  stopj(STOPJ_ACCELERATION)
end

def set_servo_pose(pose):
  cmd_servo_state = SERVO_RUNNING
  cmd_servo_q_last = cmd_servo_q
  cmd_servo_q = get_inverse_kin(pose, cmd_servo_q)
end

def tool_contact_detection():
  # Detect tool contact in the directions that the TCP is moving
  step_back = tool_contact(direction = get_actual_tcp_speed())

  # If tool contact is detected stop movement and move back to intial contact point
  if step_back > 0:
    if control_mode == MODE_FORWARD:
      kill thread_trajectory
      clear_remaining_trajectory_points()
    elif control_mode == MODE_FREEDRIVE:
      textmsg("Leaving freedrive mode")
      end_freedrive_mode()
    else:
      kill thread_move
    end

    # Set control mode to tool in contact, should be cleared by stopping tool contact detection
    control_mode = MODE_TOOL_IN_CONTACT
    stopl(3)

    # Move to initial contact point
    q = get_actual_joint_positions_history(step_back)
    movel(q)
    socket_send_int(UNTIL_TOOL_CONTACT_RESULT_SUCCESS, "script_command_socket")
    textmsg("tool contact detected")
  end
end

# Thread to receive one shot script commands, the commands shouldn't be blocking
thread script_commands():
  while control_mode > MODE_STOPPED:
    raw_command = socket_read_binary_integer(SCRIPT_COMMAND_DATA_DIMENSION, "script_command_socket", 0)
    if raw_command[0] > 0:
      command = raw_command[1]
      if command == ZERO_FTSENSOR:
        zero_ftsensor()
      elif command == SET_PAYLOAD:
        mass = raw_command[2] / MULT_jointstate
        cog = [raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate]
        set_payload(mass, cog)
      elif command == SET_TOOL_VOLTAGE:
        tool_voltage = raw_command[2] / MULT_jointstate
        set_tool_voltage(tool_voltage)
      elif command == START_FORCE_MODE:
        task_frame = p[raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate]
        selection_vector = [raw_command[8] / MULT_jointstate, raw_command[9] / MULT_jointstate, raw_command[10] / MULT_jointstate, raw_command[11] / MULT_jointstate, raw_command[12] / MULT_jointstate, raw_command[13] / MULT_jointstate]
        wrench = [raw_command[14] / MULT_jointstate, raw_command[15] / MULT_jointstate, raw_command[16] / MULT_jointstate, raw_command[17] / MULT_jointstate, raw_command[18] / MULT_jointstate, raw_command[19] / MULT_jointstate]
        force_type = raw_command[20] / MULT_jointstate
        force_limits = [raw_command[21] / MULT_jointstate, raw_command[22] / MULT_jointstate, raw_command[23] / MULT_jointstate, raw_command[24] / MULT_jointstate, raw_command[25] / MULT_jointstate, raw_command[26] / MULT_jointstate]
        force_mode(task_frame, selection_vector, wrench, force_type, force_limits)
      elif command == END_FORCE_MODE:
        end_force_mode()
      elif command == START_TOOL_CONTACT:
        tool_contact_running = True
      elif command == END_TOOL_CONTACT:
        if control_mode != MODE_TOOL_IN_CONTACT:
          # If tool contact hasn't been detected send canceled result
          socket_send_int(UNTIL_TOOL_CONTACT_RESULT_CANCELED, "script_command_socket")
        end
        tool_contact_running = False
      end
    end
  end
end

# HEADER_END

# NODE_CONTROL_LOOP_BEGINS
socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
socket_open("{{SERVER_IP_REPLACE}}", {{TRAJECTORY_SERVER_PORT_REPLACE}}, "trajectory_socket")
socket_open("{{SERVER_IP_REPLACE}}", {{SCRIPT_COMMAND_SERVER_PORT_REPLACE}}, "script_command_socket")

force_mode_set_damping({{FORCE_MODE_SET_DAMPING_REPLACE}})
force_mode_set_gain_scaling({{FORCE_MODE_SET_GAIN_SCALING_REPLACE}})

control_mode = MODE_UNINITIALIZED
thread_move = 0
thread_trajectory = 0
trajectory_points_left = 0
textmsg("ExternalControl: External control active")
global read_timeout = 0.0 # First read is blocking
thread_script_commands = run script_commands()
while control_mode > MODE_STOPPED:
  enter_critical
  params_mult = socket_read_binary_integer(REVERSE_INTERFACE_DATA_DIMENSION, "reverse_socket", read_timeout)
  if params_mult[0] > 0:
    # Convert to read timeout from milliseconds to seconds
    read_timeout = params_mult[1] / 1000.0 
    if control_mode != params_mult[REVERSE_INTERFACE_DATA_DIMENSION]:
      # Clear remaining trajectory points
      if control_mode == MODE_FORWARD:
        kill thread_trajectory
        clear_remaining_trajectory_points()
        stopj(STOPJ_ACCELERATION)
        socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket")
      # Stop freedrive
      elif control_mode == MODE_FREEDRIVE:
        textmsg("Leaving freedrive mode")
        end_freedrive_mode()
      end

      # If tool is in contact, tool contact should be ended before switching control mode
      if control_mode == MODE_TOOL_IN_CONTACT:
        if tool_contact_running == False:
          control_mode = params_mult[REVERSE_INTERFACE_DATA_DIMENSION]
        end
      else:
        control_mode = params_mult[REVERSE_INTERFACE_DATA_DIMENSION]
        join thread_move
      end
      if control_mode == MODE_SERVOJ:
        thread_move = run servoThread()
      elif control_mode == MODE_SPEEDJ:
        thread_move = run speedThread()
      elif control_mode == MODE_FORWARD:
        kill thread_move
        stopj(STOPJ_ACCELERATION)
      elif control_mode == MODE_SPEEDL:
        thread_move = run speedlThread()
      elif control_mode == MODE_POSE:
        thread_move = run servoThreadP()
      end
    end

    # Update the motion commands with new parameters
    if control_mode == MODE_SERVOJ:
      q = [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_servo_setpoint(q)
    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_FORWARD:
      if params_mult[2] == TRAJECTORY_MODE_RECEIVE:
        kill thread_trajectory
        clear_remaining_trajectory_points()
        trajectory_points_left = params_mult[3]
        thread_trajectory = run trajectoryThread()
      elif params_mult[2] == TRAJECTORY_MODE_CANCEL:
        textmsg("cancel received")
        kill thread_trajectory
        clear_remaining_trajectory_points()
        stopj(STOPJ_ACCELERATION)
        socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket")
      end
    elif control_mode == MODE_SPEEDL:
      twist = [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_speedl(twist)
    elif control_mode == MODE_POSE:
      pose = p[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_servo_pose(pose)
    elif control_mode == MODE_FREEDRIVE:
      if params_mult[2] == FREEDRIVE_MODE_START:
        textmsg("Entering freedrive mode")
        freedrive_mode()
      elif params_mult[2] == FREEDRIVE_MODE_STOP:
        textmsg("Leaving freedrive mode")
        end_freedrive_mode()
      end
    end
    # Tool contact is running, but hasn't been detected
    if tool_contact_running == True and control_mode != MODE_TOOL_IN_CONTACT:
      tool_contact_detection()
    end
  else:
    textmsg("Socket timed out waiting for command on reverse_socket. The script will exit now.")
    control_mode = MODE_STOPPED
  end
  exit_critical
end

textmsg("ExternalControl: Stopping communication and control")
kill thread_move
kill thread_trajectory
kill thread_script_commands
stopj(STOPJ_ACCELERATION)
textmsg("ExternalControl: All threads ended")
socket_close("reverse_socket")
socket_close("trajectory_socket")
socket_close("script_command_socket")

# NODE_CONTROL_LOOP_ENDS