def EulerBisect(t, x, p, hds, h, rx, n, debug=False, Zeno=False): """ trjs = EulerBisect(t, x, p, hds, h, rx, n, debug, Zeno) Compute Euler approximation to relaxed hybrid execution Inputs: t - 1 x 2 - time range x - 1 x n - initial state p - Struct - model paramters hds - hybrid model h - scalar - Euler integration stepsize rx - scalar - relaxation parameter n - int - maximum number of hybrid transitions debug - flag for printing debugging info Zeno - flag for quitting executions with short modes Outputs: trjs - list of trajectory dicts trj - trajectory dict .t - times .x - states .p - parameters .hds - hybrid model by Sam Burden, Humberto Gonzalez, Ram Vasudevan, Berkeley 2011 """ tf = t[1] # store final time h0 = h # store given stepsize p.debug = debug # enable model debugging t = np.array([t[0]]) x = x.copy() if len(x.shape) == 1: x.shape = (1,x.size) trj = Struct(t=t,x=x,p=p,hds=hds) trjs = [] # Euler loop while trj.t[-1] <= tf and not (trj.p.j == None) and len(trjs) < n: # initial data t0 = trj.t[-1] x0 = trj.x[-1,:] p0 = trj.p dx = trj.hds.F(t0, x0, p0) # Euler step j = trj.p.j t = t0 + h x = trj.hds.E(t0, x0, p0, h*dx) g = trj.hds.G(t, x, p0) # if transition occurs if ( g < -rx ): # find step size that lands trajectory on strip using bisection h = bisect( lambda s : trj.hds.G(t0+s,trj.hds.E(t0,x0,p0,s*dx),p0), (0.,h), tol=rx ) # debug if np.isnan(h): raise RuntimeError,'(euler) cannot step to strip' # Euler step t = t0 + h x = trj.hds.E(t0, x0, p0, h*dx) g = trj.hds.G(t, x, p0) # append state to trj trj.t = np.append(trj.t, [t], axis=1) trj.x = np.append(trj.x, [x], axis=0) if debug: print ' : j = %d g = %0.2e h = %0.2e t = %0.3f x = %s' % (j,g,h,t,str(x)) # if state is on strip if (g < 0): # spend time on the strip t = t + (rx + g) trj.t = np.append(trj.t, [t], axis=1) # can't move state without analytical strip trj.x = np.append(trj.x, [x], axis=0) if debug: print 'RX: j = %d g = %0.2e h = %0.2e t = %0.3f x = %s' % (j,g,h,t,str(x)) # append trj to trjs trjs += [trj] trj = trj.copy() if Zeno and (len(trj.t) <= 4): print '(euler) possible Zeno @ stepsize h = %0.6f' % h0 print 'RX: j = %d g = %0.2e h = %0.2e t = %0.3f x = %s' % (j,g,h,t,str(x)) return trjs # apply reset to modify trj t,x,p = hds.R(t,x,p0) if len(x.shape) == 1: x.shape = (1,x.size) t = np.array([t]) trj.t = t trj.x = x trj.p = p # re-initialize step size h = h0 if debug: print 'RX: j = %d g = %0.2e h = %0.2e t = %0.3f x = %s' % (j,g,h,t,str(x[0,:])) trjs += [trj] trj = trj.copy() return trjs
def EulerPlanar(t, x, p, hds, h, n, debug=False, Zeno=False): """ trjs = Euler(t, x, p, hds, h, n, debug, Zeno) *** UNFINISHED *** Compute Euler approximation to hybrid execution assuming guards are coordinate functions (thus transition time can be determined exactly) Inputs: t - 1 x 2 - time range x - 1 x n - initial state p - Struct - model paramters hds - hybrid model h - scalar - Euler integration stepsize n - int - maximum number of hybrid transitions debug - flag for printing debugging info Zeno - flag for quitting executions with short modes Outputs: trjs - list of trajectory dicts trj - trajectory dict .t - times .x - states .p - parameters .hds - hybrid model by Sam Burden, Humberto Gonzalez, Ram Vasudevan, Berkeley 2011 """ tf = t[1] # store final time h0 = h # store given stepsize p.debug = debug # enable model debugging t = np.array([t[0]]) x = x.copy() if len(x.shape) == 1: x.shape = (1,x.size) trj = Struct(t=t,x=x,p=p,hds=hds) trjs = [] # Euler loop while trj.t[-1] <= tf and not (trj.p.j == None) and len(trjs) < n: # initial data t0 = trj.t[-1] x0 = trj.x[-1,:] p0 = trj.p g0 = trj.hds.G(t, x, p0) # vector field dx = trj.hds.F(t0, x0, p0) # Euler step j = trj.p.j t = t0 + h x = trj.hds.E(t0, x0, p0, h*dx) g = trj.hds.G(t, x, p0) # if guard is triggered if g < 0: # move state exactly to transition s = g0 / trjs.hds.G(t, h*dx, p0) # halve step size until trajectory doesn't jump over strip k = 0 kmax = 50 while (g < -rx) and (k <= kmax): if debug: print 'RX: jump over strip #%d' % k h = h/2 t = t0 + h dx = h*dxdt + np.sqrt(h)*np.random.randn()*dxdw x = trj.hds.E(t, x0, p0, dx) g = trj.hds.G(t, x,p0) k += 1 if (k >= kmax): raise RuntimeError,'(euler) strip iterations exceeded' # append state to trj trj.t = np.append(trj.t, [t], axis=1) trj.x = np.append(trj.x, [x], axis=0) if debug: print ' : j = %d t = %0.3f x = %s' % (j,t,str(x)) # if state is on strip if (g < 0): # spend time on the strip t = t + (rx + g) trj.t = np.append(trj.t, [t], axis=1) # can't move state without analytical strip trj.x = np.append(trj.x, [x], axis=0) if debug: print 'RX: j = %d t = %0.3f x = %s' % (j,t,str(x)) # append trj to trjs trjs += [trj] trj = trj.copy() if Zeno and (len(trj.t) <= 4): print '(euler) possible Zeno @ stepsize h = %0.6f' % h0 print 'RX: j = %d t = %0.3f x = %s' % (j,t,str(x)) return trjs # apply reset to modify trj t,x,p = hds.R(t,x,p0) if len(x.shape) == 1: x.shape = (1,x.size) t = np.array([t]) trj.t = t trj.x = x trj.p = p # re-initialize step size h = h0 if debug: print 'RX: j = %d t = %0.3f x = %s' % (j,t,str(x[0,:])) trjs += [trj] trj = trj.copy() return trjs
def Euler(t, x, p, hds, h, rx, n, debug=False, Zeno=False): """ trjs = Euler(t, x, p, hds, h, rx, n, debug, Zeno) Compute Euler approximation to relaxed hybrid execution Inputs: t - 1 x 2 - time range x - 1 x n - initial state p - Struct - model paramters hds - hybrid model h - scalar - Euler integration stepsize rx - scalar - relaxation parameter n - int - maximum number of hybrid transitions debug - flag for printing debugging info Zeno - flag for quitting executions with short modes Outputs: trjs - list of trajectory dicts trj - trajectory dict .t - times .x - states .p - parameters .hds - hybrid model by Sam Burden, Humberto Gonzalez, Ram Vasudevan, Berkeley 2011 """ tf = t[1] # store final time h0 = h # store given stepsize p.debug = debug # enable model debugging t = np.array([t[0]]) x = x.copy() if len(x.shape) == 1: x.shape = (1,x.size) trj = Struct(t=t,x=x,p=p,hds=hds) trjs = [] # Euler loop while trj.t[-1] <= tf and not (trj.p.j == None) and len(trjs) < n: # do single Euler step t0 = trj.t[-1] x0 = trj.x[-1,:] p0 = trj.p dxdt = trj.hds.F(t0, x0, p0) dx = h*dxdt j = trj.p.j t = t0 + h x = trj.hds.E(t0, x0, p0, dx) g = trj.hds.G(t, x, p0) # halve step size until trajectory doesn't jump over strip k = 0 kmax = 50 while np.any(g < -rx) and (k <= kmax): #if debug: # print 'RX: jump over strip #%d' % k h = h/2. t = t0 + h dx = h*dxdt x = trj.hds.E(t0, x0, p0, dx) g = trj.hds.G(t, x,p0) k += 1 if (k >= kmax): raise RuntimeError,'(euler) strip iterations exceeded' # append state to trj trj.t = np.append(trj.t, [t], axis=1) trj.x = np.append(trj.x, [x], axis=0) if debug: print ' : j = %s, h = %0.2e, t = %0.3f, g = %s, x = %s' % (j,h,t,g,str(x)) # if state is on strip if np.any(g < 0): # spend time on the strip t = t + (rx + g.min()) trj.t = np.append(trj.t, [t], axis=1) # can't move state without analytical strip trj.x = np.append(trj.x, [x], axis=0) if debug: print 'RX: j = %s, t = %0.3f, g = %s, x = %s' % (j,t,g,str(x)) # append trj to trjs trjs += [trj] trj = trj.copy() if Zeno and (len(trj.t) <= 4): print '(euler) possible Zeno @ stepsize h = %0.6f' % h0 print 'RX: j = %s t = %0.3f\nx = %s' % (j,t,str(x)) return trjs # apply reset to modify trj t,x,p = hds.R(t,x,p0) if len(x.shape) == 1: x.shape = (1,x.size) t = np.array([t]) trj.t = t trj.x = x trj.p = p # re-initialize step size h = h0 if debug: j = p.j g = trj.hds.G(t[0], x[0], p) print 'RX: j = %s, t = %0.3f, g = %s, x = %s' % (j,t,g,str(x[0,:])) trjs += [trj] trj = trj.copy() return trjs