Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 10:00:48 +02:00
Code Releases Activity

edited script for use in URCaps

Browse Source
This commit is contained in:
Tristan Schnell
2019-05-23 11:13:02 +02:00
parent a0ea50a031
commit 876d4a06f5
1 changed files with 70 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
ur_rtde_driver/resources/servoj.urscript
View File

@@ -1,23 +1,22 @@
def myProg(): global steptime = get_steptime()
global steptime = get_steptime() textmsg("steptime=", steptime)
textmsg("steptime=", steptime) global MULT_jointstate = {{JOINT_STATE_REPLACE}}
MULT_jointstate = {{JOINT_STATE_REPLACE}}
SERVO_UNINITIALIZED = -1 global SERVO_UNINITIALIZED = -1
SERVO_IDLE = 0 global SERVO_IDLE = 0
SERVO_RUNNING = 1 global SERVO_RUNNING = 1
cmd_servo_state = SERVO_UNINITIALIZED global cmd_servo_state = SERVO_UNINITIALIZED
cmd_servo_q = get_actual_joint_positions() global cmd_servo_q = get_actual_joint_positions()
cmd_servo_q_last = get_actual_joint_positions() global cmd_servo_q_last = get_actual_joint_positions()
def set_servo_setpoint(q): def set_servo_setpoint(q):
enter_critical enter_critical
cmd_servo_state = SERVO_RUNNING cmd_servo_state = SERVO_RUNNING
cmd_servo_q_last = cmd_servo_q cmd_servo_q_last = cmd_servo_q
cmd_servo_q = q cmd_servo_q = q
exit_critical exit_critical
end end
def extrapolate(): def extrapolate():
enter_critical 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]] 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_last = cmd_servo_q
@@ -25,9 +24,9 @@ def myProg():
exit_critical exit_critical
return cmd_servo_q return cmd_servo_q
end end
thread servoThread(): thread servoThread():
state = SERVO_IDLE state = SERVO_IDLE
while True: while True:
enter_critical enter_critical
@@ -51,14 +50,14 @@ def myProg():
end end
end end
stopj(0.1) stopj(0.1)
end end
socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket") socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
thread_servo = run servoThread() thread_servo = run servoThread()
keepalive = -2 keepalive = -2
params_mult = socket_read_binary_integer(6+1, "reverse_socket") params_mult = socket_read_binary_integer(6+1, "reverse_socket")
keepalive = params_mult[7] keepalive = params_mult[7]
while keepalive > 0: 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 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: if params_mult[0] > 0:
keepalive = params_mult[7] keepalive = params_mult[7]
@@ -68,9 +67,7 @@ def myProg():
# TODO: Extrapolation goes here # TODO: Extrapolation goes here
keepalive = keepalive - 1 keepalive = keepalive - 1
end end
end
sleep(.1)
socket_close()
kill thread_servo
end end
sleep(.1)
socket_close()
kill thread_servo