import numpy as np import pylab as plt import scipy as sp import scipy.optimize as op try: from IPython.Debugger import Tracer; dbg = Tracer() except: dbg = lambda : 1/0 np.set_printoptions(precision=4) import relax as rx from relax import Struct euler = rx.Euler class Hop(rx.HDS): def __init__(self): """ hop = Hop() hop - HDS - Vertical Hopper hybrid model .F - vector field dx = F(t,x,p) dt .R - reset map t,x,p = R(t,x,p) .E - retraction x = E(t,x,p,v) .P - parameters p = P(q,debug) .j - discrete mode index .G - guard function g = G(t,x,p) .O - observations o = O(t,x,p) .plot(trjs) - generate plot from trajectory by Sam Burden, Berkeley 2013 """ rx.HDS.__init__(self) def P(self, q=[1.,2.,10.,0.1,1.,2,1.], debug=False): """ Parameters: q=[m,M,k,b,l,a,g] m,M - masses k,b,l - spring constant, damping coefficient, nominal length a - leg actuation magnitude g - gravitational constant debug - bool - flag for printing debugging info """ return Struct(j=None,q=q,debug=debug) def Leg(self, t, z, p): """ Leg vertical leg force """ m,M,k,b,l,a,g = p.q if p.j[1] == 1: x,y,dx,dy = z.T leg = k*(l - (y - x)) elif p.j[1] == 2: y,dy = z.T leg = a*k*(l - y) else: raise RuntimeError,"unknown discrete mode" return leg def G(self,t,z,p): """ g = G(t,z,p) g > 0 : guard inactive g = 0 : guard """ m,M,k,b,l,a,g = p.q if p.j[1] == 1: x,y,dx,dy = z.T g = [p.j[0]*dy,x] # vertical velocity, height of foot elif p.j[1] == 2: y,dy = z.T g = [p.j[0]*dy,g*m + k*(l - y)] # vertical velocity, ground reaction force in air mode else: raise RuntimeError,"unknown discrete mode" return np.asarray(g) def F(self,t,z,p): """ dz = F(t,z,p) """ m,M,k,b,l,a,g = p.q leg = self.Leg(t,z,p) if p.j[1] == 1: x,y,dx,dy = z.T ddx = (-g*m - leg - b*dx) / m ddy = (-g*M + leg) / M dz = [dx,dy,ddx,ddy] elif p.j[1] == 2: y,dy = z.T ddy = (-g*M + leg) / M dz = [dy,ddy] else: raise RuntimeError,"unknown discrete mode" return np.array(dz) def R(self,t,z,p): """ t,z,p = R(t,z,p) """ m,M,k,b,l,a,g = p.q t = t.copy() z = z.copy() p = p.copy(); p.j = p.j[:] G = self.G(t,z,p) # mid-stance / mid-flight if G[0] < 0: p.j[0] *= -1 # touchdown / liftoff if G[1] < 0: # unconstrained if p.j[1] == 1: x,y,dx,dy = z.T # compute constraint force leg = self.Leg(t,z,p) if g*m + leg > 0: # --> constrained z = [y,dy] p.j[1] = 2 else: # --> unconstrained z = [0.,y,0.,dy] p.j[1] = 1 elif p.j[1] == 2: # constrained --> unconstrained y,dy = z.T z = [0.,y,0.,dy] p.j[1] = 1 else: raise RuntimeError,"unknown discrete mode" return t,np.array(z),p def E(self,t,z,p,v): """ z = E(z,p,v) """ z = z + v return z def O(self,t,z,p): """ o = O(t,z,p) """ m,M,k,b,l,a,g = p.q if p.j[1] == 1: x,y,dx,dy = z.T elif p.j[1] == 2: y,dy = z.T x,dx = (0.*y,0.*dy) else: raise RuntimeError,"unknown discrete mode" zeros = np.zeros(x.shape) ones = np.ones(zeros.shape) T = 0.5*m*dx**2 + 0.5*M*dy**2 V = m*g*x + m*g*y L = T+V leg = self.Leg(t,z,p) F = []; for tt,zz in zip(t,z): F += [self.F(tt,zz,p)] Fn = np.sqrt(np.sum(np.array(F)**2,axis=1).reshape(-1,1)) o = np.array((x,y,dx,dy,leg,T,V,L,Fn)).T return o def Y(self,trjs,N): """ y = Y(t,o) """ T,Z,O,P = rx.stack(trjs) t = np.hstack(T) o = np.vstack(O) x,y,dx,dy,leg,TT,V,L,Fn = o.T tp = t; yp = y; dyp = dy t = np.linspace(t[0],t[-1],N,endpoint=False) y = np.interp(t,tp,yp,left=np.nan,right=np.nan) dy = np.interp(t,tp,dyp,left=np.nan,right=np.nan) return t,np.array((y)).T #return t,np.array((y,dy)).T def plot(self,trjs,fn='',dpi=None,pxh=None,pxw=None, alpha=1.,col=('r','b'),lp='',lw=4,slab='',smod=None,t0=0., clf=True,legend=True,fill=True,fsz=(10.,4.), legloc='lower left'): """ Plot trajectory Inputs: trjs - list - Hop trajectory Outputs: (T,Z,O,P) - observations """ T0,Z,O0,P = rx.stack(trjs) T = []; O = [] for k in range(len(T0)): T += [T0[k]] T += [np.nan*T0[k][0]] O += [O0[k]] O += [np.nan*O0[k][0]] if smod is None: smod = lambda s : s+slab Te,Ze,Oe,Pe = rx.trans(trjs) m,M,k,b,l,a,g = P[0].q if pxh: dpi = fsz(1)/pxh elif pxw: dpi = fsz(0)/pxw fig = plt.figure(1,figsize=fsz) #fig = plt.figure(1) if clf: fig.clf() legprop = {'size':12}; sc = 1. ms = 20.; mew = 2.; ma = 0.5 t = np.hstack(T) o = np.vstack(O) te = np.hstack(Te) oe = np.vstack(Oe) x,y,dx,dy,leg,TT,V,L,Fn = o.T xe,ye,dxe,dye,lege,TTe,Ve,Le,Fne = oe.T Vm = V.min() V = V - Vm E = TT+V ax = plt.subplot(2,1,1); plt.grid('on') if legend: H = (ax.plot(t-t0,sc*x,col[0]+lp,label='$'+smod('x')+'$',alpha=alpha), ax.plot(t-t0,sc*y,col[1]+lp,label='$'+smod('y')+'$',alpha=alpha)) else: H = (ax.plot(t-t0,sc*x,col[0]+lp,alpha=alpha), ax.plot(t-t0,sc*y,col[1]+lp,alpha=alpha)) [[hh.set_linewidth(lw) for hh in h] for h in H] ylim = np.array(ax.get_ylim())#-0.25 for te,pe in zip(Te,Pe): if pe.j[1] == 2 and fill: ax.fill([te[0]-t0,te[0]-t0,te[1]-t0,te[1]-t0], [ylim[1],ylim[0],ylim[0],ylim[1]], fc=np.array([1.,1.,1.])*0.75,ec='none',zorder=-1) if clf: ax.set_ylim(ylim) ax.set_ylabel('height') #leg = ax.legend(ncol=4,loc='lower center') if legend: leg = ax.legend(ncol=6,loc=legloc,prop=legprop) if clf: #1/0 ax.set_xlim((min(t),max(t))) ax = plt.subplot(2,1,2); plt.grid('on') if legend: H = (ax.plot(t-t0,sc*dx,col[0]+lp, label='$\\dot{x}'+slab+'$',alpha=alpha), ax.plot(t-t0,sc*dy,col[1]+lp, label='$\\dot{y}'+slab+'$',alpha=alpha)) else: H = (ax.plot(t-t0,sc*dx,col[0]+lp,alpha=alpha), ax.plot(t-t0,sc*dy,col[1]+lp,alpha=alpha)) [[hh.set_linewidth(lw) for hh in h] for h in H] ylim = np.array(ax.get_ylim())#-0.25 for te,pe in zip(Te,Pe): if pe.j[1] == 2 and fill: ax.fill([te[0]-t0,te[0]-t0,te[1]-t0,te[1]-t0], [ylim[1],ylim[0],ylim[0],ylim[1]], fc=np.array([1.,1.,1.])*0.75,ec='none',zorder=-1) if clf: ax.set_ylim(ylim) ax.set_xlabel('time (t)') ax.set_ylabel('velocity') #if legend: # leg = ax.legend(ncol=6,loc=legloc,prop=legprop) # nominal spring length #ax.plot([t[0],t[-1]],[l,l],'k--',lw=2) plt.subplot(2,1,1) if clf: ax.set_xlim((min(t),max(t))) if not fn == '': plt.savefig(fn,bbox_inches='tight',pad_inches=0.1,dpi=dpi) #plt.show() return T,Z,O,P def sch(self,t,z,p,figN=2,text=False,fn='',dpi=None,pxh=None,pxw=None): """ Plot schematic Inputs: t,z,p - time, state, parameters """ def zigzag(a=.2,b=.6,c=.2,p=4,N=100): x = np.linspace(0.,a+b+c,N); y = 0.*x mb = np.round(N*a/(a+b+c)); Mb = np.round(N*(a+b)/(a+b+c)) y[mb:Mb] = np.mod(np.linspace(0.,p-.01,Mb-mb),1.)-0.5 return np.vstack((x,y)) m,M,k,b,l,a,g = p.q o = self.O(np.array([t]),np.array([z]),p) x,y,dx,dy,leg,TT,V,L,Fn = o.T fsz = (8.,6.) if pxh: dpi = pxh/fsz(1) elif pxw: dpi = pxw/fsz(0) fig = plt.figure(figN); fig.clf() #ax = plt.axes([0.5,0.,0.5,1.]) if not text: ax = plt.axes([0.0,0.,0.35,1.]) else: ax = plt.axes([0.0,0.,0.35,0.60]) sc = 1. lw = 4.; ms = 20.; mew = 2.; ma = 0.5 fs = 20. fd = {'family':'serif','size':fs, 'weight':'bold'} xlim = (-0.75,0.75) ylim = (-0.25,2*l) sc = 1./1.5 mw = 0.5*sc; Mw = 1.*sc mh = 1.*mw; Mh = 1.*Mw gc = np.array([139.,69.,19.])/255. mc = np.array([1.0,0.4,0.4]) Mc = np.array([0.4,0.4,1.]) lc = np.array([0.,0.5,0.]) # rail #ax.plot([0.,0.],ylim,'--',lw=lw,zorder=-1,color='gray') # m ax.fill([-mw,-mw,mw,mw],[x+mh,x,x,x+mh],lw=lw,ec=0.5*mc,fc=mc) # M ax.fill([-Mw,-Mw,Mw,Mw],[y+Mh,y,y,y+Mh],lw=lw,ec=0.5*Mc,fc=Mc) # spring ly,lx = zigzag() ly = (y-x-mh)*ly+x+mh; lx = .5*lx; #ax.plot([0.,0.],[x+mh/2.,y+Mh/2.],lw=3*lw,color=lc,zorder=-1) ax.plot(lx,ly,lw=1.5*lw,color=lc,zorder=-1) # actuator #phi = np.sin(om*t + ph) #ax.arrow(-0.65*mw,(x+mh+y)/2.,0.,0.2*phi*(y-x-mh)[0],head_width=0.125,lw=lw,fc=Mc,ec=Mc) #ax.arrow(-0.65*mw,(x+mh+y)/2.,0.,-0.2*phi*(y-x-mh)[0],head_width=0.125,lw=lw,fc=mc,ec=mc) if text: xlim = (-1.2,1.2) ylim = (-0.25,1.5*l) # m ax.text(0.,x+mh/2,'$m, b$',ha='center',va='center',**fd) # x ax.text(mw+mw/2,x+0.05,'$x(t)$',ha='left',va='bottom',**fd) ax.plot([1.8*mw+0.25*mw,1.8*mw+0.25*mw],[0.,x[0]],'--',lw=lw,color='gray') ax.plot([1.8*mw,1.8*mw+0.5*mw],[x,x],lw=lw,color='gray') # M ax.text(0.,y+Mh/2,'$\\mu$',ha='center',va='center',**fd) # y ax.text(Mw+mw/2-0.1,y+0.05,'$y(t)$',ha='left',va='bottom',**fd) #ax.plot([1.8*mw+Mw-mw+0.25*mw,1.8*mw+Mw-mw+0.25*mw],[x[0]+mh,y[0]],'--',lw=lw,color='gray') ax.plot([1.8*mw+Mw-mw+0.25*mw,1.8*mw+Mw-mw+0.25*mw],[0,y[0]],'--',lw=lw,color='gray') ax.plot([1.8*mw+Mw-mw,1.8*mw+Mw-mw+0.5*mw],[y,y],lw=lw,color='gray') #ax.plot([1.8*mw+Mw-mw,1.8*mw+Mw-mw+0.5*mw],[x+mh,x+mh],lw=lw,color='gray') # leg spring ax.text(mw/1.,(x+mh+y)/2.,'$k, \\ell, a$',va='center',**fd) # leg actuator #ax.arrow(-1.*mw,mh/4+(x+mh+y)/2.+Mh/4.,0.,-0.65*Mh,head_width=0.125,lw=lw,fc='k') #ax.arrow(-0.65*mw,(x+mh+y)/2.+Mh/4.-0.75*Mh,0.,0.85*Mh,head_width=0.125,lw=lw,fc='k') # g ax.arrow(-2.75*mw,mh+Mh+(x+mh+y)/2.+Mh/2.,0.,-Mh, head_width=0.125,lw=lw,fc='k') ax.text( -3.0*mw,mh+Mh+(x+mh+y)/2.,'$g$',ha='right',va='center',**fd) # ground ax.fill([2*xlim[0],2*xlim[0],2*xlim[1],2*xlim[1]], [0.,ylim[0],ylim[0],0.],lw=lw,ec=0.5*gc,fc=gc,zorder=5) ax.axis('equal') ax.set_xlim(xlim); ax.set_xticks([]) ax.set_ylim(ylim); ax.set_yticks([]) #plt.show() if not fn == '': plt.savefig(fn,bbox_inches='tight',pad_inches=-0.1,dpi=dpi) #plt.savefig(fn) def flow(hop,p,h,eps,t,x,n=np.infty,debug=False): """ trjs = flow compute flow between two phases """ m,M,k,b,l,a,g = p.q t0 = t[0]; tf = t[-1]; x = np.array(x) # don't simulate from invalid initial condition assert hop.G( t0,x,p )[1] >= 0, 'guard satisfaction' trjs = euler((t0,tf), x, p, hop, h, eps, n, debug=debug) # clean up relaxed states,times if len(trjs[-1].t) <= 2: trjs = trjs[:-1] trj = trjs[-1] return trjs def do_fp(h=1e-3,eps=1e-10,debug=False): q = [1.,3.,10.,5.,2.,2.0,2.] m,M,k,b,l,a,g = q debug = False hop = Hop() p = hop.P(q,debug) t0 = 0.; tf = np.infty n = 5*4 t = (t0,tf) j = [+1,2] z = np.array([0.6,0.0]) #j = [-1,1] #z = np.array([0.5,3.0,0.,0.0]) p.j = j p.q = q Zeno = False import time; start = time.clock() trjs = flow(hop, p, h, eps, (t0,tf), z, n, debug=debug) #hop.plot(trjs[-10:]) hop.plot(trjs,lw=2) xlim = plt.xlim(); ylim = plt.ylim() plt.xlim(xlim); plt.ylim(ylim) print "%0.2f sec for sim & plot" % (time.clock() - start) #dbg() qq = q[:] pp = hop.P(qq); pp.j = trjs[-1].p.j zz = trjs[-1].x[-1] # Deformation of Poincare section #psi = (lambda t,x : flow(hop,pp,h,eps,(0.,2*t*np.pi/om),x)[-1].x[-1]) # Poincare map #pmap = lambda x : psi(1.,x) pmap = lambda z : flow(hop,p,h,eps,(0.,np.infty),z,n=4)[-1].x[-1] start = time.clock() # find periodic orbit z0 = rx.fp(pmap,zz,modes=[1,2]) print "%0.2f sec for fp" % (time.clock() - start) start = time.clock() trjs0 = flow(hop, p, h, eps, (t0,tf), z0, n+4) period = trjs0[4].t[0] - trjs0[0].t[0] hop.plot(trjs0,t0=trjs[-1].t[-1]-trjs0[-4].t[-1] + period, clf=False,legend=True,slab='^*',fill=False,alpha=0.4,lw=8) ax = plt.subplot(2,1,1) ax.set_ylim((-1.75,3.2)) plt.savefig('fig/hop_fp.pdf',bbox_inches='tight',pad_inches=0.1) print "%0.2f sec for plot" % (time.clock() - start) print 'z0 = %s' % z0 start = time.clock() # linearize Poincare map J = rx.jac(pmap,z0) # compute eigenvalues of P-map #print 'J = %s' % J print ' spec J = %s' % ( np.linalg.eigvals(J) ) print '|spec J| = %s' % np.abs( np.linalg.eigvals(J) ) #print np.array([(dd,np.abs(np.linalg.eigvals(rx.jac(pmap,z0,d=dd))).max()) for dd in np.logspace(-2,-6,40)]) print "%0.2f sec for jac " % (time.clock() - start) z = trjs0[-1].x[-1] #sig = np.mod(ph0 + om*(tf-t0),2*np.pi) # == phf #if len(z) == 2: # y,dy = np.array(z) # z = np.array([sig,y,dy]) #elif len(z) == 4: # sig,y,dy,x,dx = np.array(z) # z = np.array([sig,y,dy,x,dx]) #else: # dbg() return hop,trjs0,z,p def do_anim(): q=[1.,3.,10.,5.,2.,2.0,2.] m,M,k,b,l,a,g = q debug = False hop = Hop() p = hop.P(q,debug) ph0 = np.pi/2. t0 = 0 tf = 10 n = np.infty t = (t0,tf) j = 0 #z = np.array([0.2,1.75,0.,0.]) # nice transient #z = np.array([0.1,1.0,0.,2.0]) j = [+1,2] z = np.array([0.5,0.0]) #z = np.array([1.6,0.6]) #z = np.array([1.4,0.6]) p.j = j h = 1e-2 eps = 1e-12 Zeno = False import time; start = time.clock() trjs = flow(hop, p, h, eps, (t0,tf), z) #hop.plot(trjs[-10:]) #hop.plot(trjs,fn='fig/hoptrj.eps') #xlim = plt.xlim(); ylim = plt.ylim() #plt.xlim(xlim); plt.ylim(ylim) #q0 = q[:]; p0 = hop.P(q0,debug); p0.j = 1; z0 = np.array([0.25,1.75,0.,0.]) #hop.sch(0.,[0.25,2.15,0,0],p0,text=True,fn='fig/hopA.pdf') ##hop.sch(3*np.pi/2,[0.25,2.15,0,0],p0,text=False,fn='fig/hopAplain.pdf') #p0.j = 2 #hop.sch(0.,[1.5,0],p0,text=True,fn='fig/hopG.pdf') ##hop.sch(np.pi/4,[1.5,0],p0,text=False,fn='fig/hopGplain.pdf') ##hop.sch(t0,z,p,text=True,fn='fig/hop.pdf') #p0.j = 1 print "%0.2f sec for sim & plot" % (time.clock() - start) #ph0 = 0#np.pi #t0 = ph0*np.pi/om #tf = (4*2 + ph0)*np.pi/om #n = np.infty #t = (t0,tf) #j = 2 #z = np.array([1.952,1.908]) #p.j = j #h = 1e-2 #eps = 1e-12 #Zeno = False #import time; # #start = time.clock() #trjs = flow(hop, p, h, eps, (t0*om,tf*om), z) hop.plot(trjs) xlim = plt.xlim(); ylim = plt.ylim() plt.xlim(xlim); plt.ylim(ylim) lt = plt.plot([t0,t0],ylim,'y',lw=5.,alpha=0.75) T,Z,O,P = rx.stack(trjs) fps = 20. fn = 0 pxh=402; import os; if not os.path.exists('vid'): os.mkdir('vid') plt.ion() for t,z,p in zip(T,Z,P): for tt,zz in zip(t,z): if tt >= t0 + fn/fps: print '%4d : %0.2f , %0.2f' % (fn, t0+fn/fps, tt) #plt.ioff() plt.figure(1) lt[0].set_xdata([tt,tt]) plt.draw() #plt.savefig('vid/trj%04d.png'%fn); plt.savefig('vid/trj%04d.png'%fn,dpi=pxh/4); plt.figure(2) hop.sch(tt,zz,p) #plt.ion() plt.draw() plt.savefig('vid/hop%04d.png'%fn,bbox_inches='tight',pad_inches=-0.1,dpi=pxh/5.8) os.system('convert vid/hop%04d.png vid/trj%04d.png +append vid/hoptrj%04d.png'%(fn,fn,fn)) os.system('convert vid/hoptrj%04d.png -extent 1200x416 vid/hoptrj%04d_.png'%(fn,fn)) fn += 1 #break os.system('rm vid/hop.mp4 vid/hoptrj.mp4') os.system('ffmpeg -r %d -qscale 0 -i vid/hop%%04d.png vid/hop.mp4' % fps) os.system('ffmpeg -r %d -qscale 0 -i vid/hoptrj%%04d_.png vid/hoptrj.mp4' % fps) def do_id(hop=None,xi=None,p=None,T=None,tf=4.,h=1e-3,eps=1e-10,seed=49, N=60,Np=2,tol=1e-2,fn=None,noise={'obs':.1,'ic':.1}): hop = Hop() print 'seed = %d' % seed np.random.seed(seed) import time def phi(t,z0,p,debug=False): """ trjs,z = phi(t,z0,p) hybrid flow Inputs: t - scalar - time to simulate z0 - (y,dy,x,dx) or (y,dy) - initial state p - m,M,k,b,l,a,g """ t0 = 0.; tf = t; n = np.inf # don't simulate from invalid initial condition assert hop.G( t0,z0,p )[1] >= 0, 'guard satisfaction' # run simulation trjs = euler((t0,tf), z0, p, hop, h, eps, n, debug=debug) # clean up relaxed states,times if len(trjs[-1].t) <= 2: trjs = trjs[:-1] trj = trjs[-1] # terminate exactly at final time s = (tf - trj.t[-2]) / (trj.t[-1]-trj.t[-2]) trj.t[-1] = (1-s)*trj.t[-2] + s*trj.t[-1] trj.x[-1] = (1-s)*trj.x[-2] + s*trj.x[-1] # return trajectory, final state z = trj.x[-1] return trjs,z def armijo(f, x, d, fx=None, Dfx=None, alpha=1e-1, beta=0.8, gamma=1e-0, kmax=100): return rx.armijo(f,x,d,fx=fx,Dfx=Dfx, alpha=alpha,beta=beta,gamma=gamma,kmax=kmax) t_ = 1.5*T u_ = xi + np.array([-.8,0.]) p_ = p.copy() trjs,z0 = phi( t_,u_,p_ ) p0 = trjs[-1].p print "z0 = %s\np0 = (%s,%s)" % (z0,p0.j,p0.q) trjs0,_ = phi(tf, z0, p0) hop.plot(trjs0,alpha=0.5,lw=6,lp='',fsz=(14.,4.), clf=True,fill=True,legend=True,col=('r','b')) t0,eta0 = hop.Y( trjs0,N ) eta0 += np.random.randn(*eta0.shape)*noise['obs'] def err(theta,tf=tf,eta0=eta0,z0=u_,p0=p_,N=N,debug=False): """ e,z,p = err(theta) """ # extract named parameters m,M,k,b,l,a,g = p0.q # copy parameter, initial condition p0 = p0.copy(); z = z0.copy(); # extract parameters from theta if len(theta) == 6: m,k,b,a = theta[2:] p0.q = [m,M,k,b,l,a,g] # extract Poincare section data from theta t0,y0 = theta[:2] assert t0 > .1, "can't simulate for short times" # reconstruct initial condition trjs,zz = phi(t0,np.array([y0,0]),p0); pp = trjs[-1].p assert not(np.any(np.isnan(zz))) # simulate from initial condition trjs,z = phi( tf,zz,pp ) assert not(np.any(np.isnan(z))) # compute observations t,eta = hop.Y( trjs, N ) # compute mean squared error e = np.sum((eta0 - eta) * (eta0 - eta)) / eta0.shape[0] if np.isnan(e): dbg() # return error, state, parameters return e, zz, pp st = time.clock() if Np == 2: th0 = np.hstack((t_,u_[:1])) elif Np == 6: th0 = np.hstack((t_,u_[:1],m,k,b,a)) else: raise InputError('invalid number of parameters to estimate, Np = %s'%Np) #th0 = np.hstack((m,k,b,a)) e,z,p = err(th0) print "th0 = %s z = %s" % (th0,z) if Np == 2: th1 = th0 + np.random.randn(len(th0))*noise['ic'] - [T/2.,0] elif Np == 6: th1 = th0 + np.dot(np.random.randn(len(th0)), np.diag([5e-1,2.,1e-1,2.,1.,5e-1]))*noise['ic'] else: raise InputError('invalid number of parameters to estimate, Np = %s'%Np) S = None e,z,p = err(th1,debug=True) print "th1 = %s z = %s" % (th1,z) trjs,_ = phi( tf,z,p ) hop.plot(trjs,alpha=0.6,lw=3,fsz=(14.,4.), clf=False,legend=True,fill=False,lp='--',slab='_0') #return #dbgerr = True dbgerr = False dbgdescent = True #dbgdescent = False th,j = rx.descent1( lambda th : err(th,debug=dbgerr)[0],th1,eps=tol, #D = lambda f,x,fx : rx.jac(f,x,fx=fx,d=1e-3), a=armijo, #a = lambda f,x,d,fx=None,Dfx=None : 1., S=S, debug=dbgdescent,jmax=200) if np.any(np.isnan(th)): dbg() print "%0.2f sec for descent" % (time.clock() - st) print 'th1 = %s' % th1 print 'th0 = %s' % th0 print 'th = %s' % th #print ('%3d : dth1 = %0.2e, dth = %0.2e' # % ( j, np.max(np.abs(th1-th0)), np.max(np.abs(th-th0)) ) ) print (' : err1 = %0.2e, err0 = %0.2e, err = %0.2e' % ( err(th1)[0],err(th0)[0],err(th)[0] ) ) #e,z,p = err(th1) #trjs,_ = phi( 2*np.pi/om,z,p ) #hop.plot(trjs,alpha=0.5,clf=False) e,z,p = err(th) print "th = %s z = %s" % (th,z) trjs,_ = phi( tf,z,p ) hop.plot(trjs,clf=False,legend=True,fill=False,lw=2,fsz=(14.,4.), smod=lambda s : '\\widehat{'+s+'}') plt.plot(t0,eta0,'k.',label='$\\eta_i$') plt.legend(loc='lower left',prop={'size':12},ncol=8) ax = plt.subplot(2,1,1) #ax.set_ylim((-2.0,4.2)) ax = plt.subplot(2,1,2) #ax.set_ylim((-8.,4.)) if fn: #plt.savefig('fig/'+fn+'eps',bbox_inches='tight',pad_inches=0.1) plt.savefig('fig/'+fn+'.pdf',bbox_inches='tight',pad_inches=0.1) np.savetxt('data/'+fn+'.csv',np.vstack((th1,th0,th))) if __name__ == "__main__": import sys args = sys.argv h = 1e-2 eps = 1e-10 seed = np.int(100*np.random.rand()) if '--fp' in args: hop,trjs0,xi,p0 = do_fp(h=h,eps=eps) if '--pmap' in args: do_pmap() if '--anim' in args: do_anim() if '--sch' in args: hop.sch(0.,xi,p0,text=True) if '--ic' in args: seed = 89 # used in paper T = trjs0[3].t[0]-trjs0[0].t[0] do_id(hop=hop,xi=xi,p=p0,T=T,h=h,eps=eps,tf=6,tol=2e-2,N=100, #Np=2,fn='hop_ic',seed=seed,noise={'obs':xi[0]/3.,'ic':[0.,0.]}) Np=2,fn='hop_ic',seed=seed,noise={'obs':xi[0]/3.,'ic':[T/16.,xi[0]/3.]})