mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-09 17:40:47 +02:00
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
95 lines
2.8 KiB
Plaintext
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
|