external_control.urscript

{{BEGIN_REPLACE}}

# HEADER_BEGIN

steptime = get_steptime()

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

#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

TRAJECTORY_MODE_RECEIVE = 1
TRAJECTORY_MODE_CANCEL = -1

TRAJECTORY_POINT_JOINT = 0
TRAJECTORY_POINT_CARTESIAN = 1
TRAJECTORY_DATA_DIMENSION = 7

TRAJECTORY_RESULT_SUCCESS = 0
TRAJECTORY_RESULT_CANCELED = 1
TRAJECTORY_RESULT_FAILURE = 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

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(4.0)
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(5.0)
end

thread jointTrajectoryThread():
  textmsg("Executing trajectory. Number of points: ", trajectory_points_left)
  while trajectory_points_left > 0:
    enter_critical
    #reading trajectory point + blend radius + type of point (cartesian/joint based)
    raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+1+1, "trajectory_socket")
    trajectory_points_left = trajectory_points_left - 1
    exit_critical
    if raw_point[0] > 0:
      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]
      tmptime = raw_point[7] / MULT_time
      blend_radius = raw_point[8] / MULT_time

      if trajectory_points_left > 0:
        if raw_point[9] == TRAJECTORY_POINT_JOINT:
          # textmsg("Executing movej with blending")
          movej(q, t=tmptime, r=blend_radius)
        elif raw_point[9] == TRAJECTORY_POINT_CARTESIAN:
          # textmsg("Executing movel with blending")
          movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t=tmptime, r=blend_radius)
        end
      else:
        if raw_point[9] == TRAJECTORY_POINT_JOINT:
          # textmsg("Executing movej without blending")
          movej(q, t=tmptime, r=0.0)
        elif raw_point[9] == TRAJECTORY_POINT_CARTESIAN:
          # textmsg("Executing movel without blending")
          movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t=tmptime, r=0.0)
        end
      end
    end
  end
  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(5.0)
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(4.0)
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


# 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")

control_mode = MODE_UNINITIALIZED
thread_move = 0
thread_trajectory = 0
trajectory_points_left = 0
global keepalive = -2
params_mult = socket_read_binary_integer(1+6+1, "reverse_socket", 0)
textmsg("ExternalControl: External control active")
keepalive = params_mult[1]
while keepalive > 0 and control_mode > MODE_STOPPED:
  enter_critical
  params_mult = socket_read_binary_integer(1+6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
  if params_mult[0] > 0:
    keepalive = params_mult[1]
    if control_mode != params_mult[8]:
      if control_mode == MODE_FORWARD:
        kill thread_trajectory
        clear_remaining_trajectory_points()
      end
      control_mode = params_mult[8]
      join thread_move
      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
      elif control_mode == MODE_SPEEDL:
        thread_move = run speedlThread()
      elif control_mode == MODE_POSE:
        thread_move = run servoThreadP()
      end
    end
    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 jointTrajectoryThread()
      elif params_mult[2] == TRAJECTORY_MODE_CANCEL:
        textmsg("cancel received")
        kill thread_trajectory
        clear_remaining_trajectory_points()
        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)
    end
  else:
    keepalive = keepalive - 1
  end
  exit_critical
end

textmsg("ExternalControl: Stopping communication and control")
control_mode = MODE_STOPPED
join thread_move
join thread_trajectory
textmsg("ExternalControl: All threads ended")
socket_close("reverse_socket")
socket_close("trajectory_socket")

# NODE_CONTROL_LOOP_ENDS