Path traced out in state space by a double pendulum |
\[
\dot{\mathbf{q}} = I^{-1} \mathbf{p}
\]\[
\dot{\mathbf{p}} = \frac{1}{2} \dot{\boldsymbol{\theta}}^t \frac{\partial I}{\partial \mathbf{q}}\dot{\boldsymbol{\theta}} - \frac{\partial V}{\partial\mathbf q}
\]
which, for this system, is
\[
\dot{p}_1 = \frac{1}{2}\dot{\mathbf{q}}^t \begin{pmatrix} 0 & m_2\ell_1\ell_2 \sin(\theta_2-\theta_1) \\ m_2\ell_1\ell_2 \sin(\theta_2-\theta_1) &0\end{pmatrix} \dot{\mathbf{q}} + (m_1 + m_2) g\ell_1 \sin \theta_1
\]\[
\dot{p}_2 = \frac{1}{2}\dot{\mathbf{q}}^t \begin{pmatrix} 0 & -m_2\ell_1\ell_2 \sin(\theta_2-\theta_1) \\ -m_2\ell_1\ell_2 \sin(\theta_2-\theta_1) &0\end{pmatrix} \dot{\mathbf{q}} + m_2 g\ell_2 \sin \theta_2,
\]
with the symmetric moment of inertia
\[
I = \begin{pmatrix} (m_1 +m_2) \ell_1^2 & m_2\ell_1\ell_2\cos(\theta_2-\theta_1) \\ m_2 \ell_1 \ell_2 \cos(\theta_2 -\theta_1) & m_2 \ell_2^2\end{pmatrix}.
\]
We use a one-step, first order method to simulate (with time step $h$). The usual thing to do is to consider the derivatives $\dot{\mathbf{q}}$ and $\dot{\mathbf{p}}$ as difference quotients, and solving for the later time step (denoted with the superscript $n+1$) in terms of the earlier (superscript $n$):
\[
\frac{\mathbf{q}^{n+1} - \mathbf{q}^n}{h} = I^{-1} \mathbf{p}
\]and\[
\frac{\mathbf{p}^{n+1} - \mathbf{p}^n}{h} = \frac{1}{2} \mathbf{p}^t I^{-1} \frac{\partial I}{\partial \mathbf{q}} I^{-1} \mathbf{p} - \frac{\partial V}{\partial \mathbf{q}}.
\]
Now of course, the subtlety is how to evaluate the quantities on the right-hand side; since now we don't assume that we have direct access to $\dot{\mathbf q}$, we have re-expressed it in terms of $\mathbf{p}$ in the second equation. Also, since $\partial I/\partial{\mathbf{q}}$ is some higher-order tensor, the notation becomes very confusing, very fast. If we write
\[
B = \frac{\partial I}{\partial \theta_1}
\] then we find $\partial I/\partial \theta_2 = -B$, since, as we saw, $I$ only depends on $\theta_2 -\theta_1$. Therefore, we rewrite it as
\[
\frac{\mathbf{p}^{n+1} - \mathbf{p}^n}{h} = \frac{1}{2}\begin{pmatrix} \mathbf{p}^t B \mathbf{p} \\ -\mathbf{p}^t B \mathbf{p} \end{pmatrix}- \frac{\partial V}{\partial \mathbf{q}}.
\]
The Euler-B method uses $\mathbf{p}^{n+1}$ and $\mathbf{q}^n$ on on the right-hand side (we choose it instead of Euler-A, which would use $\mathbf{q}^{n+1}$ and $\mathbf{p}^n$, because the moment-of-inertia tensor $I$ depends nonlinearly on $\mathbf{q}$ and makes the resultant equations harder to solve—we'll see where that comes in, in a little bit).
Once we solve for $\mathbf{p}^{n+1}$, then it is clear how to solve for $\mathbf{q}^{n+1}$ (noting we evaluate $I$ at $\mathbf{q}^n$):
\[
\mathbf{q}^{n+1} = \mathbf{q}^n + h I^{-1} \mathbf{p}^{n+1}.
\]
Now $\mathbf{p}^{n+1}$ is harder:
\[
\mathbf{p}^{n+1} = \mathbf{p} ^n + h \left( \frac{1}{2} \begin{pmatrix} (\mathbf{p}^{n+1})^t B \mathbf{p}^{n+1} \\-(\mathbf{p}^{n+1})^t B \mathbf{p}^{n+1}\end{pmatrix}- \frac{\partial V}{\partial \mathbf{q}}\right)
\]
where, again, the $B$ and $\partial V/\partial{\mathbf{q}}$ are evaluated at $\mathbf{q}^n$. The trouble is, of course, that $\mathbf{p}^{n+1}$ appears on both sides, and it's a nonlinear function of $\mathbf{p}^{n+1}$ (though only quadratic nonlinearity, but nonlinearity, nonetheless). How do we solve this? By...
Using Newton's Method.
To clear the clutter, we let $\mathbf{x}$ be the unknown, namely $\mathbf{p}^{n+1}$, and simply write $\mathbf{p} = \mathbf{p}^n$ as known given data. Thus, bringing all of that junk to one side of the equation, we want to solve $F(\mathbf{x}) = 0$ where $F$ is:\[
F(\mathbf{x}) = \mathbf{x} - \mathbf{p} - h\left( \frac{1}{2} \mathbf{x}^t B \mathbf{x}\begin{pmatrix} 1 \\ - 1\end{pmatrix}- \frac{\partial V}{\partial \mathbf{q}}\right).
\]
Newton's method is to consider the iteration $\mathbf{x}_{m+1} = \mathbf{x}_m - F'(\mathbf{x}_m)^{-1}F(\mathbf{x}_m)$. Now
\[
F'(\mathbf{x}) = \mathrm{Id} + h \begin{pmatrix} -\mathbf{x}^t B \\ \mathbf{x}^t B \end{pmatrix}
\]
where $\mathrm{Id}$ is the identity matrix. This, for small enough $h$, is certainly invertible. In fact, the Newton iterations tend to be stable simply because the presence of an identity matrix with small $h$ makes the problem well-conditioned. And as a starting point for Newton's method, we can use $\mathbf{x}_0 = \mathbf{p} = \mathbf{p}^n$, the value of $\mathbf{p}$ at the current timestep; it should be reasonably close, because it is the smooth solution to differential equation defined by a smooth vector field. Finally, we should stop the Newton iterations when the residual $F(\mathbf{x}_m)$ falls within a small tolerance, say, $10^{-7}$; we then simply accept $\mathbf{x}_m$ as the new value $\mathbf{p}^{n+1}$. Here's how to say that in MATLAB:
A = [ mm*r1^2, m2*r1*r2*cos(q(1)-q(2));
m2*r1*r2*cos(q(1)-q(2)), m2*r2^2]; % matrix I
C = [0 , -m2*r1*r2*sin(q(1)-q(2)) ;
-m2*r1*r2*sin(q(1)-q(2)), 0]; % matrix dI/dq
B = -A\(C/A); % derivative of inverse matrix
% -I^-1 dI/dq I^-1
dVdq = [mm*g*r1*sin(q(1)); m2*g*r2*sin(q(2))];
x = p; % initialize Newton iteration
F = [1.0; 1.0]; % set initially to something that'll give a big norm
iter = 0;
while (norm(F) > 1.0e-7 && iter <= itmax)
F = x - p + h *(1/2* [x'*B*x; -x'*B*x] + dVdq); % current F
DF = eye(2) + h*([x'*B; -x'*B]); % Jacobian
x = x - DF\F; % update iteration
iter = iter+1;
end
p = x; % update p
q = q + h* (A\p); % update q, using updated p
Of course, we should save up the values of $q$ as a history if we want to see where it's been in state space. Here's a video of just that: we save the values of $q$ and plot them as a trail following the current point. But in this time we show the angles as being on the abstract phase space, a torus, rather than as the actual angular displacements of a pendulum (as shown in Part 1):
The picture that opened this post is simply the final frame of the movie, showing everywhere in state space where the pendulum has been. It is this abstract representation of a physical system that leads to intriguing visualizations! The idea is, hopefully, in some other physical system, the state space of the variables might more clearly point to interesting features of the system. We shall frequently keep this philosophy in mind in our posts!