Created
July 22, 2011 18:24
-
-
Save moorepants/1100050 to your computer and use it in GitHub Desktop.
problem that hangs in pydy
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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