Newer
Older
MSG_OUT = 1
MSG_QUIT = 2
MSG_JOINT_STATES = 3
MULT_jointstate = 1000
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
thread statePublisherThread():
def send_joint_state():
q = get_joint_positions()
qdot = get_joint_speeds()
tau = get_joint_torques()
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]))
end
socket_open(HOSTNAME, 50002)
send_joint_state()
while True:
send_joint_state()
sync()
end
sync()
end
socket_open(HOSTNAME, 50001)
socket_send_int(1)
socket_send_string("hello~")
thread_state = run statePublisherThread()
movej([2.2,0,-1.57,0,0,0],2)
movej([1.5,0,-1.57,0,0,0],2)
#i = 0
#while i < 5:
# movej([2.2,0,-1.57,0,0,0],2)
# movej([1.5,0,-1.57,0,0,0],2)
# i = i + 1
#end
sleep(1)
kill thread_state
socket_send_int(2)
end
driverProg()