def main02_pendulum_simple():
""" First tracking pendulum example """
x10, x20, y10, y20 = 0, 0, 0, 0
x0 = np.array([x10, x20, y10, y20])
omega, zeta = 10, 1
a, b, c = 10, 1, 10
k1, k2 = 400, 20
w = lambda t: 1.5 + (t > 2) * (-1.5)
t_start, t_end = 0, 3
sample_count = 1000
t = np.linspace(t_start, t_end, sample_count)
tspan = [t_start, t_end]
u = lambda t, x, e, r_dd: 1 / c * (a * sin(x[0]) + b * x[1] + r_dd - k1 *
e[0] - k2 * e[1])
rm_func = lambda t, x, w: reference_model(t, x, w, omega, zeta)
f = lambda t, x: pendulum_simple_simfunc(t, x, rm_func, w, a, b, c, u)
sol = scipy.integrate.solve_ivp(f, tspan, x0, t_eval=t)
# Collect outputs
w = w(t)
r = sol.y[2]
y = sol.y[0]
fig, ax = fc.new_figure()
ax.plot(t, r, label='$r$')
ax.plot(t, y, '--', label='$y$')
ax.set_xlabel('$t$')
ax.grid()
ax.legend()
Path('./figures').mkdir(parents=True, exist_ok=True)
plt.savefig('figures/tracking_pendulum1.pdf', pad_inches=0.0)
def main01_constant_input_simulation():
""" Simple simulation of a pendulum system. Choose a constant
control input Tss, and simulate system's output behavior. """
m, g, l, k = 1, 10, 1, 2
pend = pendulum.Pendulum(m, g, l, k)
theta0, theta_dot0 = 1, 0
x0 = np.array([theta0, theta_dot0])
t_start, t_end = 0, 10
sample_count = 1000
t = np.linspace(t_start, t_end, sample_count)
tspan = [t_start, t_end]
# Constant control input
Tss = 0
T = lambda t, x: Tss
f = lambda t, x: pend.state_space_static_controller(t, x, T)
sol = scipy.integrate.solve_ivp(f, tspan, x0, t_eval=t)
fig, ax = fc.new_figure()
ax.plot(sol.t, sol.y[0], label='$\\theta$')
ax.plot(sol.t, sol.y[1], label='$\\dot{\\theta}$')
ax.set_xlabel('$t$')
ax.grid()
ax.legend()
Path('./figures').mkdir(parents=True, exist_ok=True)
plt.savefig('figures/unforced_response.pdf', pad_inches=0.0)
def main01_reference_model():
""" Test reference modeling """
x0 = np.array([0, 0])
omega = 10
zeta = 1
w = lambda t: 1.5 + (t > 2) * (-1.5)
t_start, t_end = 0, 3
sample_count = 1000
t = np.linspace(t_start, t_end, sample_count)
tspan = [t_start, t_end]
f = lambda t, x: reference_model(t, x, w, omega, zeta)
sol = scipy.integrate.solve_ivp(f, tspan, x0, t_eval=t)
w = w(t)
r = sol.y[0]
fig, ax = fc.new_figure()
ax.plot(t, w, label='$w$')
ax.plot(t, r, label='$r$')
ax.set_xlabel('$t$')
ax.grid()
ax.legend()
Path('./figures').mkdir(parents=True, exist_ok=True)
plt.savefig('figures/tracking_reference_model.pdf', pad_inches=0.0)
def main06_robot2():
""" Second tracking pendulum example """
x10, x20, y10, y20 = 0, 0, 0, 0
x0 = np.array([x10, x20, y10, y20])
tau = 0.05
omega, zeta = 1 / tau, 1
a, b, c = 10, 1, 10
k1, k2 = 400, 20
w = lambda t: 0 * t + pi / 2
t_start, t_end = 0, 2
sample_count = 1000
t = np.linspace(t_start, t_end, sample_count)
tspan = [t_start, t_end]
umin, umax = -2, 2
u = lambda t, x, e, r_dd: 1 / c * (a * sin(x[0]) + b * x[1] + r_dd - k1 *
e[0] - k2 * e[1])
u_saturated = lambda t, x, e, r_dd: np.clip(u(t, x, e, r_dd), umin, umax)
rm_func = lambda t, x, w: reference_model(t, x, w, omega, zeta)
f = lambda t, x: pendulum_simple_simfunc(t, x, rm_func, w, a, b, c,
u_saturated)
sol = scipy.integrate.solve_ivp(f, tspan, x0, t_eval=t)
# Collect outputs
w = w(t)
y1 = sol.y[2]
y2 = sol.y[3]
r = y1
r_dot = y2
r_dd = -np.power(omega, 2) * y1 - 2 * zeta * omega * y2 + np.power(
omega, 2) * w
x = np.vstack([sol.y[0], sol.y[1]])
e1 = x[0] - r
e2 = x[1] - r_dot
e = np.vstack([e1, e2])
u = u_saturated(t, x, e, r_dd)
y = sol.y[0]
fig, axs = fc.new_figure(subplot_count=2)
axs[0].plot(t, r, label='$r$')
axs[0].plot(t, y, '--', label='$y$')
axs[0].grid()
axs[0].legend()
axs[1].plot(t, u, label='$u$')
axs[1].grid()
axs[1].legend()
Path('./figures').mkdir(parents=True, exist_ok=True)
plt.savefig('figures/tracking_robot2.pdf', pad_inches=0.0)
def main04_simulate_dynamic_g_large_tau():
A = np.array([[2.3, 0, 1], [-4.9, 3, -3], [-1, 0, 1]])
B = np.array([[0.2, -1, 1], [-0.9, -1, 0], [-0.2, 0.1, 0]])
D = np.array([[0], [0], [1]])
tau = 0.35
K = np.array([[1.6, -1]])
par = Parameters(A, B, D, tau, K)
z10, z20, z30 = 2, 2, 2
z0 = np.array([z10, z20, z30])
tsi1 = timeseries_integrator.TimeseriesIntegrator()
tsi1.add_sample(0, z10)
tsi2 = timeseries_integrator.TimeseriesIntegrator()
tsi2.add_sample(0, z20)
tsi3 = timeseries_integrator.TimeseriesIntegrator()
tsi3.add_sample(0, z30)
tsi = [tsi1, tsi2, tsi3]
g = lambda t, z: dynamic_g(t, z, tsi, par)
v = lambda t, z: control_law(t, z, tsi, par, g(t, z))
f = lambda t, z: simulation_func(t, z, v(t, z), tsi, par)
tmin, tmax = 0, 10
tspan = [tmin, tmax]
sample_count = 1000
t = np.linspace(tmin, tmax, sample_count)
sol = scipy.integrate.solve_ivp(f, tspan, z0, t_eval=t)
z = sol.y
z1 = z[0]
z2 = z[1]
z3 = z[2]
v = recreate_control(t, z, v)
fig, axs = fc.new_figure(subplot_count=2, height=2 * fc.DEFAULT_HEIGHT)
axs[0].plot(t, z1, label='$z_1$')
axs[0].plot(t, z2, label='$z_2$')
axs[0].plot(t, z3, label='$z_3$')
axs[0].set_xlabel('$t$')
axs[0].grid()
axs[0].legend()
axs[1].plot(t, v, label='$v$')
axs[1].set_xlabel('$t$')
axs[1].grid()
axs[1].legend()
Path('./figures').mkdir(parents=True, exist_ok=True)
plt.savefig('figures/smc_dynamic_g_large_tau.pdf', pad_inches=0.0)
def main03_feedback_state_stabilization():
""" Simulation of a pendulum system. The system is controlled by
a stabilizing state-feedback controller around desired angle
delta. """
m, g, l, k = 1, 10, 1, 2
a, b, c = g / l, k / m, 1 / m / l / l
delta = pi / 4 # Desired value of angle theta at equilibrium point
pend = pendulum.Pendulum(m, g, l, k)
theta0, theta_dot0 = 0, 0
x0 = np.array([theta0, theta_dot0])
# State feedback control
k1, k2 = 1, 1
T = lambda t, x: -k1 * (x[0] - delta) - k2 * x[1] + a * sin(delta) / c
f = lambda t, x: pend.state_space_static_controller(t, x, T)
t_start, t_end = 0, 10
N = 1000
t = np.linspace(t_start, t_end, N)
tspan = [t_start, t_end]
sol = scipy.integrate.solve_ivp(f, tspan, x0, t_eval=t)
control = T(sol.t, sol.y)
fig, axs = fc.new_figure(subplot_count=2, height=2 * fc.DEFAULT_HEIGHT)
axs[0].plot([t_start, t_end], [pi / 4, pi / 4],
'r--',
label='Reference $\\delta$')
axs[0].plot(sol.t, sol.y[0], label='$\\theta$')
axs[0].plot(sol.t, sol.y[1], label='$\\dot{\\theta}$')
axs[1].plot(sol.t, control, 'g', label='$T$')
axs[0].set_xlabel('$t$')
axs[0].grid()
axs[0].legend()
axs[1].set_xlabel('$t$')
axs[1].grid()
axs[1].legend()
Path('./figures').mkdir(parents=True, exist_ok=True)
plt.savefig('figures/state_feedback.pdf', pad_inches=0.0)
def main04_gain_scheduling_modified_ramp():
""" Simulate with modified gain scheduling controller """
x10, x20 = 0, 0
eta0 = 0
xhat10, xhat20 = 0, 0
x0 = np.array([x10, x20, eta0, xhat10, xhat20])
r = lambda t: (t < 100) * 1 / 100 * t + (t >= 100) * 1
alpha = lambda t: r(t)
t_start, t_end = 0, 120
t, x1, x2, eta, xhat1, xhat2, r, y = simulate_modified(
r, alpha, x0, t_start, t_end)
fig, ax = fc.new_figure()
ax.plot(t, r, 'r--', label='$r$')
ax.plot(t, y, label='$y$')
ax.set_xlabel('$t$')
ax.grid()
ax.legend()
Path('./figures').mkdir(parents=True, exist_ok=True)
plt.savefig('figures/gain_scheduling_modified_ramp.pdf', pad_inches=0.0)
def main02_gain_scheduling_unmodified():
""" Simulate with unmodified gain scheduling controller """
x10, x20 = 0, 0
sigma0 = 0
xhat10, xhat20 = 0, 0
x0 = np.array([x10, x20, sigma0, xhat10, xhat20])
r = lambda t: 0.2 + (t > 30) * 0.2 + (t > 60) * 0.2
alpha = lambda t: r(t)
t_start, t_end = 0, 140
t, x1, x2, sigma, xhat1, xhat2, r, y = simulate(r, alpha, x0, t_start,
t_end)
fig, ax = fc.new_figure()
ax.plot(t, r, 'r--', label='$r$')
ax.plot(t, y, label='$y$')
ax.set_xlabel('$t$')
ax.grid()
ax.legend()
Path('./figures').mkdir(parents=True, exist_ok=True)
plt.savefig('figures/gain_scheduling_unmodified.pdf', pad_inches=0.0)
def main05_integral_control():
""" Simulation of a pendulum system. The system is controlled by
an integral controller, which is stabilized by linearization
around desired angle delta and state-feedback. """
# Good values for m to test are: 0.1, 1, 2 (1 is nominal)
m, g, l, k = 0.1, 10, 1, 2
pend = pendulum.Pendulum(m, g, l, k)
delta = pi / 4
k1, k2, k3 = 8, 2, 10
K = np.array([k1, k2, k3])
test_integral_controller_constraints(K, m, g, l, k, delta)
theta0, theta_dot0, sigma0 = 0, 0, 0
x0 = np.array([theta0, theta_dot0, sigma0])
t_start, t_end = 0, 10
N = 1000
t = np.linspace(t_start, t_end, N)
tspan = [t_start, t_end]
f_original = pend.state_space_integral_controller
f = lambda t, x: f_original(t, x, K, delta)
sol = scipy.integrate.solve_ivp(f, tspan, x0, t_eval=t)
fig, ax = fc.new_figure()
ax.plot([t_start, t_end], [delta, delta],
'r--',
label='Reference $\\delta$')
ax.plot(sol.t, sol.y[0], label='$\\theta$')
ax.plot(sol.t, sol.y[1], label='$\\dot{\\theta}$')
ax.plot(sol.t, sol.y[2], label='$\\sigma$')
ax.set_xlabel('$t$')
ax.grid()
ax.legend()
Path('./figures').mkdir(parents=True, exist_ok=True)
plt.savefig('figures/integral_control_small_mass.pdf', pad_inches=0.0)
def main02_constant_input_simulation_fixed_delta():
""" Simple simulation of a pendulum system. Choose a constant
desired output angle delta; a constant control input Tss
will be calculated to obtain a stable equilibrium at the
desired output angle delta. """
m, g, l, k = 1, 10, 1, 2
a, b, c = g / l, k / m, 1 / m / l / l
pend = pendulum.Pendulum(m, g, l, k)
theta0, theta_dot0 = 0, 0
x0 = np.array([theta0, theta_dot0])
t_start, t_end = 0, 10
sample_count = 1000
t = np.linspace(t_start, t_end, sample_count)
tspan = [t_start, t_end]
# Constant control input derived from desired constant angle
delta = pi / 4
Tss = a / c * sin(delta)
T = lambda t, x: Tss
f = lambda t, x: pend.state_space_static_controller(t, x, T)
sol = scipy.integrate.solve_ivp(f, tspan, x0, t_eval=t)
fig, ax = fc.new_figure()
ax.plot([t_start, t_end], [delta, delta],
'r--',
label='Reference $\\delta$')
ax.plot(sol.t, sol.y[0], label='$\\theta$')
ax.plot(sol.t, sol.y[1], label='$\\dot{\\theta}$')
ax.set_xlabel('$t$')
ax.grid()
ax.legend()
Path('./figures').mkdir(parents=True, exist_ok=True)
plt.savefig('figures/constant_control.pdf', pad_inches=0.0)
def main04_output_feedback_stabilization():
""" Simulation of a pendulum system. The system is controlled by
a stabilizing output-feedback controller around desired angle
delta. States are estimated by an observer. """
m, g, l, k = 1, 10, 1, 2
delta = pi / 4
pend = pendulum.Pendulum(m, g, l, k)
x0 = np.array([0, 0, 0, 0])
t_start, t_end = 0, 10
N = 1000
t = np.linspace(t_start, t_end, N)
tspan = [t_start, t_end]
k1, k2, h1, h2 = 1, 1, 1, 1
K = np.array([k1, k2])
H = np.array([h1, h2])
f_original = pend.state_space_observer_controller
f = lambda t, x: f_original(t, x, K, H, delta)
sol = scipy.integrate.solve_ivp(f, tspan, x0, t_eval=t)
fig, ax = fc.new_figure()
ax.plot([t_start, t_end], [delta, delta],
'r--',
label='Reference $\\delta$')
ax.plot(sol.t, sol.y[0], label='$\\theta$')
ax.plot(sol.t, sol.y[1], label='$\\dot{\\theta}$')
ax.plot(sol.t, sol.y[2], label='$\\hat{\\theta}$')
ax.plot(sol.t, sol.y[3], label='$\\hat{\\dot{\\theta}}$')
ax.set_xlabel('$t$')
ax.grid()
ax.legend()
Path('./figures').mkdir(parents=True, exist_ok=True)
plt.savefig('figures/output_feedback.pdf', pad_inches=0.0)