r = 1.0
R = 1.5
g = 9.8
b = 0.1 # friction
def derivate(state, step, t, dt):
dth, th = state
_dth = ((R * dth ** 2 + g) * r * sin(th) - b * dth) / (R ** 2 + 2 * r * R * cos(th) + r ** 2)
return [_dth, dth]
if __name__ == "__main__":
times = np.linspace(0, 60, 2000)
solution = solve([0.0, pi / 12], times, integrate_rk4, derivate)
theta = solution[:, 1]
wheel_x = theta * R
mass_x = wheel_x + r * cos(theta - pi / 2)
mass_y = R - r * sin(theta - pi / 2)
w_line, = plt.plot(times, solution[:-1, 0], label="ω")
th_line, = plt.plot(times, solution[:-1, 1], label="Θ")
plt.legend([w_line, th_line], ['ω', 'Θ'])
plt.grid(True)
plt.show()
fig = plt.figure()
ax = fig.add_subplot(111, autoscale_on=False, xlim=(-2, 12), ylim=(-0.5, 3.5))
ax.set_aspect('equal')
ax.grid()
return min_value
return value
def constrain(value, limit):
return _constrain(value, -limit, limit)
if __name__ == "__main__":
times = np.linspace(0, 3.0, 5000)
pid = PIDController(k_p=100.0, k_d=50.0, k_i=2.0, target=0.0)
us = []
def derivate(state, step, t, dt):
x, v = state
u = constrain(pid.get_control(x, dt), 10)
us.append(u)
return [v, u]
solution = solve([1.0, 0.0], times, integrate_rk4, derivate)
xs = solution[:, 0]
vs = solution[:, 1]
x_line, = plt.plot(times, xs[:-1], label="x")
v_line, = plt.plot(times, vs[:-1], label="v")
u_line, = plt.plot(times, us[::4], label="u")
plt.legend([x_line, v_line, u_line], ['x', 'v', 'u'])
plt.grid(True)
plt.show()
vx1, vy1 = state_[2:4]
x2, y2 = state_[4:6]
vx2, vy2 = state_[6:]
dist2 = (x1 - x2)**2 + (y1 - y2)**2
f = G * m1 * m2 / dist2
dist = np.sqrt(dist2)
ax1 = f * (x2 - x1) / dist / m1
ay1 = f * (y2 - y1) / dist / m1
ax2 = f * (x1 - x2) / dist / m2
ay2 = f * (y1 - y2) / dist / m2
return [vx1, vy1, ax1, ay1, vx2, vy2, ax2, ay2]
solution = solve(initial_state, times, integrate_rk4,
derivatives_gravity_two_bodies)
pp.clf()
fig, ax = pp.subplots(1, 1)
pp.plot(solution[:, 0], solution[:, 1], label="first", linewidth=0.5)
pp.plot(solution[:, 4], solution[:, 5], label="second", linewidth=0.5)
pp.legend()
pp.grid(True)
obj, = pp.plot([], [], color='orange', marker='o', markersize=30)
obj2, = pp.plot([], [], color='green', marker='o', markersize=10)
def init():
obj.set_data([], [])
obj2.set_data([], [])
ddx = np.linalg.det(Ax) / det_A
Aa = np.copy(A)
Aa[:, 1] = b_vec
dda = np.linalg.det(Aa) / det_A
Ab = np.copy(A)
Ab[:, 2] = b_vec
ddb = np.linalg.det(Ab) / det_A
return [dx, ddx, da, dda, db, ddb]
times = np.linspace(0, 6, 6000)
dt = times[1] - times[0]
solution = solve(initial_state, times, integrate_rk4, derivatives)
x_solution = solution[:, 0]
a_solution = solution[:, 2]
b_solution = solution[:, 4]
x_line, = plt.plot(times, x_solution[1:])
a_line, = plt.plot(times, a_solution[1:])
b_line, = plt.plot(times, b_solution[1:])
plt.grid(True)
plt.legend([x_line, a_line, b_line], ['x', 'a', 'b'])
plt.show()
# import sys
# sys.exit(0)
x, y = xs[:, i], ys[:, i]
if i in merge_step:
x = x[:merge_step[i]]
y = y[:merge_step[i]]
pp.plot(x, y, label=f"#{i}", linewidth=0.5)
obj, = pp.plot([], [], marker='o', markersize=10)
obj.set_data([], [])
objects.append(obj)
pp.legend()
pp.grid(True)
def animate(i_):
for j in range(len(objects)):
if j in merge_step and i_ >= merge_step[j]:
objects[j].set_data([], [])
else:
objects[j].set_data(xs[i_, j], ys[i_, j])
return objects
ani = animation.FuncAnimation(fig, animate,
frames=np.arange(1, len(solution_)),
interval=10, blit=True,
repeat=False)
pp.show()
solution = solve(to_state(positions, velocities), times, integrate_rk4, derivatives_gravity_n_bodies)
render(solution)
