"""
This file will use pydy.codegen to simulate the double pendulum.
"""
from numpy import concatenate, array, linspace
from pydy.system import System
from scipy.integrate import odeint
from double_pendulum import *
constants = {l: 10.0, m: 10.0, g: 9.81}
initial_conditions = {q1: 1.0, q2: 0.0, u1: 0.0, u2: 0.0}
sys = System(KM, constants=constants,
initial_conditions=initial_conditions)
frames_per_sec = 60
final_time = 5.0
times = linspace(0.0, final_time, final_time * frames_per_sec)
sys.times = times
x = sys.integrate()
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)
sys.generate_ode_function(generator='cython')
x = sys.integrate()
plt.plot(sys.times, x)
plt.plot(sys.times, x[:, 0:3])
plt.plot(sys.times, x[:, 3:6])
plt.plot(sys.times, x[:, 6:9])
plt.plot(sys.times, x[:, 9:12])
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')
# plt.ylim(25,0)
plt.title(r'X Motion')
fig.add_subplot(142)
plt.plot(sim_time, y[:, 1], label='Unshaped')
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')
v1 = VisualizationFrame('rod', A.orientnew('rod', 'Axis', (sym.pi / 2, A.x)),
Ao, rod_shape)
v2 = VisualizationFrame(
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)
true_theta = np.asarray(true_theta_pre)
true_phi = np.asarray(true_phi_pre)
np.save('true_x', true_x)
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')
v1 = VisualizationFrame('rod',
A.orientnew('rod', 'Axis', (sym.pi / 2, A.x)),
Ao,
