Message Boards Message Boards

2 Replies
1 Total Likes
View groups...
Share this post:

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

Posted 2 years ago

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!


POSTED BY: Bogdan Udrea
2 Replies
Posted 2 years 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.


POSTED BY: Bogdan Udrea


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?



POSTED BY: Neil Singer
Reply to this discussion
Community posts can be styled and formatted using the Markdown syntax.
Reply Preview
or Discard

Group Abstract Group Abstract