This summer I got accepted into Google Summer of Code under Sympy. For those that don't know, Sympy is a computer algebra system (CAS) written in Python. It can handle all sorts of mathematics, but what I use it most for is deriving and solving for the equations of motion for physical systems using the functionality found in Sympy.physics.mechanics (as seen in this previous post).

My project this summer is adding a robust linearization routine to this package to linearize systems with constraints. The code provided in sympy.physics.mechanics currently makes it very easy to generate the equations of motion (EOM) for the dynamics of rigid, multibody systems. However, these EOM are typically nonlinear in nature, and can be quite lengthy. For analysis, linearizing the equations about equilibrium points, or trajectories is desired. Linearized equations are exceptionally useful because:

• They reduce the computational complexity of analysis and simulation
• They allow for traditional stability analysis via eigenvalue decomposition
• They are often used in control theory for controller design and analysis

Nonlinear equations can typically be written in the following form:

$$\dot{x} = f(t, x, u)$$

where $t$ is time, $x$ is the system state, and $u$ is the input to the system. For unconstrained systems (systems, where the states are only functions of time, and not of each other), linearization is as simple as taking the jacobian of $f(t, x, u)$ with respect to $x$ and $u$

$$\dot{\delta x} \approx \nabla_x f(t, x, u) |_{x^*, u^*} \delta x + \nabla_u f(t, x, u) |_{x^*, u^*} \delta u$$

where $x^*$ and $u^*$ represent the trajectory or equilibrium point that linearization is occuring about, and $\delta x$ and $\delta u$ are the changes from this point (i.e. $\delta x = x - x^*$).

However, if there are constraints, then linearization becomes more complicated, due to the fact that some variables in $x$ are now functions of other variables, leading to the need to implement the chain rule. Simply taking the jacobian as you normally would leads to a linearization that doesn't accurately represent the system. This is demonstrated below with an example of a simple pendulum.

Example: Single Pendulum with Minimum Coordinates¶

To derive the equations of motion, well first use Kane's Method with no constraints. Kane's Method is something I hadn't seen before in my dynamic's classes (we focussed more on Lagrange), so I spent part of this week learning it. It differs from Lagrange's method in that instead of only choosing generalized coordinats, generalized speeds are choosen as well. This can make the equations simpler, as the speeds do not have to be the derivatives of the coordinates.

While Sympy has functions to make the derivation using Kane's Method easier, I'm going to show each calculation step for a clearer description of the method.

For a simple pendulum, the system has one degree of freedom, so only one generalized coordinate is needed. In this case, we'll choose the angle off vertical ($q_1$). To make things simple, the generalized speed will be the angular velocity of the pendulum, which is just the derivative of this coordinate ($u_1 = \dot{q_1}$).

First we need to create the world reference frame $N$ and its origin $N^*$

Next, a rotating reference frame $A$ is created, attached to the pendulum. The rotation of this frame is $q_1$ about the $N_z$ axis, with an angular velocity of $\dot{q}_1 = u_1$. The location of the pendulum mass point $P$ can then be set as $r^{P/N^*} = L \hat{a}_x$.

The velocity of the point P can then be found:

Next we solve for the kinematic differential equations (KDE) for the system. These relate the derivatives of the generalized coordinates to the generalized velocities by

$$ kde(q, \dot{q}, u, t) = 0 $$

For this system, since the generalized speed is just the derivative of the generalized coordinates, the KDEs are rather simple:

Kane's method uses the partial derivative of the velocity of each point/body with respect to each generalized speed. In our case, with only one point and speed this is just a single value:

Next we input the resultant at each point of all forces acting on that point. For the pendulum, this is just gravity acting on the mass in the $N_x$ direction.

The generalized active forces are then found by the following expression:

$$F_r = \sum_{i=1}^v v_r^{P_i} \cdot R_i \quad (r = 1, ..., n)$$

These form the "F" part of Newton's second law (F = ma). In this case, n = 1, v = 1, so $F_1$ is then:

Next we need to solve for the generalized inertial forces. This is the "ma" part in Newton's second law. To do this, we first need to find the acceleration of the pendulum point.

The inertial force $R^*$ for the point is then

The generalized inertial forces are then found by the following expression:

$$ F_r^* = \sum_{i=1}^v v_r^{P_i} \cdot R_i^* \quad (r = 1, ..., n)$$

As before, n = 1, v = 1, so $F_1^*$ is then:

Kanes equations for the dynamic differential equations are then

$$ F_r + F_r^* = 0 \quad (r = 1, ... , n) $$

Combining the kinematic differential equations with the dynamic differential equations yields a system with 2 unknowns ($\dot{q}_1$ and $\dot{u}_1$), and 2 equations. Thus, it can be solved.

Thus, the final form for the equations of motion is:

$$ \begin{bmatrix} \dot{q}_1 \\ \dot{u}_1 \end{bmatrix} = \begin{bmatrix} u_1 \\ \frac{-9.8}{L} sin(q_1) \end{bmatrix} $$

As I'm still just learning Kane's method myself, the above derivation isn't very explanatory. I plan on making a post better explaining the procedure once I get the hang of it better. Luckily, Sympy contains a KanesMethod class that crunches all the above math for you.

Using Kanes Method Class¶

Continuing with what was already derived above, all that needs to be done is create a Particle object for the pendulum mass, and then create a KanesMethod object with the forces and bodies derived above. I won't go too in depth on these steps, as we're just replicating what was done above using the easy functionality provided by sympy.physics.mechanics.

As seen here, the results match what was solved for above by hand, but in a much easier manner using the methods provided in Sympy.

Non-minimal set of coordinates¶

The pendulum system demonstrated above is extremely simple, and a minimal set of coordinates and speeds readily found. But for more complicated systems it's often not that easy to find a such a minimal representation. In these cases additional coordinates and speeds are used, and an additional set of constraint equations are added relating the coordinates and speeds together.

To demonstrate this, the pendulum system will be re-derived using the x and y coordinates of the mass as generalized coordinates, and their derivatives as the generalized speeds. While you would never do this in practice (it makes the problem much harder), it should be a good demonstration of both configuration and velocity constraints.

As before, the a rotating reference frame $A$ is attached to the pendulum. However, this time we don't have a generalized coordinate representing the rotation angle. This can be easily calculated though as

$$\theta = \arctan(q_2/q_1)$$

The pendulum mass can easily be located in the global reference frame $N$ using the generalized coordinates, as they're just the x and y coordinates of the mass.

Next, we calculate the kinematic differential equations as before

The velocity of point P is just the time derivative of its position vector from the origin. Here we substitute all $\dot{q}$ terms for an equivalent expression using $u$.

As the system has only one degree of freedom, but two generalized coordinates, there needs to be a configuration consttraint relating the coordinates. The configuration constraint is expressed in the form

$$ f_c(q, t) = 0 $$

In this case, the length of the pendulum must always be L, so $f_c$ can be expressed as

Similarly, there are two generalized speeds, making the use of a velocity constraint equation necessary. Velocity constraints are written as

$$ f_v(q, u, t) = 0 $$

In this case, the velocity in the $A_x$ direction is always zero (the pendulum is never getting longer). This can be expressed by calculating the velocity of P in reference frame N, and setting the component in the $A_x$ direction to 0.

Acceleration constraints are then just the time derivative of the velocity constraint:

For this example, the remainder will be calculated using the KanesMethod class as above. Due to the presence of velocity constraints (nonholonomic), Kanes Method proceeds a little different than before, and this post is already long enough...

All you really need to know is that before the EOM are derived, a subset of generalized coordinates and speeds needs to be choosen to be the "independent" variables, making the remaining "dependent". There are reasons to choose some variables over others, but here we'll just let $q_1$ and $u_1$ be independent, making $q_2$ and $u_2$ dependent.

Note that the right hand side is now a 4x1 matrix, rather than a 2x1 as before. This will be important later.

To ensure that the derived equations are equivalent, a variable transformation could be done, and the minimal case trasnformed into the nonminimal equations. However, we're just going to simulate the system to show that they have the same response. Not rigorous at all, but it's good enough for now.

Simulation¶

First we need to substitute in numbers for the constants L, and m

Using lambdify, the equations of motion derived above can then be converted into equivalent Numpy expressions, allowing for fast computations. Then, a second lambda expression can be used to put the EOM into the needed for numerical integration with odeint.

Both systems can then be simulated with odeint. Let's define the initial condition to be horizontal to the right, with no initial velocity.

As can be seen by the above, the simulation results are identical. Thus, both the minimal and nonminimal sets of coordinates result in the same system equations.

Linearization¶

As mentioned at the beginning of this behemoth of a post, my project for the summer is on implementing functionality for linearizing systems of equations with and without constraints. While linearizing an unconstrained system is simple, some care must be taken for linearizing a system with constraints.

In the above sections we derived two sets of equations of motion for a simple pendulum; one set without constraints, and one with. Now let's see what happens when we apply the naive linearization method of the simple jacobian to both systems:

First, we need to determine the trim condition we wish to linearize about. This must be a valid position (satisfy the system of equations). For simplicity, lets choose the stable, straight down at rest configuration ($\theta = 0$, $\omega = 0$).

One use for a linearized system is for stability analysis at the point of linearization. This is done by computing the eigenvalues of the resulting system matrix.

Looking at the eigenvalues of each matrix (as well as the shape of each matrix) you can already see something's not quite correct with the nonminimal linearization. With only one coordinate and one speed needed to set the state for the system, there should be only 2 eigenvalues, while the nonminimal linearization gives 4.

To further the point, the linearized systems can be simulated and compared with the nonlinear system. For small angle deviations, the system responses should be similar. We'll use a deviation of 1 degree, which should be small enough that they match:

While the minimal coordinate linear and nonlinear simulation results line up nicely, the $N_x$ coordinate response of the linearized nonminimal system is significantly off. This is because for the nonminimal system, the generalized coordinates and speeds aren't independent but functions of other coordinates and speeds. Thus, when taking the jacobian the chain rule needs to be applied correctly, and the dependent states removed from the linearized expression.

This is the basis for my GSoC project. The implementation of a robust way to linearize systems that works, regardless of constraints or nonminimal system realization. A method for doing this in a systematic manner was derived by one of my mentors, and I plan on starting with that. I think it can be generalized to Lagrange's equations, as well as other systems, but I need to think more on that.

This week I spent some time teaching myself Kane's Method, and then the remainder working through a bunch of test cases to be used as unit tests for the linearization routines. Next week I'll begin work on the actual implementation.

This ipython notebook can be found here