//parameters: List 1-> joint_angle, List 2-> biceps_emg, List 3-> triceps_emg, List 4-> prev_angle, List 5-> prev_biceps_emg, List 6-> previous_triceps_emg
List <double> EstimationController.EstimateTorque(List <List <double> > parameters)
{
List <double> joint_torque = new List <double>();
List <double> joint_angle = parameters[0];
List <double> biceps_emg = parameters[1];
List <double> triceps_emg = parameters[2];
List <double> previous_joint_angle = parameters[3];
List <double> previous_biceps_emg = parameters[4];
List <double> previous_triceps_emg = parameters[5];
List <double> joint_velocity = GetJointVelocity(ref joint_angle, timestep);
List <double> joint_acceleration = GetJointAcceleration(ref joint_velocity, timestep);
//Process the EMG signals
biceps_emg = signal_processing.FilterEMG(ref biceps_emg, ref previous_biceps_emg);
triceps_emg = signal_processing.FilterEMG(ref triceps_emg, ref previous_triceps_emg);
biceps_emg = signal_processing.Rectify(ref biceps_emg);
triceps_emg = signal_processing.Rectify(ref triceps_emg);
biceps_emg = signal_processing.Normalize(ref biceps_emg, body.biceps.minimum_activation, body.biceps.maximum_activation);
triceps_emg = signal_processing.Normalize(ref triceps_emg, body.triceps.minimum_activation, body.triceps.maximum_activation);
//Neural Activation
List <double> biceps_neural_activation = signal_processing.LinearEnvelope(ref biceps_emg, ref previous_biceps_emg);
List <double> triceps_neural_activation = signal_processing.LinearEnvelope(ref triceps_emg, ref previous_triceps_emg);
//Muscle Activation
List <double> biceps_muscle_activation = MuscleActivationModel1(biceps_neural_activation, muscle_activation_coefficients[0]);
List <double> triceps_muscle_activation = MuscleActivationModel1(triceps_neural_activation, muscle_activation_coefficients[1]);
//Muscle Torque
List <double> biceps_torque = new List <double>();
List <double> triceps_torque = new List <double>();
for (int i = 0; i < joint_angle.Count - 1; i++)
{
biceps_torque.Add(EstimateMuscleTorque(biceps_muscle_activation[i + 1], joint_angle[i], joint_angle[i + 1], body.biceps, timestep));
triceps_torque.Add(EstimateMuscleTorque(triceps_muscle_activation[i + 1], joint_angle[i], joint_angle[i + 1], body.triceps, timestep));
}
//Skeletal Model
List <double> passive_torque = GetPassiveTorque(joint_velocity);
List <double> gravitational_torque = GetGravitationalTorque(ref joint_angle, body.mass_hand, body.mass_forearm, body.moment_arm_hand, body.moment_arm_forearm);
double inertial_mass = GetInertialMass(body.length_hand, body.length_forearm, body.circumference_forearm, body.mass_hand, body.mass_forearm);
List <double> acceleration_torque = GetAccelerationTorque(ref joint_acceleration, inertial_mass);
double temp = 0;
for (int i = 0; i < joint_angle.Count; i++)
{
temp = biceps_torque[i] - triceps_torque[i] - gravitational_torque[i] + passive_torque[i] + acceleration_torque[i];
joint_torque.Add(temp);
}
return(joint_torque);
}
private void EMGDataWorker_RunWorkerCompleted(object sender, System.ComponentModel.RunWorkerCompletedEventArgs e)
{
List <List <double> > samples = (List <List <double> >)e.Result;
List <List <double> > write_data;
List <double> upsampled_motor_encoder = new List <double>(window_size); //in degrees
List <double> upsampled_collection_arm_encoder = new List <double>(window_size); //in degrees
List <double> biceps_emg_copy = new List <double>(samples[0]);
List <double> triceps_emg_copy = new List <double>(samples[1]);
double duration = window_size / emg_sampling_frequency; //in seconds
double desired_frequency = emg_sampling_frequency; //match the EMG
biceps_emg = samples[0];
triceps_emg = samples[1];
if (experiment_interface.display_emg_data)
{
experiment_interface.GraphEMGData(biceps_emg, triceps_emg);
}
if (biceps_emg.Count > 0 && triceps_emg.Count > 0 && subject_positions.Count > 0 && brace_positions.Count > 0)
{
if (save_motion_data || track_motion_data)
{
//Update current parameters
subject_positions = Upsample(ref subject_positions, desired_frequency, duration);
previous_subject_positions = Upsample(ref previous_subject_positions, desired_frequency, duration);
upsampled_collection_arm_encoder = Upsample(ref subject_positions, desired_frequency, duration);
upsampled_motor_encoder = Upsample(ref brace_positions, desired_frequency, duration);
//Process data for estimation
GenerateFilterSamples(ref subject_positions, ref previous_subject_positions, signal_processing.position_filter.GetOrder());
subject_positions = signal_processing.FilterPosition(ref subject_positions, ref previous_subject_positions);
//biceps_emg = signal_processing.RemoveOffset(ref biceps_emg);
//triceps_emg = signal_processing.RemoveOffset(ref triceps_emg);
if (counter == 1)
{
System.Console.WriteLine(" ");
}
//Filter biceps and triceps EMG signals
GenerateFilterSamples(ref biceps_emg, ref previous_biceps_emg_filter1, signal_processing.emg_highpass_filter.GetOrder());
GenerateFilterSamples(ref triceps_emg, ref previous_triceps_emg_filter1, signal_processing.emg_highpass_filter.GetOrder());
biceps_emg = signal_processing.FilterEMG(ref biceps_emg, ref previous_biceps_emg_filter1);
triceps_emg = signal_processing.FilterEMG(ref triceps_emg, ref previous_triceps_emg_filter1);
previous_biceps_emg_filter1 = new List <double>(biceps_emg);
previous_triceps_emg_filter1 = new List <double>(triceps_emg);
//Rectify EMG signals
biceps_emg = signal_processing.Rectify(ref biceps_emg);
triceps_emg = signal_processing.Rectify(ref triceps_emg);
//Normalize signals
biceps_emg = signal_processing.Normalize(ref biceps_emg, subject.biceps.minimum_activation, subject.biceps.maximum_activation); //Set min and max values with main controller
triceps_emg = signal_processing.Normalize(ref triceps_emg, subject.triceps.minimum_activation, subject.triceps.maximum_activation); //Set min and max values with main controller
//Calculate Neural Activation
GenerateFilterSamples(ref biceps_emg, ref previous_biceps_emg, signal_processing.linear_envelope_filter.GetOrder());
GenerateFilterSamples(ref triceps_emg, ref previous_triceps_emg, signal_processing.linear_envelope_filter.GetOrder());
biceps_emg = signal_processing.LinearEnvelope(ref biceps_emg, ref previous_biceps_emg);
triceps_emg = signal_processing.LinearEnvelope(ref triceps_emg, ref previous_triceps_emg);
UpdateSubjectMotionData();
//Make copies of current data sources
previous_biceps_emg = new List <double>(biceps_emg);
previous_triceps_emg = new List <double>(triceps_emg);
previous_subject_positions = new List <double>(subject_positions);
previous_brace_positions = new List <double>(brace_positions);
counter++;
}
if (track_motion_data)
{
//Get position estimates
if (counter == 1)
{
estimation_controllers[current_estimation_controller].SetPreviousPosition(subject_positions[subject_positions.Count - 1]);
}
position_estimates = estimation_controllers[current_estimation_controller].EstimatePosition(subject_motion_data);
//Calculate the average position
double average_position = 0;
for (int i = 0; i < position_estimates.Count; i++)
{
average_position += position_estimates[i];
}
average_position /= position_estimates.Count;
//Position Limit Safety Check
if (IsWithinPositionLimits(average_position))
{
command_position = average_position;
}
else
{
command_position = brace_position;
}
}
if (save_motion_data)
{
write_data = new List <List <double> >();
write_data.Add(biceps_emg_copy);
write_data.Add(triceps_emg_copy);
write_data.Add(upsampled_collection_arm_encoder);
write_data.Add(position_estimates);
write_data.Add(upsampled_motor_encoder);
logger_thread.data = write_data;
logger_thread.writer = output_writer;
stop_watch3.Start();
logger_worker.RunWorkerAsync(logger_thread);
}
//Clear all data sources
biceps_emg.Clear();
triceps_emg.Clear();
subject_positions.Clear();
brace_positions.Clear();
}
stop_watch.Stop();
tracking_loop_timer = stop_watch.Elapsed.TotalMilliseconds;
//System.Console.WriteLine("EMG Thread Duration (ms): " + stop_watch.ElapsedMilliseconds);
stop_watch.Reset();
}