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]