Dear Colleagues,
I have used Andrew Moylan's most excellent blog on the n-link pendulum on a cart as inspiration for my own notebook to derive the Equations of Motion (EoMs) for the pendulum. I have included viscous and friction in the revolute joints and my notation is slightly different than Andrew's but the structure of the EoMs is similar. For example, for a two-link pendulum I get.
{Subscript[b, 0] Derivative[1][Subscript[q, 0]][t] +
Subscript[m, 0] (Subscript[q, 0]^\[Prime]\[Prime])[t] +
Subscript[m, 1] (Subscript[q, 0]^\[Prime]\[Prime])[t] +
Subscript[m, 2] (Subscript[q, 0]^\[Prime]\[Prime])[t] +
1/2 Cos[Subscript[q, 2][t]] Subscript[l, 2] Subscript[m,
2] (Subscript[q, 2]^\[Prime]\[Prime])[t] ==
u[t] + 1/2 (Sin[Subscript[q, 2][t]] Subscript[l, 2] Subscript[m, 2]
Derivative[1][Subscript[q, 2]][t]^2 +
Subscript[l,
1] (Subscript[m, 1] +
2 Subscript[m,
2]) (Sin[Subscript[q, 1][t]] Derivative[1][Subscript[q, 1]][
t]^2 - Cos[Subscript[q, 1][t]] (Subscript[q,
1]^\[Prime]\[Prime])[t])), \!\(
\*SubsuperscriptBox[\(l\), \(1\), \(2\)]\ \((
\*SubscriptBox[\(m\), \(1\)] + 4\
\*SubscriptBox[\(m\), \(2\)])\)\ \(\*
SuperscriptBox[
SubscriptBox["q", "1"], "\[Prime]\[Prime]",
MultilineFunction->None][t]\)\) +
4 (Subscript[k, 1] Subscript[q, 1][t] +
Subscript[k, 2] (Subscript[q, 1][t] - Subscript[q, 2][t]) +
Subscript[b, 1] Derivative[1][Subscript[q, 1]][t] +
Subscript[moi, 1] (Subscript[q, 1]^\[Prime]\[Prime])[t]) +
2 Subscript[l,
1] (Subscript[m,
1] (-g Sin[Subscript[q, 1][t]] +
Cos[Subscript[q, 1][t]] (Subscript[q, 0]^\[Prime]\[Prime])[
t]) + Subscript[m,
2] (-2 g Sin[Subscript[q, 1][t]] +
2 Cos[Subscript[q, 1][t]] (Subscript[q, 0]^\[Prime]\[Prime])[
t] + Subscript[l,
2] (Sin[Subscript[q, 1][t] - Subscript[q, 2][t]] Derivative[
1][Subscript[q, 2]][t]^2 +
Cos[Subscript[q, 1][t] - Subscript[q, 2][t]] (Subscript[q,
2]^\[Prime]\[Prime])[t]))) == 0,
Subscript[k, 2] (-Subscript[q, 1][t] + Subscript[q, 2][t]) +
Subscript[b, 2] Derivative[1][Subscript[q, 2]][t] + 1/4 (\!\(
\*SubsuperscriptBox[\(l\), \(2\), \(2\)]\
\*SubscriptBox[\(m\), \(2\)]\) + 4 Subscript[moi, 2]) (Subscript[q,
2]^\[Prime]\[Prime])[t] ==
1/2 Subscript[l, 2] Subscript[m,
2] (g Sin[Subscript[q, 2][t]] -
Cos[Subscript[q, 2][t]] (Subscript[q, 0]^\[Prime]\[Prime])[t] +
Subscript[l,
1] (Sin[Subscript[q, 1][t] - Subscript[q, 2][t]] Derivative[1][
Subscript[q, 1]][t]^2 -
Cos[Subscript[q, 1][t] - Subscript[q, 2][t]] (Subscript[q,
1]^\[Prime]\[Prime])[t]))}
where q0 is the position of the cart and q1 and q2 are the angles of the links measured from the positive vertical.
I have a requirement to cast the EoMs in a form that is called, in some texts, State-Dependent Coefficient form. Right now I am doing this by hand. In the first step I convert the EoMs above to a system of coupled second order ODEs of the form
$ M(\mathbf{q}) \ddot{\mathbf{q}} + C(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} + K(\mathbf{q}) \mathbf{q} = 0. $
In the second step I use standard matrix manipulation to get to the state space form of the EoMs
$ \dot{\mathbf{x}} = A(\mathbf{x}, t) \mathbf{x}, $
where the state vector is $\mathbf{x}=(\mathbf{q},\dot{\mathbf{q}})^\text{T}$
While this is time consuming but manageable for a two-link pendulum it becomes quite tedious and prone to errors for three or more links.
I have tried to find a way to program the conversion from the EoMs generated with Mathematica to the second order form using Simplify[]
with assumptions but I have made no progress. Are there any documented methods for symbolic manipulation to convert the EoMs to a vector equation? I'm sure folks in robotics have figured this out already.
Thank you!
Bogdan