11# dummyArm.py -- Python code for interfacing the sim with a virtual arm
2- #
2+ #
33# Last update: 07/21/14 ([email protected] ) 44
5- import matplotlib
5+ import matplotlib
66matplotlib .use ('TkAgg' ) # for visualization
7- from socket import *
8- import select # for socket reading
9- import struct # to pack messages for socket comms
7+ from socket import *
8+ import select # for socket reading
9+ import struct # to pack messages for socket comms
1010import numpy
1111from pylab import figure , show , ion , pause
1212
1313
14- # Initialize and setup the sockets and data format
14+ # Initialize and setup the sockets and data format
1515def setup ():
16- sendMsgFormat = "dd" # messages contain 2 doubles = joint angles of shoulder and elbow
17- receiveMsgFormat = "dd" # messages contain 2 doubles = velocities
18- receiveMsgSize = struct .calcsize (receiveMsgFormat )
19- localHostIP = "127.0.0.1" #"localhost"#"192.168.1.2"# # set IP for local connection
20-
21- print "Setting up connection..." # setup connection to model
22- sockReceive = socket (AF_INET , SOCK_DGRAM ) # create sockets
23- hostPortReceive = 31000 # set port for receiving packets
24- sockReceive .bind (('' , hostPortReceive )) # bind to port
25- sockReceive .setblocking (1 ) # Set blocking/non-blocking mode
26- print ( "Created UDP socket to receive packets from NEURON model; binded to port %d" % hostPortReceive )
27-
28- sockSend = socket (AF_INET , SOCK_DGRAM ) # connect to socket for sending packets
29- hostPortSend = 32000 # set port for sending packets
30- sockSend .connect ((localHostIP , hostPortSend ))
31- sockSend .setblocking (1 ) # Set blocking/non-blocking mode
32- print ( "Created UDP socket to send packets to NEURON model; socket connected to IP %s, port %d" % (localHostIP , hostPortSend ))
33-
34- return sendMsgFormat , receiveMsgFormat , sockReceive , sockSend
16+ sendMsgFormat = "dd" # messages contain 2 doubles = joint angles of shoulder and elbow
17+ receiveMsgFormat = "dd" # messages contain 2 doubles = velocities
18+ receiveMsgSize = struct .calcsize (receiveMsgFormat )
19+ localHostIP = "127.0.0.1" #"localhost"#"192.168.1.2"# # set IP for local connection
20+
21+ print ( "Setting up connection..." ) # setup connection to model
22+ sockReceive = socket (AF_INET , SOCK_DGRAM ) # create sockets
23+ hostPortReceive = 31000 # set port for receiving packets
24+ sockReceive .bind (('' , hostPortReceive )) # bind to port
25+ sockReceive .setblocking (1 ) # Set blocking/non-blocking mode
26+ print (( "Created UDP socket to receive packets from NEURON model; binded to port %d" % hostPortReceive ) )
27+
28+ sockSend = socket (AF_INET , SOCK_DGRAM ) # connect to socket for sending packets
29+ hostPortSend = 32000 # set port for sending packets
30+ sockSend .connect ((localHostIP , hostPortSend ))
31+ sockSend .setblocking (1 ) # Set blocking/non-blocking mode
32+ print (( "Created UDP socket to send packets to NEURON model; socket connected to IP %s, port %d" % (localHostIP , hostPortSend ) ))
33+
34+ return sendMsgFormat , receiveMsgFormat , sockReceive , sockSend
3535
3636# Send and receive packets to/from virtual arm
3737def sendAndReceivePackets (dataSend , sendMsgFormat , receiveMsgFormat , sockReceive , sockSend ):
38- dataReceived = [0 ,0 ]
39- try :
40- receiveMsgSize = struct .calcsize (receiveMsgFormat )
41- dataReceivedPacked = sockReceive .recv (receiveMsgSize ) # read packet from socket
42- if len (dataReceivedPacked ) == receiveMsgSize :
43- dataReceived = struct .unpack (receiveMsgFormat , dataReceivedPacked )
44- print "Received packet from model: (%.2f,%.2f)" % (dataReceived [0 ],dataReceived [1 ])
45- except :
46- print "Error receiving packet"
47-
48- inputready , outputready , e = select .select ([] ,[sockSend ],[], 0.0 ) # check if other side ready to receive
49- if len (outputready )> 0 :
50- try :
51- sent = sockSend .send (struct .pack (sendMsgFormat , dataSend [0 ], dataSend [1 ])) # send packet
52- print "Sent packet to virtual arm: (%.2f, %.2f)" % (dataSend [0 ], dataSend [1 ])
53- except :
54- print "Sending socket ready but error sending packet"
55- else :
56- print "Sending socket not ready"
57- return dataReceived
38+ dataReceived = [0 ,0 ]
39+ try :
40+ receiveMsgSize = struct .calcsize (receiveMsgFormat )
41+ dataReceivedPacked = sockReceive .recv (receiveMsgSize ) # read packet from socket
42+ if len (dataReceivedPacked ) == receiveMsgSize :
43+ dataReceived = struct .unpack (receiveMsgFormat , dataReceivedPacked )
44+ print ( "Received packet from model: (%.2f,%.2f)" % (dataReceived [0 ],dataReceived [1 ]) )
45+ except :
46+ print ( "Error receiving packet" )
47+
48+ inputready , outputready , e = select .select ([] ,[sockSend ],[], 0.0 ) # check if other side ready to receive
49+ if len (outputready )> 0 :
50+ try :
51+ sent = sockSend .send (struct .pack (sendMsgFormat , dataSend [0 ], dataSend [1 ])) # send packet
52+ print ( "Sent packet to virtual arm: (%.2f, %.2f)" % (dataSend [0 ], dataSend [1 ]) )
53+ except :
54+ print ( "Sending socket ready but error sending packet" )
55+ else :
56+ print ( "Sending socket not ready" )
57+ return dataReceived
5858
5959# Main code for simple virtual arm
6060duration = 4 # sec
6161interval = 0.010 # time between packets (sec)
62- L1 = 1.0 # arm segment 1 length
62+ L1 = 1.0 # arm segment 1 length
6363L2 = 0.8 # arm segment 2 length
64- shang = numpy .pi / 2 # shoulder angle (rad)
65- elang = numpy .pi / 2 # elbow angle (rad)
64+ shang = numpy .pi / 2 # shoulder angle (rad)
65+ elang = numpy .pi / 2 # elbow angle (rad)
6666shvel = 0 # shoulder velocity (rad/s)
6767elvel = 0 # elbow velocity (rad/s)
6868friction = 0.1 # friction coefficient
@@ -75,29 +75,21 @@ def sendAndReceivePackets(dataSend, sendMsgFormat, receiveMsgFormat, sockReceiv
7575ax .grid ()
7676line , = ax .plot ([], [], 'o-' , lw = 2 )
7777
78- raw_input ("Press Enter to continue..." )
78+ input ("Press Enter to continue..." )
7979for i in numpy .arange (0 , duration , interval ):
80- shang = (shang + shvel * interval ) % (2 * numpy .pi ) # update shoulder angle
81- elang = (elang + elvel * interval ) % (2 * numpy .pi )# update elbow angle
82- if shang < 0 : shang = 2 * numpy .pi + shang
83- if elang < 0 : elang = 2 * numpy .pi + shang
84- shpos = [L1 * numpy .sin (shang ), L1 * numpy .cos (shang )] # calculate shoulder x-y pos
85- elpos = [L2 * numpy .sin (shang + elang ) + shpos [0 ], L2 * numpy .cos (shang + elang ) + shpos [1 ]] # calculate elbow x-y pos
86-
87- dataSend = [shang , elang ] # set data to send
88- dataReceived = sendAndReceivePackets (dataSend , sendMsgFormat , receiveMsgFormat , sockReceive , sockSend ) # send and receive data
89- shvel = shvel + dataReceived [0 ] - (friction * shvel )# update velocities based on incoming commands (accelerations) and friction
90- elvel = elvel + dataReceived [1 ] - (friction * elvel )
91-
92- line .set_data ([0 , shpos [0 ], elpos [0 ]], [0 , shpos [1 ], elpos [1 ]]) # update line in figure
93- ax .set_title ('Time = %.1f ms, shoulder: pos=%.2f rad, vel=%.2f, acc=%.2f ; elbow: pos = %.2f rad, vel = %.2f, acc=%.2f' % (float (i )* 1000 , shang , shvel , dataReceived [0 ] - (friction * shvel ), elang , elvel , dataReceived [1 ] - (friction * elvel ) ), fontsize = 10 )
94- show ()
95- pause (0.0001 ) # pause so that the figure refreshes at every time step
96-
97-
98-
99-
100-
101-
102-
103-
80+ shang = (shang + shvel * interval ) % (2 * numpy .pi ) # update shoulder angle
81+ elang = (elang + elvel * interval ) % (2 * numpy .pi )# update elbow angle
82+ if shang < 0 : shang = 2 * numpy .pi + shang
83+ if elang < 0 : elang = 2 * numpy .pi + shang
84+ shpos = [L1 * numpy .sin (shang ), L1 * numpy .cos (shang )] # calculate shoulder x-y pos
85+ elpos = [L2 * numpy .sin (shang + elang ) + shpos [0 ], L2 * numpy .cos (shang + elang ) + shpos [1 ]] # calculate elbow x-y pos
86+
87+ dataSend = [shang , elang ] # set data to send
88+ dataReceived = sendAndReceivePackets (dataSend , sendMsgFormat , receiveMsgFormat , sockReceive , sockSend ) # send and receive data
89+ shvel = shvel + dataReceived [0 ] - (friction * shvel )# update velocities based on incoming commands (accelerations) and friction
90+ elvel = elvel + dataReceived [1 ] - (friction * elvel )
91+
92+ line .set_data ([0 , shpos [0 ], elpos [0 ]], [0 , shpos [1 ], elpos [1 ]]) # update line in figure
93+ ax .set_title ('Time = %.1f ms, shoulder: pos=%.2f rad, vel=%.2f, acc=%.2f ; elbow: pos = %.2f rad, vel = %.2f, acc=%.2f' % (float (i )* 1000 , shang , shvel , dataReceived [0 ] - (friction * shvel ), elang , elvel , dataReceived [1 ] - (friction * elvel ) ), fontsize = 10 )
94+ show ()
95+ pause (0.0001 ) # pause so that the figure refreshes at every time step
0 commit comments