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
