icub-client
allostatic_plot.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 from copy import copy
4 from time import sleep
5 import matplotlib.pyplot as plt
6 from matplotlib import get_backend
7 from pylab import get_current_fig_manager
8 from numpy import linspace
9 from collections import defaultdict
10 import yarp
11 import sys
12 
13 def change_drive_names(drive_names):
14  new_names = []
15  for name in drive_names:
16  if name == "exploration":
17  new_names.append("Knowledge acquisition")
18  elif name == "demonstration":
19  new_names.append("Knowledge expression")
20  elif name == "narration":
21  new_names.append("History narration")
22  elif name == "dummy":
23  new_names.append("This is a test")
24  else:
25  new_names.append(name)
26  return new_names
27 
28 default_geometry = dict(xpos=0.0, ypos=0.0, width=800, height=400)
29 
30 class AllostaticPlotModule(yarp.RFModule):
31  def configure(self, rf):
32 
33  for attr in ["xpos", "ypos", "width", "height"]:
34  a = rf.find(attr)
35  val = a.asInt() if not a.isNull() else default_geometry[attr]
36  setattr(self, attr, val)
37  self.win_size = 600
38 
39  self.module_name = "allostatic_plot"
40  self.setName(self.module_name)
41  self.homeo_rpc = yarp.Port()
42  self.homeo_rpc.open("/" + self.module_name +"/to_homeo_rpc")
43  yarp.Network.connect(self.homeo_rpc.getName(), "/homeostasis/rpc")
44  self.behaviorManager_rpc = yarp.Port()
45  self.behaviorManager_rpc.open("/" + self.module_name +"/to_behaviorManager_rpc")
46  yarp.Network.connect(self.behaviorManager_rpc.getName(), "/BehaviorManager/trigger:i")
47 
48  request = yarp.Bottle()
49  rep = yarp.Bottle()
50 
51  # Retrieve drive names from the homeostasis module
52  request.addString("names")
53  self.homeo_rpc.write(request, rep)
54  self.drives = []
55  names = rep.get(0).asList()
56  if not names:
57  return False
58  for i in range(names.size()):
59  self.drives.append(names.get(i).asString())
60 
61  # Retrieve behavior names from the behaviorManager module
62  request.clear()
63  rep.clear()
64  request.addString("names")
65  self.behaviorManager_rpc.write(request, rep)
66  self.behaviors = []
67  self.behavior_ports = []
68  names = rep.get(0).asList()
69  if not names:
70  return False
71  for i in range(names.size()):
72  self.behaviors.append(names.get(i).asString())
73  self.behavior_ports.append(yarp.BufferedPortBottle())
74  self.behavior_ports[-1].open("/" + self.module_name + "/behaviors/" + self.behaviors[-1] + ":i")
75  yarp.Network.connect("/BehaviorManager/" + self.behaviors[-1] + "/start_stop:o", self.behavior_ports[-1].getName())
76 
77  # Retrieve homeostasis boundaries
78  self.homeo_mins = []
79  self.homeo_maxs = []
80  for d in self.drives:
81  request.clear()
82  request.addString("ask")
83  request.addString(d)
84  request.addString("min")
85  rep.clear()
86  self.homeo_rpc.write(request, rep)
87  self.homeo_mins.append(rep.get(0).asDouble())
88 
89  request.clear()
90  request.addString("ask")
91  request.addString(d)
92  request.addString("max")
93  rep.clear()
94  self.homeo_rpc.write(request, rep)
95  self.homeo_maxs.append(rep.get(0).asDouble())
96 
97 
98  # Connect to the homeostasis module to get drive values
99  self.drive_value_ports = [yarp.BufferedPortBottle() for _ in self.drives]
100  for i, d in enumerate(self.drives):
101  self.drive_value_ports[i].open("/" + self.module_name +"/" + d + ":i")
102  yarp.Network.connect("/homeostasis/" + d + "/max:o", self.drive_value_ports[i].getName())
103 
104 
105 
106  # Init matplotlib stuff
107  min_val = min(self.homeo_mins)
108  max_val = max(self.homeo_maxs)
109  center_val = (max_val - min_val) / 2.
110  self.y_min = -0.1
111  self.y_max = 1.1
112  self.fig = plt.figure()
113  thismanager = get_current_fig_manager()
114  thismanager.window.setGeometry(self.xpos, self.ypos, self.width, self.height)
115  self.ax = plt.axes(xlim=(0, self.win_size), ylim=(self.y_min, self.y_max))
116  self.colors = plt.cm.viridis(linspace(0,1,len(self.drives)+2))
117  self.value_lines = [self.ax.plot([], [], linestyle = '-', color=self.colors[i+1], lw=2, label=d) for i,d in enumerate(self.drives)]
118  self.homeo_min_lines = [self.ax.plot([], [], linestyle = '--', color=self.colors[i+1], lw=1) for i,d in enumerate(self.drives)]
119  self.homeo_max_lines = [self.ax.plot([], [], linestyle = '--', color=self.colors[i+1], lw=1) for i,d in enumerate(self.drives)]
120  plt.legend(change_drive_names(self.drives))
121  self.ax.set_xlim(0- self.win_size, 0)
122 
123 
124  self.drive_values = [[0.] * self.win_size for _ in self.drives]
125 
126  self.behaviors_to_plot = defaultdict(list)
127 
128  self.t = 0
129 
130  return True
131 
132  def reconnect_ports(self):
133  everything_connected = True
134  for name_in, port_out in zip(self.behaviors, self.behavior_ports):
135  in_port, out_port = "/BehaviorManager/" + name_in + "/start_stop:o", port_out.getName()
136  if not yarp.Network.isConnected(in_port, out_port):
137  if not yarp.Network.connect(in_port, out_port):
138  print "Could not connect to /BehaviorManager/" + name_in + "/start_stop:o"
139  everything_connected = False
140 
141  if not yarp.Network.isConnected(self.behaviorManager_rpc.getName(), "/BehaviorManager/trigger:i"):
142  if not yarp.Network.connect(self.behaviorManager_rpc.getName(), "/BehaviorManager/trigger:i"):
143  print "Could not connect to /BehaviorManager/trigger:i"
144  everything_connected = False
145 
146  for i, d in enumerate(self.drives):
147  in_port, out_port = "/homeostasis/" + d + "/max:o", self.drive_value_ports[i].getName()
148  if not yarp.Network.isConnected(in_port, out_port):
149  if not yarp.Network.connect(in_port, out_port):
150  print "Coult not connect to /homeostasis/" + d + "/max:o", self.drive_value_ports[i].getName()
151  everything_connected = False
152 
153  if not yarp.Network.isConnected(self.homeo_rpc.getName(), "/homeostasis/rpc"):
154  if not yarp.Network.connect(self.homeo_rpc.getName(), "/homeostasis/rpc"):
155  print "Could not connect to /homeostasis/rpc"
156  everything_connected = False
157 
158  return everything_connected
159 
160 
161  def close(self):
162  print "Closing ports..."
163  self.homeo_rpc.close()
165  for p in self.behavior_ports:
166  p.close()
167  for p in self.drive_value_ports:
168  p.close()
169 
170  def interruptModule(self):
171  print "Interrupting ports..."
172  self.homeo_rpc.interrupt()
173  self.behaviorManager_rpc.interrupt()
174  for p in self.behavior_ports:
175  p.interrupt()
176  for p in self.drive_value_ports:
177  p.interrupt()
178  return True
179 
180  def getPeriod(self):
181  return 0.1
182 
183  def one_step(self,t):
184  if t % (10 / self.getPeriod()) == 0:
185  if not self.reconnect_ports():
186  yarp.Time.delay(0.1)
187  return
188 
189  self.drive_values = [values[1:] + [0.] for values in self.drive_values]
190  for i, (port, homeo_max, v_line, min_line, max_line) in enumerate(zip(self.drive_value_ports, self.homeo_maxs, self.value_lines, self.homeo_min_lines, self.homeo_max_lines)):
191  res = port.read(False)
192  if res is not None:
193  self.drive_values[i][-1] = res.get(0).asDouble() + homeo_max
194  v_line[0].set_data(range(0- self.win_size, 0), self.drive_values[i])
195  min_line[0].set_data((0- self.win_size, 0), (self.homeo_mins[i], self.homeo_mins[i]))
196  max_line[0].set_data((0- self.win_size, 0), (self.homeo_maxs[i], self.homeo_maxs[i]))
197 
198  for key, plotitem_list in self.behaviors_to_plot.iteritems():
199  for plotitem in plotitem_list:
200  plotitem[0].set_x(plotitem[0].get_x() - 1)
201  plotitem[1].set_x(plotitem[0].get_x() + 5)
202  if -(plotitem[0].get_x() + plotitem[0].get_width()) > self.win_size:
203  # the plotted rectangle is outside the x-axis from the left
204  plotitem[1].remove()
205  plotitem_list.pop(0)
206 
207  for name, port in zip(self.behaviors, self.behavior_ports):
208  res = port.read(False)
209  if res is not None:
210  msg = res.get(0).asString()
211  print name, msg
212  if msg == "start":
213  new_rectangle = plt.Rectangle(xy=(0, self.y_min), width=10000, height=(self.y_max-self.y_min)/20.)
214  plt.gca().add_patch(new_rectangle)
215  new_text = plt.text(5, self.y_min+0.025, name, horizontalalignment='left', color="gray")
216  self.behaviors_to_plot[name].append((new_rectangle, new_text))
217  print "Behavior " + name + " starts"
218  elif msg == "stop":
219  self.behaviors_to_plot[name][-1][0].set_width(-self.behaviors_to_plot[name][-1][0].get_x())
220  print "Behavior " + name + " stops"
221  print "number of behaviors to plot = ", sum([len(behs) for behs in self.behaviors_to_plot.values()]), self.behaviors_to_plot
222 
223  plt.draw()
224  plt.pause(0.1)
225 
226  def updateModule(self):
227  self.one_step(self.t)
228  self.t += 1
229  return True
230 
231 
232 if __name__ == '__main__':
233 
234  plt.ion()
235  yarp.Network.init()
237  rf = yarp.ResourceFinder()
238  rf.configure(sys.argv)
239 
240  mod.runModule(rf)
def change_drive_names(drive_names)