Load the script from an external file

2026-04-10 01:50:46 +02:00 · 2019-05-20 12:28:35 +02:00
parent ec31db35b2
commit 89ee1aae8a
7 changed files with 110 additions and 87 deletions
--- a/ur_rtde_driver/resources/servoj.urscript
+++ b/ur_rtde_driver/resources/servoj.urscript
@@ -0,0 +1,76 @@
+def myProg():
+  global steptime = get_steptime()
+  textmsg("steptime=", steptime)
+  MULT_jointstate = {{JOINT_STATE_REPLACE}}
+
+  SERVO_UNINITIALIZED = -1
+  SERVO_IDLE = 0
+  SERVO_RUNNING = 1
+  cmd_servo_state = SERVO_UNINITIALIZED
+  cmd_servo_q = get_actual_joint_positions()
+  cmd_servo_q_last = get_actual_joint_positions()
+  def set_servo_setpoint(q):
+    enter_critical
+    cmd_servo_state = SERVO_RUNNING
+    cmd_servo_q_last = cmd_servo_q
+    cmd_servo_q = q
+    exit_critical
+  end
+
+  def extrapolate():
+    enter_critical
+    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]]
+    exit_critical
+
+    return cmd_servo_q
+  end
+
+  thread servoThread():
+    state = SERVO_IDLE
+    while True:
+      enter_critical
+      q = cmd_servo_q
+      do_extrapolate = False
+      if (cmd_servo_state == SERVO_IDLE):
+        do_extrapolate = True
+      end
+      state = cmd_servo_state
+      cmd_servo_state = SERVO_IDLE
+      exit_critical
+      if do_extrapolate:
+        textmsg("No new setpoint received. Extrapolating.")
+        q = extrapolate()
+        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
+      elif state == SERVO_RUNNING:
+        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
+      else:
+        textmsg("Should not be here")
+        sync()
+      end
+    end
+    stopj(0.1)
+  end
+  socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
+
+  thread_servo = run servoThread()
+  keepalive = -2
+  params_mult = socket_read_binary_integer(6+1, "reverse_socket")
+  keepalive = params_mult[7]
+  while keepalive > 0:
+    params_mult = socket_read_binary_integer(6+1, "reverse_socket", 0.2) # steptime could work as well, but does not work in simulation
+    if params_mult[0] > 0:
+      keepalive = params_mult[7]
+      q = [params_mult[1] / MULT_jointstate, 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]
+      set_servo_setpoint(q)
+    else:
+      # TODO: Extrapolation goes here
+      keepalive = keepalive - 1
+    end
+  end
+  sleep(.1)
+  socket_close()
+  kill thread_servo
+end
+