# GSoC, Week 5

At the end of last week, a lot of the functionality I was aiming for was completed.  I think the code can now compute equations of motion.  I’m still working on some more advanced tests, but for simple cases I know it works. I submitted a pull request for pulling my work into sympy:master.  It involved cleaning up a lot of ugly commit history.  I believe the last time I tried to rebase, Luke and I both rebased and pushed, leading to a lot of double commits.  This was obviously bad.  So I rebased again to remove the double commits, squashed a lot of the small commits, and added a lot of comments to commits. I think now that a lot of the desired functionality is present future commits won’t be all over the place, and I’ll do a better job with commit messages.

One thing that is important on github, and in bright letters on the rebasing help page, is that if you rebase and edit the commit history and someone else has pulled from your branch pre-commit, then they will have a bad day when they pull next…. So I would recommend avoiding that. I wish I could say I felt more comfortable with git, but every time I try and do something new, I have trouble.  I guess that means I’m learning though, even though I am scared of git still…

During the week, Luke and I discussed formulation of Fr* and Fr in a different manner when using Kane’s Method. Typically R* = -ma, where a is the translational acceleration vector of a body’s mass center. Now when you consider the final form of the equations, Fr + Fr* = 0, you have all the terms on one side. For analysis, often this will be arranged in a form: M(q) udot = f(u,q,t,…). M(q) is the mass matrix, and it is the coefficients of the accelerations. Now, accelerations are the time derivatives of velocity, and can be represented in the form: c udot + g(q,u,t,…). This c matrix is the start of contributions to the mass matrix from the accelerations (it needs to be multiplied by the body’s mass and partials). This matrix is actually in the form v1, v2, v3, … where each v is the partial velocity for the respective u dot.  This is a little confusing, so I’ll try and tie this back a step.  Velocities can be defined as follows: the sum of (V_r* u_r) + other terms. Vr is said to be the partial velocity of V with respect to generalized speed u_r. Then there is a remainder term.

Next when you take the derivative of the velocity, and use the chain rule, each element in the sum is evaluated (in a frame, N) as ^N d/dt(V_r*u_r) which is ^N d/dt(u_r) * V_r + ^N d/dt(V_r) * u_r.  The d/dt(u_r) * V_r term is then V_r * Udot_r.  Then we can see that it is the partial velocities which are the elements of the “c” matrix in the acceleration definition. This goes back the acceleration definition:  c udot + g(q,u,t,…). The next step when forming the generalized active force, Fr*, is to take multiply R* by the V_r for each generalized speed. It can then be seen that this will be of the form [v_1, v_2, v_3,…]. Next is taking the “outer product” of this with itself.  Outer product is not really correct here, but it leads to a matrix in the form -m * [[v1&v1, v1&v2, v1&v3],[v2&v1,v2&v2,v2&v3],[v3&v1,v3&v2,v3&v3]] where & represents the dot product (remember, partial velocities are still vectors). So it is this matrix which is the mass matrix.  What I’ve gone over is just the mass matrix contribution from the translational components of a body; it extends as you would expect.  Also, all those additional terms end up in the “f” term in our EoM formulation: M(q) udot = f(u,q,t,…).

This is really confusing, but I hope by adding some more text to what is in Kane’s book (http://ecommons.library.cornell.edu/handle/1813/638), that this is more visible.  He covers everything in there, but it can be really hard to follow.  I’m going to be writing all of this up in the documentation for PyDy in the next few weeks, and I’ll be using LaTeX & mathjax with the sphinx documentation so all these things will show up correctly. I hope by that point, I’ll be able to describe these things better (and I’m sure displaying actual math will help).

Finally, a little example of how this will work.  This is a rolling disc, infinitely thin, with no slip constraints.  This is actually taken right from the test code, so you can copy, paste, and run it (assuming you have a branch of sympy with my pydy code).

    # Rolling Disc Example
# Here the rolling disc is formed from the contact point up, removing the
# need to introduce generalized speeds. Only 3 configuration and three
# speed variables are need to describe this system, along with the disc's
# mass and radius, and the local graivty (note that mass will drop out).
q1, q2, q3, q1d, q2d, q3d = dynamicsymbols('q1 q2 q3 q1d q2d q3d')
u1, u2, u3, u1d, u2d, u3d = dynamicsymbols('u1 u2 u3 u1d u2d u3d')
r, m, g = symbols('r m g')

# The kinematics are formed by a series of simple rotations. Each simple
# rotation creates a new frame, and the next rotation is defined by the new
# frame's basis vectors. This example uses a 3-1-2 series of rotations, or
# Z, X, Y series of rotations. Angular velocity for this is defined using
# the second frame's basis (the lean frame).
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Simple', q1, 3)
L = Y.orientnew('L', 'Simple', q2, 1)
R = L.orientnew('R', 'Simple', q3, 2)
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
R.set_ang_acc(N, R.ang_vel_in(N).dt(R) + (R.ang_vel_in(N) ^ R.ang_vel_in(N)))

# This is the translational kinematics. We create a point with no velocity
# in N; this is the contact point between the disc and ground. Next we form
# the position vector from the contact point to the disc mass center.
# Finally we form the velocity and acceleration of the disc.
C = Point('C')
C.set_vel(N, 0)
Dmc = C.newpoint('Dmc', r * L.z)
Dmc.v2pt(C, N, R)
Dmc.a2pt(C, N, R)

# This is a simple way to form the inertia dyadic.
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)

# Kinematic differential equations; how the generalized coordinate time
# derivatives relate to generalized speeds.
kd = dict({q1d: (u3/cos(q3)),
q2d: (u1),
q3d: (u2 - u3 * tan(q2))})

# Creation of the force list; it is the gravitational force at the mass
# center of the disc. Then we create the disc by assigning a Point to the
# mass center attribute, a ReferenceFrame to the frame attribute, and mass
# and inertia. Then we form the body list.
ForceList = [(Dmc, - m * g * Y.z)]
BodyD = RigidBody()
BodyD.mc = Dmc
BodyD.inertia = (I, Dmc)
BodyD.frame = R
BodyD.mass = m
BodyList = [BodyD]

# Finally we form the equations of motion, using the same steps we did
# before. Specify inertial frame, supply generalized speeds, supply
# kinematic differential equation dictionary, compute Fr from the force
# list and Fr* fromt the body list, compute the mass matrix and forcing
# terms, then solve for the u dots (time derivatives of the generalized
# speeds).
KM = Kane(N)
KM.gen_speeds([u1, u2, u3])
KM.kindiffeq(kd)
fr = KM.form_fr(ForceList)
frstar = KM.form_frstar(BodyList)
MM = KM.mass_matrix()
forcing = KM.forcing()
rhs = MM.inv() * forcing
print rhs.expand()