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()
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):
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
end
def extrapolate():
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
@@ -25,9 +24,9 @@ def myProg():
exit_critical
return cmd_servo_q
end
end
thread servoThread():
thread servoThread():
state = SERVO_IDLE
while True:
enter_critical
@@ -51,14 +50,14 @@ def myProg():
end
end
stopj(0.1)
end
socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
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:
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]
@@ -68,9 +67,7 @@ def myProg():
# TODO: Extrapolation goes here
keepalive = keepalive - 1
end
end
sleep(.1)
socket_close()
kill thread_servo
end
sleep(.1)
socket_close()
kill thread_servo