| """Reference assembler of exported component operators, independent of FEniCS.""" |
| import json |
| from pathlib import Path |
| import numpy as np |
| from scipy.linalg import eigh |
|
|
|
|
| def load(folder): |
| folder=Path(folder) |
| c=dict(np.load(folder/'component.npz')) |
| c.update(json.loads((folder/'metadata.json').read_text())) |
| return c |
|
|
|
|
| def rz(a): |
| c,s=np.cos(a),np.sin(a) |
| return np.array([[c,-s,0],[s,c,0],[0,0,1.]]) |
|
|
|
|
| def rx(a): |
| c,s=np.cos(a),np.sin(a) |
| return np.array([[1.,0,0],[0,c,-s],[0,s,c]]) |
|
|
|
|
| def assemble(chain,rolls=None): |
| n=len(chain); nm=[c['Kr'].shape[0]-12 for c in chain]; nq=6*(n+1) |
| K=np.zeros((nq,nq)); fT=np.zeros(nq); fG=np.zeros((nq,3)) |
| KC=np.zeros((nq+sum(nm),)*2);MC=KC.copy() |
| origin=np.zeros(3);orient=np.eye(3);places=[]; offset=nq |
| for i,c in enumerate(chain): |
| orient=orient@rx((rolls or [0]*n)[i]) |
| r=orient.copy();d=np.kron(np.eye(4),r.T) |
| ix=np.arange(6*i,6*i+12) |
| K[np.ix_(ix,ix)]+=d.T@c['S']@d |
| fT[ix]+=d.T@c['fT']; fG[ix]+=d.T@c['g']@r.T |
| ixm=np.r_[ix,np.arange(offset,offset+nm[i])] |
| dc=np.eye(12+nm[i]);dc[:12,:12]=d |
| KC[np.ix_(ixm,ixm)]+=dc.T@c['Kr']@dc |
| MC[np.ix_(ixm,ixm)]+=dc.T@c['Mr']@dc |
| places.append(dict(origin=origin.copy(),rotation=r,index=ix,mindex=ixm,transform=d,cbtransform=dc)) |
| origin=origin+r@c['ports'][1] |
| if c['spec']['kind']=='elbow':orient=orient@rz(np.pi/2) |
| offset+=nm[i] |
| return dict(K=K,fT=fT,fG=fG,KC=KC,MC=MC,places=places,nq=nq) |
|
|
|
|
| def solve(chain,rolls=None,deltaT=100.,gravity=(0,0,-9.81),supports=None,nmodes=6): |
| a=assemble(chain,rolls); K=a['K'].copy();KC=a['KC'].copy() |
| supports=supports or [{'node':0,'fixed':list(range(6))}] |
| fixed=[] |
| for s in supports: |
| fixed.extend([6*s['node']+k for k in s.get('fixed',[])]) |
| for k,v in enumerate(s.get('spring',[0]*6)): |
| ix=6*s['node']+k;K[ix,ix]+=v;KC[ix,ix]+=v |
| fixed=np.unique(fixed);free=np.setdiff1d(np.arange(len(K)),fixed) |
| F=a['fT']*deltaT+a['fG']@gravity |
| q=np.zeros(len(K));q[free]=np.linalg.solve(K[np.ix_(free,free)],F[free]) |
| reaction=K@q-F |
| |
| freeC=np.setdiff1d(np.arange(len(KC)),fixed) |
| ev,vec=eigh(KC[np.ix_(freeC,freeC)],a['MC'][np.ix_(freeC,freeC)],subset_by_index=(0,min(nmodes,len(freeC))-1)) |
| if ev[0]<=0:raise ValueError('Unrestrained or invalid assembly') |
| modes=np.zeros((len(KC),len(ev)));modes[freeC]=vec |
| paths=[];disps=[];mpaths=[] |
| for c,p in zip(chain,a['places']): |
| R=p['rotation'];local=p['transform']@q[p['index']] |
| u=c['recovery']@local+c['thermal_recovery']*deltaT+c['gravity_recovery']@(R.T@gravity) |
| phi=np.concatenate([c['recovery'],c['mode_recovery']],axis=2)@(p['cbtransform']@modes[p['mindex']]) |
| paths.append(c['path']@R.T+p['origin']);disps.append(u@R.T) |
| mpaths.append(np.einsum('ij,sjk->sik',R,phi)) |
| return dict(q=q,reaction=reaction,frequency=np.sqrt(ev)/(2*np.pi),modes=modes, |
| paths=paths,displacements=disps,mode_displacements=mpaths, |
| free_residual=float(np.linalg.norm(reaction[free])/max(1,np.linalg.norm(F))), |
| strain_energy=float(q@a['K']@q/2)) |
|
|
