mb: 1.0
}
sys.initial_conditions = {
q[0]: 0.0,
q[1]: 0.0,
q[2]: 0.0,
q[3]: 0.0,
q[4]: 0.03,
q[5]: 0.0,
q[6]: 0.0,
q[7]: 0.0,
q[8]: 0.0,
q[9]: 0.0,
q[10]: 0.14,
q[11]: 0.0,
u[0]: 0.0,
u[1]: 0.0,
u[2]: 0.0,
u[3]: 0.0,
u[4]: 0.0,
u[5]: 0.0,
u[6]: 0.0,
u[7]: 0.0,
u[8]: 0.0,
u[9]: 0.0,
u[10]: 0.0,
u[11]: 0.0
}
sys.times = np.linspace(0, 10.0, 1000000)
bb_sys.initial_conditions = {
quat[0]: 0.,
quat[1]: 0.,
quat[2]: 0.,
quat[3]: 1.,
pos[0]: 0.,
pos[1]: 0.,
pos[2]: 0.,
motor_theta[0]: 0.,
motor_theta[1]: 0.,
motor_theta[2]: 0.,
motor_theta[3]: 0.,
blade_theta[0]: 0.0,
blade_theta[1]: 0.0,
blade_theta[2]: 0.0,
blade_theta[3]: 0.0,
blade_theta[4]: 0.0,
blade_theta[5]: 0.0,
blade_theta[6]: 0.0,
blade_theta[7]: 0.0,
omega[0]:0., #roll
omega[1]:0, #pitch
omega[2]:0.,
vel[0]: 0.,
vel[1]: 0.,
vel[2]: 0.001,
motor_omega[0]: 1.,
motor_omega[1]: 0.,
motor_omega[2]: 0.,
motor_omega[3]: 0.,
blade_omega[0]: 0.,
blade_omega[1]: 0.,
blade_omega[2]: 0.,
blade_omega[3]: 0.,
blade_omega[4]: 0.,
blade_omega[5]: 0.,
blade_omega[6]: 0.,
blade_omega[7]: 0.,
}
sys = System(kane)
sys.constants = {
m: mass_of_rod,
M: mass_of_plate,
g: 9.81,
H: workspace_width,
a: plate_width,
b: plate_height,
c: cable_c,
c_rod: rod_c,
k_rod: rod_k,
Izz: inertia_of_plate,
Izz_rod: inertia_of_rod,
}
sys.initial_conditions = {x: x_init, y: y_init, beta: beta_init, e: -rod_init}
sys.specifieds = {
L1: lambda x, t: s_curve(t, L1_init_s, L1_end_s, RiseTime, StartTime),
L2: lambda x, t: s_curve(t, L2_init_s, L2_end_s, RiseTime, StartTime)
}
sys.times = np.linspace(0.0, Simulation_Time, 1000)
sys.generate_ode_function(generator='cython')
y = sys.integrate()
sim_time = np.linspace(0.0, Simulation_Time, 1000)
fig = plt.figure(figsize=(18, 4))
# fig = plt.figure(0)
fig.add_subplot(141)
plt.plot(sim_time, y[:, 0], label='Unshaped')
fr, frstar = kane.kanes_equations(loads, bodies)
sys = System(kane)
sys.constants = {
lB: 0.2, # m
h: 0.1, # m
w: 0.2, # m
mA: 0.01, # kg
mB: 0.1, # kg
g: 9.81, # m/s**2
}
sys.initial_conditions = {
theta: np.deg2rad(90.0),
phi: np.deg2rad(0.5),
omega: 0,
alpha: 0
}
sys.times = np.linspace(0, 10, 500)
x = sys.integrate()
plt.plot(sys.times, x)
plt.legend([sym.latex(s, mode='inline') for s in sys.coordinates + sys.speeds])
# visualize
rod_shape = Cylinder(2 * lA, 0.005, color='red')
plate_shape = Plane(h, w, color='blue')
rod_length = 3.0
mass_of_rod = 2.0
inertia_of_plate = (plate_width**2 + plate_height**2) * (mass_of_plate/12.0)
sys = System(kane)
sys.constants = {
M:12.0,
g:9.81,
F1:Lengths_and_Moments(inital_x, inital_y)[0],
F2:Lengths_and_Moments(inital_x, inital_y)[1],
H:20.0,
a:4.0,
b:2.0,
Izz:inertia_of_plate,
}
sys.initial_conditions = {x:inital_x, y:inital_y, beta:0}
sys.times = np.linspace(0.0, 20.0, 1000)
y = sys.integrate()
sim_time = np.linspace(0.0, 20.0, 1000)
fig = plt.figure(figsize=(18, 4))
# fig = plt.figure(0)
fig.add_subplot(141)
plt.plot(sim_time, y[:,0], label='Unshaped')
# plt.ylim(25,0)
plt.title(r'X Motion')
fig.add_subplot(142)
plt.plot(sim_time, y[:,1], label='Unshaped')
x6 = consts[l45] + consts[l56]
sys.initial_conditions = {
q[0]: 0.0,
q[1]: 0.0,
q[2]: 0.0,
q[3]: 0.0,
q[4]: 0.0,
q[5]: 0.0,
q[6]: 0.0,
q[7]: yi1,
q[8]: 0.0,
q[9]: 0.0,
q[10]: yi2,
q[11]: 0.0,
q[12]: 0.0,
q[13]: np.pi,
q[14]: x6,
u[0]: 0.0,
u[1]: 0.0,
u[2]: 0.0,
u[3]: 0.0,
u[4]: 0.0,
u[5]: 0.0,
u[6]: 0.0,
u[7]: 0.0,
u[8]: 0.0,
u[9]: 0.0,
u[10]: 0.0,
u[11]: 0.0,
u[12]: 1.0 #, u[13]: 0.0, u[14]: 0.0
}
sys.generate_ode_function(generator='cython')
b: plate_height,
kr: rod_spring,
cr: rod_damper,
k: cable_k,
c: cable_c,
Ip: plate_inertia,
Ir: rod_inertia,
L1: Length1_values[i],
L2: Length2_values[i],
e_offset: e_offset_scalar,
k_C: spring_on_frame_C
}
sys.initial_conditions = {
x: x_values_heat_np[i],
z: z_values_heat_np[i],
theta: theta_values_heat_np[i, 0],
e: e_equil,
phi: theta_values_heat_np[i, 0]
}
sys.times = np.linspace(0.0, runtime, runtime * 30)
sys.generate_ode_function(generator='cython')
resp = sys.integrate()
true_x_pre.append(resp[:, 0][-1])
true_z_pre.append(resp[:, 1][-1])
true_e_pre.append(resp[:, 2][-1])
true_theta_pre.append(resp[:, 3][-1])
true_phi_pre.append(resp[:, 4][-1])
print(i)
true_x = np.asarray(true_x_pre)
true_z = np.asarray(true_z_pre)
true_e = np.asarray(true_e_pre)
# calculate Kane's equations
fr, frstar = kane.kanes_equations(loads, bodies)
sys = System(kane)
sys.constants = {lB: 0.2, # m
h: 0.1, # m
w: 0.2, # m
mA: 0.01, # kg
mB: 0.1, # kg
g: 9.81, # m/s**2
}
sys.initial_conditions = {theta: np.deg2rad(90.0),
phi: np.deg2rad(0.5),
omega: 0,
alpha: 0}
sys.times = np.linspace(0, 10, 500)
x = sys.integrate()
plt.plot(sys.times, x)
plt.legend([sym.latex(s, mode='inline') for s in sys.coordinates + sys.speeds])
# visualize
rod_shape = Cylinder(2 * lA, 0.005, color='red')
plate_shape = Plane(h, w, color='blue')
