diff --git a/stl/test/wstl_syntax_tests.py b/stl/test/wstl_syntax_tests.py new file mode 100644 index 0000000..3c6e1ce --- /dev/null +++ b/stl/test/wstl_syntax_tests.py @@ -0,0 +1,38 @@ +''' + Copyright (C) 2020 Cristian Ioan Vasile + Explainable Robotics Lab, Lehigh University + See license.txt file for license information. +''' + +from StringIO import StringIO + +from antlr4 import InputStream, CommonTokenStream + +import sys +sys.path.append('..') + +from wstlLexer import wstlLexer +from wstlParser import wstlParser +from wstlVisitor import wstlVisitor + +# Parse the WSTL formulae strings + +with open('wstl_syntax_tests.txt', 'rt') as fin: + text = StringIO() + for line in fin: + if line.strip(): + text.write(line) + else: + input_string = text.getvalue() + print(input_string.strip()) + lexer = wstlLexer(InputStream(input_string)) + tokens = CommonTokenStream(lexer) + parser = wstlParser(tokens) + t = parser.wstlProperty() + if t.weight is None: + print(None) + else: + print(t.weight.text) + + text.close() + text = StringIO() diff --git a/stl/test/wstl_syntax_tests.txt b/stl/test/wstl_syntax_tests.txt new file mode 100644 index 0000000..d8659b2 --- /dev/null +++ b/stl/test/wstl_syntax_tests.txt @@ -0,0 +1,49 @@ +<>[1, 3] (y <= 2) + +[][1, 3] (y <= 2) + +(y > 1) U[1, 3] (y <= 2) + +(y > 1) R[1, 3] (y <= 2) + +(y > 1) & (y <= 2) + +(y > 1) & (y <= 2) & (y > 3) + +(y > 1) | (y <= 2) | (y > 3) + +(y > 1) | (y <= 2) + +(y > 1) & (y <= 2) | (y > 3) + +& (y <= 2, y > 3, x < 0.2) + +| (y <= 2, y > 3, x < 0.2) + +<>[1, 3]^weight1 (y <= 2) + +[][1, 3]^weight1 (y <= 2) + +(y > 1) U[1, 3]^weight1 (y <= 2) + +(y > 1) R[1, 3]^weight1 (y <= 2) + +(y > 1) &^weight1 (y <= 2) + +(y > 1) &^weight1 (y <= 2) &^weight2 (y > 3) + +(y > 1) |^weight1 (y <= 2) |^weight2 (y > 3) + +(y > 1) |^weight1 (y <= 2) + +(y > 1) &^weight1 (y <= 2) |^weight2 (y > 3) + +&^weight1 (y <= 2, y > 3, x < 0.2) + +|^weight1 (y <= 2, y > 3, x < 0.2) + +&^weight ( + [][1, 2]^w1 (x < 3), + <>[3, 5]^w2 (y > 2), + (z < 6) |^p3 (e = 2) +) diff --git a/stl/test/wstl_test.py b/stl/test/wstl_test.py new file mode 100644 index 0000000..ddd62cb --- /dev/null +++ b/stl/test/wstl_test.py @@ -0,0 +1,78 @@ +from antlr4 import InputStream, CommonTokenStream + +import numpy as np + +import sys +sys.path.append('..') + +from stl import Operation, RelOperation, STLFormula +from wstlLexer import wstlLexer +from wstlParser import wstlParser +from wstlVisitor import wstlVisitor + +from wstl import WSTLAbstractSyntaxTreeExtractor + +from wstl2milp import wstl2milp + +from gurobipy import * + + +# Create a new model +m = Model("mip1") + +# Parse the WSTL formula string +# lexer = wstlLexer(InputStream("<>[1, 3]^weight1 (y <= 2)")) +# lexer = wstlLexer(InputStream("[][1, 3]^weight1 (y <= 2)")) +# lexer = wstlLexer(InputStream("(y > 1) U[1, 3]^weight1 (y <= 2)")) +# lexer = wstlLexer(InputStream("(y > 1) &^weight1 (y <= 2)")) +# lexer = wstlLexer(InputStream("(y > 1) &^weight1 (y <= 2) &^weight2 (y > 3)")) +# lexer = wstlLexer(InputStream("(y > 1) |^weight1 (y <= 2) |^weight2 (y > 3)")) +# lexer = wstlLexer(InputStream("(y > 1) |^weight1 (y <= 2)")) +# lexer = wstlLexer(InputStream("(y > 1) &^weight1 (y <= 2) |^weight2 (y > 3)")) +# lexer = wstlLexer(InputStream("&&^weight1 (F[2, 5] (y <= 2), y > 3, x < 0.2)")) + +lexer = wstlLexer(InputStream("&&^weight1 (F[1, 4]^weight2 (x >= 2), x > 0.1)")) +tokens = CommonTokenStream(lexer) +parser = wstlParser(tokens) +t = parser.wstlProperty() + +# Get AST from parse tree +weights = {'weight1': lambda x: 0.5, 'weight2': lambda x: 0.25} +ast = WSTLAbstractSyntaxTreeExtractor(weights).visit(t) + +# Translate WSTL to MILP and retrieve integer variable for the formula +wstl_milp = wstl2milp(ast, model=m) +z_formula, rho_formula = wstl_milp.translate() + +# Get/create state variables +varname = 'x' +end_time = 4 +x = [wstl_milp.variables[varname].get(time, m.addVar(lb= -GRB.INFINITY, + ub= GRB.INFINITY)) + for time in range(end_time + 1)] +m.update() + +# Add dynamics constraints +for time in range(end_time): # example of state transition (system dynamics) + m.addConstr(x[time+1] == x[time] - 0.1) + +for time in range(end_time + 1): # example if state constraints (e.g., safety) + m.addConstr(x[time] >= 0) + +# Set objective +m.setObjective(rho_formula, GRB.MAXIMIZE) +m.update() + +m.write('milp.mps') + +# Solve problem +m.optimize() + +if (m.status == 2): + y = np.array([x[i].X for i in range(end_time+1)], dtype=np.float) +else: + print("cannot solve...") + y = None + +# Print solution +print(y) diff --git a/stl/wstl.g4 b/stl/wstl.g4 new file mode 100644 index 0000000..37dbc3a --- /dev/null +++ b/stl/wstl.g4 @@ -0,0 +1,58 @@ +grammar wstl; + +@header { +''' + Copyright (C) 2020 Cristian Ioan Vasile + Explainable Robotics Lab, Lehigh University + See license.txt file for license information. +''' +} + + +wstlProperty: + '(' child=wstlProperty ')' #parprop + | booleanExpr #booleanPred + | op=NOT child=wstlProperty #formula + | op=EVENT '[' low=RATIONAL ',' high=RATIONAL ']' ('^' weight=VARIABLE)? + child=wstlProperty #formula + | op=ALWAYS '[' low=RATIONAL ',' high=RATIONAL ']' ('^' weight=VARIABLE)? + child=wstlProperty #formula + | left=wstlProperty op=IMPLIES right=wstlProperty #formula + | left=wstlProperty op=AND ('^' weight=VARIABLE)? + right=wstlProperty #formula + | op=AND ('^' weight=VARIABLE)? + '(' wstlProperty (',' wstlProperty)+ ')' #longFormula + | left=wstlProperty op=OR ('^' weight=VARIABLE)? + right=wstlProperty #formula + | op=OR ('^' weight=VARIABLE)? + '(' wstlProperty (',' wstlProperty)+ ')' #longFormula + | left=wstlProperty op=UNTIL '[' low=RATIONAL ',' high=RATIONAL ']' + ('^' weight=VARIABLE)? right=wstlProperty #formula + | left=wstlProperty op=RELEASE '[' low=RATIONAL ',' high=RATIONAL ']' + ('^' weight=VARIABLE)? right=wstlProperty #formula + ; +expr: + ( '-(' | '(' ) expr ')' + | expr '^' expr + | VARIABLE '(' expr ')' + | expr ( '*' | '/' ) expr + | expr ( '+' | '-' ) expr + | RATIONAL + | VARIABLE + ; +booleanExpr: + left=expr op=( '<' | '<=' | '=' | '>=' | '>' ) right=expr + | op=BOOLEAN + ; +AND : '&' | '&&' | '/\\' ; +OR : '|' | '||' | '\\/' ; +IMPLIES : '=>' ; +NOT : '!' | '~' ; +EVENT : 'F' | '<>' ; +ALWAYS : 'G' | '[]' ; +UNTIL : 'U' ; +RELEASE : 'R' ; +BOOLEAN : 'true' | 'True' | 'false' | 'False' ; +VARIABLE : ( [a-z] | [A-Z] )( [a-z] | [A-Z] | [0-9] | '_' )* ; +RATIONAL : ('-')? [0-9]* ('.')? [0-9]+ ( 'E' | 'E-' )? [0-9]* ; +WS : ( ' ' | '\t' | '\r' | '\n' )+ -> skip ; diff --git a/stl/wstl.py b/stl/wstl.py new file mode 100644 index 0000000..3e37a34 --- /dev/null +++ b/stl/wstl.py @@ -0,0 +1,311 @@ +''' + Copyright (C) 2020 Cristian Ioan Vasile + Explainable Robotics Lab, Lehigh University + See license.txt file for license information. +''' + +import itertools as it + +import numpy as np +from antlr4 import InputStream, CommonTokenStream + +from stl import Operation, RelOperation, STLFormula, Trace +from wstlLexer import wstlLexer +from wstlParser import wstlParser +from wstlVisitor import wstlVisitor + + +class WSTLFormula(STLFormula): + '''Abstract Syntax Tree representation of an WSTL formula. The class is + derived from STLFormula. + ''' + + def __init__(self, operation, **kwargs): + '''Constructor + + Parameters + ---------- + operation (stl.Operation) operation code. + value (bool, optional) value of a Boolean constant. + variable (string, optional) name of a predicate's variable. + relation (stl.RelOperation, optional) relation of a predicate. + threshold (number, optional) threshold of a predicate. + left (WSTLFormula, optional) left AST subtree of binary operations. + right (WSTLFormula, optional) right AST subtree of binary operations. + child (WSTLFormula, optional) child AST subtree of unary operations. + children (WSTLFormula, optional) children AST subtrees of n-ary + operations (i.e., disjuction and conjuction). + low (int, optional) lower bound of the time interval associate with a + temporal operator. + high (int, optional) upper bound of the time interval associate with a + temporal operator. + weight (function, optional, default: None) weight function associated + with weighted operators. + ''' + STLFormula.__init__(self, operation, **kwargs) + + if self.op in (Operation.AND, Operation.OR, Operation.ALWAYS, + Operation.EVENT, Operation.UNTIL): + self.weight = kwargs.get('weight', None) + + self.__string = None # string representation cache for quick lookup + + def robustness(self, s, t, maximumRobustness=1): + '''Computes the robustness of the STL formula. + + Parameters + ---------- + s (stl.Trace) a signal + t (number) time instant + + Returns + ------- + (number) the traditional robustness of signal `s` at time `t` with + respect to the formula `self` + ''' + if self.op in (Operation.BOOL, Operation.PRED, Operation.NOT, + Operation.IMPLIES): + return STLFormula.robustness(self, s, t, maximumRobustness) + elif self.weight is None: + return STLFormula.robustness(self, s, t, maximumRobustness) + elif self.op == Operation.AND: + return min(child.robustness(s, t) * self.weight(k) + for k, child in enumerate(self.children)) + elif self.op == Operation.OR: + return max(child.robustness(s, t) * self.weight(k) + for k, child in enumerate(self.children)) + elif self.op == Operation.UNTIL: + r_acc = min(self.left.robustness(s, t+tau) + for tau in np.arange(self.low+1)) + rleft = (self.left.robustness(s, t+tau) + for tau in np.arange(self.low, self.high+1)) + rright = (self.right.robustness(s, t+tau) * self.weight(tau) + for tau in np.arange(self.low, self.high+1)) + value = -maximumRobustness + for rl, rr in zip(rleft, rright): + r_acc = min(r_acc, rl) + r_conj = min(r_acc, rr) + value = max(value, r_conj) + return value + elif self.op == Operation.ALWAYS: + return min(self.child.robustness(s, t+tau) * self.weight(tau) + for tau in np.arange(self.low, self.high+1)) + elif self.op == Operation.EVENT: + return max(self.child.robustness(s, t+tau) * self.weight(tau) + for tau in np.arange(self.low, self.high+1)) + + def __str__(self): + '''Computes the string representation. The result is cached internally + for quick subsequent calls. + + Returns + ------- + (string) formula string + ''' + if self.__string is not None: + return self.__string + + opname = Operation.getString(self.op) + if self.op == Operation.BOOL: + s = str(self.value) + elif self.op == Operation.PRED: + s = '({v} {rel} {th})'.format(v=self.variable, th=self.threshold, + rel=RelOperation.getString(self.relation)) + elif self.op == Operation.IMPLIES: + s = '{left} {op} {right}'.format(left=self.left, op=opname, + right=self.right) + elif self.op == Operation.NOT: + s = '{op} {child}'.format(op=opname, child=self.child) + else: + if self.weight is None: + op_weight = '' + else: + op_weight = '^{weight}'.format(weight=self.weight.__name__) + + if self.op in (Operation.AND, Operation.OR): + children = [str(child) for child in self.children] + join_str = ' {op}{weight} '.format(op=opname, weight=op_weight) + s = '(' + join_str.join(children) + ')' + elif self.op == Operation.UNTIL: + s = '({left} {op}[{low}, {high}]{weight} {right})'.format( + op=opname, weight=op_weight, left=self.left, + right=self.right, low=self.low, high=self.high) + elif self.op in (Operation.ALWAYS, Operation.EVENT): + s = '({op}[{low}, {high}]{weight} {child})'.format(op=opname, + weight=op_weight, low=self.low, high=self.high, + child=self.child) + self.__string = s + return self.__string + + +class WSTLAbstractSyntaxTreeExtractor(wstlVisitor): + '''Parse Tree visitor that constructs the AST of an WSTL formula''' + + def __init__(self, predicate_weights): + '''Constructor + + Parameters + ---------- + predicate_weights (dictionary) maps the names of predicate weights to + funtions implementing them. + ''' + wstlVisitor.__init__(self) + self.predicate_weights = predicate_weights + + def getWeight(self, ctx): + '''Returns the weight function from the node/rule context. + + Parameters + ---------- + ctx (ParseRuleContext) the context of a rule. + + Returns + ------- + weight (function) the implementation of weight + ''' + if ctx.weight is None: + weight = None + else: + weight = self.predicate_weights[ctx.weight.text] + # make sure the function retains the name from the specification + if weight.__name__ != ctx.weight.text: + weight.__name = ctx.weight.text + return weight + + def visitFormula(self, ctx): + '''Transforms a parse tree associated with Boolean and temporal + operators into an AST of the associated WSTL formula. + + Parameters + ---------- + ctx (ParseRuleContext) the context of a rule. + + Returns + ------- + (wstl.WSTLFormula) AST of the WSTL formula. + ''' + op = Operation.getCode(ctx.op.text) + ret = None + low = -1 + high = -1 + if op in (Operation.AND, Operation.OR): + left = self.visit(ctx.left) + right = self.visit(ctx.right) + weight = self.getWeight(ctx) + assert op != right.op + if left.op == op and left.weight == weight: + children = left.children + else: + children = [left] + children.append(right) + ret = WSTLFormula(op, children=children, weight=weight) + elif op == Operation.IMPLIES: + ret = WSTLFormula(op, left=self.visit(ctx.left), + right=self.visit(ctx.right)) + elif op == Operation.NOT: + ret = WSTLFormula(op, child=self.visit(ctx.child)) + elif op == Operation.UNTIL: + low = float(ctx.low.text) + high = float(ctx.high.text) + weight = self.getWeight(ctx) + ret = WSTLFormula(op, weight=weight, left=self.visit(ctx.left), + right=self.visit(ctx.right), low=low, high=high) + elif op in (Operation.ALWAYS, Operation.EVENT): + low = float(ctx.low.text) + high = float(ctx.high.text) + ret = WSTLFormula(op, weight=self.getWeight(ctx), + child=self.visit(ctx.child), low=low, high=high) + else: + raise ValueError('Error: unknown operation {}!'.format(ctx.op.text)) + return ret + + def visitLongFormula(self, ctx): + '''Transforms a parse tree associated with Boolean disjuction and + conjuction operators in long format into an AST of the associated WSTL + formula. + + Parameters + ---------- + ctx (ParseRuleContext) the context of a rule. + + Returns + ------- + (wstl.WSTLFormula) AST of the WSTL formula. + ''' + op = Operation.getCode(ctx.op.text) + assert op in (Operation.AND, Operation.OR) + # extract child sub-formulae + children = (self.visit(child) for child in ctx.getChildren()) + children = [child for child in children if child is not None] + return WSTLFormula(op, children=children, weight=self.getWeight(ctx)) + + def visitBooleanPred(self, ctx): + '''Parses Boolean predicates. + + Parameters + ---------- + ctx (ParseRuleContext) the context of a rule. + + Returns + ------- + (wstl.WSTLFormula) AST of the WSTL predicate. + ''' + return self.visit(ctx.booleanExpr()) + + def visitBooleanExpr(self, ctx): + '''Transforms a parse tree associated with Boolean expression of a + predicate into an AST terminal leaf. + + Parameters + ---------- + ctx (ParseRuleContext) the context of a rule. + + Returns + ------- + (wstl.WSTLFormula) AST of the WSTL predicate. + ''' + return WSTLFormula(Operation.PRED, + relation=RelOperation.getCode(ctx.op.text), + variable=ctx.left.getText(), threshold=float(ctx.right.getText())) + + def visitParprop(self, ctx): + '''Parses properties within parantheses. + + Parameters + ---------- + ctx (ParseRuleContext) the context of a rule. + + Returns + ------- + (wstl.WSTLFormula) AST of the WSTL predicate. + ''' + return self.visit(ctx.child); + + +if __name__ == '__main__': + lexer = stlLexer(InputStream("!(x < 10) &&^p1 F[0, 2]^w1 y > 2" + " ||^p2 G[1, 3]^w2 z<=8")) + tokens = CommonTokenStream(lexer) + + parser = stlParser(tokens) + t = parser.stlProperty() + print(t.toStringTree()) + + predicate_weights = { + 'p1': lambda i: i + 1, + 'p2': lambda i: 2 - i, + 'w1': lambda x: 2 - np.abs(x - 1), + 'w2': lambda x: 1 + (x-2)**2 + } + ast = WSTLAbstractSyntaxTreeExtractor().visit(t) + print('AST:', ast) + + varnames = ['x', 'y', 'z'] + data = [[8, 8, 11, 11, 11], [2, 3, 1, 2, 2], [3, 9, 8, 9, 9]] + timepoints = [0, 1, 2, 3, 4] + s = Trace(varnames, timepoints, data) + + print('r:', ast.robustness(s, 0)) + + pnf = ast.pnf() + print(pnf) diff --git a/stl/wstl2milp.py b/stl/wstl2milp.py new file mode 100644 index 0000000..309c806 --- /dev/null +++ b/stl/wstl2milp.py @@ -0,0 +1,272 @@ +''' + Copyright (C) 2020 Cristian Ioan Vasile + Explainable Robotics Lab, Lehigh University + See license.txt file for license information. +''' + +import gurobipy as grb + +from stl import Operation, RelOperation, STLFormula + + +class wstl2milp(object): + '''Translate an WSTL formula to an MILP.''' + + def __init__(self, formula, ranges=None, vtypes=None, model=None): + self.formula = formula + + self.ranges = ranges + if ranges is None: + self.ranges = {v: (0, 10) for v in self.formula.variables()} + + self.vtypes = vtypes + if vtypes is None: + self.vtypes = {v: grb.GRB.CONTINUOUS for v in self.ranges} + + self.model = model + if model is None: + self.model = grb.Model('WSTL formula: {}'.format(formula)) + + self.M = 1000 + self.variables = dict() + self.hat_variables = dict() + + self.__milp_call = { + Operation.PRED : self.predicate, + Operation.AND : self.conjunction, + Operation.OR : self.disjunction, + Operation.EVENT : self.eventually, + Operation.ALWAYS : self.globally, + Operation.UNTIL : self.until, + # Operation.RELEASE : self.release #TODO: + } + + def translate(self, satisfaction=True): + '''Translates the STL formula to MILP from time 0.''' + z, rho = self.to_milp(self.formula) + if satisfaction: + self.model.addConstr(z == 1, 'formula_satisfaction') + return z, rho + + def to_milp(self, formula, t=0): + '''Generates the MILP from the STL formula.''' + (z, rho), added = self.add_formula_variables(formula, t) + if added: + self.__milp_call[formula.op](formula, z, rho, t) + return z, rho + + def add_formula_variables(self, formula, t): + '''Adds a variable for the `formula` at time `t`.''' + if formula not in self.variables: + self.variables[formula] = dict() + if t not in self.variables[formula]: + opname = Operation.getString(formula.op) + identifier = formula.identifier() + z_name = 'z_{}_{}_{}'.format(opname, identifier, t) + if formula.op == Operation.PRED: + z = self.model.addVar(vtype=grb.GRB.BINARY, name=z_name) + else: + z = self.model.addVar(vtype=grb.GRB.CONTINUOUS, name=z_name, + lb=0, ub=1) + rho_name = 'rho_{}_{}_{}'.format(opname, identifier, t) + rho = self.model.addVar(vtype=grb.GRB.CONTINUOUS, name=rho_name, + lb=-grb.GRB.INFINITY,ub=grb.GRB.INFINITY) + self.variables[formula][t] = (z, rho) + self.model.update() + return self.variables[formula][t], True + return self.variables[formula][t], False + + def add_hat_variable(self, formula, parent, t): + '''Adds a hat variable for the `formula` at time `t` + TODO: + + NOTE: Is caching correct, or does every disjunction, eventually, + and until need their own version? + ''' + if parent not in self.hat_variables: + self.hat_variables[parent] = dict() + if formula not in self.hat_variables[parent]: + self.hat_variables[parent][formula] = dict() + if t not in self.hat_variables[parent][formula]: + opname = Operation.getString(formula.op) + identifier = formula.identifier() + parent_identifier = parent.identifier() + z_name = 'zhat_{}_{}_{}_{}'.format(opname, identifier, + parent_identifier, t) + self.hat_variables[parent][formula][t] = self.model.addVar( + vtype=grb.GRB.CONTINUOUS, name=z_name, lb=0, ub=1) + self.model.update() + return self.hat_variables[parent][formula][t], True + return self.hat_variables[parent][formula][t], False + + def add_state(self, state, t): + '''Adds the `state` at time `t` as a variable.''' + if state not in self.variables: + self.variables[state] = dict() + if t not in self.variables[state]: + low, high = self.ranges[state] + vtype = self.vtypes[state] + name='{}_{}'.format(state, t) + v = self.model.addVar(vtype=vtype, lb=low, ub=high, name=name) + self.variables[state][t] = v + print 'Added state:', state, 'time:', t + self.model.update() + return self.variables[state][t] + + def predicate(self, pred, z, rho, t): + '''Adds a predicate to the model.''' + assert pred.op == Operation.PRED + v = self.add_state(pred.variable, t) + if pred.relation in (RelOperation.GE, RelOperation.GT): + self.model.addConstr(rho == v - pred.threshold) + self.model.addConstr(rho >= self.M * (1 - z)) + self.model.addConstr(rho <= self.M * z) + elif pred.relation in (RelOperation.LE, RelOperation.LT): + raise NotImplementedError + # self.model.addConstr(rho == pred.threshold - v) + else: + raise NotImplementedError + + def conjunction(self, formula, z, rho, t): + '''Adds a conjunction to the model.''' + assert formula.op == Operation.AND + vars_children = [self.to_milp(f, t) for f in formula.children] + z_sum = 0 + for k, (z_child, rho_child) in enumerate(vars_children): + self.model.addConstr(z <= z_child) + self.model.addConstr(rho <= (1 - formula.weight(k)) * rho_child + + self.M * (1 - z)) + self.model.addConstr(rho <= formula.weight(k) * rho_child + + self.M * z) + + def disjunction(self, formula, z, rho, t): + '''Adds a disjunction to the model.''' + assert formula.op == Operation.OR + z_children, rho_children = zip(*[self.to_milp(f, t) + for f in formula.children]) + z_hat_children,_ = zip(*[self.add_hat_variable(f, formula, t) + for f in formula.children]) + vars_children = zip(z_children, z_hat_children, rho_children) + for k, (z_child, z_hat_child, rho_child) in enumerate(vars_children): + weight = formula.weight(k) + self.model.addConstr(rho <= weight * rho_child + + self.M * (2 - z_hat_child - z)) + self.model.addConstr(rho <= (1 - weight) * rho_child + + self.M * (1 - z_hat_child + z)) + self.model.addConstr(z <= sum(z_children)) + self.model.addConstr(sum(z_hat_children) >= 1) + + def eventually(self, formula, z, rho, t): + '''Adds an eventually to the model.''' + assert formula.op == Operation.EVENT + a, b = int(formula.low), int(formula.high) + child = formula.child + z_children, rho_children = zip(*[self.to_milp(child, t + tau) + for tau in range(a, b+1)]) + z_hat_children, _ = zip(*[self.add_hat_variable(child, formula, t + tau) + for tau in range(a, b+1)]) + vars_children = zip(range(a, b+1), z_children, z_hat_children, + rho_children) + for tau, z_child, z_hat_child, rho_child in vars_children: + weight = formula.weight(tau) + self.model.addConstr(rho <= weight * rho_child + + self.M * (2 - z_hat_child - z)) + self.model.addConstr(rho <= (1 - weight) * rho_child + + self.M * (1 - z_hat_child + z)) + self.model.addConstr(z <= sum(z_children)) + self.model.addConstr(sum(z_hat_children) >= 1) + + def globally(self, formula, z, rho, t): + '''Adds a globally to the model.''' + assert formula.op == Operation.ALWAYS + a, b = int(formula.low), int(formula.high) + child = formula.child + vars_children = [self.to_milp(child, t + tau) for tau in range(a, b+1)] + for tau, (z_child, rho_child) in zip(range(a, b+1), vars_children): + self.model.addConstr(z <= z_child) + self.model.addConstr(rho <= formula.weight(tau) * rho_child + + self.M * (1 - z)) + self.model.addConstr(rho <= formula.weight(tau) * rho_child + + self.M * z) + + def until(self, formula, z, rho, t): + '''Adds an until to the model.''' + assert formula.op == Operation.UNTIL + + raise NotImplementedError #TODO: under construction + + a, b = formula.low, formula.high + z_children_left = [self.to_milp(formula.left, tau) + for tau in range(t, t+b+1)] + z_children_right = [self.to_milp(formula.right, tau) + for tau in range(t+a, t+b+1)] + + z_aux = [] + phi_alw = None + if a > 0: + phi_alw = STLFormula(Operation.ALWAYS, child=formula.left, + low=t, high=t+a-1) + for tau in range(t+a, t+b+1): + if tau > t+a: + phi_alw_u = STLFormula(Operation.ALWAYS, child=formula.left, + low=t+a, high=tau) + else: + phi_alw_u = formula.left + children = [formula.right, phi_alw_u] + if phi_alw is not None: + children.append(phi_alw) + phi = STLFormula(Operation.AND, children=children) + z_aux.append(self.add_formula_variables(phi, t)) + + for k, z_right in enumerate(z_children_right): + z_conj = z_aux[k] + self.model.addConstr(z_conj <= z_right) + for z_left in z_children_left[:t+a+k+1]: + self.model.addConstr(z_conj <= z_left) + m = 1 + (t + a + k) + self.model.addConstr(z_conj >= 1-m + z_right + + sum(z_children_left[:t+a+k+1])) + + self.model.addConstr(z >= z_conj) + self.model.addConstr(z <= sum(z_aux)) + + def release(self, formula, z, rho, t): + '''Adds a release to the model.''' + assert formula.op == Operation.RELEASE + + raise NotImplementedError #TODO: under construction + + a, b = formula.low, formula.high + z_children_left = [self.to_milp(formula.left, tau) + for tau in range(t, t+b+1)] + z_children_right = [self.to_milp(formula.right, tau) + for tau in range(t+a, t+b+1)] + + z_aux = [] + phi_alw = None + if a > 0: + phi_alw = STLFormula(Operation.ALWAYS, child=formula.left, + low=t, high=t+a-1) + for tau in range(t+a, t+b+1): + if tau > t+a: + phi_alw_u = STLFormula(Operation.ALWAYS, child=formula.left, + low=t+a, high=tau) + else: + phi_alw_u = formula.left + children = [formula.right, phi_alw_u] + if phi_alw is not None: + children.append(phi_alw) + phi = STLFormula(Operation.AND, children=children) + z_aux.append(self.add_formula_variables(phi, t)) + + for k, z_right in enumerate(z_children_right): + z_conj = z_aux[k] + self.model.addConstr(z_conj <= z_right) + for z_left in z_children_left[:t+a+k+1]: + self.model.addConstr(z_conj <= z_left) + m = 1 + (t + a + k) + self.model.addConstr(z_conj >= 1-m + z_right + + sum(z_children_left[:t+a+k+1])) + + self.model.addConstr(z >= z_conj) + self.model.addConstr(z <= sum(z_aux))