From 876d4a06f50da953259c0a5330fb09542daba90f Mon Sep 17 00:00:00 2001
From: Tristan Schnell <schnell@fzi.de>
Date: Thu, 23 May 2019 11:13:02 +0200
Subject: [PATCH] edited script for use in URCaps

---
 ur_rtde_driver/resources/servoj.urscript | 143 +++++++++++------------
 1 file changed, 70 insertions(+), 73 deletions(-)

diff --git a/ur_rtde_driver/resources/servoj.urscript b/ur_rtde_driver/resources/servoj.urscript
index 02be93d..a79eda2 100644
--- a/ur_rtde_driver/resources/servoj.urscript
+++ b/ur_rtde_driver/resources/servoj.urscript
@@ -1,76 +1,73 @@
-def myProg():
-  global steptime = get_steptime()
-  textmsg("steptime=", steptime)
-  MULT_jointstate = {{JOINT_STATE_REPLACE}}
+global steptime = get_steptime()
+textmsg("steptime=", steptime)
+global 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
+global SERVO_UNINITIALIZED = -1
+global SERVO_IDLE = 0
+global SERVO_RUNNING = 1
+global cmd_servo_state = SERVO_UNINITIALIZED
+global cmd_servo_q = get_actual_joint_positions()
+global 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