import numpy as np
import matplotlib.pyplot as plt
import bicycledataprocessor as bdp
import canonical_system_id as csi
# This gives the proportion of the lateral force which should be added to the
# steer torque and roll torque equations in the canonical equations.
F = {}
for rider in ['Charlie', 'Jason', 'Luke']:
F[rider] = csi.whipple_state_space(rider, 1.0)[2][2:]
# find the runs that we want to id
dataset = bdp.DataSet()
dataset.open()
table = dataset.database.root.runTable
runs = []
for row in table.iterrows():
con = []
con.append(row['Rider'] in ['Jason', 'Charlie', 'Luke'])
con.append(row['Maneuver'] in ['Balance',
'Track Straight Line',
'Balance With Disturbance',
'Track Straight Line With Disturbance'])
con.append(row['Environment'] == 'Horse Treadmill')
con.append(row['corrupt'] is not True)
con.append(int(row['RunID']) > 100)
if False not in con:
runs.append(row['RunID'])
def test_benchmark_lstsq_matrices():
dataset = bdp.DataSet()
trial = bdp.Run('700', dataset)
A, B, F = csi.whipple_state_space(trial.metadata['Rider'], 1.0)
H = np.dot(np.linalg.inv(B[2:]), F[2:])
timeSeries = csi.benchmark_time_series(trial, subtractMean=False)
M, C1, K0, K2 = trial.bicycle.canonical(nominal=True)
fixedValues = csi.benchmark_canon_to_dict(M, C1, K0, K2, H)
rollParams = ['Mpp', 'Mpd',
'C1pp', 'C1pd',
'K0pp', 'K0pd',
'K2pp', 'K2pd',
'HpF']
A, B = csi.benchmark_lstsq_matrices(rollParams, timeSeries, fixedValues)
testing.assert_allclose(trial.taskSignals['RollRate'].time_derivative(),
A[:, 0])
testing.assert_allclose(trial.taskSignals['SteerRate'].time_derivative(),
A[:, 1])
testing.assert_allclose(trial.taskSignals['ForwardSpeed'] *
trial.taskSignals['RollRate'], A[:, 2])
testing.assert_allclose(trial.taskSignals['ForwardSpeed'] *
trial.taskSignals['SteerRate'], A[:, 3])
testing.assert_allclose(9.81 *
trial.taskSignals['RollAngle'], A[:, 4])
testing.assert_allclose(9.81 *
trial.taskSignals['SteerAngle'], A[:, 5])
testing.assert_allclose(trial.taskSignals['ForwardSpeed']**2 *
trial.taskSignals['RollAngle'], A[:, 6])
testing.assert_allclose(trial.taskSignals['ForwardSpeed']**2 *
trial.taskSignals['SteerAngle'], A[:, 7])
testing.assert_allclose(-trial.taskSignals['PullForce'], A[:, 8])
testing.assert_allclose(np.zeros_like(trial.taskSignals['PullForce']), B)
rollParams = ['Mpp', 'Mpd',
'C1pp', 'C1pd',
'K0pp', 'K0pd',
'K2pp', 'K2pd']
A, B = csi.benchmark_lstsq_matrices(rollParams, timeSeries, fixedValues)
testing.assert_allclose(trial.taskSignals['RollRate'].time_derivative(),
A[:, 0])
testing.assert_allclose(trial.taskSignals['SteerRate'].time_derivative(),
A[:, 1])
testing.assert_allclose(trial.taskSignals['ForwardSpeed'] *
trial.taskSignals['RollRate'], A[:, 2])
testing.assert_allclose(trial.taskSignals['ForwardSpeed'] *
trial.taskSignals['SteerRate'], A[:, 3])
testing.assert_allclose(9.81 *
trial.taskSignals['RollAngle'], A[:, 4])
testing.assert_allclose(9.81 *
trial.taskSignals['SteerAngle'], A[:, 5])
testing.assert_allclose(trial.taskSignals['ForwardSpeed']**2 *
trial.taskSignals['RollAngle'], A[:, 6])
testing.assert_allclose(trial.taskSignals['ForwardSpeed']**2 *
trial.taskSignals['SteerAngle'], A[:, 7])
testing.assert_allclose(H[0] * trial.taskSignals['PullForce'], B)
steerParams = ['Mdp', 'Mdd', 'C1dp', 'C1dd', 'K0dp', 'K0dd', 'K2dp',
'K2dd', 'HdF']
A, B = csi.benchmark_lstsq_matrices(steerParams, timeSeries, fixedValues)
testing.assert_allclose(trial.taskSignals['RollRate'].time_derivative(),
A[:, 0])
testing.assert_allclose(trial.taskSignals['SteerRate'].time_derivative(),
A[:, 1])
testing.assert_allclose(trial.taskSignals['ForwardSpeed'] *
trial.taskSignals['RollRate'], A[:, 2])
testing.assert_allclose(trial.taskSignals['ForwardSpeed'] *
trial.taskSignals['SteerRate'], A[:, 3])
testing.assert_allclose(9.81 *
trial.taskSignals['RollAngle'], A[:, 4])
testing.assert_allclose(9.81 *
trial.taskSignals['SteerAngle'], A[:, 5])
testing.assert_allclose(trial.taskSignals['ForwardSpeed']**2 *
trial.taskSignals['RollAngle'], A[:, 6])
testing.assert_allclose(trial.taskSignals['ForwardSpeed']**2 *
trial.taskSignals['SteerAngle'], A[:, 7])
testing.assert_allclose(-trial.taskSignals['PullForce'], A[:, 8])
testing.assert_allclose(trial.taskSignals['SteerTorque'], B)
steerParams = ['Mdp', 'Mdd', 'C1dp', 'C1dd', 'K0dp', 'K0dd', 'K2dp',
'K2dd']
A, B = csi.benchmark_lstsq_matrices(steerParams, timeSeries, fixedValues)
testing.assert_allclose(trial.taskSignals['RollRate'].time_derivative(),
A[:, 0])
testing.assert_allclose(trial.taskSignals['SteerRate'].time_derivative(),
A[:, 1])
testing.assert_allclose(trial.taskSignals['ForwardSpeed'] *
trial.taskSignals['RollRate'], A[:, 2])
testing.assert_allclose(trial.taskSignals['ForwardSpeed'] *
trial.taskSignals['SteerRate'], A[:, 3])
testing.assert_allclose(9.81 *
trial.taskSignals['RollAngle'], A[:, 4])
testing.assert_allclose(9.81 *
trial.taskSignals['SteerAngle'], A[:, 5])
testing.assert_allclose(trial.taskSignals['ForwardSpeed']**2 *
trial.taskSignals['RollAngle'], A[:, 6])
testing.assert_allclose(trial.taskSignals['ForwardSpeed']**2 *
trial.taskSignals['SteerAngle'], A[:, 7])
testing.assert_allclose(trial.taskSignals['SteerTorque'] +
H[1] * trial.taskSignals['PullForce'], B)
steerParams = ['Mdp', 'C1dd', 'K0dp', 'K2dp', 'K2dd']
A, B = csi.benchmark_lstsq_matrices(steerParams, timeSeries, fixedValues)
testing.assert_allclose(trial.taskSignals['RollRate'].time_derivative(),
A[:, 0])
testing.assert_allclose(trial.taskSignals['ForwardSpeed'] *
trial.taskSignals['SteerRate'], A[:, 1])
testing.assert_allclose(9.81 *
trial.taskSignals['RollAngle'], A[:, 2])
testing.assert_allclose(trial.taskSignals['ForwardSpeed']**2 *
trial.taskSignals['RollAngle'], A[:, 3])
testing.assert_allclose(trial.taskSignals['ForwardSpeed']**2 *
trial.taskSignals['SteerAngle'], A[:, 4])
testing.assert_allclose(
H[1] * trial.taskSignals['PullForce'] +
trial.taskSignals['SteerTorque'] -
fixedValues['Mdd'] * trial.taskSignals['SteerRate'].time_derivative() -
fixedValues['C1dp'] * trial.taskSignals['ForwardSpeed'] * trial.taskSignals['RollRate'] -
fixedValues['K0dd'] * 9.81 * trial.taskSignals['SteerAngle'], B)
