1
0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-09 17:40:47 +02:00
Files
universal_robots_ros_driver/ur_rtde_driver/resources/servoj.urscript
Rune Søe-Knudsen 0b4da2dab5 Change extrapolation log messages strategy
Change the log strategy regarding extrapolation from
make a message every time, to show the maximum number of
extrapolation steps observed in a row and the current count

In this way the log will not be filled up with identical messages,
hiding other messages.

Valuable when running on an unstable network like WIFI
2019-06-25 09:57:47 +00:00

95 lines
2.8 KiB
Plaintext

{{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
params_mult = socket_read_binary_integer(6+1, "reverse_socket")
keepalive = params_mult[7]
while keepalive > 0:
enter_critical
socket_send_line(1, "reverse_socket")
params_mult = socket_read_binary_integer(6+1, "reverse_socket", 0.02) # 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:
keepalive = keepalive - 1
end
exit_critical
end
textmsg("Stopping communication and servoing")
cmd_servo_state = SERVO_STOPPED
sleep(.1)
socket_close()
kill thread_servo