Implementation of a 2-link arm.
$q = \left[\theta_1, \theta_2, \dot{\theta}_1, \dot{\theta}_2\right] \rightarrow \dot{q} = \left[q_3, q_4, \ddot{q}_1, \ddot{q}_2\right]$
ODE is defined as:
$\ddot{q} = B^{-1}(q)(-C(\dot{q}, q) - g(q) + F)$
With:
$B(q) = \begin{bmatrix} (m_1 + m_2)l_1^2 + m_2l_2^2 + 2m_2l_1l_2\cos(q_2) & m_2l_2^2 + m_2l_1l_2\cos(q_2) \\ m_2l_2^2 + m_2l_1l_2\cos(q_2) & m_2l_2^2 \end{bmatrix}\\ C(\dot{q}, q) = \begin{bmatrix} -m_2l_1l_2\sin(q_2)(2q_3q_4 + q_4^2) \\ -m_2l_1l_2\sin(q_2)q_3q_4 \end{bmatrix}\\ g(q) = \begin{bmatrix} -(m_1+m_2)gl_1\sin(q_1) - m_2gl_2\sin(q_1+q_2) \\ -m_2gl_2\sin(q_1+q_2) \end{bmatrix}$
In [1]:
import numpy as np
from scipy.integrate import ode
In [2]:
import matplotlib
import matplotlib.pyplot as plt
from matplotlib import animation
%matplotlib notebook
In [3]:
def eom(t, q, params, ths, Kpd):
m1 , m2, l1, l2, g = params
q1, q2, q3, q4, q5, q6 = q
b11 = (m1 + m2)*l1**2 + m2*l2**2 + 2*m2*l1*l2*np.cos(q2)
b12 = m2*l2**2 + m2*l1*l2*np.cos(q2)
b21 = m2*l2**2 + m2*l1*l2*np.cos(q2)
b22 = m2*l2**2
B = np.array([[b11, b12], [b21, b22]])
c1 = -m2*l1*l2*np.sin(q2)*(2*q3*q4 + q4**2)
c2 = -m2*l1*l2*np.sin(q2)*q3*q4
C = np.array([[c1], [c2]])
g1 = -(m1 + m2)*g*l1*np.sin(q1) - m2*g*l2*np.sin(q1 + q2)
g2 = -m2*g*l2*np.sin(q1 + q2)
g = np.array([[g1], [g2]])
Kp1, Kd1, Ki1, Kp2, Kd2, Ki2 = Kpd
th1s, th2s = ths
f1 = Kp1*(th1s - q1) - Kd1*q3 + Ki1*q5
f2 = Kp2*(th2s - q2) - Kd2*q4 + Ki2*q6
Fhat = np.array([[f1], [f2]])
F = np.dot(B, Fhat)
qdotdot = np.dot(np.linalg.inv(B), -C - g + F)
qdot = [q3, q4] + qdotdot.T[0].tolist() + [th1s-q1, th2s-q2]
return qdot
Solve system with with initial position $q_0 = \left[\pi/2, \pi/2, 0., 0.\right]$ and desired position $\theta_s = \left[\pi/2, -\pi/2\right]$. Parameters are set to $m_1 = m_2 = l_1 = l_2 = 1$ and $g = 9.81$.
Solve with simple PID controller, with $K_p = \left[15, 15\right]$ and $K_d = \left[7, 10\right]$ and $K_i = \left[10, 10\right]$.
In [4]:
q0 = [-np.pi/2, np.pi/2, 0., 0., 0., 0.]
ths = (np.pi/2, -np.pi/2)
Ts = 20
m1 = 1
m2 = 1
l1 = 1
l2 = 1
g = 9.81
params = (m1, m2, l1, l2, g)
Kp1 = 15
Kd1 = 7
Ki1 = 10
Kp2 = 15
Kd2 = 10
Ki2 = 10
Kpd = (Kp1, Kd1, Ki1, Kp2, Kd2, Ki2)
r = ode(eom).set_integrator('dopri5')
r.set_initial_value(q0, 0).set_f_params(params, ths, Kpd)
dt = 0.1
sol = []
while r.successful() and r.t < Ts:
q = r.integrate(r.t + dt)
sol.append(q)
sol = np.array(sol)
In [5]:
t = np.linspace(0, Ts, Ts*10)
q1d = np.ones(len(t))*ths[0]
q2d = np.ones(len(t))*ths[1]
plt.figure(figsize=(9,4))
plt.subplot(221)
plt.plot(t, sol[:, 0], 'b', label='Actual')
plt.plot(t, q1d, 'g--', label='Desired')
plt.legend(loc='best')
plt.xlabel('t')
plt.ylabel(r'$\theta_1$')
plt.grid()
plt.subplot(222)
plt.plot(t, q1d-sol[:, 0])
plt.xlabel('t')
plt.ylabel('error')
plt.grid()
plt.subplot(223)
plt.plot(t, sol[:, 1], 'b', label='Actual')
plt.plot(t, q2d, 'g--', label='Desired')
plt.legend(loc='best')
plt.xlabel('t')
plt.ylabel(r'$\theta_2$')
plt.grid()
plt.subplot(224)
plt.plot(t, q2d-sol[:, 1])
plt.xlabel('t')
plt.ylabel('error')
plt.grid()
In [6]:
x1 = np.sin(sol[:, 0])*l1
y1 = np.cos(sol[:, 0])*l1
x2 = x1 + np.sin(sol[:, 0] + sol[:, 1])*l2
y2 = y1 + np.cos(sol[:, 0] + sol[:, 1])*l2
x1d = np.sin(ths[0])*l1
y1d = np.cos(ths[0])*l1
x2d = x1d + np.sin(ths[0] + ths[1])*l2
y2d = y1d + np.cos(ths[0] + ths[1])*l2
In [7]:
fig, ax = plt.subplots()
ax.set_xlim((-l1-l2, l1+l2))
ax.set_ylim((-l1-l2, l1+l2))
desire_position = ax.plot(x1d, y1d, 'r*')
desire_position = ax.plot(x2d, y2d, 'r*')
line1, = ax.plot([], [], 'b')
line2, = ax.plot([], [], 'b')
lines = [line1, line2]
def init():
line1.set_data([], [])
line2.set_data([], [])
return [line1, line2]
def animate(i):
line1.set_data([0, x1[i]], [0, y1[i]])
line2.set_data([x1[i], x2[i]], [y1[i], y2[i]])
return [line1, line2]
anim = animation.FuncAnimation(fig, animate, init_func=init, frames=100, interval=len(x1), blit=True)
In [8]:
def eom_new(t, q, u, params):
m1 , m2, l1, l2, g = params
u1, u2 = u
q1, q2, q3, q4 = q
b11 = (m1 + m2)*l1**2 + m2*l2**2 + 2*m2*l1*l2*np.cos(q2)
b12 = m2*l2**2 + m2*l1*l2*np.cos(q2)
b21 = m2*l2**2 + m2*l1*l2*np.cos(q2)
b22 = m2*l2**2
B = np.array([[b11, b12], [b21, b22]])
c1 = -m2*l1*l2*np.sin(q2)*(2*q3*q4 + q4**2)
c2 = -m2*l1*l2*np.sin(q2)*q3*q4
C = np.array([[c1], [c2]])
g1 = -(m1 + m2)*g*l1*np.sin(q1) - m2*g*l2*np.sin(q1 + q2)
g2 = -m2*g*l2*np.sin(q1 + q2)
g = np.array([[g1], [g2]])
Fhat = np.array([[u1], [u2]])
F = np.dot(B, Fhat)
qdotdot = np.dot(np.linalg.inv(B), -C - g + F)
qdot = [q3, q4] + qdotdot.T[0].tolist()
return qdot
In [17]:
q0 = [-np.pi/2, np.pi/2, 0., 0.]
ths = (np.pi/2, -np.pi/2)
Ts = 20
m1 = 1
m2 = 1
l1 = 1
l2 = 1
g = 9.81
params = (m1, m2, l1, l2, g)
Kp1 = 15
Kd1 = 7
Ki1 = 10
Kp2 = 15
Kd2 = 10
Ki2 = 10
def get_input(q, x_int):
q1, q2, q3, q4 = q
u1 = Kp1*(ths[0] - q1) - Kd1*q3 + Ki1*x_int[0]
u2 = Kp2*(ths[1] - q2) - Kd2*q4 + Ki2*x_int[1]
return (u1, u2)
x_int = [0., 0.]
u0 = get_input(q0, x_int)
f_old = np.array([ths[0]-q0[0], ths[1]-q0[1]])
f_int = np.array([0., 0.])
r = ode(eom_new).set_integrator('dopri5')
r.set_initial_value(q0, 0).set_f_params(u0, params)
dt = 0.1
sol = []
while r.successful() and r.t < Ts:
q = r.integrate(r.t + dt)
sol.append(q)
# Calculate integral of error numerically
f_new = np.array([ths[0]-q[0], ths[1]-q[1]])
f_int = f_int + (f_old + f_new) * dt / 2.
f_old = f_new
x_int = [f_int[0], f_int[1]]
u = get_input(q, x_int)
r.set_f_params(u, params)
sol = np.array(sol)
In [23]:
t = np.linspace(0, Ts, Ts*10)
q1d = np.ones(len(t))*ths[0]
q2d = np.ones(len(t))*ths[1]
plt.figure(figsize=(9,4))
plt.subplot(221)
plt.plot(t, sol[:, 0], 'b', label='Actual')
plt.plot(t, q1d, 'g--', label='Desired')
plt.legend(loc='best')
plt.xlabel('t')
plt.ylabel(r'$\theta_1$')
plt.grid()
plt.subplot(222)
plt.plot(t, q1d-sol[:, 0])
plt.xlabel('t')
plt.ylabel('error')
plt.grid()
plt.subplot(223)
plt.plot(t, sol[:, 1], 'b', label='Actual')
plt.plot(t, q2d, 'g--', label='Desired')
plt.legend(loc='best')
plt.xlabel('t')
plt.ylabel(r'$\theta_2$')
plt.grid()
plt.subplot(224)
plt.plot(t, q2d-sol[:, 1])
plt.xlabel('t')
plt.ylabel('error')
plt.grid()