Posted on May 24, 2014

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.

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.

In [1]:

```
from IPython.display import SVG
SVG(filename='Pendulum_Min_Coord.svg')
```

Out[1]:

In [2]:

```
# Import required functionality
from sympy import symbols, sin, cos, atan, simplify, solve, Matrix
from sympy.physics.mechanics import *
```

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}$).

In [3]:

```
# Create generalized coordinate and speed variables. The `dynamicsymbols`
# function creates each symbol as a function of time (i.e. q1 = q1(t))
q1 = dynamicsymbols('q1') # Generalized coordinate: angle of pendulum
q1d = dynamicsymbols('q1', 1) # Derivative of generalized coordinate
u1 = dynamicsymbols('u1') # Generalized speed: angular velocity of pendulum
u1d = dynamicsymbols('u1', 1) # Derivative of generalized speed
# `symbols` is used for things that are constant, not functions of other
# variables.
t = symbols('t') # We need t later to take derivatives with respect to time
L = symbols('L') # Length of the pendulum
m = symbols('m') # Mass of the pendulum
g = 9.8; # Gravity
```

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

In [4]:

```
N = ReferenceFrame('N')
pN = Point('N*')
# Set the velocity of the origin to 0, as it's not moving
pN.set_vel(N, 0)
```

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$.

In [5]:

```
# Create and orient reference frame A, and set its angular velocity
A = N.orientnew('A', 'axis', [q1, N.z])
A.set_ang_vel(N, u1*N.z)
# Locate point P relative to the origin N*
P = pN.locatenew('P', L*A.x)
```

The velocity of the point P can then be found:

In [6]:

```
# Calculate velocity at the point
P.v2pt_theory(pN, N, A)
```

Out[6]:

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:

In [7]:

```
# Create Kinematic Differential Equations
kde = Matrix([q1d - u1])
# Solve for d/dt q in terms of u. This will be used throughout to eliminate
# d/dt q from the equations
dq_dict = solve(kde, [q1d])
```

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:

In [8]:

```
# Calculate Partial velocity at the point
# This command chain in english is:
# "The velocity of P in frame N, with each d/dt q replaced by expression in terms of u,
# differentiated in terms of u_1 in frame N."
v1_p = P.vel(N).subs(dq_dict).diff(u1, N)
```

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.

In [9]:

```
# Input the force resultant at P
R = m*g*N.x
```

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 (

In [10]:

```
# Solve for F1:
F1 = v1_p.dot(R)
```

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.

In [11]:

```
# Solve for accelerations at the point
a = P.acc(N)
```

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

In [12]:

```
# Solve for generalized inertial forces
R_star = -m*a
```

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:

In [13]:

```
# Solve for F1_star
F1_star = v1_p.dot(R_star)
```

Kanes equations for the dynamic differential equations are then

$$ F_r + F_r^* = 0 \quad (r = 1, ... , n) $$In [14]:

```
# Solve for the dynamic differential equations
dyn_eqs = Matrix([F1_star + F1]).subs(dq_dict)
```

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.

In [15]:

```
# The equations of motion are composed of the kinematic differential equations,
# and the dynamic differential equations.
eom = Matrix([kde,
dyn_eqs])
mprint(eom)
```

In [16]:

```
# Solving the equations of motion for q1' and u1'
sols = solve(eom, [q1d, u1d])
```

In [17]:

```
# Recomposing the system into a first order differential equation
qudots = Matrix([sols[q1d],
sols[u1d]])
qudots.simplify()
mprint(qudots)
```

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.

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`

.

In [18]:

```
# Create a particle object representing the pendulum mass
pP = Particle('pP', P, m)
# Create a KanesMethod object. This takes the global reference frame N,
# the independent coordinates (q1), the independent velocities (q2), and
# the Kinematic equations derived above (kde)
KM = KanesMethod(N, q_ind=[q1], u_ind=[u1], kd_eqs=kde)
```

In [19]:

```
# Make a list of tuples of the form (Particle/Body, Force on Particle/Body)
FL = [(P, R)]
# Make a list of Particles/Bodies in the system
BL = [pP]
# Derive the generalized active forces, and the generalized inertial forces
(fr, frstar) = KM.kanes_equations(FL, BL)
```

In [20]:

```
# Solve for the equations of motion in first order form.
# What's happening here is the mass matrix (M) and the forcing
# matrix (F) are being solved for. This results in a system of
# equations of the form M*[q', u']^T = F.
kdd = KM.kindiffdict()
mm = KM.mass_matrix_full
fo = KM.forcing_full
# The final first order form is then solved for as
# [q', u']^T = M^-1 * F.
qudots_km = mm.inv() * fo
qudots_km = qudots_km.subs(kdd)
qudots_km.simplify()
mprint(qudots_km)
```

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.

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.

In [21]:

```
from IPython.display import SVG
SVG(filename='Pendulum_NonMin_Coord.svg')
```

Out[21]:

In [22]:

```
# Create generalized coordinates and speeds for this non-minimal realization
q1, q2 = qs = dynamicsymbols('q1:3')
q1d, q2d = qds = dynamicsymbols('q1:3', level=1)
u1, u2 = us = dynamicsymbols('u1:3')
u1d, u2d = uds = dynamicsymbols('u1:3', level=1)
```

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)$$In [23]:

```
theta1 = atan(q2/q1)
A = N.orientnew('A', 'axis', [theta1, N.z])
```

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.

In [24]:

```
P = pN.locatenew('P1', q1*N.x + q2*N.y)
mprint(P.pos_from(pN))
```

Next, we calculate the kinematic differential equations as before

In [25]:

```
kde = Matrix([q1d - u1,
q2d - u2])
dq_dict = solve(kde, [q1d, q2d])
```

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$.

In [26]:

```
P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict))
mprint(P.vel(N))
```

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

In [27]:

```
f_c = Matrix([P.pos_from(pN).magnitude() - L])
mprint(f_c)
```

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

In [28]:

```
f_v = Matrix([P.vel(N).express(A).dot(A.x)])
f_v.simplify()
mprint(f_v)
```

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

In [29]:

```
f_a = f_v.diff(t)
f_a.simplify()
mprint(f_a)
```

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.

In [30]:

```
# Derive the equations of motion using the KanesMethod class. This proceeds much the
# same as it did in the previous section. The only difference is the addition
# of the dependent variable and constraint kwargs.
pP = Particle('pP', P, m)
FL = [(P, R)]
BL = [pP]
KM = KanesMethod(N, q_ind=[q1], u_ind=[u1], q_dependent=[q2], u_dependent=[u2],
configuration_constraints=f_c, velocity_constraints=f_v, acceleration_constraints=f_a, kd_eqs=kde)
(fr, frstar) = KM.kanes_equations(FL, BL)
kdd = KM.kindiffdict()
mm = KM.mass_matrix_full
fo = KM.forcing_full
mm.simplify()
fo.simplify()
# Solve for the first order form of the EOM
qudots_nm = mm.inv() * fo
qudots_nm = qudots_nm.subs(kdd)
qudots_nm.simplify()
mprint(qudots_nm)
```

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.

In [31]:

```
from sympy import lambdify
from scipy.integrate import odeint
from scipy import linspace, array, pi, cos, sin
import matplotlib.pyplot as plt
%matplotlib inline
```

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

In [32]:

```
val_dict = {L: 1, # Length of 1 m
m: 0.5} # Mass of 1/2 kg
# Substitute into the minimal and nonminimal EOM expressions:
qudots = qudots.subs(val_dict)
qudots_nm = qudots_nm.subs(val_dict)
```

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`

.

In [33]:

```
f_nonminimal = lambdify(Matrix([qs, us]),qudots_nm)
f_minimal = lambdify(Matrix([q1, u1]), qudots)
# Lambda expressions for use with odeint
rhs_minimal = lambda x, t: array(f_minimal(*x)).reshape(2)
rhs_nonminimal = lambda x, t: array(f_nonminimal(*x)).reshape(4)
```

Both systems can then be simulated with `odeint`

. Let's define the initial condition to be horizontal to the right, with no initial velocity.

In [34]:

```
# Define initial condition to be horizontal to the right, with no velocity
x_0_m = [pi/2, 0]
x_0_nm = [0, 1, 0, 0]
# Create time vector, and run the simulation
ts = linspace(0, 5, 1000)
x_nm = odeint(rhs_nonminimal, x_0_nm, ts)
x_m = odeint(rhs_minimal, x_0_m, ts)
# Plot both simulations on top of each other
plt.subplot(2,1,1)
plt.title('Simulation Results')
plt.plot(ts, val_dict[L]*sin(x_m[:,0]), label="Minimal")
plt.plot(ts, x_nm[:,1], label="Nonminimal")
plt.xlabel('t (s)')
plt.ylabel('$N_y$ coordinate (m)')
plt.legend()
plt.subplot(2,1,2)
plt.plot(ts, val_dict[L]*cos(x_m[:,0]), ts, x_nm[:,0])
plt.xlabel('t (s)')
plt.ylabel('$N_x$ coordinate (m)');
```

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.

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$).

In [35]:

```
# Set the trim condition for the minimal approach
trim_cond_m = array([0, 0])
# Perform simple linearization by computing the jacobian of the rhs
min_lin = qudots.jacobian([q1, u1]).subs(dict(zip([q1, u1], trim_cond_m)))
min_lin
```

Out[35]:

In [36]:

```
# Set the trim condition for the nonminimal approach
trim_cond_nm = array([val_dict[L], 0, 0, 0])
# Perform naive linearization by computing the jacobian of the rhs
nonmin_lin = qudots_nm.jacobian([q1, q2, u1, u2]).subs(dict(zip([q1, q2, u1, u2], trim_cond_nm)))
nonmin_lin
```

Out[36]:

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.

In [37]:

```
print("Minimal Eigenvalues:\n", min_lin.eigenvals())
print("Nonminimal Eigenvalues:\n", nonmin_lin.eigenvals())
```

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:

In [38]:

```
# Create the initial conditions based on the trim a deviation of 1 degree
# from the trim condition, with no initial velocity
delta = 1*pi/180
x0min = array([delta, 0]) - trim_cond_m
x0nonmin = array([val_dict[L]*cos(delta), val_dict[L]*sin(delta), 0, 0]) - trim_cond_nm
```

In [39]:

```
# Create the required functions for simulation with odeint
f_nonminimal_lin = lambdify(qs + us, nonmin_lin*Matrix(qs + us))
f_minimal_lin = lambdify([q1, u1], min_lin*Matrix([q1, u1]))
rhs_nonminimal_lin = lambda x, t: array(f_nonminimal_lin(*x)).reshape(4)
rhs_minimal_lin = lambda x, t: array(f_minimal_lin(*x)).reshape(2)
```

In [40]:

```
# Run the simulation
t = linspace(0, 5, 1000)
x_nm = odeint(rhs_nonminimal, x0nonmin + trim_cond_nm, t)
x_m = odeint(rhs_minimal, x0min + trim_cond_m, t)
x_nm_lin = odeint(rhs_nonminimal_lin, x0nonmin, t) + trim_cond_nm
x_m_lin = odeint(rhs_minimal_lin, x0min, t) + trim_cond_m
```

In [41]:

```
# Plot the results
plt.figure(1)
plt.subplot(2,1,1)
plt.plot(t, val_dict[L]*sin(x_m[:,0]), t, val_dict[L]*sin(x_m_lin[:,0]))
plt.xlabel('t, (s)')
plt.ylabel('$N_y$ Coordinate (m)')
plt.title('Minimal Coordinates Simulation Results')
plt.legend(['Nonlinear', 'Linear'])
plt.subplot(2,1,2)
plt.plot(t, val_dict[L]*cos(x_m[:,0]), t, val_dict[L]*cos(x_m_lin[:,0]))
plt.xlabel('t, (s)')
plt.ylabel('$N_x$ Coordinate (m)');
plt.figure(2)
plt.subplot(2,1,1)
plt.plot(t, x_nm[:,1], t, x_nm_lin[:,1])
plt.xlabel('t, (s)')
plt.ylabel('$N_y$ Coordinate (m)')
plt.title('Nonminimal Coordinates Simulation Results')
plt.legend(['Nonlinear', 'Linear'])
plt.subplot(2,1,2)
plt.plot(t, x_nm[:,0], t, x_nm_lin[:,0])
plt.xlabel('t, (s)')
plt.ylabel('$N_x$ Coordinate (m)');
```

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*