144 lines
4.2 KiB
Plaintext
144 lines
4.2 KiB
Plaintext
# HEADER_BEGIN
|
|
|
|
{{BEGIN_REPLACE}}
|
|
|
|
steptime = get_steptime()
|
|
|
|
textmsg("ExternalControl: steptime=", steptime)
|
|
MULT_jointstate = {{JOINT_STATE_REPLACE}}
|
|
|
|
#Constants
|
|
SERVO_UNINITIALIZED = -1
|
|
SERVO_IDLE = 0
|
|
SERVO_RUNNING = 1
|
|
|
|
MODE_STOPPED = -2
|
|
MODE_UNINITIALIZED = -1
|
|
MODE_IDLE = 0
|
|
MODE_SERVOJ = 1
|
|
MODE_SPEEDJ = 2
|
|
|
|
#Global variables are also showed in the Teach pendants variable list
|
|
global cmd_servo_state = SERVO_UNINITIALIZED
|
|
global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
|
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
|
|
global control_mode = MODE_UNINITIALIZED
|
|
cmd_speedj_active = True
|
|
|
|
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():
|
|
textmsg("ExternalControl: Starting servo thread")
|
|
state = SERVO_IDLE
|
|
while control_mode == MODE_SERVOJ:
|
|
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("ExternalControl: servo thread ended")
|
|
stopj(4.0)
|
|
end
|
|
|
|
# Helpers for speed control
|
|
def set_speed(qd):
|
|
cmd_servo_qd = qd
|
|
control_mode = MODE_SPEEDJ
|
|
end
|
|
|
|
thread speedThread():
|
|
textmsg("ExternalControl: Starting speed thread")
|
|
while control_mode == MODE_SPEEDJ:
|
|
qd = cmd_servo_qd
|
|
speedj(qd, 40.0, steptime)
|
|
end
|
|
textmsg("ExternalControl: speedj thread ended")
|
|
stopj(5.0)
|
|
end
|
|
|
|
# HEADER_END
|
|
|
|
# NODE_CONTROL_LOOP_BEGINS
|
|
|
|
socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
|
|
|
|
control_mode = MODE_UNINITIALIZED
|
|
thread_move = 0
|
|
global keepalive = -2
|
|
params_mult = socket_read_binary_integer(1+6+1, "reverse_socket", 0)
|
|
textmsg("ExternalControl: External control active")
|
|
keepalive = params_mult[1]
|
|
while keepalive > 0 and control_mode > MODE_STOPPED:
|
|
enter_critical
|
|
params_mult = socket_read_binary_integer(1+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[1]
|
|
if control_mode != params_mult[8]:
|
|
control_mode = params_mult[8]
|
|
join thread_move
|
|
if control_mode == MODE_SERVOJ:
|
|
thread_move = run servoThread()
|
|
elif control_mode == MODE_SPEEDJ:
|
|
thread_move = run speedThread()
|
|
end
|
|
end
|
|
if control_mode == MODE_SERVOJ:
|
|
q = [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, params_mult[7] / MULT_jointstate]
|
|
set_servo_setpoint(q)
|
|
elif control_mode == MODE_SPEEDJ:
|
|
qd = [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, params_mult[7] / MULT_jointstate]
|
|
set_speed(qd)
|
|
end
|
|
else:
|
|
keepalive = keepalive - 1
|
|
end
|
|
exit_critical
|
|
end
|
|
|
|
textmsg("ExternalControl: Stopping communication and control")
|
|
control_mode = MODE_STOPPED
|
|
join thread_move
|
|
textmsg("ExternalControl: All threads ended")
|
|
socket_close("reverse_socket")
|
|
|
|
# NODE_CONTROL_LOOP_ENDS
|