!pip freeze

%pylab inline

import pandas
from numpy import deg2rad
from dtk import walk, process

obinna = pandas.read_csv('../data/obinna-walking.txt', delimiter='\t',
                         index_col="TimeStamp", na_values='0.000000')

obinna

[col for col in obinna.columns if col.endswith('.Ang') or col.endswith('ForY') or col.endswith('.Mom')]

for col in obinna.columns:
    if col.endswith('.Ang'):
        obinna[col] = deg2rad(obinna[col])

rcParams['figure.figsize'] = 14, 8
obinna['FP2.ForY'].plot()

start = 500
stop = 3500

data = walk.WalkingData(obinna.iloc[start:stop].copy())

to_diff = [col for col in obinna.columns if col.endswith('.Ang')]
new_names = [name.split('.')[0] + '.Rate' for name in to_diff]

data.time_derivative(to_diff, new_names)

data.raw_data[new_names]

rcParams['figure.figsize'] = 14, 10
right_strikes, left_strikes, right_offs, left_offs = \
    data.grf_landmarks('FP2.ForY', 'FP1.ForY', do_plot=True, threshold=28.0)

rcParams['figure.figsize'] = 14, 10
right_steps = data.split_at('right', num_samples=15)
data.plot_steps('FP2.ForY', 'RKneeFlexion.Ang', 'RKneeFlexion.Rate', 'RKneeFlexion.Mom', marker='o')

mean_right_steps = right_steps.mean(axis='items')
mean_right_steps[['FP2.ForY', 'RKneeFlexion.Ang', 'RKneeFlexion.Rate', 'RKneeFlexion.Mom']].plot(subplots=True)

sensors = ['RKneeFlexion.Ang',
           'RKneeFlexion.Rate',
           'RHipFlexion.Ang',
           'RHipFlexion.Rate',
           'LKneeFlexion.Ang',
           'LKneeFlexion.Rate',
           'LHipFlexion.Ang',
           'LHipFlexion.Rate']

controls = ['RKneeFlexion.Mom',
            'RHipFlexion.Mom',
            'LKneeFlexion.Mom',
            'LHipFlexion.Mom']

solver = walk.SimpleControlSolver(right_steps, sensors, controls)

gains, controls_star, variance, gain_var, control_var, estimated_controls = solver.solve()

gains.shape

axes = solver.plot_gains(gains, gain_var)

variance

rcParams['figure.figsize'] = 14, 14
solver.plot_estimated_vs_measure_controls(estimated_controls, variance)

solver.plot_control_contributions(estimated_controls)

gain_omission_matrix = np.ones((len(controls), len(sensors))).astype(bool)
gain_omission_matrix[2:, :4] = False
gain_omission_matrix[:2, 4:] = False
gains, controls, variance, gain_var, control_var, estimated_controls = solver.solve(gain_omission_matrix=gain_omission_matrix)

gain_omission_matrix

rcParams['figure.figsize'] = 14, 10
axes = solver.plot_gains(gains, gain_var)

rcParams['figure.figsize'] = 14, 14
solver.plot_estimated_vs_measure_controls(estimated_controls, variance)

rcParams['figure.figsize'] = 14, 14
solver.plot_control_contributions(estimated_controls)

coordinates = pandas.read_csv('../data/CAREN_Trial04_dof.txt', delimiter='\t', index_col="TimeStamp")
speeds = pandas.read_csv('../data/CAREN_Trial04_dofvel.txt', delimiter='\t', index_col="TimeStamp")
generalized_forces = pandas.read_csv('../data/CAREN_Trial04_jointmoment.txt', delimiter='\t', index_col="TimeStamp")
ground_reaction_forces = pandas.read_csv('../data/CAREN_Trial04_grf.txt', delimiter='\t', index_col="TimeStamp")
raw_data = pandas.read_csv('../data/CAREN_Trial04.txt', delimiter='\t', index_col="TimeStamp")

coordinates.columns = [name + '.Ang' for name in coordinates.columns]
speeds.columns = [name + '.Rate' for name in speeds.columns]
generalized_forces.columns = [name + '.Mom' for name in generalized_forces.columns]

all_data = coordinates.join(speeds).join(generalized_forces).join(ground_reaction_forces)

data = walk.WalkingData(all_data[all_data.index > 15.0])

rcParams['figure.figsize'] = 14, 10
right_strikes, left_strikes, right_offs, left_offs = \
    data.grf_landmarks('RFy','LFy', do_plot=True, threshold=5.0)

right_steps = data.split_at('right', num_samples=15)
axes = data.plot_steps('RFy', 'RKneeFlexion.Ang', 'RKneeFlexion.Rate', 'RKneeFlexion.Mom', marker='o')

sensors = ['RKneeFlexion.Ang',
           'RKneeFlexion.Rate',
           'RHipFlexion.Ang',
           'RHipFlexion.Rate']

controls = ['RKneeFlexion.Mom',
            'RHipFlexion.Mom']

solver = walk.SimpleControlSolver(right_steps, sensors, controls)

gains, controls, variance, gain_var, control_var, estimated_controls = solver.solve()

variance

rcParams['figure.figsize'] = 14, 10
axes = solver.plot_gains(gains, gain_var)

solver.plot_estimated_vs_measure_controls(estimated_controls, variance)

rcParams['figure.figsize'] = 14, 14
solver.plot_control_contributions(estimated_controls)