Newer
Older
HOSTNAME = "%(driver_hostname)s"
MSG_OUT = 1
MSG_QUIT = 2
MSG_JOINT_STATES = 3
MSG_SET_DIGITAL_OUT = 10
MSG_GET_IO = 11
MSG_SET_FLAG = 12
MSG_SET_TOOL_VOLTAGE = 13
MSG_SET_ANALOG_OUT = 14
MULT_analog = 1000000.0
Stuart Glaser
committed
def send_out(msg):
enter_critical
Stuart Glaser
committed
socket_send_string(msg)
socket_send_string("~")
exit_critical
end
def send_waypoint_finished(waypoint_id):
enter_critical
socket_send_int(MSG_WAYPOINT_FINISHED)
socket_send_int(waypoint_id)
exit_critical
end
ipa-fxm
committed
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
## For joint states the real-time communication interface connection is used
#thread statePublisherThread():
# def send_joint_state():
# q = get_joint_positions()
# qdot = get_joint_speeds()
# tau = get_joint_torques()
# enter_critical
# socket_send_int(MSG_JOINT_STATES)
# socket_send_int(floor(MULT_jointstate * q[0]))
# socket_send_int(floor(MULT_jointstate * q[1]))
# socket_send_int(floor(MULT_jointstate * q[2]))
# socket_send_int(floor(MULT_jointstate * q[3]))
# socket_send_int(floor(MULT_jointstate * q[4]))
# socket_send_int(floor(MULT_jointstate * q[5]))
# socket_send_int(floor(MULT_jointstate * qdot[0]))
# socket_send_int(floor(MULT_jointstate * qdot[1]))
# socket_send_int(floor(MULT_jointstate * qdot[2]))
# socket_send_int(floor(MULT_jointstate * qdot[3]))
# socket_send_int(floor(MULT_jointstate * qdot[4]))
# socket_send_int(floor(MULT_jointstate * qdot[5]))
# socket_send_int(floor(MULT_jointstate * tau[0]))
# socket_send_int(floor(MULT_jointstate * tau[1]))
# socket_send_int(floor(MULT_jointstate * tau[2]))
# socket_send_int(floor(MULT_jointstate * tau[3]))
# socket_send_int(floor(MULT_jointstate * tau[4]))
# socket_send_int(floor(MULT_jointstate * tau[5]))
# #socket_send_int(7895160) # Recognizable ".xxx" or 00787878
# exit_critical
# end
# #socket_open(HOSTNAME, 50002)
# send_joint_state()
# while True:
# send_joint_state()
# sync()
# end
# sync()
#end
## For tcp wrenches the real-time communication interface connection is used
#thread wrench_statePublisherThread():
# def send_wrench_state():
# wrench = get_tcp_force()
# enter_critical
# socket_send_int(MSG_WRENCH)
# socket_send_int(floor(MULT_wrench * wrench[0]))
# socket_send_int(floor(MULT_wrench * wrench[1]))
# socket_send_int(floor(MULT_wrench * wrench[2]))
# socket_send_int(floor(MULT_wrench * wrench[3]))
# socket_send_int(floor(MULT_wrench * wrench[4]))
# socket_send_int(floor(MULT_wrench * wrench[5]))
# exit_critical
# end
# send_wrench_state()
# while True:
# send_wrench_state()
# sync()
# end
# sync()
#end
#thread io_statePublisherThread():
# def send_io_state():
# enter_critical
# socket_send_int(MSG_GET_IO)
# socket_send_int(1*get_digital_in(0) + 2*get_digital_in(1) + 4 * get_digital_in(2) + 8 * get_digital_in(3) + 16 * get_digital_in(4) + 32 * get_digital_in(5) + 64 * get_digital_in(6) + 128 * get_digital_in(7) + 256 * get_digital_in(8) + 512 * get_digital_in(9))
# socket_send_int(1*get_digital_out(0) + 2*get_digital_out(1) + 4 * get_digital_out(2) + 8 * get_digital_out(3) + 16 * get_digital_out(4) + 32 * get_digital_out(5) + 64 * get_digital_out(6) + 128 * get_digital_out(7) + 256 * get_digital_out(8) + 512 * get_digital_out(9))
# socket_send_int(pow(2,0)*get_flag(0) + pow(2,1)*get_flag(1) + pow(2,2)*get_flag(2) + pow(2,3)*get_flag(3) + pow(2,4)*get_flag(4) + pow(2,5)*get_flag(5) + pow(2,6)*get_flag(6) + pow(2,7)*get_flag(7) + pow(2,8)*get_flag(8) + pow(2,9)*get_flag(9) + pow(2,10)*get_flag(10) + pow(2,11)*get_flag(11) + pow(2,12)*get_flag(12) + pow(2,13)*get_flag(13) + pow(2,14)*get_flag(14) + pow(2,15)*get_flag(15) + pow(2,16)*get_flag(16) + pow(2,17)*get_flag(17) + pow(2,18)*get_flag(18) + pow(2,19)*get_flag(19) + pow(2,20)*get_flag(20) + pow(2,21)*get_flag(21) + pow(2,22)*get_flag(22) + pow(2,23)*get_flag(23) + pow(2,24)*get_flag(24) + pow(2,25)*get_flag(25) + pow(2,26)*get_flag(26) + pow(2,27)*get_flag(27) + pow(2,28)*get_flag(28) + pow(2,29)*get_flag(29) + pow(2,30)*get_flag(30) + pow(2,31)*get_flag(31) + pow(2,32)*get_flag(32))
# socket_send_int(get_analog_out(0)*1000000)
# socket_send_int(get_analog_out(1)*1000000)
# socket_send_int(get_analog_in(0)*1000000)
# socket_send_int(get_analog_in(1)*1000000)
# socket_send_int(get_analog_in(2)*1000000)
# socket_send_int(get_analog_in(3)*1000000)
# exit_critical
# end
# send_io_state()
# while True:
# send_io_state()
# sync()
# end
# sync()
#end
ipa-fxm
committed
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
SERVO_IDLE = 0
SERVO_RUNNING = 1
cmd_servo_state = SERVO_IDLE
cmd_servo_id = 0 # 0 = idle, -1 = stop
cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
cmd_servo_dt = 0.0
def set_servo_setpoint(id, q, dt):
enter_critical
cmd_servo_state = SERVO_RUNNING
cmd_servo_id = id
cmd_servo_q = q
cmd_servo_dt = dt
exit_critical
end
thread servoThread():
state = SERVO_IDLE
while True:
# Latches the new command
enter_critical
q = cmd_servo_q
dt = cmd_servo_dt
id = cmd_servo_id
do_brake = False
if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE):
# No command pending
do_brake = True
end
state = cmd_servo_state
cmd_servo_state = SERVO_IDLE
exit_critical
# Executes the command
if do_brake:
#stopj(1.0) # TODO
send_out("Braking")
sync()
elif state == SERVO_RUNNING:
servoj(q, 0, 0, dt)
Stuart Glaser
committed
#send_out("Idle")
#thread_state = run statePublisherThread()
#thread_wrench_state = run wrench_statePublisherThread()
#thread_io_state = run io_statePublisherThread()
thread_servo = run servoThread()
#movej([1.5,-0.4,-1.57,0,0,0], 3, 0.75, 1.0)
#t = 0
#while True:
# q = [1.5,0,-1.57,0,0,0]
# q[0] = q[0] + 0.2 * sin(0.25 * t*(2*pi))
# q[1] = q[1] - 0.2 + 0.2 * cos(0.25 * t*(2*pi))
# #servoj(q, 3, 1, 0.08)
# #send_out("servoed")
# set_servo_setpoint(t, q, 0.08)
# t = t + 0.08
# sleep(0.08)
#end
while True:
#send_out("Listening")
Stuart Glaser
committed
ll = socket_read_binary_integer(1)
#send_out("Received nothing")
elif ll[0] > 1:
send_out("Received too many things")
Stuart Glaser
committed
else:
mtype = ll[1]
if mtype == MSG_QUIT:
send_out("Received QUIT")
break
elif mtype == MSG_MOVEJ:
send_out("Received movej")
params_mult = socket_read_binary_integer(1+6+4)
if params_mult[0] == 0:
send_out("Received no parameters for movej message")
end
# Unpacks the parameters
waypoint_id = params_mult[1]
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]
a = params_mult[8] / MULT_jointstate
v = params_mult[9] / MULT_jointstate
t = params_mult[10] / MULT_time
r = params_mult[11] / MULT_blend
# Sends the command
send_out("movej started")
movej(q, a, v, t, r)
send_waypoint_finished(waypoint_id)
send_out("movej finished")
elif mtype == MSG_SERVOJ:
# Reads the parameters
params_mult = socket_read_binary_integer(1+6+1)
if params_mult[0] == 0:
send_out("Received no parameters for movej message")
end
# Unpacks the parameters
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]
t = params_mult[8] / MULT_time
#servoj(q, 3, 0.1, t)
#send_waypoint_finished(waypoint_id)
set_servo_setpoint(waypoint_id, q, t)
params = socket_read_binary_integer(1)
if params[0] == 0:
send_out("Received no parameters for setPayload message")
end
#send_out(payload)
send_out("Received new payload")
elif mtype == MSG_STOPJ:
send_out("Received stopj")
elif mtype == MSG_SET_DIGITAL_OUT:
#send_out("Received Digital Out Signal")
# Reads the parameters
params_mult = socket_read_binary_integer(2)
send_out("Received no parameters for set_digital_out message")
set_digital_out(params_mult[1], True)
elif params_mult[2] == 0:
set_digital_out(params_mult[1], False)
end
elif mtype == MSG_SET_FLAG:
#send_out("Received Set Flag Signal")
# Reads the parameters
params_mult = socket_read_binary_integer(2)
send_out("Received no parameters for set_flag message")
set_flag(params_mult[1], True)
elif params_mult[2] == 0:
set_flag(params_mult[1], False)
end
elif mtype == MSG_SET_ANALOG_OUT:
#send_out("Received Analog Out Signal")
# Reads the parameters
params_mult = socket_read_binary_integer(2)
send_out("Received no parameters for set_analog_out message")
set_analog_out(params_mult[1], (params_mult[2] / MULT_analog))
elif mtype == MSG_SET_TOOL_VOLTAGE:
#send_out("Received Tool Voltage Signal")
# Reads the parameters (also reads second dummy '0' integer)
params_mult = socket_read_binary_integer(2)
send_out("Received no parameters for set_tool_voltage message")
else:
send_out("Received unknown message type")
end
Stuart Glaser
committed
end
end
Stuart Glaser
committed
#sleep(1)
#kill thread_state
#kill thread_wrench_state
#kill thread_io_state