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)