Skip to content

Instantly share code, notes, and snippets.

@moorepants
Created July 22, 2011 18:24
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save moorepants/1100050 to your computer and use it in GitHub Desktop.
Save moorepants/1100050 to your computer and use it in GitHub Desktop.
problem that hangs in pydy
from sympy import *
from sympy.physics.mechanics import *
# the unknown steer torque
Tdelta = dynamicsymbols('Tdelta')
# constants we measure on the bicycle
d, ds1, ds3 = symbols('d ds1 ds2')
c = symbols('c')
# time varying signals we measure on the bicycle
Tm, Tf = dynamicsymbols('Tm Tf')
# steer angle and body fixed handlebar rate about steer axis
delta, wh3 = dynamicsymbols('delta wh3')
# body fixed angular rates of the bicycle frame
wb1, wb2, wb3 = dynamicsymbols('wb1 wb2 wb3')
# body fixed acceleration of the vectornav point
av1, av2, av3 = dynamicsymbols('av1 av2 av3')
# time varying signals we calculate
deltad, wh3d = dynamicsymbols('delta wh3', 1)
# body fixed angular acceleration
wb1d, wb2d, wb3d = dynamicsymbols('wb1 wb2 wb3', 1)
# newtonian frame
N = ReferenceFrame('N')
# bicycle frame
B = ReferenceFrame('B')
# handlebar frame
H = B.orientnew('H', 'Simple', delta, 3)
I11, I22, I33, I31 = symbols('I11 I22 I33 I31')
# handlebar inertia
IH = inertia(H, I11, I22, I33, 0, 0, I31)
mH = symbols('mH')
B.set_ang_vel(N, wb1 * B.x + wb2 * B.y + wb3 * B.z)
H.set_ang_vel(N, (wb1 * cos(delta) + wb2 * sin(delta)) * H.x + (-wb1 *
sin(delta) + wb2 * cos(delta)) * H.y + wh3 * H.z)
# vectornav center
v = Point('v')
v.set_acc(N, av1 * B.x + av2 * B.y + av3 * B.z)
# point on the steer axis
s = Point('s')
s.set_pos(v, ds1 * B.x + ds3 * B.z)
s.a2pt(v, N, B)
# handlebar center of mass
ho = Point('ho')
ho.set_pos(s, d * H.x)
ho.a2pt(s, N, H)
# calculate the angular momentum of the handlebar in N about the center of mass
# of the handlebar
H_H_N_ho = IH.dot(H.ang_vel_in(N))
# the derivative of the angular momentum of the handlebar in N about the com
Hdot = H_H_N_ho.dt(N)
# formulate euler's equation about an arbitrary point
s_ho = ho.pos_from(s)
mvdot = mH * ho.acc(N)
part2 = s_ho.cross(mvdot)
sumOfTorques = Hdot + part2
# calculate the steer torque
Tdelta = (sumOfTorques).dot(H.z) + Tm + c * deltad + Tf
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment