GSoC, Week 9

Week 9 already….? Also, my internet connection is down, so this post is a little later than I planned….

So there was some progress this week. A lot of it was in doing the math for the linearization process. Linearization is normally easy, but when dealing with dependent quantities becomes more complicated. It requires treating the dependent quantities as functions of the independent quantities, then taking partial derivatives. It ends up being simplest (that is the current theory at least) when using the chain rule; this was done by hand in order to ease the work done by SymPy’s routines. These expressions can get quite large, so minimizing the buildup of expression size is important. Or, to put it another way, we figured out the best way to operate on smaller chunks of expressions, rather than larger ones, and then combine them afterwards, minimizing the size of some of the expressions which have to be dealt with.

In our lab at UC Davis, one of the big projects currently is studying bicycle dynamics. A paper was written in 2007 which sought to provide benchmark/reference values to validate models of the bicycle against: J.P Meijaard, Jim M Papadopoulos, Andy Ruina and A.L Schwab. Linearized dynamics equations for the balance and steer of a bicycle: a benchmark and review. Proceedings of The Royal Society (2007) 463, 1955-1982 doi: 10.1098/rspa.2007.1857 . One of the goals for PyDy this year was to be able to generate the full nonlinear equations for the bicycle, and the linear equations for comparison of eigenvalues using the reference values. Well, it looks like my code can do that successfully now:

The code at the bottom of the post calculates the “A” matrix, and writes a few eigenvalues out. You can compare them to those presented in the paper. There are still 2 issues left to investigate as part of the linearization process, there is pretty printing, latex printing, and finally code output and all of the functionality will be “done”. There is also still lots of documentation to write…

One other issue, that is proving to be somewhat serious, is the speed of the code. The execution time of the code is usually not too terrible, and if it does get bad, turning of Vector’s auto simplification (Vector.simp = False) usually helps. The linearization can also be kinda slow, but not overwhelmingly so. What seems to really be an issue is both printing (to screen or file) and substituting. The expressions get fairly big (I think the forcing term for the bicycle is on the order of 1MB when in Ascii form) so I’m not completely unhappy or surprised about the printing issue. I am more unhappy about the substitution being slow. Right now I’m using .subs(), and it seems to take a few minutes. I’ve also tried .evalf(subs=dict) and lambdify, but .evalf hits a maximum recursion depth error, and I’m getting a syntax error with lambdify (it actually looks like this is due to the use of unknown functions; I’m sure I can solve this somehow).

Use the pydy-pull-funcderiv branch; that’s the current branch. Some pushes have been forced in, so beware….sorry. Also, Brian Granger’s code got pulled in, so I’ll be defining ‘dynamic symbols’ as undefined functions of time. My branches need a little work to get them organized and unified again.

Code Output:

Calculation of Linearized Bicycle "A" Matrix, with States: Roll, Steer, Roll Rate, Steer Rate
Before Forming the List of Nonholonomic Constraints.
Before Handling of Dependent Speeds
Before Forming Generalized Active Forces, Fr
Before Forming Generalized Inertia Forces, Fr*
Base Equations of Motion Computed
Before Linearization of the "Forcing" Term
Before Substitution of Numerical Values
[               0,                                           0,                  1.0,                    0]
[               0,                                           0,                    0,                  1.0]
[9.48977444677355, -0.891197738059088*v**2 - 0.571523173729246, -0.105522449805691*v, -0.330515398992311*v]
[11.7194768719633,   -1.97171508499972*v**2 + 30.9087533932407,   3.67680523332153*v,  -3.08486552743311*v]
v = 1
{-3.13423125066578: 1, 3.52696170990069 - 0.80774027519931*I: 1, 3.52696170990069 + 0.80774027519931*I: 1, -7.11008014637441: 1}
v = 2
{-8.67387984831737: 1, -3.07158645641514: 1, 2.68234517512745 + 1.68066296590676*I: 1, 2.68234517512745 - 1.68066296590676*I: 1}
v = 3
{-2.63366137253665: 1, 1.70675605663973 + 2.31582447384324*I: 1, 1.70675605663973 - 2.31582447384324*I: 1, -10.3510146724592: 1}
v = 4
{0.41325331521124 - 3.07910818603205*I: 1, -12.1586142657644: 1, 0.41325331521124 + 3.07910818603205*I: 1, -1.42944427361326: 1}
v = 5
{-14.0783896927982: 1, -0.322866429004087: 1, -0.775341882195845 + 4.46486771378823*I: 1, -0.775341882195845 - 4.46486771378823*I: 1}

Actual Code:

from sympy import *
from sympy.physics.mechanics import *

# Code to get equations of motion for a bicycle modeled as in:
# J.P Meijaard, Jim M Papadopoulos, Andy Ruina and A.L Schwab. Linearized
# dynamics equations for the balance and steer of a bicycle: a benchmark and
# review. Proceedings of The Royal Society (2007) 463, 1955-1982
# doi: 10.1098/rspa.2007.1857

print('Calculation of Linearized Bicycle \"A\" Matrix, with States: Roll, '
      'Steer, Roll Rate, Steer Rate')

# Note that this code has been crudely ported from Autolev, which is the reason
# for some of the unusual naming conventions. It was purposefully as similar as
# possible in order to aide debugging.

# Vector's simplification routines need to be turned off, otherwise the
# expressions get too big and too slow to simplify
Vector.simp = False

# Declare Coordinates & Speeds
q1, q2, q4, q5 = dynamicsymbols('q1 q2 q4 q5')
q1d, q2d, q4d, q5d = dynamicsymbols('q1 q2 q4 q5', 1)
u1, u2, u3, u4, u5, u6 = dynamicsymbols('u1 u2 u3 u4 u5 u6')
u1d, u2d, u3d, u4d, u5d, u6d = dynamicsymbols('u1 u2 u3 u4 u5 u6', 1)

# Declare System's Parameters
WFrad, WRrad, htangle, forkoffset = symbols('WFrad WRrad htangle forkoffset')
forklength, framlength, forkcg1 = symbols('forklength framlength forkcg1')
forkcg3, framcg1, framcg3, Iwr11 = symbols('forkcg3 framcg1 framcg3 Iwr11')
Iwr22, Iwf11, Iwf22, Ifram11 = symbols('Iwr22 Iwf11 Iwf22 Ifram11')
Ifram22, Ifram33, Ifram31, Ifork11 = symbols('Ifram22 Ifram33 Ifram31 Ifork11')
Ifork22, Ifork33, Ifork31, g = symbols('Ifork22 Ifork33 Ifork31 g')
mframe, mfork, mwf, mwr = symbols('mframe mfork mwf mwr')

# Set up reference frames for the system
# N - inertial
# Y - yaw
# R - roll
# WR - rear wheel, rotation angle is ignorable coordinate so not oriented
# FRAM - bicycle frame
# TEMPFRAM - statically rotated frame for easier reference inertia definition
# FORK - bicycle fork
# TEMPFORK - statically rotated frame for easier reference inertia definition
# WF - front wheel, again posses a ignorable coordinate
N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Simple', q1, 3)
R = Y.orientnew('R', 'Simple', q2, 1)
FRAM = R.orientnew('FRAM', 'Simple', q4 + htangle, 2)
WR = ReferenceFrame('WR')
TEMPFRAM = FRAM.orientnew('TEMPFRAM', 'Simple', -htangle, 2)
FORK = FRAM.orientnew('FORK', 'Simple', q5, 1)
TEMPFORK = FORK.orientnew('TEMPFORK', 'Simple', -htangle, 2)
WF = ReferenceFrame('WF')

# Declaration of the RigidBody containers
BodyFram = RigidBody()
BodyFork = RigidBody()
BodyWR = RigidBody()
BodyWF = RigidBody()

# Setting the masses for the bodies
BodyFram.mass = mframe
BodyFork.mass = mfork
BodyWF.mass = mwf
BodyWR.mass = mwr

# Assigning the appropriate frames to each body
BodyFram.frame = FRAM
BodyFork.frame = FORK
BodyWR.frame = WR
BodyWF.frame = WF

# Kinematics of the Bicycle
# First block of code is forming the positions of the relevant points
# rear wheel contact -> rear wheel mass center -> frame mass center +
# frame/fork connection -> fork mass center + front wheel mass center -> front
# wheel contact point
WRhat = Point('WRhat')
WRmc = WRhat.newpoint('WRmc', WRrad * R.z)
STEER = WRmc.newpoint('STEER', framlength * FRAM.z)
FRAMmc = WRmc.newpoint('FRAMmc', -framcg1 * FRAM.x + framcg3 * FRAM.z)
FORKmc = STEER.newpoint('FORKmc', -forkcg1 * FORK.x + forkcg3 * FORK.z)
WFmc = STEER.newpoint('WFmc', forklength * FORK.x + forkoffset * FORK.z)
WFhat = WFmc.newpoint('WFhat', WFrad*(dot(FORK.y, Y.z)*FORK.y - Y.z).unit)

# Set the angular velocity of each frame.
# Angular accelerations end up being calculated automatically by
# differentiating the angular velocities when first needed.
# u1 is yaw rate
# u2 is roll rate
# u3 is rear wheel rate
# u4 is frame pitch rate
# u5 is fork steer rate
# u6 is front wheel rate
Y.set_ang_vel(N, u1 * Y.z)
R.set_ang_vel(Y, u2 * R.x)
WR.set_ang_vel(FRAM, u3 * FRAM.y)
FRAM.set_ang_vel(R, u4 * FRAM.y)
FORK.set_ang_vel(FRAM, u5 * FORK.x)
WF.set_ang_vel(FORK, u6 * FORK.y)

# Form the velocities of the previously defined points, using the 2 - point
# theorem (written out by hand here).
# Accelerations again are calculated automatically when first needed.
WRhat.set_vel(N, 0)
WRmc.set_vel(N, WRhat.vel(N) + (WR.ang_vel_in(N) ^ WRmc.pos_from(WRhat)))
STEER.set_vel(N, WRmc.vel(N) + (FRAM.ang_vel_in(N) ^ STEER.pos_from(WRmc)))
FRAMmc.set_vel(N, WRmc.vel(N) + (FRAM.ang_vel_in(N) ^ FRAMmc.pos_from(WRmc)))
FORKmc.set_vel(N, STEER.vel(N) + (FORK.ang_vel_in(N) ^ FORKmc.pos_from(STEER)))
WFmc.set_vel(N, STEER.vel(N) + (FORK.ang_vel_in(N) ^ WFmc.pos_from(STEER)))
WFhat.set_vel(N, WFmc.vel(N) + (WF.ang_vel_in(N) ^ WFhat.pos_from(WFmc)))

# Assign the relevant points to each body. = FRAMmc = FORKmc = WFmc = WRmc

# Sets the inertias of each body. Uses the inertia frame to construct the
# inertia dyadics. Wheel inertias are only defined by principle moments of
# inertia, and are in fact constant in the frame and fork reference frames; it
# is for this reason that the orientations of the wheels does not need to be
# defined. The frame and fork inertias are defined in the 'TEMP' frames which
# are fixed to the appropriate body frames; this is to allow easier input of
# the reference values of the benchmark paper. Note that due to slightly
# different orientations, the products of inertia need to have their signs
# flipped; this is done later when entering the numerical value.
BodyFram.inertia = (inertia(TEMPFRAM, Ifram11, Ifram22, Ifram33, 0, 0, Ifram31), FRAMmc)
BodyFork.inertia = (inertia(TEMPFORK, Ifork11, Ifork22, Ifork33, 0, 0, Ifork31), FORKmc)
BodyWR.inertia = (inertia(FRAM, Iwr11, Iwr22, Iwr11), WRmc)
BodyWF.inertia = (inertia(FORK, Iwf11, Iwf22, Iwf11), WFmc)

print 'Before Forming the List of Nonholonomic Constraints.'

# The kinematic differential equations; they are defined quite simply. Each
# entry in this list is equal to zero.
kd = [q1d - u1, q2d - u2, q4d - u4, q5d - u5]

# The nonholonomic constraints are the velocity of the front wheel contact
# point dotted into the X, Y, and Z directions; the yaw frame is used as it is
# "closer" to the front wheel (1 less DCM connecting them). These constraints
# force the velocity of the front wheel contact point to be 0 in the inertial
# frame; the X and Y direction constraints enforce a "no-slip" condition, and
# the Z direction constraint forces the front wheel contact point to not move
# away from the ground frame, essentially replicating the holonomic constraint
# which does not allow the frame pitch to change in an invalid fashion.
conlspeed = [WFhat.vel(N) & Y.x, WFhat.vel(N) & Y.y, WFhat.vel(N) & Y.z]
# The holonomic constraint is that the position from the rear wheel contact
# point to the front wheel contact point when dotted into the normal-to-ground
# plane direction must be zero; effectively that the front and rear wheel
# contact points are always touching the ground plane. This is actually not
# part of the dynamic equations, but instead is necessary for the lineraization
# process.
conlcoord = [WFhat.pos_from(WRhat) & Y.z]

# The force list; each body has the appropriate gravitational force applied
# at its mass center.
FL = [(FRAMmc, -mframe * g * Y.z), (FORKmc, -mfork * g * Y.z), (WFmc,
      -mwf * g * Y.z), (WRmc, -mwr * g * Y.z)]
BL = [BodyFram, BodyFork, BodyWR, BodyWF]

# The N frame is the inertial frame, coordinates are supplied in the order of
# independent, dependent coordinates, as are the speeds. The kinematic
# differential equation are also entered here.
KM = Kane(N)
KM.coords([q1, q2, q5, q4])
KM.speeds([u2, u3, u5, u1, u4, u6])

print 'Before Handling of Dependent Speeds'

# Here the dependent speeds are specified, in the same order they were provided
# in earlier, along with the non-holonomic constraints.
# The dependent coordinate is also provided, with the holonomic constraint.
# Again, this is only provided for the linearization process.
KM.dependent_speeds([u1, u4, u6], conlspeed)
KM.dependent_coords([q4], conlcoord)

print 'Before Forming Generalized Active Forces, Fr'
fr = KM.form_fr(FL)
print 'Before Forming Generalized Inertia Forces, Fr*'
frstar = KM.form_frstar(BL)
print 'Base Equations of Motion Computed'

# This is the start of entering in the numerical values from the benchmark
# paper to validate the eigen values of the linearized equations from this
# model to the reference eigen values. Look at the aforementioned paper for
# more information. Some of these are intermediate values, used to transform
# values from the paper into the coordinate systems used in this model.
PaperRadRear                    =  0.3
PaperRadFront                   =  0.35
HTA                             =  evalf.N(pi/2-pi/10)
TrailPaper                      =  0.08
rake                            =  evalf.N(-(TrailPaper*sin(HTA)-(PaperRadFront*cos(HTA))))
PaperWb                         =  1.02
PaperFramCgX                    =  0.3
PaperFramCgZ                 	=  0.9
PaperForkCgX                  	=  0.9
PaperForkCgZ                	=  0.7
FramLength                  	=  evalf.N(PaperWb*sin(HTA)-(rake-(PaperRadFront-PaperRadRear)*cos(HTA)))
FramCGNorm                     	=  evalf.N((PaperFramCgZ-PaperRadRear-(PaperFramCgX/sin(HTA))*cos(HTA))*sin(HTA))
FramCGPar                      	=  evalf.N((PaperFramCgX/sin(HTA) + (PaperFramCgZ-PaperRadRear-PaperFramCgX/sin(HTA)*cos(HTA))*cos(HTA)))
tempa                         	=  evalf.N((PaperForkCgZ - PaperRadFront))
tempb                         	=  evalf.N((PaperWb-PaperForkCgX))
tempc                         	=  evalf.N(sqrt(tempa**2+tempb**2))
PaperForkL                      =  evalf.N((PaperWb*cos(HTA)-(PaperRadFront-PaperRadRear)*sin(HTA)))
ForkCGNorm                  	=  evalf.N(rake+(tempc * sin(pi/2-HTA-acos(tempa/tempc))))
ForkCGPar              		=  evalf.N(tempc * cos((pi/2-HTA)-acos(tempa/tempc))-PaperForkL)

# Here is the final assembly of the numerical values. The symbol 'v' is the
# forward speed of the bicycle (a concept which only makes sense in the
# upright, static equilibrium case?). These are in a dictionary which will
# later be substituted in. Again the sign on the *product* of inertia values is
# flipped here, due to different orientations of coordinate systems.
v = Symbol('v')
val_dict = {WFrad: PaperRadFront,
            WRrad: PaperRadRear,
            htangle: HTA,
            forkoffset: rake,
            forklength: PaperForkL,
            framlength: FramLength,
            forkcg1: ForkCGPar,
            forkcg3: ForkCGNorm,
            framcg1: FramCGNorm,
            framcg3: FramCGPar,
            Iwr11: 0.0603,
            Iwr22: 0.12,
            Iwf11: 0.1405,
            Iwf22: 0.28,
            Ifork11: 0.05892,
            Ifork22: 0.06,
            Ifork33: 0.00708,
            Ifork31: 0.00756,
            Ifram11: 9.2,
            Ifram22: 11,
            Ifram33: 2.8,
            Ifram31: -2.4,
            mfork: 4,
            mframe: 85,
            mwf: 3,
            mwr: 2,
            g: 9.81,
            q1: 0,
            q2: 0,
            q4: 0,
            q5: 0,
            u1: 0,
            u2: 0,
            u3: v/PaperRadRear,
            u4: 0,
            u5: 0,
            u6: v/PaperRadFront}

# Here a dictionary is formed using the kinematic differential equations. The
# expression is perhaps slightly more complicated then necessary in this case,
# but should work in all cases in order the generate a dictionary in the form
# {qd: f(u)}.
sub_dict = solve_linear_system_LU(Matrix([KM._k_kqdot.T,
        -(KM._k_ku*Matrix(KM._u) + KM._f_k).T]).T, KM._qdot)

print 'Before Linearization of the \"Forcing\" Term'
# Linearizes the forcing vector; the equations are set up as MM udot = forcing,
# where MM is the mass matrix, udot is the vector representing the time
# derivatives of the generalized speeds, and forcing is a vector which contains
# both external forcing terms and internal forcing terms, such as centripital
# or coriolis forces.
# This actually returns a matrix with as many rows as *total* coordinates and
# speeds, but only as many columns as independent coordinates and speeds.
forcing_lin = KM.linearize().subs(sub_dict)

# As mentioned above, the size of the linearized forcing terms is expanded to
# include both q's and u's, so the mass matrix must have this done as well.
# This will likely be changed to be part of the linearized process, for future
# reference.
MM_full = (KM._k_kqdot).row_join(zeros((4, 6))).col_join((zeros((6,

print 'Before Substitution of Numerical Values'
# I think this is pretty self explanatory. It takes a really long time though.
# I've experimented with using evalf with substitution, this failed due to
# maximum recursion depth being exceeded; I also tried lambdifying this, and am
# not sure what the error message I got there meant.
MM_full = MM_full.subs(val_dict).evalf()
forcing_lin = forcing_lin.subs(val_dict).evalf()

# Finally, we construct an "A" matrix for the form xdot = A x (x being the
# state vector), although in this case, the sizes are a little off. The
# following line extracts only the minimum entries required for eigenvalue
# analysis, which correspond to rows and columns for lean, steer, lean rate,
# and steer rate.
Amat = MM_full.inv() * forcing_lin
A = Amat.extract([1,2,4,6],[1,2,3,5])
print A
print 'v = 1'
print A.subs(v, 1).eigenvals()
print 'v = 2'
print A.subs(v, 2).eigenvals()
print 'v = 3'
print A.subs(v, 3).eigenvals()
print 'v = 4'
print A.subs(v, 4).eigenvals()
print 'v = 5'
print A.subs(v, 5).eigenvals()
About these ads

One response to “GSoC, Week 9

  1. This is exciting, that your code can do this example.

    Ask on the list about your lambdify() error. I’m sure you just need to pass in a variables dict or something.

Leave a Reply

Fill in your details below or click an icon to log in: Logo

You are commenting using your account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s