def parallel_simulation_boosted(args):
"""Run the simulations in parallel.
"""
# unpack the arguments
fname, params = args
nu_theta, theta_max, f_theta, d_psi, psi_max, L = params
# if undulation is turned off, set parameters for f = 1.2 Hz
f_theta_orig = f_theta
if f_theta_orig == 0:
f_theta = 1.2
# setup the body
body_dict = setup_body(L=L,
ds=.01,
theta_max=theta_max,
nu_theta=nu_theta,
f_theta=f_theta,
phi_theta=0,
psi_max=psi_max,
frac_theta_max=0,
d_theta=0,
d_psi=d_psi,
display_ho=0,
bl=1.36,
bd=.6) # BOOST THE FORCES
vscale = body_dict['vscale']
dt = float(body_dict['dt'])
# find the initial Euler angle offsets so minimize inertial effects at the start
ang0 = find_rotational_ic(body_dict, print_time=False)
# now turn undulation back off --- for both waves
if f_theta_orig != f_theta:
body_dict['theta_dict']['f_theta'] = 0
body_dict['psi_dict']['f_psi'] = 0
# velocity initial condition
v0_non = .2409 # 1.7 m/s for a 29 N/m^2 snake
v0_dim = v0_non * vscale # near 1.7 m/s
# initial conditions
tend = None
Ro0 = np.r_[0, 0, 10]
dRo0 = np.r_[0, v0_dim, 0]
dang0 = np.deg2rad(np.r_[0, 0, 0]) # yaw rate, pitch rate, roll rate
C0 = sim.euler2C(ang0[0], ang0[1], ang0[2])
omg0_body = np.dot(sim.dang2omg(ang0[0], ang0[1], ang0[2]), dang0)
omg0 = np.dot(C0.T, omg0_body)
soln0 = np.r_[Ro0, dRo0, omg0, ang0]
# run the dynamics simulation
out = sim.integrate(soln0, body_dict, dt, tend=tend, print_time=False)
# extract values
ts, Ro, dRo, omg, ang = out
yaw, pitch, roll = np.rad2deg(ang.T)
# reconstruct all values from the simulation
out_recon = simulation_recon(body_dict, ts, Ro, dRo, omg, ang)
# save the values
np.savez(fname,
vscale=vscale,
dt=dt,
ts=ts,
Ro=Ro,
dRo=dRo,
omg=omg,
ang=ang,
yaw=yaw,
pitch=pitch,
roll=roll,
f_theta=f_theta,
psi_max=psi_max,
d_psi=d_psi,
**out_recon)
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 cube_parallel_simulation(trial):
"""Run the simulations in parallel.
"""
save_fname = '../Output/s_serp3d_compare_to_cube/{}_{}.npz'
# load the saved trial information
snake_id = int(trial['Snake ID'])
trial_id = int(trial['Trial ID'])
data_fname = ret_fnames(snake=snake_id, trial=trial_id)[0]
d = np.load(data_fname)
# setup simulation parameters
body_dict = setup_body(L=trial['SVL'] / 100,
ds=.01,
theta_max=trial['theta_max'],
nu_theta=trial['nu_theta'],
f_theta=trial['f_theta'],
phi_theta=0,
psi_max=trial['psi_max'],
frac_theta_max=0,
d_theta=0,
d_psi=trial['d_psi_avg'],
display_ho=0,
ho_shift=True,
bl=1,
bd=1,
known_mass=trial['mass'] / 1000)
# setup the simulation
dt = float(body_dict['dt'])
# find the initial Euler angle offsets so minimize inertial
# effects at the start
ang0 = find_rotational_ic(body_dict, print_time=False)
# initial conditions
tend = None
Ro0 = d['Ro_I'][0] / 1000
dRo0 = d['dRo_I'][0] / 1000
dang0 = np.deg2rad(np.r_[0, 0, 0]) # yaw rate, pitch rate, roll rate
C0 = sim.euler2C(ang0[0], ang0[1], ang0[2])
omg0_body = np.dot(sim.dang2omg(ang0[0], ang0[1], ang0[2]), dang0)
omg0 = np.dot(C0.T, omg0_body)
soln0 = np.r_[Ro0, dRo0, omg0, ang0]
# run the dynamics simulation
out = sim.integrate(soln0, body_dict, dt, tend=tend, print_time=False)
# extract values
ts, Ro, dRo, omg, ang = out
yaw, pitch, roll = np.rad2deg(ang.T)
# reconstruct all values from the simulation
out_recon = simulation_recon(body_dict, ts, Ro, dRo, omg, ang)
# save the values
np.savez(
save_fname.format(trial_id, snake_id),
Ro_I=d['Ro_I'] / 1000, # trial position
dRo_I=d['dRo_I'] / 1000, # trial velocity
ddRo_I=d['ddRo_I'] / 1000, # trial acceleration
dt=dt,
ts=ts,
Ro=Ro,
dRo=dRo,
omg=omg,
ang=ang,
yaw=yaw,
pitch=pitch,
roll=roll,
**out_recon)
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]
now = time.time()
vscale = body_dict['vscale']
dt = float(body_dict['dt'])
# find the initial Euler angle offsets so minimize inertial effects at the start
ang0 = find_rotational_ic(body_dict, print_time=True)
# initial conditions
tend = None
Ro0 = d['Ro_I'][0] / 1000
dRo0 = d['dRo_I'][0] / 1000
dang0 = np.deg2rad(np.r_[0, 0, 0]) # yaw rate, pitch rate, roll rate
C0 = sim.euler2C(ang0[0], ang0[1], ang0[2])
omg0_body = np.dot(sim.dang2omg(ang0[0], ang0[1], ang0[2]), dang0)
omg0 = np.dot(C0.T, omg0_body)
soln0 = np.r_[Ro0, dRo0, omg0, ang0]
# run the dynamics simulation
out = sim.integrate(soln0, body_dict, dt, tend=tend, print_time=True)
print('{0:.3f} sec'.format(time.time() - now))
# extract values
ts, Ro, dRo, omg, ang = out
yaw, pitch, roll = np.rad2deg(ang.T)
# reconstruct all values from the simulation
out_recon = simulation_recon(body_dict, ts, Ro, dRo, omg, ang)
