Skip to content

Commit

Permalink
Merge pull request #213 from rcnl-org/debug-mtp
Browse files Browse the repository at this point in the history 
Debug MTP cost terms
  • Loading branch information
@cvhammond
cvhammond committed Jun 15, 2023
2 parents b55942d + 5cac387 commit 0d70c90
Show file tree
Hide file tree
Showing 4 changed files with 100 additions and 81 deletions.
2 changes: 2 additions & 0 deletions src/MuscleTendonPersonalization/MuscleTendonPersonalizationTool.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,8 @@ function MuscleTendonPersonalizationTool(settingsFileName)
optimizedInitialGuess = MuscleTendonLengthInitialization(precalInputs);
inputs = updateMtpInitialGuess(inputs, precalInputs, ...
optimizedInitialGuess);
else
precalInputs = struct('optimizeIsometricMaxForce', false);
end

optimizedParams = MuscleTendonPersonalization(inputs, params);
Expand Down
6 changes: 4 additions & 2 deletions src/MuscleTendonPersonalization/Optimization/CostTerms/calcMuscleExcitationPenaltyCost.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,13 +27,15 @@

function cost = calcMuscleExcitationPenaltyCost(modeledValues, ...
experimentalData, costTerm)
errorCenter = valueOrAlternate(costTerm, "errorCenter", 0.5);
maximumAllowableError = valueOrAlternate(costTerm, "maxAllowableError", 0.25);
muscleExcitationsConstraint = modeledValues.muscleExcitationsNoTDelay(: , ...
setdiff(1 : size(modeledValues.muscleExcitationsNoTDelay, 2), ...
[experimentalData.synergyExtrapolation.missingEmgChannelGroups{:}]), ...
experimentalData.numPaddingFrames + 1 : ...
size(modeledValues.muscleExcitationsNoTDelay, 3) - ...
experimentalData.numPaddingFrames);
cost = 120 * (muscleExcitationsConstraint - 0.5) .^ 8;
cost = 30 / maximumAllowableError * (muscleExcitationsConstraint - errorCenter) .^ 8;
cost(isnan(cost))=0;
cost = sum([sqrt(0.1) .* cost ].^ 2, 'all');
cost = sum((sqrt(0.1) .* cost).^ 2, 'all');
end
90 changes: 45 additions & 45 deletions src/MuscleTendonPersonalization/SynergyExtrapolation/calcMuscleExcitationsSynX.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,11 +7,11 @@
% data:
% time - timestamp vectors
% timeDelay - electromechanical delay
% emgScalingFactor - EMG scaling factors
% emgScalingFactor - EMG scaling factors
% synergyExtrapolation - vector for SynX (including both SynX weights and residual weights)
% experimentalData.extrapolationCommands - synergy excitations extracted from measured muscle
% excitations for SynX
% experimentalData.residualCommands - synergy excitations extracted from measured muscle excitations
% experimentalData.residualCommands - synergy excitations extracted from measured muscle excitations
% for residual muscle excitations
% params.missingEmgChannelGroups - Index for expanding SynX EMG to unmeasured muscles
% MeasuredMuscIndexExpand - Index for expanding residual EMG to measured muscles
Expand All @@ -22,9 +22,9 @@
% size(params.taskNames,2) - number of tasks - double
% experimentalData.emgData - number of time frames for each trial - double
% params.matrixFactorizationMethod - matrix factorization algorithm - 'PCA' or 'NMF'
% params.synergyExtrapolationCategorization - variability of synergy vector weights across trials for
% params.synergyExtrapolationCategorization - variability of synergy vector weights across trials for
% SynX reconstruction
% params.residualCategorization - variability of synergy vector weights across trials for
% params.residualCategorization - variability of synergy vector weights across trials for
% residual excitation reconstruction
% TrialIndex - trial index for each task - cell array (nTask cells)
% nOtherDesignVariables - number of other design variables - double
Expand Down Expand Up @@ -56,7 +56,7 @@

function [muscleExcitations, muscleExcitationsNoTDelay] = ...
calcMuscleExcitationsSynX(experimentalData, timeDelay, ...
emgScalingFactor, synergyExtrapolationVariables, params)
emgScalingFactor, synergyExtrapolationVariables, params)

experimentalData.emgDataExpanded = round(experimentalData.emgDataExpanded, 4);

Expand All @@ -79,10 +79,10 @@
% Create EMG splines
emgSplines = createEmgSignals(muscleExcitationsNoTDelay, ...
experimentalData.emgTime, timeDelay);
% Interpolate Emg
% Interpolate Emg
muscleExcitations = evaluateEmgSplines(experimentalData.emgTime, ...
emgSplines, timeDelay);
% muscleExcitations = permute(muscleExcitations,[1 3 2]);
% muscleExcitations = permute(muscleExcitations,[1 3 2]);
end

function UnmeasuredExcitations = getUnmeasuredMuscleExcitations(params, ...
Expand All @@ -93,33 +93,33 @@
size(params.missingEmgChannelGroups,2), ...
length(params.synergyCategorizationOfTrials));
for j = 1:size(params.missingEmgChannelGroups,2)
if strcmpi(params.matrixFactorizationMethod,'PCA')
SynxVars = reshape(extrapolationWeights',(params.numberOfSynergies + 1) * ...
length(params.synergyCategorizationOfTrials), ...
size(params.missingEmgChannelGroups, 2))';
for i = 1 : length(params.synergyCategorizationOfTrials)
UnmeasuredExcitations(:, j, i) = extrapolationCommands{i} * SynxVars(j, ...
(i - 1) * (params.numberOfSynergies + 1) + 1 : (i - 1) * ...
(params.numberOfSynergies + 1) + params.numberOfSynergies)' + ...
repmat(SynxVars(j, i * (params.numberOfSynergies + 1)), ...
size(emgData, 3) * ...
size(params.synergyCategorizationOfTrials{i}, 2), 1);
end
elseif strcmp(params.matrixFactorizationMethod,'NMF')
SynxVars = reshape(synxVars', params.numberOfSynergies * ...
length(params.synergyCategorizationOfTrials), ...
size(params.missingEmgChannelGroups, 2))';
for i = 1 : length(params.synergyCategorizationOfTrials)
UnmeasuredExcitations = zeros(emgData * size( ...
params.synergyCategorizationOfTrials{i}, 2), ...
size(params.missingEmgChannelGroups, ...
2), length(params.synergyCategorizationOfTrials));
UnmeasuredExcitations(:, j, i) = extrapolationCommands{i} * ...
SynxVars(j, (i - 1) * params.numberOfSynergies + 1 : ...
i * params.numberOfSynergies, :)';
if strcmpi(params.matrixFactorizationMethod,'PCA')
SynxVars = reshape(extrapolationWeights',(params.numberOfSynergies + 1) * ...
length(params.synergyCategorizationOfTrials), ...
size(params.missingEmgChannelGroups, 2))';
for i = 1 : length(params.synergyCategorizationOfTrials)
UnmeasuredExcitations(:, j, i) = extrapolationCommands{i} * SynxVars(j, ...
(i - 1) * (params.numberOfSynergies + 1) + 1 : (i - 1) * ...
(params.numberOfSynergies + 1) + params.numberOfSynergies)' + ...
repmat(SynxVars(j, i * (params.numberOfSynergies + 1)), ...
size(emgData, 3) * ...
size(params.synergyCategorizationOfTrials{i}, 2), 1);
end
elseif strcmp(params.matrixFactorizationMethod,'NMF')
SynxVars = reshape(synxVars', params.numberOfSynergies * ...
length(params.synergyCategorizationOfTrials), ...
size(params.missingEmgChannelGroups, 2))';
for i = 1 : length(params.synergyCategorizationOfTrials)
UnmeasuredExcitations = zeros(emgData * size( ...
params.synergyCategorizationOfTrials{i}, 2), ...
size(params.missingEmgChannelGroups, ...
2), length(params.synergyCategorizationOfTrials));
UnmeasuredExcitations(:, j, i) = extrapolationCommands{i} * ...
SynxVars(j, (i - 1) * params.numberOfSynergies + 1 : ...
i * params.numberOfSynergies, :)';
end
end
end
end
UnmeasuredExcitations = permute(UnmeasuredExcitations, [3 2 1]);
end

Expand All @@ -146,7 +146,7 @@
end
concatResidualExcitations = [];
for i = 1:size(params.residualCategorizationOfTrials, 2)
concatResidualExcitations = [concatResidualExcitations;
concatResidualExcitations = [concatResidualExcitations;
residualExcitations{i}];
end
residualExcitations = concatResidualExcitations;
Expand All @@ -155,11 +155,11 @@
function emgData = updateEmgSignals(missingEmgChannelGroups, emgData, ...
unmeasuredEmgSignals)

for i = 1 : size(missingEmgChannelGroups, 2)
for j = 1 : size(missingEmgChannelGroups{i}, 2)
emgData(:, missingEmgChannelGroups{i}(1, j), :) = ...
unmeasuredEmgSignals(:, i, :);
end
for i = 1 : size(missingEmgChannelGroups, 2)
for j = 1 : size(missingEmgChannelGroups{i}, 2)
emgData(:, missingEmgChannelGroups{i}(1, j), :) = ...
unmeasuredEmgSignals(:, i, :);
end
end
end

Expand All @@ -168,12 +168,12 @@

%Distribute residual muscle excitations to measured muscles
residualExcitationsExpanded = zeros(size(emgData, 3) * ...
size(emgData, 1), size(emgData, 2));
size(emgData, 1), size(emgData, 2));
for i = 1 : size(currentEmgChannelGroups, 2)
for j = 1 : size(currentEmgChannelGroups{i}, 2)
residualExcitationsExpanded(:, currentEmgChannelGroups{i}(1, j)) = ...
residualExcitations(:, i);
end
for j = 1 : size(currentEmgChannelGroups{i}, 2)
residualExcitationsExpanded(:, currentEmgChannelGroups{i}(1, j)) = ...
residualExcitations(:, i);
end
end
residualExcitationsExpanded = reshape(residualExcitationsExpanded, ...
size(emgData, 3), size(emgData, 1), size(emgData, 2));
Expand All @@ -186,13 +186,13 @@
if size(timeDelay, 2) <= 2
for j = 1 : size(emgData, 1)
emgSplines{j} = spline(emgTime(j, 1 : 4 : end), ...
emgData(j, :, 1 : 4 : end)');
emgData(j, :, 1 : 4 : end)');
end
else
for i = 1 : size(emgData, 2)
for j = 1:size(emgData, 1)
emgSplines{j, i} = spline(emgTime(j, 1 : 4 : end), ...
emgData(j, i, 1 : 4 : end));
emgData(j, i, 1 : 4 : end));
end
end
end
Expand Down
83 changes: 49 additions & 34 deletions src/MuscleTendonPersonalization/SynergyExtrapolation/getLinearInequalityConstraints.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,42 +49,57 @@
function [A,b] = getLinearInequalityConstraints(params, ...
numOtherDesignVariables, synergyCommands, emgData)

if strcmp(params.matrixFactorizationMethod, 'PCA')
matrixFactorizationFactor = 1;
elseif strcmp(params.matrixFactorizationMethod, 'NMF')
matrixFactorizationFactor = 0;
end
% Construct the linear constraint, SynX and Residual A Matrix
aMatrixSynergy = getSynxAMatrix(params, synergyCommands, ...
emgData, matrixFactorizationFactor);
aMatrixResidual = getResidualAMatrix(emgData, params.numberOfSynergies, ...
params.missingEmgChannelGroups, params.residualCategorizationOfTrials, ...
matrixFactorizationFactor);
% Concatenate non-SynX design variables with SynX and Residual Matrices
A = [zeros(2 * size(params.missingEmgChannelGroups, 2) * size(emgData, 1) * ...
size(emgData, 2), numOtherDesignVariables) [-aMatrixSynergy ...
aMatrixResidual; aMatrixSynergy aMatrixResidual]];
% b (between 0 and 1)
b = [zeros(size(emgData, 1) * size(emgData, 2) * size( ...
params.missingEmgChannelGroups, 2), 1); ones(size(emgData, 1) * ...
size(emgData, 2) * size(params.missingEmgChannelGroups, 2), 1)];

% fullCommands = synergyCommands{1};
% for i = 2:length(synergyCommands)
% fullCommands = cat(1, fullCommands, synergyCommands{i});
% end
% fullCommands(:, end+1) = ones(size(fullCommands, 1), 1);
% fullEmg = reshape(emgData, size(emgData, 1) * size(emgData, 2), ...
% size(emgData, 3));
%
% if strcmp(params.matrixFactorizationMethod, 'PCA')
% matrixFactorizationFactor = 1;
% elseif strcmp(params.matrixFactorizationMethod, 'NMF')
% matrixFactorizationFactor = 0;
% totalGroups = length(params.missingEmgChannelGroups) * size(emgData, 2) + ...
% length(params.currentEmgChannelGroups);
% A = zeros(totalGroups * size(fullCommands, 1), ...
% numOtherDesignVariables + totalGroups * size(fullCommands, 2));
% for i = 1:totalGroups
% A(size(fullCommands, 1) * (i - 1) + 1 : size(fullCommands, 1) * i, ...
% numOtherDesignVariables + size(fullCommands, 2) * (i - 1) + 1 : ...
% numOtherDesignVariables + size(fullCommands, 2) * i) = fullCommands;
% end
% A = cat(1, A, -A);
%
% b = ones(length(params.missingEmgChannelGroups) * ...
% size(emgData, 2) * size(fullCommands, 1), 1);
% for i = 1:length(params.currentEmgChannelGroups)
% b = [b; 1 - fullEmg(:, i)];
% end
% b = [b; zeros(length(params.missingEmgChannelGroups) * ...
% size(emgData, 2) * size(fullCommands, 1), 1)];
% for i = 1:length(params.currentEmgChannelGroups)
% b = [b; fullEmg(:, i)];
% end
% end
% % Construct the linear constraint, SynX and Residual A Matrix
% aMatrixSynergy = getSynxAMatrix(params, synergyCommands, ...
% emgData, matrixFactorizationFactor);
% aMatrixResidual = getResidualAMatrix(emgData, params.numberOfSynergies, ...
% params.missingEmgChannelGroups, params.residualCategorizationOfTrials, ...
% matrixFactorizationFactor);
% % Concatenate non-SynX design variables with SynX and Residual Matrices
% A = [zeros(2 * size(params.missingEmgChannelGroups, 2) * size(emgData, 1) * ...
% size(emgData, 2), numOtherDesignVariables) [-aMatrixSynergy ...
% aMatrixResidual; aMatrixSynergy aMatrixResidual]];
% % b (between 0 and 1)
% b = [zeros(size(emgData, 1) * size(emgData, 2) * size( ...
% params.missingEmgChannelGroups, 2), 1); ones(size(emgData, 1) * ...
% size(emgData, 2) * size(params.missingEmgChannelGroups, 2), 1)];

fullCommands = synergyCommands{1};
for i = 2:length(synergyCommands)
fullCommands = cat(1, fullCommands, synergyCommands{i});
end
fullCommands(:, end+1) = ones(size(fullCommands, 1), 1);

totalGroups = length(params.missingEmgChannelGroups) + ...
length(params.currentEmgChannelGroups);
A = zeros(totalGroups * size(fullCommands, 1), numOtherDesignVariables + totalGroups * size(fullCommands, 2));
for i = 1:totalGroups
A(size(fullCommands, 1) * (i - 1) + 1 : size(fullCommands, 1) * i, ...
numOtherDesignVariables + size(fullCommands, 2) * (i - 1) + 1 : ...
numOtherDesignVariables + size(fullCommands, 2) * i) = fullCommands;
end
A = cat(1, A, -A);
b = ones(totalGroups * size(fullCommands, 1) * 2, 1);

end

Expand Down

0 comments on commit 0d70c90

Please sign in to comment.