def whipple_state_space(rider, speed):
""" x' = Ax + Bu + Fv
x = [phi,
delta,
phiDot,
deltaDot]
u = [Tphi,
Tdel]
v = [Fcl]
"""
bicycleModel = LinearLateralForce()
pathToData='/media/Data/Documents/School/UC Davis/Bicycle Mechanics/BicycleParameters/data/'
if rider == 'Jason':
bicycleName = 'Rigid'
elif rider == 'Charlie' or rider == 'Luke':
bicycleName = 'Rigidcl'
bike = bp.Bicycle(bicycleName, pathToData)
bike.add_rider(rider)
# set the model parameters
benchmarkPar = bp.io.remove_uncertainties(bike.parameters['Benchmark'])
benchmarkPar['xcl'] = bike.parameters['Measured']['xcl']
benchmarkPar['zcl'] = bike.parameters['Measured']['zcl']
moorePar = bicycle.benchmark_to_moore(benchmarkPar)
bicycleModel.set_parameters(moorePar)
# set the default equilibrium point
pitchAngle = bicycle.pitch_from_roll_and_steer(0., 0., moorePar['rf'],
moorePar['rr'], moorePar['d1'], moorePar['d2'], moorePar['d3'])
wheelAngSpeed = -speed / moorePar['rr']
equilibrium = np.zeros(len(bicycleModel.stateNames))
equilibrium[bicycleModel.stateNames.index('q5')] = pitchAngle
equilibrium[bicycleModel.stateNames.index('u6')] = wheelAngSpeed
bicycleModel.linear(equilibrium)
states = ['q4', 'q7', 'u4', 'u7']
inputs = ['Fcl', 'T4', 'T7']
outputs = ['q4', 'q7', 'u4', 'u7']
A, B, C, D = bicycleModel.reduce_system(states, inputs, outputs)
F = B[:, 0]
B = B[:, 1:]
return A, B, F
# load the benchmark parameters
pathToData='/media/Data/Documents/School/UC Davis/Bicycle Mechanics/BicycleParameters/data/'
gyro = bp.Bicycle('Gyro', pathToData, forceRawCalc=True)
gyro.add_rider('Child', reCalc=True)
benchmarkPar = bp.io.remove_uncertainties(gyro.parameters['Benchmark'])
# convert to my parameter set
moorePar = bicycle.benchmark_to_moore(benchmarkPar, oldMassCenter=False)
moorePar['mg'] = benchmarkPar['mD']
moorePar['ig11'] = benchmarkPar['IDxx']
moorePar['ig22'] = benchmarkPar['IDyy']
gyroNonLinear.set_parameters(moorePar)
# set the initial conditions to match Meijaard2007
speedNaught = 0.5
rollRateNaught = 0.5
pitchAngle = bicycle.pitch_from_roll_and_steer(0., 0., moorePar['rf'], moorePar['rr'],
moorePar['d1'], moorePar['d2'], moorePar['d3'])
wheelAngSpeed = -speedNaught / moorePar['rr']
gyroNonLinear.initialConditions[gyroNonLinear.stateNames.index('q5')] = pitchAngle
gyroNonLinear.initialConditions[gyroNonLinear.stateNames.index('u4')] = rollRateNaught
gyroNonLinear.initialConditions[gyroNonLinear.stateNames.index('u6')] = wheelAngSpeed
gyroNonLinear.initialConditions[gyroNonLinear.stateNames.index('u9')] = -5000. / 60. * 2 * pi
# integration settings
ts = 0.01 # step size
tf = 5. # final time
gyroNonLinear.intOpts['ts'] = ts
gyroNonLinear.intOpts['tf'] = tf
gyroNonLinear.simulate()
# There is currenlty a bug in which the parameters do not get saved with
whip = LinearWhipple()
# load the benchmark parameters
pathToData = '/media/Data/Documents/School/UC Davis/Bicycle Mechanics/BicycleParameters/data/'
benchmark = bp.Bicycle('Benchmark', pathToData)
benchmarkPar = bp.io.remove_uncertainties(benchmark.parameters['Benchmark'])
# convert to my parameter set
moorePar = bicycle.benchmark_to_moore(benchmarkPar, oldMassCenter=False)
whip.set_parameters(moorePar)
# set the initial conditions to match Meijaard2007
speedNaught = 4.6
u6Naught = -speedNaught / moorePar['rr']
rollRateNaught = 0.5
pitchAngle = bicycle.pitch_from_roll_and_steer(0., 0., moorePar['rf'],
moorePar['rr'], moorePar['d1'],
moorePar['d2'], moorePar['d3'])
# linearize about the nominal configuration
equilibrium = np.zeros(len(whip.stateNames))
equilibrium[whip.stateNames.index('q5')] = pitchAngle
equilibrium[whip.stateNames.index('u6')] = u6Naught
whip.linear(equilibrium)
# set up the simulation
whip.set_initial_conditions('q5', pitchAngle, 'u4', rollRateNaught, 'u6',
u6Naught)
# integration settings
ts = 0.01 # step size
tf = 5. # final time
