def simulation_recon(body_dict, ts, Ro, dRo, omg, ang): """Reconstruct the body and forces from the simulation. """ # state vector yaw, pitch, roll = ang.T # unpack additional arguments s, m, n_neck = body_dict['s'], body_dict['m'], body_dict['n_neck'] theta_dict, psi_dict = body_dict['theta_dict'], body_dict['psi_dict'] rho, g = body_dict['rho'], body_dict['g'] ds, c, aero_interp = body_dict['ds'], body_dict['c'], body_dict[ 'aero_interp'] nbody = body_dict['nbody'] ntime = len(ts) # kinematics simulation theta = np.zeros((ntime, nbody)) psi = np.zeros((ntime, nbody)) dthetads = np.zeros((ntime, nbody)) dpsids = np.zeros((ntime, nbody)) p = np.zeros((ntime, nbody, 3)) dp = np.zeros((ntime, nbody, 3)) ddp = np.zeros((ntime, nbody, 3)) dpds = np.zeros((ntime, nbody, 3)) ddpdds = np.zeros((ntime, nbody, 3)) tv = np.zeros((ntime, nbody, 3)) cv = np.zeros((ntime, nbody, 3)) bv = np.zeros((ntime, nbody, 3)) Tv = np.zeros((ntime, nbody, 3)) Cv = np.zeros((ntime, nbody, 3)) Bv = np.zeros((ntime, nbody, 3)) Crs = np.zeros((ntime, nbody, 3, 3)) Crs_I = np.zeros((ntime, nbody, 3, 3)) C = np.zeros((ntime, 3, 3)) Fl = np.zeros((ntime, nbody, 3)) Fd = np.zeros((ntime, nbody, 3)) Fa = np.zeros((ntime, nbody, 3)) dR_BC = np.zeros((ntime, nbody, 3)) dR_T = np.zeros((ntime, nbody, 3)) aoa = np.zeros((ntime, nbody)) Re = np.zeros((ntime, nbody)) Ml = np.zeros((ntime, nbody, 3)) Md = np.zeros((ntime, nbody, 3)) Ma = np.zeros((ntime, nbody, 3)) dynP = np.zeros((ntime, nbody)) U_BC = np.zeros((ntime, nbody)) U_tot = np.zeros((ntime, nbody)) Dh = np.zeros((ntime, nbody, 3)) Lh = np.zeros((ntime, nbody, 3)) cl = np.zeros((ntime, nbody)) cd = np.zeros((ntime, nbody)) clcd = np.zeros((ntime, nbody)) dR_B = np.zeros((ntime, nbody, 3)) dR_TC = np.zeros((ntime, nbody, 3)) U_TC = np.zeros((ntime, nbody)) beta = np.zeros((ntime, nbody)) dynP_frac = np.zeros((ntime, nbody)) Fl_B = np.zeros((ntime, nbody, 3)) Fd_B = np.zeros((ntime, nbody, 3)) Fa_B = np.zeros((ntime, nbody, 3)) Ml_B = np.zeros((ntime, nbody, 3)) Md_B = np.zeros((ntime, nbody, 3)) Ma_B = np.zeros((ntime, nbody, 3)) # rotational power = tau \dot \omega power = np.zeros((ntime, nbody)) r = np.zeros((ntime, nbody, 3)) dr = np.zeros((ntime, nbody, 3)) ddr = np.zeros((ntime, nbody, 3)) dR = np.zeros((ntime, nbody, 3)) Nframe = np.eye(3) nframe = np.zeros((ntime, 3, 3)) for i in np.arange(ntime): t = ts[i] out = sim.aerialserp_pos_vel_acc_tcb(s, t, m, n_neck, theta_dict, psi_dict) # store the values theta[i] = out['theta'] psi[i] = out['psi'] dthetads[i] = out['dthetads'] dpsids[i] = out['dpsids'] # position, velocity, acceleration p[i] = out['p'] dp[i] = out['dp'] ddp[i] = out['ddp'] # derivatives along spine dpds[i] = out['dpds'] ddpdds[i] = out['ddpdds'] # body coordinate system tv[i] = out['tv'] cv[i] = out['cv'] bv[i] = out['bv'] Crs[i] = out['Crs'] C[i] = sim.euler2C(yaw[i], pitch[i], roll[i]) r[i] = sim.rotate(C[i].T, p[i]) dr[i] = sim.rotate(C[i].T, dp[i]) ddr[i] = sim.rotate(C[i].T, ddp[i]) dR[i] = dRo[i] + dr[i] + sim.cross(omg[i], r[i]) Tv[i] = sim.rotate(C[i].T, tv[i]) Cv[i] = sim.rotate(C[i].T, cv[i]) Bv[i] = sim.rotate(C[i].T, bv[i]) for j in np.arange(nbody): Crs_I[i, j] = np.dot(C[i].T, Crs[i, j]) out_aero = sim.aero_forces(tv[i], cv[i], bv[i], C[i], dR[i], ds, c, rho, aero_interp, full_out=True) # forces in the inertial frame Fl[i] = out_aero['Fl'] Fd[i] = out_aero['Fd'] Fa[i] = out_aero['Fa'] aoa[i] = out_aero['aoa'] Re[i] = out_aero['Re'] dR_T[i] = out_aero['dR_T'] dR_BC[i] = out_aero['dR_BC'] dynP[i] = out_aero['dynP'] U_BC[i] = out_aero['U_BC'] U_tot[i] = out_aero['U_tot'] Dh[i] = out_aero['Dh'] Lh[i] = out_aero['Lh'] cl[i] = out_aero['cl'] cd[i] = out_aero['cd'] clcd[i] = out_aero['clcd'] dR_B[i] = out_aero['dR_B'] dR_TC[i] = out_aero['dR_TC'] U_TC[i] = out_aero['U_TC'] beta[i] = out_aero['beta'] dynP_frac[i] = out_aero['dynP_frac'] # aero moments in the inertial frame Ml[i] = np.cross(r[i], Fl[i]) Md[i] = np.cross(r[i], Fd[i]) Ma[i] = np.cross(r[i], Fa[i]) # aero forces and moments in the body frame Fl_B[i] = sim.rotate(C[i], Fl[i]) Fd_B[i] = sim.rotate(C[i], Fd[i]) Fa_B[i] = sim.rotate(C[i], Fa[i]) Ml_B[i] = np.cross(p[i], Fl_B[i]) Md_B[i] = np.cross(p[i], Fd_B[i]) Ma_B[i] = np.cross(p[i], Fa_B[i]) power[i] = np.dot(Ma[i], omg[i]) nframe[i] = sim.rotate(C[i].T, Nframe) # airfoil in inertial frame foils_I, foil_color = sim.apply_airfoil_shape(r, c, Crs_I) foils_B, foil_color = sim.apply_airfoil_shape(r, c, Crs) # put everything into a dictionary out = dict(theta=theta, psi=psi, dthetads=dthetads, dpsids=dpsids, p=p, dp=dp, ddp=ddp, dpds=dpds, ddpdds=ddpdds, tv=tv, cv=cv, bv=bv, Tv=Tv, Cv=Cv, Bv=Bv, Crs=Crs, Crs_I=Crs_I, C=C, Fl=Fl, Fd=Fd, Fa=Fa, dR_BC=dR_BC, dR_T=dR_T, aoa=aoa, Re=Re, Ml=Ml, Md=Md, Ma=Ma, dynP=dynP, U_BC=U_BC, U_tot=U_tot, Dh=Dh, Lh=Lh, cl=cl, cd=cd, clcd=clcd, dR_B=dR_B, dR_TC=dR_TC, U_TC=U_TC, beta=beta, dynP_frac=dynP_frac, Fl_B=Fl_B, Fd_B=Fd_B, Fa_B=Fa_B, Ml_B=Ml_B, Md_B=Md_B, Ma_B=Ma_B, power=power, r=r, dr=dr, ddr=ddr, dR=dR, Nframe=Nframe, nframe=nframe, foils_I=foils_I, foils_B=foils_B, foil_color=foil_color) return out
def cyc_avg_moments(body_dict, ts_cyc, vy_cyc, vz_cyc, print_time=False): ntime_orig = len(ts_cyc) # unpack needed variables s, m, n_neck, g = body_dict['s'], body_dict['m'], body_dict['n_neck'], body_dict['g'] theta_dict, psi_dict = body_dict['theta_dict'], body_dict['psi_dict'] vscale, rho = body_dict['vscale'], body_dict['rho'] ds, c, aero_interp = body_dict['ds'], body_dict['c'], body_dict['aero_interp'] nbody = body_dict['nbody'] # phases of the wave to evaluate at nphase = 60 phi_theta_phases = np.linspace(0, 2 * np.pi, nphase + 1)[:-1] phi_psi_phases = 2 * (phi_theta_phases - np.pi / 2) # make the velocity dimensional dRo = np.c_[np.zeros(ntime_orig), vy_cyc, vz_cyc] * vscale C = np.eye(3) omg = np.r_[0, 0, 0] ntime = 5 * ntime_orig ts_phs = np.linspace(ts_cyc[0], ts_cyc[-1], ntime) # interpolate the velocity dRo_x_many = np.interp(ts_phs, ts_cyc, dRo[:, 0]) dRo_y_many = np.interp(ts_phs, ts_cyc, dRo[:, 1]) dRo_z_many = np.interp(ts_phs, ts_cyc, dRo[:, 2]) dRo_many = np.c_[dRo_x_many, dRo_y_many, dRo_z_many] dRo = dRo_many npntnb3 = np.zeros((nphase, ntime, nbody, 3)) M_B = npntnb3.copy() F_B = npntnb3.copy() ho_B = npntnb3.copy() dho_B = npntnb3.copy() p = npntnb3.copy() dp = npntnb3.copy() ddp = npntnb3.copy() tv = npntnb3.copy() cv = npntnb3.copy() bv = npntnb3.copy() if print_time: now = time.time() # i is index through phases for i in np.arange(nphase): # phase in the undulation cycle theta_dict['phi_theta'] = phi_theta_phases[i] psi_dict['phi_psi'] = phi_psi_phases[i] # j is index through cycle for j in np.arange(ntime): # time through cycle t = ts_phs[j] out = sim.aerialserp_pos_vel_acc_tcb(s, t, m, n_neck, theta_dict, psi_dict) pi, dpi, ddpi = out['p'], out['dp'], out['ddp'] tvi, cvi, bvi = out['tv'], out['cv'], out['bv'] p[i, j] = pi dp[i, j] = dpi ddp[i, j] = ddpi tv[i, j] = tvi cv[i, j] = cvi bv[i, j] = bvi # positions, velocities, and accelerations ri, dri = sim.rotate(C.T, pi), sim.rotate(C.T, dpi) dRi = dRo[j] + dri + np.cross(omg, ri) # ho ho_B[i, j] = np.cross((m * pi.T).T, dpi) dho_B[i, j] = np.cross((m * pi.T).T, ddpi) # aerodynamic forces aout = sim.aero_forces(tvi, cvi, bvi, C, dRi, ds, c, rho, aero_interp, full_out=True) # aerodynamic force Fi_iner = aout['Fa'] F_B[i, j] = sim.rotate(C, Fi_iner) # aerodynamic moments Mi_iner = sim.cross(ri, Fi_iner) M_B[i, j] = sim.rotate(C, Mi_iner) if print_time: print('Cycle avg moment time: {0:.3f} sec'.format(time.time() - now)) return ts_phs, dRo, F_B, M_B, ho_B, dho_B, p, dp, ddp, tv, cv, bv, phi_theta_phases, phi_psi_phases
def rotational_dynamics_eom(t, state, vel_body_dict): """ """ # unpack the arguments dRo, body_dict = vel_body_dict # current angles and angular velocity omg, ang = np.split(state, 2) yaw, pitch, roll = ang # unpack needed kinematics variables s, m, n_neck, g = body_dict['s'], body_dict['m'], body_dict[ 'n_neck'], body_dict['g'] theta_dict, psi_dict = body_dict['theta_dict'], body_dict['psi_dict'] vscale, weight, rho = body_dict['vscale'], body_dict['weight'], body_dict[ 'rho'] ds, c, aero_interp = body_dict['ds'], body_dict['c'], body_dict[ 'aero_interp'] # body kinematics out = sim.aerialserp_pos_vel_acc_tcb(s, t, m, n_neck, theta_dict, psi_dict) p, dp, ddp = out['p'], out['dp'], out['ddp'] tv, cv, bv = out['tv'], out['cv'], out['bv'] # rotation matrix from inertial to body C = sim.euler2C(yaw, pitch, roll) # control tv, cv, bv based on head orientation head_control = body_dict['head_control'] if head_control: tv, cv, bv, _ = sim.serp3d_tcb_head_control(out['dpds'], C) # positions, velocities, and accelerations r, dr, ddr = sim.rotate(C.T, p), sim.rotate(C.T, dp), sim.rotate(C.T, ddp) dR = dRo + dr + np.cross(omg, r) # gravitational force in inertial frame F = np.zeros(r.shape) F[:, 2] = -m * g # aerodynamic forces if aero_interp is not None: Fa = sim.aero_forces(tv, cv, bv, C, dR, ds, c, rho, aero_interp) F += Fa # form the dynamic equations M, N, _, _ = sim.dynamics_submatrices(r, dr, ddr, omg, m, F) # extract only rotational dynamics M = M[3:, 3:] # lower right N = N[3:] # solve for domg domg = np.linalg.solve(M, -N) # solve for change in Euler angles (kinematic differential equations) omg_body = sim.rotate(C, omg) dang = np.dot(sim.euler2kde(yaw, pitch, roll), omg_body) # combine our derivatives as the return parameter return np.r_[domg, dang]
def cycle_avg_dynamics_eom(tintegrator, state, body_dict): """Called by fixed_point. """ # non-dimensional velocities y, z, vy, vz = state # unpack needed variables s, m, n_neck, g = body_dict['s'], body_dict['m'], body_dict['n_neck'], body_dict['g'] theta_dict, psi_dict = body_dict['theta_dict'], body_dict['psi_dict'] rho = body_dict['rho'] ds, c, aero_interp = body_dict['ds'], body_dict['c'], body_dict['aero_interp'] # phase coupling parameters A_phi, B_phi = body_dict['A_phi'], body_dict['B_phi'] # phases of the wave to evaluate at nphase = 10 # every 2 pi / 10 = pi / 5 radians phi_theta_phases = np.linspace(0, 2 * np.pi, nphase + 1)[:-1] phi_psi_phases = A_phi * (phi_theta_phases + B_phi) # trivial dynamics variables C = np.eye(3) omg = np.r_[0, 0, 0] # CoM velocity dRo = np.r_[0, vy, vz] F = np.zeros((nphase, 3)) for i in np.arange(nphase): t = 0 # phase in the undulation cycle theta_dict['phi_theta'] = phi_theta_phases[i] psi_dict['phi_psi'] = phi_psi_phases[i] out = sim.aerialserp_pos_vel_acc_tcb(s, t, m, n_neck, theta_dict, psi_dict) p, dp = out['p'], out['dp'] tv, cv, bv = out['tv'], out['cv'], out['bv'] # positions, velocities, and accelerations r, dr = sim.rotate(C.T, p), sim.rotate(C.T, dp) dR = dRo + dr + np.cross(omg, r) Fi = np.zeros(r.shape) Fi[:, 2] = -m * g # aerodynamic forces aout = sim.aero_forces(tv, cv, bv, C, dR, ds, c, rho, aero_interp, full_out=True) Fi += aout['Fa'] # Fl + Fd F[i] = Fi.sum(axis=0) # average force F_avg = F.mean(axis=0) accel = F_avg / m.sum() ay, az = accel[1], accel[2] # don't itegrate the x component return np.r_[vy, vz, ay, az]