Message Boards Message Boards


Conversion to a system of second order ODEs then first order ODEs

Posted 2 months ago
2 Replies
1 Total Likes

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 + 
       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\)])\)\ \(\*
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] + 
      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, 

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!


2 Replies
Posted 2 months ago


Thank you for your suggestions. I should have opened my question with details of the application.

I am working on a few control and estimation algorithms.The system is highly dynamical and I have to operate away from equilibrium so linearization is a last resort. One of the nonlinear controls uses the solution of the State Dependent Riccati Equation (SDRE) that requires the dynamics of the system to be cast in the State-Dependent Coefficients (SDC) form. $ \dot{\mathbf{x}}=A(\mathbf{x}) \mathbf{x}+B(\mathbf{x}) \mathbf{u} $ I have to verify all sort of requirements on the $A(\mathbf{x})$ and $B(\mathbf{x})$ matrices and I would like to do that by hand or symbolically.

Moreover, the control designer has quite a bit of freedom in converting the nonlinear system state function to the SDC form, i.e., $f(\mathbf{x} = A(\mathbf{x}) \mathbf{x}$. Some papers suggest that this factorization can be somewhat optimized and I want to put that to test.

For the estimator I would like to have a "truth model" of the dynamics so I can determine on-line the "goodness" of the estimation.



I have two suggestions:

First, If your goal is to simulate and explore the dynamics and control of an inverted N link pendulum, it is significantly easier if you use Wolfram System Modeler (either from within Mathematica or using it stand-alone). In SystemModeler, this double pendulum is already modeled as an example using six built-in components and connecting them.

enter image description here

You can easily invert the pendulum and add a cart (There are some examples of a single link inverted pendulum as well). This would give you a simulation and 3D animation of the links. Additionally, you can export the equations of motion to Mathematica for manipulation. (including a linearized state space model)

Second, In the blog post, the author linearizes the model around equilibrium and creates his model from StateSpaceModel[]. He also could have generated the nonlinear model with NonlinearStateSpaceModel[]. Why can't you use NonlinearStateSpaceModel or StateSpaceModel since you have the same equations?



Reply to this discussion
Community posts can be styled and formatted using the Markdown syntax.
Reply Preview
or Discard

Group Abstract Group Abstract