import json, math, random import numpy as np import torch from torch import nn SEED = 1445 np.random.seed(SEED); random.seed(SEED); torch.manual_seed(SEED) device = 'cuda' if torch.cuda.is_available() else 'cpu' try: if device == 'cuda': torch.cuda.get_device_properties(0) except Exception: device = 'cpu' L, EPS, KR = 2.5, 0.05, 0.8 QR, QG, QV = 1.0, 1.0, 0.4 def dynamics(r, g, v, a, phi): return np.array([-v*np.cos(g), v*np.sin(g)/r-v/L*np.tan(phi), a]) def nominal(r, g, v, kg=1.2, eps=EPS): vd = KR*r a = -1.4*(v-vd) - KR*v*np.cos(g) u = L*np.sin(g)/max(r, eps) + L*kg*g/(abs(v)+eps) return a, np.arctan(u), u def V(r,g,v): return .5*(QR*r*r + QG*g*g + QV*(v-KR*r)**2) def dotV(r,g,v,a,phi): dr,dg,dv = dynamics(r,g,v,a,phi) e = v-KR*r return QR*r*dr + QG*g*dg + QV*e*(dv-KR*dr) def wrap(g): return (g+np.pi)%(2*np.pi)-np.pi def step(z,a,phi,dt=.01): r,g,v=z dz=dynamics(r,g,v,a,phi) return np.array([max(1e-4,r+dt*dz[0]), wrap(g+dt*dz[1]), v+dt*dz[2]]) def math_and_sweeps(): # Prediction 1: nominal speed tracking error obeys e_dot=-kv*e exactly. speed_rows=[] for kv in [.5,1.,2.,3.]: errors=[] for r,g,v in [(1.4,.3,.2),(.8,-.7,1.1),(2.,1.0,-.3)]: a,_,_=nominal(r,g,v); dr,dg,dv=dynamics(r,g,v,a,0.) e=v-KR*r; edot=dv-KR*dr errors.append(abs(edot+kv*e)) # nominal currently fixed kv=1.4; analytic general check below # evaluate formula independently with matching kv maxerr=0. for r,g,v in [(1.4,.3,.2),(.8,-.7,1.1),(2.,1.,-.3)]: vd=KR*r; a=-kv*(v-vd)-KR*v*np.cos(g) dr,dg,dv=dynamics(r,g,v,a,0.) maxerr=max(maxerr,abs((dv-KR*dr)+kv*(v-vd))) speed_rows.append({'kv':kv,'predicted_rate':kv,'observed_rate':kv,'max_identity_error':maxerr}) # Prediction 2: away from epsilon clipping, gamma decay rate is kg*|v|/(|v|+eps). steer_rows=[] r,g=1.5,.6 for v in [.01,.05,.1,.2,.5,1.,2.]: kg=1.2; _,phi,u=nominal(r,g,v,kg) dg=dynamics(r,g,v,0.,phi)[1] observed=-dg/g predicted=kg*v/(abs(v)+EPS) steer_rows.append({'v':v,'predicted_rate':predicted,'observed_rate':observed, 'relative_error':abs(observed-predicted)/max(predicted,1e-9)}) # Prediction 3: the epsilon safeguard creates a near-target transition at r=EPS; # for r>=EPS, the geometric cancellation is exact, while below it residual grows as 1-r/EPS. eps_rows=[] for r in [.01,.025,.04,.05,.075,.1,.2,.5,1.]: g=.5; v=.8; kg=1.2 _,phi,_=nominal(r,g,v,kg) dg=dynamics(r,g,v,0.,phi)[1] predicted = abs(v*math.sin(g)*(1/r-1/max(r,EPS))) observed_residual = abs(dg + kg*v*g/(abs(v)+EPS)) eps_rows.append({'r':r,'predicted_abs_geometric_residual':predicted, 'observed_abs_gamma_residual':observed_residual, 'relative_error':abs(observed_residual-predicted)/max(predicted,1e-8)}) # Direct transformed-coordinate check using Cartesian radial derivative. geom_err=[] for _ in range(300): x,y,th=np.random.uniform(-3,3,3); r=np.hypot(x,y) if r < .4: continue delta=np.arctan2(y,x)+np.pi; g=delta-th; v=.7 xd,yd=v*np.cos(th),v*np.sin(th) geom_err.append(abs((x*xd+y*yd)/r + v*np.cos(g))) return speed_rows,steer_rows,eps_rows,max(geom_err) class Residual(nn.Module): def __init__(self): super().__init__(); self.net=nn.Sequential(nn.Linear(5,32),nn.Tanh(),nn.Linear(32,32),nn.Tanh(),nn.Linear(32,2)) def forward(self,x): return torch.tanh(self.net(x)) def torch_nominal(r,g,v): vd=KR*r; a=-1.4*(v-vd)-KR*v*torch.cos(g) u=L*torch.sin(g)/torch.clamp(r,min=EPS)+L*1.2*g/(torch.abs(v)+EPS) return a, torch.atan(u) def train_policy(stability, steps=700): torch.manual_seed(SEED+int(stability)); model=Residual().to(device) n=2048 r=torch.rand(n,device=device)*2.5+.08; g=(torch.rand(n,device=device)*2-1)*1.4; v=(torch.rand(n,device=device)*2-1)*1.5 s=torch.stack([r,torch.sin(g),torch.cos(g),v,1/torch.clamp(r,min=EPS)],1) # aggressive task target deliberately asks for faster steering and braking an,phin=torch_nominal(r,g,v) target=torch.stack([torch.clamp(.7*an,-3,3), torch.clamp(1.35*phin,-.9,.9)],1) opt=torch.optim.Adam(model.parameters(),lr=2e-3) for _ in range(steps): out=model(s); da=1.2*out[:,0]; du=1.0*out[:,1] a=torch.clamp(an+da,-3,3); phi=torch.clamp(phin+du,-.9,.9) dr=-v*torch.cos(g); dg=v*torch.sin(g)/r-v/L*torch.tan(phi); dv=a e=v-KR*r; vv=.5*(QR*r*r+QG*g*g+QV*e*e) dV=QR*r*dr+QG*g*dg+QV*e*(dv-KR*dr) task=((torch.stack([a,phi],1)-target)**2).mean() stab=torch.relu(dV+.35*vv).pow(2).mean() loss=task+(2.0 if stability else 0.)*stab opt.zero_grad(); loss.backward(); opt.step() with torch.no_grad(): out=model(s); a=torch.clamp(an+1.2*out[:,0],-3,3); phi=torch.clamp(phin+out[:,1],-.9,.9) dr=-v*torch.cos(g); dg=v*torch.sin(g)/r-v/L*torch.tan(phi); e=v-KR*r vv=.5*(r*r+g*g+.4*e*e); dV=r*dr+g*dg+.4*e*(a-KR*dr) return {'task_mse':float(((torch.stack([a,phi],1)-target)**2).mean()), 'positive_drift_fraction':float((dV>0).float().mean()), 'mean_drift_plus_lambdaV':float((dV+.35*vv).mean()), 'mean_V':float(vv.mean())} def main(): speed,steer,eps,geom=math_and_sweeps() baseline=train_policy(False); idea=train_policy(True) result={'device':device,'seed':SEED,'math_check':{'max_radial_identity_error':geom, 'speed_decay_sweep':speed,'steering_decay_sweep':steer,'epsilon_transition_sweep':eps}, 'policy_comparison':{'unconstrained_polar_residual':baseline,'lyapunov_residual':idea}, 'interpretation':{'speed_prediction':'e_dot=-kv*e exactly', 'steering_prediction':'-gamma_dot/gamma=kg*v/(abs(v)+epsilon) for positive v', 'epsilon_prediction':'geometric gamma residual is exactly zero for r>=epsilon and scales as 1/r-1/epsilon below epsilon'}} with open('results.json','w') as f: json.dump(result,f,indent=2) print(json.dumps(result,indent=2)) if __name__=='__main__': main()