cbd

materials/lib/CBDSimulatorInterpreter/cbd/models/TrainCostModelBlock.py

@@ -0,0 +1,73 @@
+#!/usr/bin/env python
+from cbd.src.CBD import *
+
+class CostFunctionBlock(BaseBlock):        
+    def __init__(self, block_name):
+        BaseBlock.__init__(self, block_name, ["InVi","InVTrain","InDelta","InXPerson"], ["OutCost"])
+        self.viChanged = False
+        self.timeInWhichViChanged = 0.0
+        self.cummulativeCost = 0.0
+        
+    def compute(self, curIteration):
+        displacement_person = self.getInputSignal(curIteration, "InXPerson").value
+        velocity_train = self.getInputSignal(curIteration, "InVTrain").value
+        
+        if abs(displacement_person) > 0.4 or velocity_train<0.0:
+            raise StopSimulationException()
+        
+        currentVi = self.getInputSignal(curIteration, "InVi").value
+        currentTime = self.getClock().getTime()
+        lastVi = self.getInputSignal(curIteration-1, "InVi").value
+        if lastVi != currentVi:
+            self.viChanged = True
+            self.timeInWhichViChanged = currentTime
+            self.attainedVelocity = False
+        else:
+            self.viChanged = False
+        
+        lastVTrain = self.getInputSignal(curIteration-1, "InVTrain").value
+        currentVTrain = self.getInputSignal(curIteration, "InVTrain").value
+        if ((lastVTrain-currentVi)*(currentVTrain-currentVi) <= 0):
+            self.attainedVelocity = True;
+        
+        if (not self.attainedVelocity):
+            instantCostTime = currentTime - self.timeInWhichViChanged
+            assert instantCostTime >= 0
+            delta_t = self.getInputSignal(curIteration, "InDelta").value
+            self.cummulativeCost = self.cummulativeCost + instantCostTime*delta_t
+        
+        self.appendToSignal(self.cummulativeCost, name_output="OutCost")
+
+
+
+class AboveThresholdBlock(BaseBlock):
+    def __init__(self, block_name, threshold):
+        BaseBlock.__init__(self, block_name, ["IN1"], ["OUT1"])
+        self.threshold = threshold
+
+    def compute(self, curIteration):
+        self.appendToSignal(1.0 if self.getInputSignal(curIteration).value > self.threshold else -1.0)
+
+
+
+class StopSimulationBlock(BaseBlock):
+    def __init__(self, block_name):
+        BaseBlock.__init__(self, block_name, ["IN1"], [])
+
+    def compute(self, curIteration):
+        inSignalValue = self.getInputSignal(curIteration).value
+        if inSignalValue > 0.0:
+            raise StopSimulationException()
+        
+
+class StopSimulationException(Exception):
+    pass
+        
+
+
+
+
+
+
+
+

materials/lib/CBDSimulatorInterpreter/cbd/models/power_window.py

@@ -0,0 +1,149 @@
+from bokeh.plotting import output_file, figure, show
+
+from cbd.src.CBD import ConstantBlock, NegatorBlock, \
+    IntegratorBlock, CBD, ProductBlock, AdderBlock, InverterBlock, DecisionBlock
+from cbd.src.CBDDraw import draw
+
+
+DELTA = 0.01
+
+class PowerWindowUp(CBD):
+    def __init__(self, blockName, delta_t = DELTA):
+        CBD.__init__(self,blockName, input_ports=[], output_ports=["out_w0","out_v0", "out_Fo100","out_Fo","out_acceleration","out_acceleration_w"])
+        
+        self.addBlock(ConstantBlock("delta_t", value=delta_t))
+        self.addBlock(ConstantBlock("torque", value=2.0))
+        self.addBlock(ConstantBlock("1", value=1.0))
+        self.addBlock(ConstantBlock("0.4", value=0.4))
+        self.addBlock(ConstantBlock("force_threshold", value=-100))
+        
+        self.addBlock(ConstantBlock("in_w0", value=0.0))
+        self.addBlock(ConstantBlock("in_v0", value=0.0))
+        
+        self.addBlock(AdderBlock("compare_force"))
+        
+        self.addBlock(PowerWindowPlant("plant"))
+        
+        self.addConnection("delta_t", "plant", input_port_name="delta_t")
+        self.addConnection("torque", "plant", input_port_name="motor")
+        self.addConnection("1", "plant", input_port_name="object_detect")
+        self.addConnection("0.4", "plant", input_port_name="object_position")
+        self.addConnection("force_threshold", "compare_force")
+        
+        self.addConnection("in_w0", "plant", input_port_name="init_position")
+        self.addConnection("in_v0", "plant", input_port_name="init_velocity")
+        
+        self.addConnection("compare_force", "out_Fo100")
+        
+        self.addConnection("plant", "out_w0", output_port_name="position_out")
+        self.addConnection("plant", "out_v0", output_port_name="velocity_out")
+        self.addConnection("plant", "compare_force", output_port_name="force_out")
+        self.addConnection("plant", "out_Fo", output_port_name="force_out")
+        self.addConnection("plant", "out_acceleration", output_port_name="acceleration_out")
+        self.addConnection("plant", "out_acceleration_w", output_port_name="acceleration_w_out")
+        
+        
+class PowerWindowPlant(CBD):
+    def __init__(self, blockName, delta_t = 0.01):
+        CBD.__init__(self,blockName, input_ports=[
+                                    "object_detect",
+                                    "object_position",
+                                    "motor", 
+                                    "init_position",
+                                    "init_velocity",
+                                    "delta_t"
+                        ], output_ports=["position_out","velocity_out","force_out","acceleration_out","acceleration_w_out"])
+        
+        # See power_window_object.pdf
+        
+        self.addBlock(ConstantBlock("m", value=10.0))
+        self.addBlock(ConstantBlock("cw",value=-10.0))
+        self.addBlock(ConstantBlock("zero",value=0.0))
+        self.addBlock(ConstantBlock("k0",value=1000.0))
+        self.addBlock(ConstantBlock("c0",value=-1000.0))
+        
+        self.addBlock(InverterBlock("invert_mass"))
+        
+        self.addBlock(NegatorBlock("position_object_neg"))
+        
+        self.addBlock(ProductBlock("cw_p"))
+        self.addBlock(ProductBlock("divide_m"))
+        self.addBlock(ProductBlock("c0_p"))
+        self.addBlock(ProductBlock("k0_p"))
+        self.addBlock(ProductBlock("object_detect_enable"))
+        
+        self.addBlock(AdderBlock("torque_friction"))
+        self.addBlock(AdderBlock("total_forces"))
+        self.addBlock(AdderBlock("object_compare"))
+        self.addBlock(AdderBlock("object_compression"))
+        self.addBlock(AdderBlock("object_displacement"))
+        
+        self.addBlock(IntegratorBlock("velocity"))
+        self.addBlock(IntegratorBlock("position"))
+        
+        self.addBlock(DecisionBlock("obj_detected"))
+        
+        self.addConnection("object_detect", "object_detect_enable")
+        self.addConnection("object_position", "position_object_neg")
+        self.addConnection("motor", "torque_friction")
+        self.addConnection("init_position", "position", input_port_name="IC")
+        self.addConnection("init_velocity", "velocity", input_port_name="IC")
+        self.addConnection("delta_t", "position", input_port_name="delta_t")
+        self.addConnection("delta_t", "velocity", input_port_name="delta_t")
+        
+        self.addConnection("m", "invert_mass")
+        self.addConnection("zero", "obj_detected", input_port_name="F")
+        
+        self.addConnection("invert_mass", "divide_m")
+        
+        self.addConnection("cw", "cw_p")
+        self.addConnection("k0", "k0_p")
+        self.addConnection("c0", "c0_p")
+        self.addConnection("position_object_neg", "object_compare")
+        self.addConnection("position_object_neg", "object_displacement")
+        
+        
+        self.addConnection("cw_p", "torque_friction")
+        self.addConnection("divide_m", "velocity")
+        self.addConnection("c0_p", "object_compression")
+        self.addConnection("k0_p", "object_compression")
+        self.addConnection("object_detect_enable", "total_forces")
+        self.addConnection("object_detect_enable", "force_out")
+        
+        self.addConnection("torque_friction", "total_forces")
+        self.addConnection("torque_friction", "acceleration_w_out")
+        self.addConnection("total_forces", "divide_m")
+        self.addConnection("total_forces", "acceleration_out")
+        self.addConnection("object_compare", "obj_detected", input_port_name="C")
+        self.addConnection("object_compression", "obj_detected", input_port_name="T")
+        self.addConnection("object_displacement", "k0_p")
+        
+        self.addConnection("velocity", "position")
+        self.addConnection("velocity", "cw_p")
+        self.addConnection("velocity", "c0_p")
+        self.addConnection("velocity", "velocity_out")
+        self.addConnection("position", "object_displacement")
+        self.addConnection("position", "position_out")
+        self.addConnection("position", "object_compare")
+        
+        self.addConnection("obj_detected", "object_detect_enable")
+        
+
+cbd = PowerWindowUp("powerwindow")
+plantDot = draw(cbd.getBlockByName("plant"))
+plantDot.render('plant.pdf')
+
+cbd.run(int(6.0 / DELTA))
+
+times = []
+output = []
+
+for timeValuePair in cbd.getSignal("out_w0"):
+    times.append(timeValuePair.time)
+    output.append(timeValuePair.value)
+            
+#Plot
+output_file("./results.html", title="Even Numbers")
+p = figure(title="Plot", x_axis_label='time', y_axis_label='N')
+p.circle(x=times, y=output, legend="out_w0")
+show(p)

materials/lib/CBDSimulatorInterpreter/cbd/src/CBDDraw.py

@@ -0,0 +1,42 @@
+from graphviz import Digraph
+
+from cbd.src.CBD import ConstantBlock, CBD
+
+
+def draw(cbd):
+	'''
+	Output CBD as a dot digraph
+
+	:param cbd: the model
+	:return: a dot digraph
+	'''
+
+	dot = Digraph(comment='The model')
+
+	def writeBlock(block):
+		if isinstance(block, ConstantBlock):
+			label = block.getBlockType() + " (" + block.getBlockName() + ")\\n" + str(block.getValue())
+		else:
+			label = block.getBlockType() + " (" + block.getBlockName() + ")"
+
+		shape = ""
+		if isinstance(block, CBD):
+			shape="Msquare"
+
+		dot.node(block.getBlockName(), label, {'shape': shape})
+
+
+	for block in cbd.getBlocks():
+		writeBlock(block)
+		for (name, other) in  block.getLinksIn().items():
+			label = ""
+
+			if not name.startswith("IN"):
+				label = name
+
+			if not other.output_port.startswith("OUT"):
+				label = label + " / " + other.output_port
+
+			dot.edge(other.block.getBlockName(), block.getBlockName(), label)
+
+	return dot

materials/lib/CBDSimulatorInterpreter/cbd/src/naivelog.py

@@ -0,0 +1,129 @@
+import os
+import sys
+import datetime
+(DEBUG, INFO, WARNING, ERROR, FATAL) = (0, 1, 2, 3, 4)
+
+def strToLevel(elvl):
+	if elvl == "DEBUG":
+		return DEBUG
+	if elvl == "INFO":
+		return INFO
+	if elvl == "WARNING":
+		return WARNING
+	if elvl == "ERROR":
+		return ERROR
+	if elvl == "FATAL":
+		return FATAL
+	else:
+		return None
+
+def levelToStr(lvl):
+	if lvl == DEBUG:
+		return "DEBUG"
+	if lvl == INFO:
+		return "INFO"
+	if lvl == WARNING:
+		return "WARNING"
+	if lvl == ERROR:
+		return "ERROR"
+	if lvl == FATAL:
+		return "FATAL"
+	return None
+
+
+def levelToShortStr(lvl):
+	if lvl == DEBUG:
+		return "DBUG"
+	if lvl == INFO:
+		return "INFO"
+	if lvl == WARNING:
+		return "WARN"
+	if lvl == ERROR:
+		return "ERROR"
+	if lvl == FATAL:
+		return "FATAL"
+	return None
+
+class Logger:
+	def __init__(self, modulename, level, crashlevel):
+		self.__modulename = modulename
+		self.__level = level
+		self.__crashlevel = crashlevel
+
+	def debug(self, mainstr, *args, **kwargs):
+		self.log(DEBUG, mainstr, *args, **kwargs)
+	def info(self, mainstr, *args, **kwargs):
+		self.log(INFO, mainstr, *args, **kwargs)
+	def warning(self, mainstr, *args, **kwargs):
+		self.log(WARNING, mainstr, *args, **kwargs)
+	def error(self, mainstr, *args, **kwargs):
+		self.log(ERROR, mainstr, *args, **kwargs)
+	def fatal(self, mainstr, *args, **kwargs):
+		self.log(FATAL, mainstr, *args, **kwargs)
+
+	def log(self, level, mainstr, *args, **kwargs):
+		if level >= self.__level:
+			sys.stdout.write(self.formatmsg(level,str(mainstr).format(*args, **kwargs)))
+
+		if level >= self.__crashlevel:
+			exit(1)
+
+	def setLevel(self, level):
+		self.__level = level
+
+	def formatmsg(self, level, mainstr):
+		class bcolors:
+			HEADER = '\033[95m'
+			OKBLUE = '\033[94m'
+			OKGREEN = '\033[92m'
+			WARNING = '\033[93m'
+			FAIL = '\033[91m'
+			ENDC = '\033[0m'
+	
+
+		col = bcolors.OKGREEN
+		if level >= WARNING:
+			col = bcolors.WARNING
+		if level >= ERROR:
+			col = bcolors.FAIL
+
+		return "{startcol}[{now:%H:%M:%S.%f} {module} {lvl}] {mainstr}{endcol}\n".format(
+				lvl=levelToShortStr(level),
+				module=self.__modulename,
+				now=datetime.datetime.today(),
+				mainstr=mainstr,
+				startcol=col,
+				endcol=bcolors.ENDC);
+
+defaultLogLevel = INFO
+defaultCrashLevel = FATAL
+
+def getAbstractLogLevel(env, default):
+	elvl = os.environ[env] if env in os.environ else ''
+
+	lvl = strToLevel(elvl)
+	if lvl:
+		return lvl
+	else:
+		return default
+
+def getLogLevel():
+	return getAbstractLogLevel('NAIVE_LOGLEVEL', defaultLogLevel)
+
+def getCrashLevel():
+	return getAbstractLogLevel('NAIVE_CRASHLEVEL', defaultCrashLevel)
+
+def getLogger(modulename):
+	return Logger(modulename, getLogLevel(), getCrashLevel())
+
+if __name__ == "__main__":
+	l = getLogger('testmodule')
+	l.info("bla");
+	l.info("test nummer {}{}", 2, " is good")
+	l.info("test {hier} is ook ok", hier=3, daar=4)
+	l.info("should not see this")
+
+
+	l2 = getLogger('testmodule.m2')
+	l2.info("More info")
+	l2.info("and even more")

materials/lib/CBDSimulatorInterpreter/cbd/src/plot.py

@@ -0,0 +1,49 @@
+__author__ = 'joachimdenil'
+
+from math import ceil
+import matplotlib
+matplotlib.use('TkAgg')
+from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg, NavigationToolbar2TkAgg
+from matplotlib.figure import Figure
+from matplotlib.lines import Line2D
+import numpy as np
+import threading
+
+try:
+    import Tkinter as Tk
+except ImportError:
+    import tkinter as Tk
+
+class ScopeWindow():
+    def __init__(self, theOutportRefs, names):
+        """
+        Plot results in a Tk window using matlplotlib
+        @param theOutportRefs: array of values to plot: [[x1,x2,xn], ... ,[y1,y2, , yn]]
+        @param names: the labels for each of the plots: list [name1, name2, ...,  namen]
+        @return:
+        """
+        self.root = Tk.Tk()
+        self.f = Figure()
+        n = len(theOutportRefs)
+        n = int(ceil(n*1.00/2))
+        index = 1
+        self.ax = []
+        for outport in theOutportRefs:
+            self.ax.append(self.f.add_subplot(n, 2, index))
+            # add values:
+            self.ax[index-1].plot(outport, 'ro')
+            self.ax[index-1].set_xlabel('t')
+            self.ax[index-1].set_ylabel(names[index-1])
+            index+=1
+
+        self.canvas = FigureCanvasTkAgg(self.f, master=self.root)
+        self.canvas.get_tk_widget().pack(side=Tk.TOP, fill=Tk.BOTH, expand=1)
+        self.canvas.show()
+
+        self.toolbar = NavigationToolbar2TkAgg( self.canvas, self.root )
+        self.toolbar.pack(side=Tk.TOP, fill=Tk.BOTH, expand=1)
+        self.toolbar.update()
+
+        self.button = Tk.Button(master=self.root, text='Quit', command=self.root.destroy)
+        self.button.pack(side=Tk.BOTTOM)
+        self.root.mainloop()