Renamed the driver to ur_robot_driver

ur_robot_driver/resources/rtde_input_recipe.txt

@@ -0,0 +1,12 @@
+speed_slider_mask
+speed_slider_fraction
+standard_digital_output_mask
+standard_digital_output
+configurable_digital_output_mask
+configurable_digital_output
+tool_digital_output_mask
+tool_digital_output
+standard_analog_output_mask
+standard_analog_output_type
+standard_analog_output_0
+standard_analog_output_1

ur_robot_driver/resources/rtde_output_recipe.txt

@@ -0,0 +1,22 @@
+timestamp
+actual_q
+actual_qd
+speed_scaling
+target_speed_fraction
+runtime_state
+actual_TCP_force
+actual_TCP_pose
+actual_digital_input_bits
+actual_digital_output_bits
+standard_analog_input0
+standard_analog_input1
+standard_analog_output0
+standard_analog_output1
+analog_io_types
+tool_mode
+tool_analog_input_types
+tool_analog_input0
+tool_analog_input1
+tool_output_voltage
+tool_output_current
+tool_temperature

ur_robot_driver/resources/servoj.urscript

@@ -0,0 +1,97 @@
+{{BEGIN_REPLACE}}
+
+steptime = get_steptime()
+textmsg("steptime=", steptime)
+MULT_jointstate = {{JOINT_STATE_REPLACE}}
+
+#Constants
+SERVO_STOPPED = -2
+SERVO_UNINITIALIZED = -1
+SERVO_IDLE = 0
+SERVO_RUNNING = 1
+
+#Global variables are also showed in the Teach pendants variable list
+global cmd_servo_state = SERVO_UNINITIALIZED
+global cmd_servo_q = get_actual_joint_positions()
+global cmd_servo_q_last = get_actual_joint_positions()
+global extrapolate_count = 0
+global extrapolate_max_count = 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():
+  state = SERVO_IDLE
+  while state > SERVO_STOPPED:
+    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("servo thread ended")
+  stopj(0.1)
+end
+socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
+
+thread_servo = run servoThread()
+keepalive = -2
+textmsg("External control active")
+params_mult = socket_read_binary_integer(1+6, "reverse_socket")
+keepalive = params_mult[1]
+while keepalive > 0:
+  enter_critical
+  socket_send_line(1, "reverse_socket")
+  params_mult = socket_read_binary_integer(1+6, "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 params_mult[1] > 1:
+      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)
+    end
+  else:
+    keepalive = keepalive - 1
+  end
+  exit_critical
+end
+
+textmsg("Stopping communication and servoing")
+cmd_servo_state = SERVO_STOPPED
+sleep(.1)
+socket_close("reverse_socket")
+kill thread_servo