diff --git a/resources/project/D7EGYpvIiO_nGXQuTOYjsSP_JQw/70qGohwUKdJepml9XrUSQBg8iXAp.xml b/resources/project/D7EGYpvIiO_nGXQuTOYjsSP_JQw/70qGohwUKdJepml9XrUSQBg8iXAp.xml deleted file mode 100644 index 13873994b..000000000 --- a/resources/project/D7EGYpvIiO_nGXQuTOYjsSP_JQw/70qGohwUKdJepml9XrUSQBg8iXAp.xml +++ /dev/null @@ -1,2 +0,0 @@ - - \ No newline at end of file diff --git a/resources/project/D7EGYpvIiO_nGXQuTOYjsSP_JQw/jwQJUlUQLZW-X5QWOUQ_Ed6LSTUp.xml b/resources/project/D7EGYpvIiO_nGXQuTOYjsSP_JQw/jwQJUlUQLZW-X5QWOUQ_Ed6LSTUp.xml deleted file mode 100644 index bd8c96c4a..000000000 --- a/resources/project/D7EGYpvIiO_nGXQuTOYjsSP_JQw/jwQJUlUQLZW-X5QWOUQ_Ed6LSTUp.xml +++ /dev/null @@ -1,2 +0,0 @@ - - \ No newline at end of file diff --git a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/oXIaZ30PV4AuIRRG1GZvJ_SIyRMd.xml b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/oXIaZ30PV4AuIRRG1GZvJ_SIyRMd.xml new file mode 100644 index 000000000..78ad5b1a1 --- /dev/null +++ b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/oXIaZ30PV4AuIRRG1GZvJ_SIyRMd.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/oXIaZ30PV4AuIRRG1GZvJ_SIyRMp.xml b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/oXIaZ30PV4AuIRRG1GZvJ_SIyRMp.xml new file mode 100644 index 000000000..7a38a9153 --- /dev/null +++ b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/oXIaZ30PV4AuIRRG1GZvJ_SIyRMp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/NDPaR6DsgCrsBoJWF55J1vGaXlA/8fabh-WBmBGbDz7_h2NqYd4ovLcd.xml b/resources/project/NDPaR6DsgCrsBoJWF55J1vGaXlA/8fabh-WBmBGbDz7_h2NqYd4ovLcd.xml new file mode 100644 index 000000000..a75f7a81b --- /dev/null +++ b/resources/project/NDPaR6DsgCrsBoJWF55J1vGaXlA/8fabh-WBmBGbDz7_h2NqYd4ovLcd.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/NDPaR6DsgCrsBoJWF55J1vGaXlA/8fabh-WBmBGbDz7_h2NqYd4ovLcp.xml b/resources/project/NDPaR6DsgCrsBoJWF55J1vGaXlA/8fabh-WBmBGbDz7_h2NqYd4ovLcp.xml new file mode 100644 index 000000000..842de6ab3 --- /dev/null +++ b/resources/project/NDPaR6DsgCrsBoJWF55J1vGaXlA/8fabh-WBmBGbDz7_h2NqYd4ovLcp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/D7EGYpvIiO_nGXQuTOYjsSP_JQw/70qGohwUKdJepml9XrUSQBg8iXAd.xml b/resources/project/NDPaR6DsgCrsBoJWF55J1vGaXlA/liHb9kbr54OOKjgYDq6G1l2UsfId.xml similarity index 77% rename from resources/project/D7EGYpvIiO_nGXQuTOYjsSP_JQw/70qGohwUKdJepml9XrUSQBg8iXAd.xml rename to resources/project/NDPaR6DsgCrsBoJWF55J1vGaXlA/liHb9kbr54OOKjgYDq6G1l2UsfId.xml index 7de97d4a9..7a6326b99 100644 --- a/resources/project/D7EGYpvIiO_nGXQuTOYjsSP_JQw/70qGohwUKdJepml9XrUSQBg8iXAd.xml +++ b/resources/project/NDPaR6DsgCrsBoJWF55J1vGaXlA/liHb9kbr54OOKjgYDq6G1l2UsfId.xml @@ -1,6 +1,6 @@ - \ No newline at end of file diff --git a/resources/project/NDPaR6DsgCrsBoJWF55J1vGaXlA/liHb9kbr54OOKjgYDq6G1l2UsfIp.xml b/resources/project/NDPaR6DsgCrsBoJWF55J1vGaXlA/liHb9kbr54OOKjgYDq6G1l2UsfIp.xml new file mode 100644 index 000000000..a84fd8d2c --- /dev/null +++ b/resources/project/NDPaR6DsgCrsBoJWF55J1vGaXlA/liHb9kbr54OOKjgYDq6G1l2UsfIp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/_2w7KV6EHc1SMHPJ5sv5BJ_TE_4/NDPaR6DsgCrsBoJWF55J1vGaXlAd.xml b/resources/project/_2w7KV6EHc1SMHPJ5sv5BJ_TE_4/NDPaR6DsgCrsBoJWF55J1vGaXlAd.xml new file mode 100644 index 000000000..a75f7a81b --- /dev/null +++ b/resources/project/_2w7KV6EHc1SMHPJ5sv5BJ_TE_4/NDPaR6DsgCrsBoJWF55J1vGaXlAd.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/_2w7KV6EHc1SMHPJ5sv5BJ_TE_4/NDPaR6DsgCrsBoJWF55J1vGaXlAp.xml b/resources/project/_2w7KV6EHc1SMHPJ5sv5BJ_TE_4/NDPaR6DsgCrsBoJWF55J1vGaXlAp.xml new file mode 100644 index 000000000..ecec5d64b --- /dev/null +++ b/resources/project/_2w7KV6EHc1SMHPJ5sv5BJ_TE_4/NDPaR6DsgCrsBoJWF55J1vGaXlAp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/D7EGYpvIiO_nGXQuTOYjsSP_JQw/jwQJUlUQLZW-X5QWOUQ_Ed6LSTUd.xml b/resources/project/jwn0mMf3vUOgeAWeaJY8jqgb0nM/g0w2BX3hbHutyPMvNe1ZC1Hym4sd.xml similarity index 77% rename from resources/project/D7EGYpvIiO_nGXQuTOYjsSP_JQw/jwQJUlUQLZW-X5QWOUQ_Ed6LSTUd.xml rename to resources/project/jwn0mMf3vUOgeAWeaJY8jqgb0nM/g0w2BX3hbHutyPMvNe1ZC1Hym4sd.xml index 7de97d4a9..7a6326b99 100644 --- a/resources/project/D7EGYpvIiO_nGXQuTOYjsSP_JQw/jwQJUlUQLZW-X5QWOUQ_Ed6LSTUd.xml +++ b/resources/project/jwn0mMf3vUOgeAWeaJY8jqgb0nM/g0w2BX3hbHutyPMvNe1ZC1Hym4sd.xml @@ -1,6 +1,6 @@ - \ No newline at end of file diff --git a/resources/project/jwn0mMf3vUOgeAWeaJY8jqgb0nM/g0w2BX3hbHutyPMvNe1ZC1Hym4sp.xml b/resources/project/jwn0mMf3vUOgeAWeaJY8jqgb0nM/g0w2BX3hbHutyPMvNe1ZC1Hym4sp.xml new file mode 100644 index 000000000..2861ed5de --- /dev/null +++ b/resources/project/jwn0mMf3vUOgeAWeaJY8jqgb0nM/g0w2BX3hbHutyPMvNe1ZC1Hym4sp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/src/DesignOptimization/DesignOptimization.m b/src/DesignOptimization/DesignOptimization.m index 489549639..82357d72c 100644 --- a/src/DesignOptimization/DesignOptimization.m +++ b/src/DesignOptimization/DesignOptimization.m @@ -1,7 +1,7 @@ % This function is part of the NMSM Pipeline, see file for full license. % % () -> () -% +% % ----------------------------------------------------------------------- % % The NMSM Pipeline is a toolkit for model personalization and treatment % @@ -11,7 +11,7 @@ % National Institutes of Health (R01 EB030520). % % % % Copyright (c) 2021 Rice University and the Authors % -% Author(s): Marleny Vega % +% Author(s): Marleny Vega, Claire V. Hammond % % % % Licensed under the Apache License, Version 2.0 (the "License"); % % you may not use this file except in compliance with the License. % @@ -26,19 +26,9 @@ % ----------------------------------------------------------------------- % function [output, inputs] = DesignOptimization(inputs, params) -pointKinematics(inputs.mexModel); -inverseDynamics(inputs.mexModel); -inputs = getStateDerivatives(inputs); -inputs = setupGroundContact(inputs); -inputs = getSplines(inputs); -inputs = checkStateGuess(inputs); -inputs = checkControlGuess(inputs); -inputs = checkParameterGuess(inputs); -inputs = getIntegralBounds(inputs); -inputs = getPathConstraintBounds(inputs); -inputs = getTerminalConstraintBounds(inputs); -inputs = getDesignVariableInputBounds(inputs); -if strcmp(inputs.controllerType, 'synergy_driven') +inputs = makeTreatmentOptimizationInputs(inputs, params); +initializeMexOrMatlabParallelFunctions(inputs.mexModel); +if strcmp(inputs.controllerType, 'synergy_driven') inputs = setupMuscleSynergies(inputs); end output = computeDesignOptimizationMainFunction(inputs, params); @@ -47,4 +37,4 @@ inputs.splineSynergyActivations = spaps(inputs.initialGuess.time, ... inputs.initialGuess.control(:, inputs.numCoordinates + 1:end)', 0.0000001); inputs.synergyLabels = inputs.initialGuess.controlLabels(:, inputs.numCoordinates + 1:end); -end \ No newline at end of file +end diff --git a/src/DesignOptimization/DesignOptimizationTool.m b/src/DesignOptimization/DesignOptimizationTool.m index 6afa845de..a1861897b 100644 --- a/src/DesignOptimization/DesignOptimizationTool.m +++ b/src/DesignOptimization/DesignOptimizationTool.m @@ -33,6 +33,6 @@ function DesignOptimizationTool(settingsFileName) settingsTree = xml2struct(settingsFileName); [inputs, params] = parseDesignOptimizationSettingsTree(settingsTree); [outputs, inputs] = DesignOptimization(inputs, params); -reportDesignOptimizationResults(outputs, inputs); +reportTreatmentOptimizationResults(outputs, inputs); saveDesignOptimizationResults(outputs, inputs); -end \ No newline at end of file +end diff --git a/src/DesignOptimization/calcDesignOptimizationDiscreteObjective.m b/src/DesignOptimization/calcDesignOptimizationDiscreteObjective.m index 1e3a2be7c..36d38ae1c 100644 --- a/src/DesignOptimization/calcDesignOptimizationDiscreteObjective.m +++ b/src/DesignOptimization/calcDesignOptimizationDiscreteObjective.m @@ -25,34 +25,11 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function discrete = calcDesignOptimizationDiscreteObjective(values, params) +function discrete = calcDesignOptimizationDiscreteObjective(values, ... + modeledValues, auxdata) -discrete = []; -if isfield(params, 'discrete') - for i = 1:length(params.discrete.tracking) - costTerm = params.discrete.tracking{i}; - if costTerm.isEnabled - switch costTerm.type - case "synergy_vectors" - discrete = cat(2, discrete, ... - calcTrackingCoordinateIntegrand(params, ... - values.time, values.statePositions, ... - costTerm.coordinate)); - otherwise - throw(MException('', ['Cost term type ' costTerm.type ... - ' does not exist for this tool.'])) - end - end - end - for i = 1:length(params.discrete.minimizing) - costTerm = params.discrete.minimizing{i}; - if costTerm.isEnabled - switch costTerm.type - otherwise - throw(MException('', ['Cost term type ' costTerm.type ... - ' does not exist for this tool.'])) - end - end - end -end +[costTermCalculations, allowedTypes] = ... + generateCostTermStruct("discrete", "DesignOptimization"); +discrete = calcTreatmentOptimizationCost( ... + costTermCalculations, allowedTypes, values, modeledValues, auxdata); end \ No newline at end of file diff --git a/src/DesignOptimization/calcDesignOptimizationIntegrand.m b/src/DesignOptimization/calcDesignOptimizationIntegrand.m index 91c0464eb..66a4be42b 100644 --- a/src/DesignOptimization/calcDesignOptimizationIntegrand.m +++ b/src/DesignOptimization/calcDesignOptimizationIntegrand.m @@ -1,7 +1,7 @@ % This function is part of the NMSM Pipeline, see file for full license. % % () -> () -% +% % ----------------------------------------------------------------------- % % The NMSM Pipeline is a toolkit for model personalization and treatment % @@ -11,7 +11,7 @@ % National Institutes of Health (R01 EB030520). % % % % Copyright (c) 2021 Rice University and the Authors % -% Author(s): Marleny Vega % +% Author(s): Marleny Vega, Claire V. Hammond % % % % Licensed under the Apache License, Version 2.0 (the "License"); % % you may not use this file except in compliance with the License. % @@ -25,42 +25,12 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function integrand = calcDesignOptimizationIntegrand(values, params, ... - phaseout) -integrand = []; -for i = 1:length(params.integral.tracking) - costTerm = params.integral.tracking{i}; - if costTerm.isEnabled - switch costTerm.type - case "coordinate" - integrand = cat(2, integrand, ... - calcTrackingCoordinateIntegrand(params, ... - values.time, values.statePositions, ... - costTerm.coordinate)); - case "controller" - integrand = cat(2, integrand, ... - calcTrackingControllerIntegrand(params, values, ... - costTerm.controller)); - otherwise - throw(MException('', ['Cost term type ' costTerm.type ... - ' does not exist for this tool.'])) - end - end -end -for i = 1:length(params.integral.minimizing) - costTerm = params.integral.minimizing{i}; - if costTerm.isEnabled - switch costTerm.type - case "joint_jerk" - integrand = cat(2, integrand, ... - calcMinimizingJointJerkIntegrand(values.controlJerks, ... - params, costTerm.coordinate)); - otherwise - throw(MException('', ['Cost term type ' costTerm.type ... - ' does not exist for this tool.'])) - end - end -end -integrand = scaleToBounds(integrand, params.maxIntegral, params.minIntegral); +function integrand = calcDesignOptimizationIntegrand(values, ... + modeledValues, auxdata) +[costTermCalculations, allowedTypes] = ... + generateCostTermStruct("continuous", "DesignOptimization"); +integrand = calcTreatmentOptimizationCost( ... + costTermCalculations, allowedTypes, values, modeledValues, auxdata); +integrand = integrand ./ (auxdata.maxIntegral - auxdata.minIntegral); integrand = integrand .^ 2; end \ No newline at end of file diff --git a/src/DesignOptimization/calcDesignOptimizationObjective.m b/src/DesignOptimization/calcDesignOptimizationObjective.m index 5856be4c7..55d0c8883 100644 --- a/src/DesignOptimization/calcDesignOptimizationObjective.m +++ b/src/DesignOptimization/calcDesignOptimizationObjective.m @@ -25,8 +25,12 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function objective = calcDesignOptimizationObjective(discrete, continuous) +function objective = calcDesignOptimizationObjective(discrete, ... + continuous, finalTime, inputs) continuousObjective = sum(continuous) / length(continuous); +if isfield(inputs, "finalTimeRange") + continuousObjective = continuousObjective / finalTime; +end discreteObjective = sum(discrete) / length(discrete); if isnan(discreteObjective); discreteObjective = 0; end objective = continuousObjective + discreteObjective; diff --git a/src/DesignOptimization/computeDesignOptimizationContinuousFunction.m b/src/DesignOptimization/computeDesignOptimizationContinuousFunction.m index 808e1b6d7..301a6643d 100644 --- a/src/DesignOptimization/computeDesignOptimizationContinuousFunction.m +++ b/src/DesignOptimization/computeDesignOptimizationContinuousFunction.m @@ -1,7 +1,7 @@ % This function is part of the NMSM Pipeline, see file for full license. % % () -> () -% +% % ----------------------------------------------------------------------- % % The NMSM Pipeline is a toolkit for model personalization and treatment % @@ -25,13 +25,20 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function phaseout = computeDesignOptimizationContinuousFunction(inputs) +function modeledValues = computeDesignOptimizationContinuousFunction(inputs) values = getDesignOptimizationValueStruct(inputs.phase, inputs.auxdata); -phaseout = calcTorqueBasedModeledValues(values, inputs.auxdata); -phaseout = calcSynergyBasedModeledValues(values, inputs.auxdata, phaseout); -phaseout.dynamics = calcDesignOptimizationDynamicsConstraint(values, inputs.auxdata); -phaseout.path = calcDesignOptimizationPathConstraint(values, phaseout, inputs.auxdata); -phaseout.integrand = calcDesignOptimizationIntegrand(values, inputs.auxdata, ... - phaseout); -end \ No newline at end of file +inputs = updateSystemFromUserDefinedFunctions(inputs, values); +modeledValues = calcTorqueBasedModeledValues(values, inputs.auxdata); +modeledValues = calcSynergyBasedModeledValues(values, inputs.auxdata, ... + modeledValues); +modeledValues.dynamics = calcDesignOptimizationDynamicsConstraint(values, ... + inputs.auxdata); +if ~isempty(inputs.auxdata.path) + modeledValues.path = calcDesignOptimizationPathConstraint(values, ... + modeledValues, inputs.auxdata); +end +modeledValues.integrand = calcDesignOptimizationIntegrand(values, ... + modeledValues, inputs.auxdata); +end + diff --git a/src/DesignOptimization/computeDesignOptimizationEndpointFunction.m b/src/DesignOptimization/computeDesignOptimizationEndpointFunction.m index f8c28e07a..1fcf3e286 100644 --- a/src/DesignOptimization/computeDesignOptimizationEndpointFunction.m +++ b/src/DesignOptimization/computeDesignOptimizationEndpointFunction.m @@ -1,7 +1,7 @@ % This function is part of the NMSM Pipeline, see file for full license. % % () -> () -% +% % ----------------------------------------------------------------------- % % The NMSM Pipeline is a toolkit for model personalization and treatment % @@ -11,7 +11,7 @@ % National Institutes of Health (R01 EB030520). % % % % Copyright (c) 2021 Rice University and the Authors % -% Author(s): Marleny Vega % +% Author(s): Marleny Vega, Claire V. Hammond % % % % Licensed under the Apache License, Version 2.0 (the "License"); % % you may not use this file except in compliance with the License. % @@ -31,14 +31,35 @@ inputs.phase.time = [inputs.phase.initialtime; inputs.phase.finaltime]; inputs.phase.control = ones(size(inputs.phase.time,1), ... length(inputs.auxdata.minControl)); -values = getDesignOptimizationValueStruct(inputs.phase, inputs.auxdata); +phase = inputs.phase; +if isfield(inputs, "parameter") + phase.parameter = inputs.parameter; +end +values = getDesignOptimizationValueStruct(phase, inputs.auxdata); +inputs = updateSystemFromUserDefinedFunctions(inputs, values); modeledValues = calcTorqueBasedModeledValues(values, inputs.auxdata); if ~isempty(inputs.auxdata.terminal) -output.eventgroup.event = calcDesignOptimizationTerminalConstraint( ... - values, modeledValues, inputs.auxdata); + output.eventgroup.event = calcDesignOptimizationTerminalConstraint( ... + values, modeledValues, inputs.auxdata); end -discrete = calcDesignOptimizationDiscreteObjective(values, inputs.auxdata); +discrete = calcDesignOptimizationDiscreteObjective(values, ... + modeledValues, inputs.auxdata); +% discrete = computeStaticParameterCost(inputs); output.objective = calcDesignOptimizationObjective(discrete, ... - inputs.phase.integral); + inputs.phase.integral, values.time(end), inputs.auxdata); +end + +function cost = computeStaticParameterCost(inputs) +costTerms = inputs.auxdata.costTerms; +cost = 0; +for i = 1:length(costTerms) + costTerm = costTerms{i}; + if strcmp(costTerm.type, "user_defined") && ... + strcmp(costTerm.cost_term_type, "discrete") + func = str2func(costTerm.function_name); + cost = cost + func(inputs); + end +end end + diff --git a/src/DesignOptimization/computeDesignOptimizationMainFunction.m b/src/DesignOptimization/computeDesignOptimizationMainFunction.m index 210c97f9d..990a459f9 100644 --- a/src/DesignOptimization/computeDesignOptimizationMainFunction.m +++ b/src/DesignOptimization/computeDesignOptimizationMainFunction.m @@ -26,8 +26,9 @@ % ----------------------------------------------------------------------- % function output = computeDesignOptimizationMainFunction(inputs, params) -bounds = setupProblemBounds(inputs); guess = setupCommonOptimalControlInitialGuess(inputs); +bounds = setupProblemBounds(inputs, guess); +guess = addUserDefinedTermsToGuess(guess, inputs); setup = setupCommonOptimalControlSolverSettings(inputs, ... bounds, guess, params, ... @computeDesignOptimizationContinuousFunction, ... @@ -35,11 +36,12 @@ solution = gpops2(setup); solution = solution.result.solution; solution.auxdata = inputs; +solution.phase.parameter = [solution.parameter]; output = computeDesignOptimizationContinuousFunction(solution); output.solution = solution; end -function bounds = setupProblemBounds(inputs) +function bounds = setupProblemBounds(inputs, guess) bounds = setupCommonOptimalControlBounds(inputs); % setup parameter bounds if strcmp(inputs.controllerType, 'synergy_driven') @@ -48,4 +50,36 @@ bounds.parameter.upper = 0.5 * ones(1, length(inputs.minParameter)); end end +for i = 1:length(inputs.userDefinedVariables) + variable = inputs.userDefinedVariables{i}; + if ~isfield(bounds, "parameter") || ... + ~isfield(bounds.parameter, "lower") + bounds.parameter.lower = [-0.5]; + bounds.parameter.upper = [0.5]; + else + bounds.parameter.lower = [bounds.parameter.lower, ... + -0.5]; + bounds.parameter.upper = [bounds.parameter.upper, ... + 0.5]; + end +end +if isfield(inputs, "finalTimeRange") + bounds.phase.finaltime.lower = guess.phase.time(end) - (0.5 - guess.phase.time(end)); + bounds.phase.finaltime.upper = 0.5; +end +end + +function guess = addUserDefinedTermsToGuess(guess, inputs) +for i = 1:length(inputs.userDefinedVariables) + variable = inputs.userDefinedVariables{i}; + if ~isfield(guess, "phase") || ... + ~isfield(guess.phase, "parameter") + guess.parameter = []; + end + guess.parameter = [guess.parameter, ... + scaleToBounds( ... + variable.initial_values, ... + variable.upper_bounds, ... + variable.lower_bounds)]; end +end \ No newline at end of file diff --git a/src/DesignOptimization/getDesignOptimizationValueStruct.m b/src/DesignOptimization/getDesignOptimizationValueStruct.m index 994caa750..783581645 100644 --- a/src/DesignOptimization/getDesignOptimizationValueStruct.m +++ b/src/DesignOptimization/getDesignOptimizationValueStruct.m @@ -1,7 +1,7 @@ % This function is part of the NMSM Pipeline, see file for full license. % % () -> () -% +% % ----------------------------------------------------------------------- % % The NMSM Pipeline is a toolkit for model personalization and treatment % @@ -11,7 +11,7 @@ % National Institutes of Health (R01 EB030520). % % % % Copyright (c) 2021 Rice University and the Authors % -% Author(s): Marleny Vega % +% Author(s): Marleny Vega, Claire V. Hammond % % % % Licensed under the Apache License, Version 2.0 (the "License"); % % you may not use this file except in compliance with the License. % @@ -26,19 +26,9 @@ % ----------------------------------------------------------------------- % function values = getDesignOptimizationValueStruct(inputs, params) - -values.time = scaleToOriginal(inputs.time, params.maxTime, ... - params.minTime); -state = scaleToOriginal(inputs.state, ones(size(inputs.state, 1), 1) .* ... - params.maxState, ones(size(inputs.state, 1), 1) .* params.minState); -control = scaleToOriginal(inputs.control, ones(size(inputs.control, 1), 1) .* ... - params.maxControl, ones(size(inputs.control, 1), 1) .* params.minControl); -values.statePositions = getCorrectStates(state, 1, params.numCoordinates); -values.stateVelocities = getCorrectStates(state, 2, params.numCoordinates); -values.stateAccelerations = getCorrectStates(state, 3, params.numCoordinates); -values.controlJerks = control(:, 1 : params.numCoordinates); -if strcmp(params.controllerType, 'synergy_driven') - if params.optimizeSynergyVectors +values = getTreatmentOptimizationValueStruct(inputs, params); +if strcmp(params.controllerType, 'synergy_driven') + if params.optimizeSynergyVectors values.synergyWeights = scaleToOriginal(inputs.parameter(1,:), ... params.maxParameter, params.minParameter); values.synergyWeights = getSynergyWeightsFromGroups(... @@ -47,10 +37,20 @@ values.synergyWeights = getSynergyWeightsFromGroups(... params.synergyWeightsGuess, params); end - values.controlSynergyActivations = control(:, params.numCoordinates + 1 : ... - params.numCoordinates + params.numSynergies); -else - values.controlTorques = control(:, params.numCoordinates + 1 : ... - params.numCoordinates + params.numTorqueControls); + if params.splineJointMoments.dim > 1 + values.controlSynergyActivations = ... + fnval(params.splineSynergyActivations, values.time)'; + else + values.controlSynergyActivations = ... + fnval(params.splineSynergyActivations, values.time); + end +end +if isfield(params, 'userDefinedVariables') + for i = 1:length(params.userDefinedVariables) + values.(params.userDefinedVariables{i}.type) = scaleToOriginal( ... + inputs.parameter(i, 1), ... + params.userDefinedVariables{i}.upper_bounds, ... + params.userDefinedVariables{i}.lower_bounds); + end +end end -end \ No newline at end of file diff --git a/src/DesignOptimization/parseDesignOptimizationSettingsTree.m b/src/DesignOptimization/parseDesignOptimizationSettingsTree.m index e9a900fc6..84cce51f7 100644 --- a/src/DesignOptimization/parseDesignOptimizationSettingsTree.m +++ b/src/DesignOptimization/parseDesignOptimizationSettingsTree.m @@ -40,6 +40,14 @@ inputs.synergyWeights = parseTreatmentOptimizationStandard(... {getTextFromField(getFieldByName(tree, 'synergy_vectors_file'))}); end +inputs.systemFns = parseSpaceSeparatedList(tree, "model_functions"); +parameterTree = getFieldByNameOrError(tree, "RCNLParameterTermSet"); +if isstruct(parameterTree) && isfield(parameterTree, "RCNLParameterTerm") + inputs.userDefinedVariables = parseRcnlCostTermSet( ... + parameterTree.RCNLParameterTerm); +else + inputs.userDefinedVariables = {}; +end end function inputs = getDesignVariableBounds(tree, inputs) @@ -79,7 +87,13 @@ if(isstruct(maxControlTorques)) inputs.maxControlTorquesMultiple = getDoubleFromField(maxControlTorques); end +finalTimeRange = getFieldByName(designVariableTree, ... + 'final_time_range'); +if(isstruct(finalTimeRange)) + inputs.finalTimeRange = getDoubleFromField(finalTimeRange); +end end +inputs.toolName = "DesignOptimization"; end function params = getParams(tree) diff --git a/src/core/osimx/addGcpSpring.m b/src/DesignOptimization/updateSystemFromUserDefinedFunctions.m similarity index 54% rename from src/core/osimx/addGcpSpring.m rename to src/DesignOptimization/updateSystemFromUserDefinedFunctions.m index 188de6ce9..cdd467d7e 100644 --- a/src/core/osimx/addGcpSpring.m +++ b/src/DesignOptimization/updateSystemFromUserDefinedFunctions.m @@ -1,10 +1,7 @@ % This function is part of the NMSM Pipeline, see file for full license. % -% This function adds the information about a spring to the -% in a struct made fom buildGcpOsimxTemplate +% () -> () % -% (struct, Model, number, 1D array of number) -> (struct) -% Adds a spring to the contact surface of an osimx file % ----------------------------------------------------------------------- % % The NMSM Pipeline is a toolkit for model personalization and treatment % @@ -28,24 +25,9 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function contactSurface = addGcpSpring(contactSurface, model, ... - markerNumber, springConstant) -markerName = "spring_marker_" + markerNumber; -springMarker = model.getMarkerSet.get(markerName); -contactSurface.GCPSpringSet.objects.GCPSpring{markerNumber}.Attributes.name = ... - convertStringsToChars(markerName); -contactSurface.GCPSpringSet.objects.GCPSpring{markerNumber}.parent_body.Comment = ... - 'The body that the spring is attached to'; -contactSurface.GCPSpringSet.objects.GCPSpring{markerNumber}.parent_body.Text = ... - getMarkerBodyName(model, markerName); -location = springMarker.get_location(); -contactSurface.GCPSpringSet.objects.GCPSpring{markerNumber}.location.Comment = ... - 'The location of the spring in the body it is attached to'; -contactSurface.GCPSpringSet.objects.GCPSpring{markerNumber}.location.Text = ... - num2str([location.get(0) location.get(1) location.get(2)], 15); -contactSurface.GCPSpringSet.objects.GCPSpring{markerNumber}.spring_constant.Comment = ... - 'The modeled spring constant for the spring'; -contactSurface.GCPSpringSet.objects.GCPSpring{markerNumber}.spring_constant.Text = ... - num2str(springConstant, 15); +function inputs = updateSystemFromUserDefinedFunctions(inputs, values) +for i = 1:length(inputs.auxdata.systemFns) + func = str2func(inputs.auxdata.systemFns(i)); + inputs = func(inputs, values); end - +end \ No newline at end of file diff --git a/src/GroundContactPersonalization/GroundContactPersonalization.m b/src/GroundContactPersonalization/GroundContactPersonalization.m index 8b49a7ed6..0bae13e0c 100644 --- a/src/GroundContactPersonalization/GroundContactPersonalization.m +++ b/src/GroundContactPersonalization/GroundContactPersonalization.m @@ -28,7 +28,7 @@ % ----------------------------------------------------------------------- % function results = GroundContactPersonalization(inputs, params) -inputs = prepareGroundContactPersonalizationInputs(inputs, params); +inputs = prepareGroundContactPersonalizationInputs(inputs); % Optionally initializes the resting spring length. if params.restingSpringLengthInitialization inputs = initializeRestingSpringLength(inputs); diff --git a/src/GroundContactPersonalization/GroundContactPersonalizationTool.m b/src/GroundContactPersonalization/GroundContactPersonalizationTool.m index 6c17d39c0..f9b90bc65 100644 --- a/src/GroundContactPersonalization/GroundContactPersonalizationTool.m +++ b/src/GroundContactPersonalization/GroundContactPersonalizationTool.m @@ -34,6 +34,7 @@ function GroundContactPersonalizationTool(settingsFileName) [inputs, params, resultsDirectory] = ... parseGroundContactPersonalizationSettingsTree(settingsTree); results = GroundContactPersonalization(inputs, params); -saveGroundContactPersonalizationResults(results, resultsDirectory); +saveGroundContactPersonalizationResults(results, params, ... + resultsDirectory, inputs.inputOsimxFile); end diff --git a/src/GroundContactPersonalization/Saving/saveGroundContactPersonalizationResults.m b/src/GroundContactPersonalization/Saving/saveGroundContactPersonalizationResults.m index 1b66c5451..b7007b3a8 100644 --- a/src/GroundContactPersonalization/Saving/saveGroundContactPersonalizationResults.m +++ b/src/GroundContactPersonalization/Saving/saveGroundContactPersonalizationResults.m @@ -29,7 +29,7 @@ % ----------------------------------------------------------------------- % function saveGroundContactPersonalizationResults(inputs, params, ... - resultsDirectory) + resultsDirectory, osimxFileName) [~, name, ~] = fileparts(inputs.bodyModel); if ~exist(resultsDirectory, "dir") mkdir(resultsDirectory); @@ -39,7 +39,7 @@ function saveGroundContactPersonalizationResults(inputs, params, ... writeReplacedExperimentalGroundReactionsToSto(inputs, ... resultsDirectory, name); writeOptimizedGroundReactionsToSto(inputs, params, resultsDirectory, name); -writeGroundContactPersonalizationOsimxFile(inputs,... - fullfile(resultsDirectory, strcat(name, "_groundContactModel.xml"))); +writeGroundContactPersonalizationOsimxFile(inputs, resultsDirectory, ... + osimxFileName); end diff --git a/src/GroundContactPersonalization/Saving/writeExperimentalFootKinematicsToSto.m b/src/GroundContactPersonalization/Saving/writeExperimentalFootKinematicsToSto.m index 5c5fa160c..310be26f9 100644 --- a/src/GroundContactPersonalization/Saving/writeExperimentalFootKinematicsToSto.m +++ b/src/GroundContactPersonalization/Saving/writeExperimentalFootKinematicsToSto.m @@ -29,9 +29,10 @@ function writeExperimentalFootKinematicsToSto(inputs, resultsDirectory, ... modelName) -columnLabels = ["Toe.Angle", "Y.Rotation", "X.Rotation", "Z.Rotation", ... - "X.Translation", "Y.Translation", "Z.Translation"]; for foot = 1:length(inputs.surfaces) + columnLabels = [convertCharsToStrings(inputs.surfaces{foot} ... + .toesCoordinateName), "Rotation1", "Rotation2", "Rotation3", ... + "Translation1", "Translation2", "Translation3"]; writeToSto( ... columnLabels, ... inputs.surfaces{foot}.time, ... diff --git a/src/GroundContactPersonalization/Saving/writeGroundContactPersonalizationOsimxFile.m b/src/GroundContactPersonalization/Saving/writeGroundContactPersonalizationOsimxFile.m index 359b03f19..86c9db2b3 100644 --- a/src/GroundContactPersonalization/Saving/writeGroundContactPersonalizationOsimxFile.m +++ b/src/GroundContactPersonalization/Saving/writeGroundContactPersonalizationOsimxFile.m @@ -28,21 +28,70 @@ % ----------------------------------------------------------------------- % function writeGroundContactPersonalizationOsimxFile(inputs, ... - groundContactModelFileName) - -bodyModel = Model(inputs.bodyModel); -osimx = buildGcpOsimxTemplate(... - replace(bodyModel.getName().toCharArray',".","_dot_"), ... - inputs.bodyModel, ... - inputs.restingSpringLength, ... - inputs.dynamicFrictionCoefficient, ... - inputs.dampingFactor ... - ); - -for surface = 1:length(inputs.surfaces) - osimx = addGcpContactSurface(osimx,inputs.surfaces{surface}, ... - inputs.springConstants); + resultsDirectory, osimxFileName) +modelFileName = inputs.bodyModel; +model = Model(modelFileName); + +if isfile(osimxFileName) + osimx = parseOsimxFile(inputs.inputOsimxFile); + [~, name, ~] = fileparts(inputs.inputOsimxFile); + outfile = fullfile(resultsDirectory, strcat(name, "_gcp.xml")); +else + osimx = buildGcpOsimxTemplate(... + replace(model.getName().toCharArray',".","_dot_"), modelFileName); + [~, name, ~] = fileparts(modelFileName); + outfile = fullfile(resultsDirectory, strcat(name, "_gcp.xml")); +end +osimx.modelName = name; +osimx.model = modelFileName; + +osimx = addGcpContactSurfaces(osimx, inputs); + +writeOsimxFile(buildOsimxFromOsimxStruct(osimx), outfile); +end + +function osimx = addGcpContactSurfaces(osimx, inputs) +if ~isfield(osimx, 'groundContact') + osimx.groundContact.contactSurface = {}; +end +for foot = 1:length(inputs.surfaces) + newSurface.isLeftFoot = inputs.surfaces{foot}.isLeftFoot; + newSurface.beltSpeed = inputs.surfaces{foot}.beltSpeed; + newSurface.forceColumns = string(inputs.surfaces{foot}.forceColumns)'; + newSurface.momentColumns = string(inputs.surfaces{foot}.momentColumns)'; + newSurface.electricalCenterColumns = string(inputs.surfaces{foot} ... + .electricalCenterColumns)'; + newSurface.toesCoordinateName = inputs.surfaces{foot} ... + .toesCoordinateName; + newSurface.toesJointName = inputs.surfaces{foot}.toesJointName; + newSurface.toeMarker = inputs.surfaces{foot}.markerNames.toe; + newSurface.medialMarker = inputs.surfaces{foot}.markerNames.medial; + newSurface.lateralMarker = inputs.surfaces{foot}.markerNames.lateral; + newSurface.heelMarker = inputs.surfaces{foot}.markerNames.heel; + newSurface.midfootSuperiorMarker = inputs.surfaces{foot} ... + .markerNames.midfootSuperior; + newSurface.restingSpringLength = inputs.restingSpringLength; + newSurface.dynamicFrictionCoefficient = inputs ... + .dynamicFrictionCoefficient; + newSurface.viscousFrictionCoefficient = inputs ... + .viscousFrictionCoefficient; + newSurface.dampingFactor = inputs.dampingFactor; + newSurface.latchingVelocity = inputs.latchingVelocity; + + for i = 1:length(inputs.springConstants) + newSurface.springs{i} = addGcpSpring(inputs, foot, i); + end + + index = 1 + length(osimx.groundContact.contactSurface); + osimx.groundContact.contactSurface{index} = newSurface; +end end -writeOsimxFile(osimx, groundContactModelFileName); +function spring = addGcpSpring(inputs, foot, springNumber) + spring.name = "spring_marker_" + springNumber; + model = Model("footModel_" + foot + ".osim"); + springMarker = model.getMarkerSet.get(spring.name); + spring.parentBody = getMarkerBodyName(model, spring.name); + spring.location = Vec3ToArray(springMarker.get_location()); + spring.springConstant = inputs.springConstants(springNumber); end diff --git a/src/GroundContactPersonalization/Saving/writeOptimizedFootKinematicsToSto.m b/src/GroundContactPersonalization/Saving/writeOptimizedFootKinematicsToSto.m index 58d0489cb..18a1fd21f 100644 --- a/src/GroundContactPersonalization/Saving/writeOptimizedFootKinematicsToSto.m +++ b/src/GroundContactPersonalization/Saving/writeOptimizedFootKinematicsToSto.m @@ -29,9 +29,10 @@ function writeOptimizedFootKinematicsToSto(inputs, resultsDirectory, ... modelName) -columnLabels = ["Toe.Angle", "Y.Rotation", "X.Rotation", "Z.Rotation", ... - "X.Translation", "Y.Translation", "Z.Translation"]; for foot = 1:length(inputs.surfaces) + columnLabels = [convertCharsToStrings(inputs.surfaces{foot} ... + .toesCoordinateName), "Rotation1", "Rotation2", "Rotation3", ... + "Translation1", "Translation2", "Translation3"]; [modeledJointPositions, ~] = calcGCPJointKinematics( ... inputs.surfaces{foot}.experimentalJointPositions, ... inputs.surfaces{foot}.jointKinematicsBSplines, ... diff --git a/src/GroundContactPersonalization/parseGroundContactPersonalizationSettingsTree.m b/src/GroundContactPersonalization/parseGroundContactPersonalizationSettingsTree.m index ac74ffaa0..562c74882 100644 --- a/src/GroundContactPersonalization/parseGroundContactPersonalizationSettingsTree.m +++ b/src/GroundContactPersonalization/parseGroundContactPersonalizationSettingsTree.m @@ -42,6 +42,8 @@ import org.opensim.modeling.* inputDirectory = getTextFromField(getFieldByNameOrAlternate(tree, ... 'input_directory', pwd)); +inputs.inputOsimxFile = getTextFromField(getFieldByNameOrAlternate( ... + tree, 'input_osimx_file', '')); inputs.bodyModel = getFieldByNameOrError(tree, 'input_model_file').Text; motionFile = getFieldByNameOrError(tree, 'input_motion_file').Text; grfFile = getFieldByNameOrError(tree, 'input_grf_file').Text; @@ -51,11 +53,11 @@ getFieldByNameOrAlternate(tree, 'latching_velocity', '0.05'))); if(~isempty(inputDirectory)) try - bodyModel = Model(fullfile(inputDirectory, inputs.bodyModel)); + bodyModel = Model(inputs.bodyModel); inputs.motionFileName = fullfile(inputDirectory, motionFile); inputs.grfFileName = fullfile(inputDirectory, grfFile); catch - bodyModel = Model(fullfile(pwd, inputDirectory, inputs.bodyModel)); + bodyModel = Model(pwd, inputs.bodyModel); inputs.motionFileName = fullfile(pwd, inputDirectory, motionFile); inputs.grfFileName = fullfile(pwd, inputDirectory, grfFile); inputDirectory = fullfile(pwd, inputDirectory); @@ -88,8 +90,7 @@ output{counter} = getMotionTime(inputs.bodyModel, ... inputs.motionFileName, output{counter}); verifyTime(output{counter}.grfTime, output{counter}.time); - tempFields = {'forceColumns', 'momentColumns', ... - 'electricalCenterColumns', 'grfTime', 'startTime', 'endTime'}; + tempFields = {'grfTime', 'startTime', 'endTime'}; output{counter} = rmfield(output{counter}, tempFields); counter = counter + 1; end diff --git a/src/JointModelPersonalization/KinematicCalibration/computeInnerOptimizationHeuristic.m b/src/JointModelPersonalization/KinematicCalibration/computeInnerOptimizationHeuristic.m index 2863aef06..ec194aac5 100644 --- a/src/JointModelPersonalization/KinematicCalibration/computeInnerOptimizationHeuristic.m +++ b/src/JointModelPersonalization/KinematicCalibration/computeInnerOptimizationHeuristic.m @@ -37,6 +37,7 @@ error = computeInverseKinematicsSquaredError(model, trialIKSolver, ... markersReference, params); trialIKSolver = libpointer; -error = error / params.desiredError; +desiredError = valueOrAlternate(params, "desiredError", 0.01); +error = error / desiredError; end diff --git a/src/MuscleTendonLengthInitialization/updateNcpInitialGuess.m b/src/MuscleTendonLengthInitialization/updateNcpInitialGuess.m index 0452dc1a4..44bedc255 100644 --- a/src/MuscleTendonLengthInitialization/updateNcpInitialGuess.m +++ b/src/MuscleTendonLengthInitialization/updateNcpInitialGuess.m @@ -2,8 +2,12 @@ precalInputs, optimizedInitialGuess) values = makeMuscleTendonLengthInitializationValuesAsStruct( ... optimizedInitialGuess, precalInputs); -nonMtpEntries = ismember(ncpInputs.muscleTendonColumnNames, ... - ncpInputs.mtpActivationsColumnNames); +if isfield(ncpInputs, 'mtpActivationsColumnNames') + nonMtpEntries = ~ismember(ncpInputs.muscleTendonColumnNames, ... + ncpInputs.mtpActivationsColumnNames); +else + nonMtpEntries = ones(1, length(ncpInputs.muscleTendonColumnNames)); +end ncpInputs.optimalFiberLength(nonMtpEntries) = ... precalInputs.optimalFiberLength(nonMtpEntries) ... .* values.optimalFiberLengthScaleFactors(nonMtpEntries); diff --git a/src/MuscleTendonPersonalization/MuscleTendonPersonalizationTool.m b/src/MuscleTendonPersonalization/MuscleTendonPersonalizationTool.m index 9f3f41186..a17db3f4f 100644 --- a/src/MuscleTendonPersonalization/MuscleTendonPersonalizationTool.m +++ b/src/MuscleTendonPersonalization/MuscleTendonPersonalizationTool.m @@ -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); @@ -48,7 +50,11 @@ function MuscleTendonPersonalizationTool(settingsFileName) % reportMuscleTendonPersonalizationResults(optimizedParams, inputs); % end finalValues = makeMtpValuesAsStruct([], optimizedParams, zeros(1, 7)); +if precalInputs.optimizeIsometricMaxForce + finalValues.maxIsometricForce = inputs.maxIsometricForce; +end results = calcMtpSynXModeledValues(finalValues, inputs, params); + results.time = inputs.emgTime(:, inputs.numPaddingFrames + 1 : ... end - inputs.numPaddingFrames); saveMuscleTendonPersonalizationResults(inputs.model, ... diff --git a/src/MuscleTendonPersonalization/Optimization/CostTerms/calcMuscleExcitationPenaltyCost.m b/src/MuscleTendonPersonalization/Optimization/CostTerms/calcMuscleExcitationPenaltyCost.m index 5de8a3e2f..505251aee 100644 --- a/src/MuscleTendonPersonalization/Optimization/CostTerms/calcMuscleExcitationPenaltyCost.m +++ b/src/MuscleTendonPersonalization/Optimization/CostTerms/calcMuscleExcitationPenaltyCost.m @@ -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 \ No newline at end of file diff --git a/src/MuscleTendonPersonalization/Optimization/calcMtpCost.m b/src/MuscleTendonPersonalization/Optimization/calcMtpCost.m index 575b4c9b1..7ec645d8d 100644 --- a/src/MuscleTendonPersonalization/Optimization/calcMtpCost.m +++ b/src/MuscleTendonPersonalization/Optimization/calcMtpCost.m @@ -73,7 +73,7 @@ synxModeledValues, experimentalData, costTerm); case "residual_muscle_activation" cost = calcResidualMuscleActivationCost( ... - synxModeledValues, modeledValues, experimentalData, params); + synxModeledValues, modeledValues, experimentalData, costTerm); case "muscle_excitation_penalty" cost = calcMuscleExcitationPenaltyCost( ... synxModeledValues, experimentalData, costTerm); diff --git a/src/MuscleTendonPersonalization/SynergyExtrapolation/calcMuscleExcitationsSynX.m b/src/MuscleTendonPersonalization/SynergyExtrapolation/calcMuscleExcitationsSynX.m index e1f15c59c..eae6b2404 100644 --- a/src/MuscleTendonPersonalization/SynergyExtrapolation/calcMuscleExcitationsSynX.m +++ b/src/MuscleTendonPersonalization/SynergyExtrapolation/calcMuscleExcitationsSynX.m @@ -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 @@ -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 @@ -56,7 +56,7 @@ function [muscleExcitations, muscleExcitationsNoTDelay] = ... calcMuscleExcitationsSynX(experimentalData, timeDelay, ... - emgScalingFactor, synergyExtrapolationVariables, params) + emgScalingFactor, synergyExtrapolationVariables, params) experimentalData.emgDataExpanded = round(experimentalData.emgDataExpanded, 4); @@ -79,11 +79,12 @@ % 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, ... emgData, extrapolationCommands, extrapolationWeights) @@ -92,35 +93,36 @@ 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 + function residualExcitations = getResidualMuscleExcitation(params, ... residualCommands, residualWeights) @@ -144,54 +146,58 @@ end concatResidualExcitations = []; for i = 1:size(params.residualCategorizationOfTrials, 2) - concatResidualExcitations = [concatResidualExcitations; + concatResidualExcitations = [concatResidualExcitations; residualExcitations{i}]; end residualExcitations = concatResidualExcitations; end + 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 + function emgData = distributeResidualExcitations(emgData, ... currentEmgChannelGroups, residualExcitations) %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)); emgData = emgData + permute(residualExcitationsExpanded, [2 3 1]); end + function emgSplines = createEmgSignals(emgData, emgTime, timeDelay) emgSplines = cell(size(emgData, 1), size(emgData, 2)); 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 end + function emgData = evaluateEmgSplines(emgTime, emgSplines, timeDelay) if size(timeDelay, 2) == 1 diff --git a/src/MuscleTendonPersonalization/SynergyExtrapolation/getLinearInequalityConstraints.m b/src/MuscleTendonPersonalization/SynergyExtrapolation/getLinearInequalityConstraints.m index 817d40bfc..67f50956e 100644 --- a/src/MuscleTendonPersonalization/SynergyExtrapolation/getLinearInequalityConstraints.m +++ b/src/MuscleTendonPersonalization/SynergyExtrapolation/getLinearInequalityConstraints.m @@ -68,7 +68,45 @@ 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') +% 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 + end + + + + + function [aMatrix, aMatrixSynergy] = allocateSynxMatrixAMemory(emgData, ... numberOfSynergies, missingEmgChannelGroups, thirdMatrixDimension, ... matrixFactorizationFactor) @@ -79,6 +117,7 @@ size(missingEmgChannelGroups, 2), (numberOfSynergies + ... matrixFactorizationFactor) * thirdMatrixDimension * numberOfSynergies); end + function aMatrixSynergy = updateHalfMatrixA(emgData, aMatrix,... numberOfSynergies, missingEmgChannelGroups, thirdMatrixDimension, ... matrixFactorizationFactor) @@ -91,6 +130,7 @@ matrixFactorizationFactor) * thirdMatrixDimension) = aMatrix; end end + function aMatrixSynergy = getSynxAMatrix(params, synergyCommands, ... emgData, matrixFactorizationFactor) @@ -118,6 +158,7 @@ params.numberOfSynergies, params.missingEmgChannelGroups, ... length(params.synergyCategorizationOfTrials), matrixFactorizationFactor); end + function aMatrixResidual = getResidualAMatrix(emgData, ... numberOfSynergies, missingEmgChannelGroups, residualCategorization, ... matrixFactorizationFactor) diff --git a/src/MuscleTendonPersonalization/SynergyExtrapolation/getSynergyCommands.m b/src/MuscleTendonPersonalization/SynergyExtrapolation/getSynergyCommands.m index 12aefafe6..48ecdb469 100644 --- a/src/MuscleTendonPersonalization/SynergyExtrapolation/getSynergyCommands.m +++ b/src/MuscleTendonPersonalization/SynergyExtrapolation/getSynergyCommands.m @@ -53,7 +53,7 @@ maxEmgOverAllTrials = max(max(emgData, [], 3), [], 1); normalizedEMG = permute(emgData ./ maxEmgOverAllTrials, [3 2 1]); %--Extract synergy excitations from measured muscle excitations -if strcmpi(matrixFactorizationMethod, 'PCA') + if strcmpi(matrixFactorizationMethod, 'PCA') extrapolationCommands = getPcaCommands(normalizedEMG, numberOfSynergies, ... synergyCategorizationOfTrials); residualCommands = getPcaCommands(normalizedEMG, numberOfSynergies, ... diff --git a/src/MuscleTendonPersonalization/parseMuscleTendonPersonalizationSettingsTree.m b/src/MuscleTendonPersonalization/parseMuscleTendonPersonalizationSettingsTree.m index f568ae441..1b877362e 100644 --- a/src/MuscleTendonPersonalization/parseMuscleTendonPersonalizationSettingsTree.m +++ b/src/MuscleTendonPersonalization/parseMuscleTendonPersonalizationSettingsTree.m @@ -69,6 +69,7 @@ [inputs.fullEmgData, inputs.emgDataColumnNames] = parseMtpStandard(emgDataFileNames); collectedEmgGroupNamesMembers = ismember(inputs.emgDataColumnNames, collectedEmgGroupNames); inputs.emgData = inputs.fullEmgData(:, collectedEmgGroupNamesMembers, :); +inputs.emgDataColumnNames = inputs.emgDataColumnNames(collectedEmgGroupNamesMembers); firstEmgDataExpanded = expandEmgDatas(inputs.model, squeeze(inputs.emgData(1, :, :)), collectedEmgGroupNames, inputs.muscleNames); inputs.emgDataExpanded = zeros(size(inputs.emgData, 1), size(firstEmgDataExpanded, 1), size(firstEmgDataExpanded, 2)); inputs.emgDataExpanded(1, :, :) = firstEmgDataExpanded; diff --git a/src/NeuralControlPersonalization/NeuralControlPersonalizationTool.m b/src/NeuralControlPersonalization/NeuralControlPersonalizationTool.m index 6afb90db6..3b84d2b1c 100644 --- a/src/NeuralControlPersonalization/NeuralControlPersonalizationTool.m +++ b/src/NeuralControlPersonalization/NeuralControlPersonalizationTool.m @@ -47,5 +47,5 @@ function NeuralControlPersonalizationTool(settingsFileName) [synergyWeights, synergyCommands] = normalizeSynergiesByMaximumWeight(... synergyWeights, synergyCommands); saveNeuralControlPersonalizationResults(synergyWeights, ... - synergyCommands, inputs, resultsDirectory); + synergyCommands, inputs, resultsDirectory, precalInputs); end \ No newline at end of file diff --git a/src/NeuralControlPersonalization/Optimization/calcNcpCost.m b/src/NeuralControlPersonalization/Optimization/calcNcpCost.m index bc5ea8e20..c6e7ac074 100644 --- a/src/NeuralControlPersonalization/Optimization/calcNcpCost.m +++ b/src/NeuralControlPersonalization/Optimization/calcNcpCost.m @@ -33,9 +33,13 @@ error = []; % Split activations into subsets ahead of cost computation -[activationsWithMtpData, activationsWithoutMtpData] = ... - makeMtpActivatonSubset(activations, ... - inputs.mtpActivationsColumnNames, inputs.muscleTendonColumnNames); +if isfield(inputs, 'mtpActivationsColumnNames') + [activationsWithMtpData, activationsWithoutMtpData] = ... + makeMtpActivatonSubset(activations, ... + inputs.mtpActivationsColumnNames, inputs.muscleTendonColumnNames); +else + activationsWithoutMtpData = activations; +end for term = 1:length(params.costTerms) costTerm = params.costTerms{term}; if costTerm.isEnabled @@ -51,7 +55,11 @@ rawCost = muscleJointMoments - ... inputs.inverseDynamicsMoments; case "activation_tracking" - rawCost = activationsWithMtpData - inputs.mtpActivations; + if isfield(inputs, 'mtpActivations') + rawCost = activationsWithMtpData - inputs.mtpActivations; + else + rawCost = 0; + end case "activation_minimization" rawCost = reshape(activationsWithoutMtpData, [], 1); case "grouped_activations" diff --git a/src/NeuralControlPersonalization/parseNeuralControlPersonalizationSettingsTree.m b/src/NeuralControlPersonalization/parseNeuralControlPersonalizationSettingsTree.m index 127873d85..6efe6a877 100644 --- a/src/NeuralControlPersonalization/parseNeuralControlPersonalizationSettingsTree.m +++ b/src/NeuralControlPersonalization/parseNeuralControlPersonalizationSettingsTree.m @@ -43,14 +43,22 @@ inputs = parseMtpNcpSharedInputs(tree); inputs.synergyGroups = getSynergyGroups(tree, Model(inputs.model)); inputs = matchMuscleNamesFromCoordinatesAndSynergyGroups(inputs); -inputs = loadMtpData(tree, inputs); inputs = reorderPreprocessedDataByMuscleNames(inputs, inputs.muscleNames); [inputs.maxIsometricForce, inputs.optimalFiberLength, ... inputs.tendonSlackLength, inputs.pennationAngle] = ... getMuscleInputs(inputs, inputs.muscleTendonColumnNames); -[inputs.optimalFiberLengthScaleFactors, ... - inputs.tendonSlackLengthScaleFactors] = getMtpDataInputs( ... - inputs.mtpMuscleData, inputs.muscleTendonColumnNames); +mtpResults = getFieldByName(tree, "mtp_results_directory"); +if isstruct(mtpResults) && ~isempty(mtpResults.Text) + inputs = loadMtpData(tree, inputs); + [inputs.optimalFiberLengthScaleFactors, ... + inputs.tendonSlackLengthScaleFactors, ... + inputs.maxIsometricForce] = getMtpDataInputs(inputs); +else + inputs.optimalFiberLengthScaleFactors = ... + ones(1, length(inputs.muscleTendonColumnNames)); + inputs.tendonSlackLengthScaleFactors = ... + ones(1, length(inputs.muscleTendonColumnNames)); +end end function inputs = loadMtpData(tree, inputs) @@ -59,8 +67,11 @@ [inputs.mtpActivations, inputs.mtpActivationsColumnNames] = ... parseMtpStandard(findFileListFromPrefixList( ... fullfile(mtpResultsDirectory, "muscleActivations"), inputs.prefixes)); -inputs.mtpMuscleData = parseOsimxFile(fullfile(mtpResultsDirectory, ... - "model.osimx")); +osimxFileName = getFieldByName(tree, "input_osimx_file"); +if ~isstruct(osimxFileName) || isempty(osimxFileName.Text) + throw(MException('', 'An input .osimx file is required if using data from MTP.')) +end +inputs.mtpMuscleData = parseOsimxFile(osimxFileName.Text); % Remove activations of muscles from coordinates not included includedSubset = ismember(inputs.mtpActivationsColumnNames, ... inputs.muscleTendonColumnNames); @@ -101,6 +112,11 @@ params.normalizedFiberLengthGroupNames, model); params.costTerms = parseRcnlCostTermSet( ... getFieldByNameOrError(tree, 'RCNLCostTermSet').objects.RCNLCostTerm); +if strcmpi('true', getTextFromField(getFieldByName(tree, ... + 'enforce_bilateral_symmetry'))) + params.costTerms{end+1} = struct('type', 'bilateral_symmetry', ... + 'isEnabled', true, 'maxAllowableError', 1e-6, 'errorCenter', 0); +end params.diffMinChange = str2double(getTextFromField(... getFieldByNameOrAlternate(tree, 'diff_min_change', '1e-6'))); params.stepTolerance = str2double(getTextFromField(... @@ -140,14 +156,31 @@ end function [optimalFiberLengthScaleFactors, ... - tendonSlackLengthScaleFactors] = getMtpDataInputs(mtpData, muscleNames) + tendonSlackLengthScaleFactors, maxIsometricForce] = ... + getMtpDataInputs(inputs) +mtpData = inputs.mtpMuscleData; +muscleNames = inputs.muscleTendonColumnNames; + optimalFiberLengthScaleFactors = zeros(1, length(muscleNames)); tendonSlackLengthScaleFactors = zeros(1, length(muscleNames)); -mtpDataMuscleNames = fieldnames(mtpData); +maxIsometricForce = inputs.maxIsometricForce; +mtpDataMuscleNames = fieldnames(mtpData.muscles); for i = 1 : length(muscleNames) if ismember(muscleNames(i), mtpDataMuscleNames) - optimalFiberLengthScaleFactors(i) = mtpData.(muscleNames(i)).optimalFiberLengthScaleFactor; - tendonSlackLengthScaleFactors(i) = mtpData.(muscleNames(i)).tendonSlackLengthScaleFactor; + currentMuscle = mtpData.muscles.(muscleNames(i)); + if isfield(currentMuscle, 'optimalFiberLength') + optimalFiberLengthScaleFactors(i) = currentMuscle.optimalFiberLength / inputs.optimalFiberLength(i); + else + optimalFiberLengthScaleFactors(i) = 1; + end + if isfield(currentMuscle, 'tendonSlackLength') + tendonSlackLengthScaleFactors(i) = currentMuscle.tendonSlackLength / inputs.tendonSlackLength(i); + else + tendonSlackLengthScaleFactors(i) = 1; + end + if isfield(currentMuscle, 'maxIsometricForce') + maxIsometricForce(i) = currentMuscle.maxIsometricForce; + end else optimalFiberLengthScaleFactors(i) = 1; tendonSlackLengthScaleFactors(i) = 1; diff --git a/src/NeuralControlPersonalization/saveNeuralControlPersonalizationResults.m b/src/NeuralControlPersonalization/saveNeuralControlPersonalizationResults.m index dbe6dd4c3..adf43e70e 100644 --- a/src/NeuralControlPersonalization/saveNeuralControlPersonalizationResults.m +++ b/src/NeuralControlPersonalization/saveNeuralControlPersonalizationResults.m @@ -1,5 +1,5 @@ function saveNeuralControlPersonalizationResults(synergyWeights, ... - synergyCommands, inputs, resultsDirectory) + synergyCommands, inputs, resultsDirectory, precalInputs) if ~exist(resultsDirectory, "dir") mkdir(resultsDirectory); end @@ -23,4 +23,8 @@ function saveNeuralControlPersonalizationResults(synergyWeights, ... ) ... ) end +if isstruct(precalInputs) + writeNeuralControlPersonalizationOsimxFile(inputs, ... + resultsDirectory, precalInputs) +end end diff --git a/src/Preprocessing/preprocessing.m b/src/Preprocessing/preprocessing.m index 3fd5739f6..be5d0c662 100644 --- a/src/Preprocessing/preprocessing.m +++ b/src/Preprocessing/preprocessing.m @@ -13,8 +13,8 @@ % All values required rawEmgFileName = "input_data\Patient3_NormalGait_1pt0_02_Right_01_EMG.mot"; filterOrder = 4; -highPassCutoff = 10; -lowPassCutoff = 50; +highPassCutoff = 40; +lowPassCutoff = 10; processedEmgFileName = "Patient3_NormalGait_1pt0_02_Right_01_processedEmg.sto"; processRawEmgFile(rawEmgFileName, filterOrder, highPassCutoff, ... @@ -39,7 +39,7 @@ ]; % Required: Associated .osim model file -inputSettings.model = "UF_Patient3_correctHeight_JMP_MTPGroups.osim"; +inputSettings.model = "UF_Patient3_correctHeight_JMP_MTPGroups_NCPGroups.osim"; % All values optional: files and directories of data to be split inputSettings.ikFileName = "input_data\Patient3_NormalGait_1pt0_02_Right_01_IK.mot"; diff --git a/src/Preprocessing/splitIntoTrials.m b/src/Preprocessing/splitIntoTrials.m index a45fc5597..2ac1e7494 100644 --- a/src/Preprocessing/splitIntoTrials.m +++ b/src/Preprocessing/splitIntoTrials.m @@ -36,6 +36,7 @@ function splitIntoTrials(timePairs, inputSettings, outputSettings) for i=1:length(filesToSection) delete(filesToSection(i)); end +delete(fullfile(outputDir, emgOutputDir, trialName + ".sto")); moveMAFilesToSeparateDirectories(trialName, outputDir, ... maOutputDir, timePairs) end @@ -173,7 +174,7 @@ function throwCantFindMAFileException(fileName) function moveMAFilesToSeparateDirectories(trialName, outputDir, ... maOutputDir, timePairs) -for i=1:length(timePairs) +for i=1:size(timePairs, 1) mkdir(fullfile(outputDir, maOutputDir, trialName + "_" ... + i)); files = dir(fullfile(outputDir, maOutputDir)); diff --git a/src/TrackingOptimization/TrackingOptimization.m b/src/TrackingOptimization/TrackingOptimization.m index 44aa793a1..46f06dea7 100644 --- a/src/TrackingOptimization/TrackingOptimization.m +++ b/src/TrackingOptimization/TrackingOptimization.m @@ -1,7 +1,7 @@ % This function is part of the NMSM Pipeline, see file for full license. % % () -> () -% +% % ----------------------------------------------------------------------- % % The NMSM Pipeline is a toolkit for model personalization and treatment % @@ -11,7 +11,7 @@ % National Institutes of Health (R01 EB030520). % % % % Copyright (c) 2021 Rice University and the Authors % -% Author(s): Marleny Vega % +% Author(s): Marleny Vega, Claire V. Hammond % % % % Licensed under the Apache License, Version 2.0 (the "License"); % % you may not use this file except in compliance with the License. % @@ -26,18 +26,7 @@ % ----------------------------------------------------------------------- % function [output, inputs] = TrackingOptimization(inputs, params) -pointKinematics(inputs.mexModel); -inverseDynamics(inputs.mexModel); -inputs -inputs = getStateDerivatives(inputs); -inputs = setupGroundContact(inputs); -inputs = getSplines(inputs); -inputs = checkStateGuess(inputs); -inputs = checkControlGuess(inputs); -inputs = checkParameterGuess(inputs); -inputs = getIntegralBounds(inputs); -inputs = getPathConstraintBounds(inputs); -inputs = getTerminalConstraintBounds(inputs); -inputs = getDesignVariableInputBounds(inputs); +inputs = makeTreatmentOptimizationInputs(inputs, params); +initializeMexOrMatlabParallelFunctions(inputs.mexModel); output = computeTrackingOptimizationMainFunction(inputs, params); end diff --git a/src/TrackingOptimization/TrackingOptimizationTool.m b/src/TrackingOptimization/TrackingOptimizationTool.m index 3f33a5a46..d8d9592f1 100644 --- a/src/TrackingOptimization/TrackingOptimizationTool.m +++ b/src/TrackingOptimization/TrackingOptimizationTool.m @@ -33,6 +33,6 @@ function TrackingOptimizationTool(settingsFileName) settingsTree = xml2struct(settingsFileName); [inputs, params] = parseTrackingOptimizationSettingsTree(settingsTree); [outputs, inputs] = TrackingOptimization(inputs, params); -reportTrackingOptimizationResults(outputs, inputs); +reportTreatmentOptimizationResults(outputs, inputs); saveTrackingOptimizationResults(outputs, inputs); -end \ No newline at end of file +end diff --git a/src/TrackingOptimization/calcFootGroundReactions.m b/src/TrackingOptimization/calcFootGroundReactions.m index 60cfe09dc..7d90e3cf2 100644 --- a/src/TrackingOptimization/calcFootGroundReactions.m +++ b/src/TrackingOptimization/calcFootGroundReactions.m @@ -42,6 +42,9 @@ function [forces, moments] = calcGroundReactionForcesAndMoments(markerPositions, ... markerVelocities, springConstants, midfootSuperiorPosition, contactSurface) +markerPositions = reshape(markerPositions, [], 3, size(springConstants, 2)); +markerVelocities = reshape(markerVelocities, [], 3, size(springConstants, 2)); + for i = 1:size(markerPositions, 1) markerKinematics.xPosition = squeeze(markerPositions(i, 1, :))'; markerKinematics.height = squeeze(markerPositions(i, 2, :))'; diff --git a/src/TrackingOptimization/calcSynergyBasedModeledValues.m b/src/TrackingOptimization/calcSynergyBasedModeledValues.m index e69237637..c31902e6d 100644 --- a/src/TrackingOptimization/calcSynergyBasedModeledValues.m +++ b/src/TrackingOptimization/calcSynergyBasedModeledValues.m @@ -1,7 +1,7 @@ % This function is part of the NMSM Pipeline, see file for full license. % % () -> () -% +% % ----------------------------------------------------------------------- % % The NMSM Pipeline is a toolkit for model personalization and treatment % @@ -25,21 +25,23 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function phaseout = calcSynergyBasedModeledValues(values, params, phaseout) -if strcmp(params.controllerType, 'synergy_driven') -[jointAngles, jointVelocities] = getMuscleActuatedDOFs(values, params); -[params.muscleTendonLength, params.momentArms, ... - params.muscleTendonVelocity] = calcSurrogateModel(params, ... - jointAngles, jointVelocities); -[phaseout.normalizedFiberLength, phaseout.normalizedFiberVelocity] = ... - calcNormalizedMuscleFiberLengthsAndVelocities(params, ... - ones(1, params.numMuscles), ones(1, params.numMuscles)); -phaseout.muscleActivations = calcMuscleActivationFromSynergies(values); -muscleJointMoments = calcMuscleJointMoments(params, ... - phaseout.muscleActivations, phaseout.normalizedFiberLength, ... - phaseout.normalizedFiberVelocity); -phaseout.muscleJointMoments = muscleJointMoments(:, params.surrogateModelIndex); -phaseout.muscleJointMoments = phaseout.muscleJointMoments(:, params.dofsActuatedIndex); +function modeledValues = calcSynergyBasedModeledValues(values, params, modeledValues) +if strcmp(params.controllerType, 'synergy_driven') + [jointAngles, jointVelocities] = getMuscleActuatedDOFs(values, params); + [params.muscleTendonLength, params.momentArms, ... + params.muscleTendonVelocity] = calcSurrogateModel(params, ... + jointAngles, jointVelocities); + [modeledValues.normalizedFiberLength, modeledValues.normalizedFiberVelocity] = ... + calcNormalizedMuscleFiberLengthsAndVelocities(params, ... + ones(1, params.numMuscles), ones(1, params.numMuscles)); + modeledValues.muscleActivations = calcMuscleActivationFromSynergies(values); + modeledValues.metabolicCost = calcMetabolicCost(values.time, ... + values.statePositions, modeledValues.muscleActivations, params); + muscleJointMoments = calcMuscleJointMoments(params, ... + modeledValues.muscleActivations, modeledValues.normalizedFiberLength, ... + modeledValues.normalizedFiberVelocity); + modeledValues.muscleJointMoments = muscleJointMoments(:, params.surrogateModelIndex); + modeledValues.muscleJointMoments = modeledValues.muscleJointMoments(:, params.dofsActuatedIndex); end end diff --git a/src/TrackingOptimization/calcTorqueBasedModeledValues.m b/src/TrackingOptimization/calcTorqueBasedModeledValues.m index a4257aace..1c7d9548e 100644 --- a/src/TrackingOptimization/calcTorqueBasedModeledValues.m +++ b/src/TrackingOptimization/calcTorqueBasedModeledValues.m @@ -28,6 +28,7 @@ function phaseout = calcTorqueBasedModeledValues(values, params) appliedLoads = [zeros(length(values.time), params.numTotalMuscles)]; if ~isempty(params.contactSurfaces) + clear pointKinematics [springPositions, springVelocities] = getSpringLocations(values.time, .... values.statePositions, values.stateVelocities, params); phaseout.bodyLocations = getBodyLocations(values.time, .... @@ -41,7 +42,7 @@ end phaseout.inverseDynamicMoments = inverseDynamics(values.time, ... values.statePositions, values.stateVelocities, ... - values.stateAccelerations, params.coordinateNames, appliedLoads); + values.stateAccelerations, params.coordinateNames, appliedLoads, params.mexModel); end function [springPositions, springVelocities] = getSpringLocations(time, .... statePositions, stateVelocities, params) @@ -49,16 +50,16 @@ for i = 1:length(params.contactSurfaces) [springPositions.parent{i}, springVelocities.parent{i}] = ... pointKinematics(time, statePositions, stateVelocities, ... - params.contactSurfaces{i}.parentSpringPointsOnBody', ... + params.contactSurfaces{i}.parentSpringPointsOnBody, ... params.contactSurfaces{i}.parentBody * ones(1, ... size(params.contactSurfaces{i}.parentSpringPointsOnBody, 1)), ... - params.coordinateNames); + params.mexModel, params.coordinateNames); [springPositions.child{i}, springVelocities.child{i}] = ... pointKinematics(time, statePositions, stateVelocities, ... - params.contactSurfaces{i}.childSpringPointsOnBody', ... + params.contactSurfaces{i}.childSpringPointsOnBody, ... params.contactSurfaces{i}.childBody * ones(1, ... size(params.contactSurfaces{i}.childSpringPointsOnBody, 1)), ... - params.coordinateNames); + params.mexModel, params.coordinateNames); end end function bodyLocations = getBodyLocations(time, statePositions, ... @@ -66,15 +67,15 @@ for i = 1:length(params.contactSurfaces) bodyLocations.midfootSuperior{i} = pointKinematics(time, statePositions, ... - stateVelocities, params.contactSurfaces{i}.midfootSuperiorPointOnBody', ... - params.contactSurfaces{i}.midfootSuperiorBody, params.coordinateNames); + stateVelocities, params.contactSurfaces{i}.midfootSuperiorPointOnBody, ... + params.contactSurfaces{i}.midfootSuperiorBody, params.mexModel, params.coordinateNames); bodyLocations.midfootSuperior{i}(:, 2) = 0; bodyLocations.parent{i} = pointKinematics(time, statePositions, ... - stateVelocities, [0 0 0]', params.contactSurfaces{i}.parentBody, ... - params.coordinateNames); + stateVelocities, [0 0 0], params.contactSurfaces{i}.parentBody, ... + params.mexModel, params.coordinateNames); bodyLocations.child{i} = pointKinematics(time, statePositions, ... - stateVelocities, [0 0 0]', params.contactSurfaces{i}.childBody, ... - params.coordinateNames); + stateVelocities, [0 0 0], params.contactSurfaces{i}.childBody, ... + params.mexModel, params.coordinateNames); end end function groundReactionsBody = tranferGroundReactionMoments( ... diff --git a/src/TrackingOptimization/calcTrackingOptimizationIntegrand.m b/src/TrackingOptimization/calcTrackingOptimizationIntegrand.m index 5443eef18..66efa6ece 100644 --- a/src/TrackingOptimization/calcTrackingOptimizationIntegrand.m +++ b/src/TrackingOptimization/calcTrackingOptimizationIntegrand.m @@ -25,58 +25,12 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function integrand = calcTrackingOptimizationIntegrand(values, params, ... - phaseout) -integrand = []; -for i = 1:length(params.integral.tracking) - costTerm = params.integral.tracking{i}; - if costTerm.isEnabled - switch costTerm.type - case "coordinate" - integrand = cat(2, integrand, ... - calcTrackingCoordinateIntegrand(params, ... - values.time, values.statePositions, ... - costTerm.coordinate)); - case "inverse_dynamics_load" - integrand = cat(2, integrand, ... - calcTrackingInverseDynamicLoadsIntegrand(params, ... - values.time, phaseout.inverseDynamicMoments, ... - costTerm.load)); - case "external_force" - integrand = cat(2, integrand, ... - calcTrackingExternalForcesIntegrand(params, ... - phaseout.groundReactionsLab.forces, values.time, ... - costTerm.force)); - case "external_moment" - integrand = cat(2, integrand, ... - calcTrackingExternalMomentsIntegrand(params, ... - phaseout.groundReactionsLab.moments, values.time, ... - costTerm.moment)); - case "muscle_activation" - integrand = cat(2, integrand, ... - calcTrackingMuscleActivationIntegrand( ... - phaseout.muscleActivations, ... - values.time, params, costTerm.muscle)); - otherwise - throw(MException('', ['Cost term type ' costTerm.type ... - ' does not exist for this tool.'])) - end - end -end -for i = 1:length(params.integral.minimizing) - costTerm = params.integral.minimizing{i}; - if costTerm.isEnabled - switch costTerm.type - case "joint_jerk" - integrand = cat(2, integrand, ... - calcMinimizingJointJerkIntegrand(values.controlJerks, ... - params, costTerm.coordinate)); - otherwise - throw(MException('', ['Cost term type ' costTerm.type ... - ' does not exist for this tool.'])) - end - end -end -integrand = integrand ./ (params.maxIntegral - params.minIntegral); +function integrand = calcTrackingOptimizationIntegrand(values, ... + modeledValues, auxdata) +[costTermCalculations, allowedTypes] = ... + generateCostTermStruct("continuous", "TrackingOptimization"); +integrand = calcTreatmentOptimizationCost( ... + costTermCalculations, allowedTypes, values, modeledValues, auxdata); +integrand = integrand ./ (auxdata.maxIntegral - auxdata.minIntegral); integrand = integrand .^ 2; end \ No newline at end of file diff --git a/src/TrackingOptimization/calcTrackingOptimizationPathConstraint.m b/src/TrackingOptimization/calcTrackingOptimizationPathConstraint.m index d724658f1..87bf1163f 100644 --- a/src/TrackingOptimization/calcTrackingOptimizationPathConstraint.m +++ b/src/TrackingOptimization/calcTrackingOptimizationPathConstraint.m @@ -25,7 +25,7 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function path = calcTrackingOptimizationPathConstraint(values, phaseout, ... +function path = calcTrackingOptimizationPathConstraint(values, modeledValues, ... params) path = []; for i = 1:length(params.path) @@ -37,15 +37,15 @@ calcRootSegmentResidualsPathConstraints(... constraintTerm.load, ... params.inverseDynamicMomentLabels, ... - phaseout.inverseDynamicMoments)); + modeledValues.inverseDynamicMoments)); case "muscle_model_moment_consistency" path = cat(2, path, ... calcMuscleActuatedMomentsPathConstraints(params, ... - phaseout, constraintTerm.load)); + modeledValues, constraintTerm.load)); case "torque_model_moment_consistency" path = cat(2, path, ... calcTorqueActuatedMomentsPathConstraints(params, ... - phaseout, values.controlTorques, constraintTerm.load)); + modeledValues, values.controlTorques, constraintTerm.load)); otherwise throw(MException('', ['Constraint term type ' ... constraintTerm.type ' does not exist for this tool.'])) diff --git a/src/TrackingOptimization/calcTrackingOptimizationTerminalConstraint.m b/src/TrackingOptimization/calcTrackingOptimizationTerminalConstraint.m index 3009ff0c9..0ab2bd362 100644 --- a/src/TrackingOptimization/calcTrackingOptimizationTerminalConstraint.m +++ b/src/TrackingOptimization/calcTrackingOptimizationTerminalConstraint.m @@ -57,13 +57,13 @@ modeledValues.inverseDynamicMoments, ... params.inverseDynamicMomentLabels, ... constraintTerm.load)); - case "external_force_periodicity" + case "external_force_tracking_periodicity" event = cat(2, event, ... calcExternalForcesPeriodicity(... modeledValues.groundReactionsLab.forces, ... params.contactSurfaces, ... constraintTerm.force)); - case "external_moment_periodicity" + case "external_moment_tracking_periodicity" event = cat(2, event, ... calcExternalMomentsPeriodicity(... modeledValues.groundReactionsLab.moments, ... diff --git a/src/TrackingOptimization/computeTrackingOptimizationContinuousFunction.m b/src/TrackingOptimization/computeTrackingOptimizationContinuousFunction.m index 8c641b69e..9c34e4b56 100644 --- a/src/TrackingOptimization/computeTrackingOptimizationContinuousFunction.m +++ b/src/TrackingOptimization/computeTrackingOptimizationContinuousFunction.m @@ -25,13 +25,11 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function phaseout = computeTrackingOptimizationContinuousFunction(inputs) - +function modeledValues = computeTrackingOptimizationContinuousFunction(inputs) values = getTrackingOptimizationValueStruct(inputs.phase, inputs.auxdata); -phaseout = calcTorqueBasedModeledValues(values, inputs.auxdata); -phaseout = calcSynergyBasedModeledValues(values, inputs.auxdata, phaseout); -phaseout.dynamics = calcTrackingOptimizationDynamicsConstraint(values, inputs.auxdata); -phaseout.path = calcTrackingOptimizationPathConstraint(values, phaseout, inputs.auxdata); -phaseout.integrand = calcTrackingOptimizationIntegrand(values, inputs.auxdata, ... - phaseout); +modeledValues = calcTorqueBasedModeledValues(values, inputs.auxdata); +modeledValues = calcSynergyBasedModeledValues(values, inputs.auxdata, modeledValues); +modeledValues.dynamics = calcTrackingOptimizationDynamicsConstraint(values, inputs.auxdata); +modeledValues.path = calcTrackingOptimizationPathConstraint(values, modeledValues, inputs.auxdata); +modeledValues.integrand = calcTrackingOptimizationIntegrand(values, modeledValues, inputs.auxdata); end \ No newline at end of file diff --git a/src/TrackingOptimization/computeTrackingOptimizationMainFunction.m b/src/TrackingOptimization/computeTrackingOptimizationMainFunction.m index 031218884..28091e0e7 100644 --- a/src/TrackingOptimization/computeTrackingOptimizationMainFunction.m +++ b/src/TrackingOptimization/computeTrackingOptimizationMainFunction.m @@ -32,7 +32,6 @@ bounds, guess, params, ... @computeTrackingOptimizationContinuousFunction, ... @computeTrackingOptimizationEndpointFunction); - solution = gpops2(setup); solution = solution.result.solution; solution.auxdata = inputs; diff --git a/src/TrackingOptimization/getTrackingOptimizationValueStruct.m b/src/TrackingOptimization/getTrackingOptimizationValueStruct.m index f5aa1e386..80d2cb2f4 100644 --- a/src/TrackingOptimization/getTrackingOptimizationValueStruct.m +++ b/src/TrackingOptimization/getTrackingOptimizationValueStruct.m @@ -1,7 +1,7 @@ % This function is part of the NMSM Pipeline, see file for full license. % % () -> () -% +% % ----------------------------------------------------------------------- % % The NMSM Pipeline is a toolkit for model personalization and treatment % @@ -11,7 +11,7 @@ % National Institutes of Health (R01 EB030520). % % % % Copyright (c) 2021 Rice University and the Authors % -% Author(s): Marleny Vega % +% Author(s): Marleny Vega, Claire V. Hammond % % % % Licensed under the Apache License, Version 2.0 (the "License"); % % you may not use this file except in compliance with the License. % @@ -26,19 +26,9 @@ % ----------------------------------------------------------------------- % function values = getTrackingOptimizationValueStruct(inputs, params) - -values.time = scaleToOriginal(inputs.time, params.maxTime, ... - params.minTime); -state = scaleToOriginal(inputs.state, ones(size(inputs.state, 1), 1) .* ... - params.maxState, ones(size(inputs.state, 1), 1) .* params.minState); -control = scaleToOriginal(inputs.control, ones(size(inputs.control, 1), 1) .* ... - params.maxControl, ones(size(inputs.control, 1), 1) .* params.minControl); -values.statePositions = getCorrectStates(state, 1, params.numCoordinates); -values.stateVelocities = getCorrectStates(state, 2, params.numCoordinates); -values.stateAccelerations = getCorrectStates(state, 3, params.numCoordinates); -values.controlJerks = control(:, 1 : params.numCoordinates); -if strcmp(params.controllerType, 'synergy_driven') - if params.optimizeSynergyVectors +values = getTreatmentOptimizationValueStruct(inputs, params); +if strcmp(params.controllerType, 'synergy_driven') + if params.optimizeSynergyVectors values.synergyWeights = scaleToOriginal(inputs.parameter(1,:), ... params.maxParameter, params.minParameter); values.synergyWeights = getSynergyWeightsFromGroups(... @@ -47,10 +37,5 @@ values.synergyWeights = getSynergyWeightsFromGroups(... params.synergyWeightsGuess, params); end - values.controlSynergyActivations = control(:, params.numCoordinates + 1 : ... - params.numCoordinates + params.numSynergies); -else - values.controlTorques = control(:, params.numCoordinates + 1 : ... - params.numCoordinates + params.numTorqueControls); end -end \ No newline at end of file +end diff --git a/src/TrackingOptimization/inverseDynamics.mexw64 b/src/TrackingOptimization/inverseDynamics.mexw64 deleted file mode 100644 index b9220e37d..000000000 Binary files a/src/TrackingOptimization/inverseDynamics.mexw64 and /dev/null differ diff --git a/src/TrackingOptimization/parseTrackingOptimizationSettingsTree.m b/src/TrackingOptimization/parseTrackingOptimizationSettingsTree.m index fa0683977..af01d9007 100644 --- a/src/TrackingOptimization/parseTrackingOptimizationSettingsTree.m +++ b/src/TrackingOptimization/parseTrackingOptimizationSettingsTree.m @@ -77,6 +77,7 @@ inputs.maxControlTorquesMultiple = getDoubleFromField(maxControlTorques); end end +inputs.toolName = "TrackingOptimization"; end function params = getParams(tree) diff --git a/src/TrackingOptimization/pointKinematics.mexw64 b/src/TrackingOptimization/pointKinematics.mexw64 deleted file mode 100644 index b8ca6b73f..000000000 Binary files a/src/TrackingOptimization/pointKinematics.mexw64 and /dev/null differ diff --git a/src/TrackingOptimization/reportTrackingOptimizationResults.m b/src/TrackingOptimization/reportTrackingOptimizationResults.m deleted file mode 100644 index 801b40e65..000000000 --- a/src/TrackingOptimization/reportTrackingOptimizationResults.m +++ /dev/null @@ -1,118 +0,0 @@ -% This function is part of the NMSM Pipeline, see file for full license. -% -% () -> () -% - -% ----------------------------------------------------------------------- % -% The NMSM Pipeline is a toolkit for model personalization and treatment % -% optimization of neuromusculoskeletal models through OpenSim. See % -% nmsm.rice.edu and the NOTICE file for more information. The % -% NMSM Pipeline is developed at Rice University and supported by the US % -% National Institutes of Health (R01 EB030520). % -% % -% Copyright (c) 2021 Rice University and the Authors % -% Author(s): Marleny Vega % -% % -% Licensed under the Apache License, Version 2.0 (the "License"); % -% you may not use this file except in compliance with the License. % -% You may obtain a copy of the License at % -% http://www.apache.org/licenses/LICENSE-2.0. % -% % -% Unless required by applicable law or agreed to in writing, software % -% distributed under the License is distributed on an "AS IS" BASIS, % -% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or % -% implied. See the License for the specific language governing % -% permissions and limitations under the License. % -% ----------------------------------------------------------------------- % - -function reportTrackingOptimizationResults(solution, inputs) - -values = getTrackingOptimizationValueStruct(solution.solution.phase, inputs); -if strcmp(inputs.controllerType, 'synergy_driven') -% plot Muscle Activations -plotMuscleActivations(solution.muscleActivations, values.time, ... - inputs.experimentalMuscleActivations, inputs.experimentalTime, ... - inputs.muscleLabels); -% plot synergy activations -synergyTitles = {}; -for i = 1 : inputs.numSynergies - synergyTitles{end + 1} = strcat('synergy activation', num2str(i)); -end -plotResultsWithOutComparison(values.controlSynergyActivations, values.time, ... - synergyTitles, ["Synergy" "Activations"]); -end -% plot joint angles -plotResultsWithComparison(values.statePositions, values.time, ... - inputs.experimentalJointAngles, inputs.experimentalTime, ... - strcat(inputs.coordinateNames, ' [rad]'), ["Joint" "Angles [rad]"]); -% plot joint moments -plotResultsWithComparison(solution.inverseDynamicMoments, values.time, ... - inputs.experimentalJointMoments, inputs.experimentalTime, ... - strcat(inputs.inverseDynamicMomentLabels, ' [Nm]'), ["Joint" "Moments"]); -% plot ground reactions -for i = 1:length(inputs.contactSurfaces) -plotResultsWithComparison(solution.groundReactionsLab.forces{i}, values.time, ... - inputs.contactSurfaces{i}.experimentalGroundReactionForces, inputs.experimentalTime, ... - ["GRFx", "GRFy", "GRFz"], ["Ground" "Reaction Forces"]); -plotResultsWithComparison(solution.groundReactionsLab.moments{i}, values.time, ... - inputs.contactSurfaces{i}.experimentalGroundReactionMoments, inputs.experimentalTime, ... - ["GRTx", "GRTy", "GRTz"], ["Ground" "Reaction Moments"]); -end -end -function plotMuscleActivations(muscleActivations, time, ... - experimentalMuscleActivations, experimentalTime, muscleLabels) - -figure('name', 'Muscle Activations') -nplots = ceil(sqrt(size(muscleActivations, 2))); -for i = 1 : size(muscleActivations,2) -subplot(nplots, nplots, i) -plot(time, muscleActivations(:, i)); hold on -plot(experimentalTime, experimentalMuscleActivations(:, i), 'r') -axis([0 1 0 experimentalTime(end)]) -title(muscleLabels(i)) -figureXLabels(numel(muscleLabels), nplots, i, "Time") -figureYLabels(numel(muscleLabels), nplots, i, ["Muscle" "Activation"]) -end -end -function plotResultsWithComparison(solutionData, solutionTime, ... - experimentalData, experimentalTime, labels, figureTitle) - -figure('name', strjoin(figureTitle)) -nplots = ceil(sqrt(numel(labels))); -for i = 1 : size(solutionData,2) -subplot(nplots, nplots, i) -plot(solutionTime, solutionData(:, i)); hold on -plot(experimentalTime, experimentalData(:, i), 'r') -xlim([0 experimentalTime(end)]) -title(labels(i)) -figureXLabels(numel(labels), nplots, i, "Time") -end -end -function plotResultsWithOutComparison(solutionData, solutionTime, ... - labels, figureTitle) - -figure('name', strjoin(figureTitle)) -for i = 1 : size(solutionData,2) -subplot(numel(labels), 1, i) -plot(solutionTime, solutionData(:, i)); hold on -xlim([0 solutionTime(end)]) -title(labels(i)) -figureXLabels(numel(labels), 1, i, "Time") -end -end -function figureXLabels(numTotalPlots, numColumnPlots, plotIndex, xLabel) - -if plotIndex > numTotalPlots - numColumnPlots - xlabel(xLabel); -else - xticklabels(''); -end -end -function figureYLabels(numTotalPlots, numColumnPlots, plotIndex, yLabel) - -if ismember(plotIndex, 1 : numColumnPlots : numTotalPlots) - ylabel({yLabel(1); yLabel(2)}); -else - yticklabels(''); -end -end \ No newline at end of file diff --git a/src/VerificationOptimization/VerificationOptimization.m b/src/VerificationOptimization/VerificationOptimization.m index 4a09333fd..f8510cc61 100644 --- a/src/VerificationOptimization/VerificationOptimization.m +++ b/src/VerificationOptimization/VerificationOptimization.m @@ -1,7 +1,7 @@ % This function is part of the NMSM Pipeline, see file for full license. % % () -> () -% +% % ----------------------------------------------------------------------- % % The NMSM Pipeline is a toolkit for model personalization and treatment % @@ -11,7 +11,7 @@ % National Institutes of Health (R01 EB030520). % % % % Copyright (c) 2021 Rice University and the Authors % -% Author(s): Marleny Vega % +% Author(s): Marleny Vega, Claire V. Hammond % % % % Licensed under the Apache License, Version 2.0 (the "License"); % % you may not use this file except in compliance with the License. % @@ -26,19 +26,9 @@ % ----------------------------------------------------------------------- % function [output, inputs] = VerificationOptimization(inputs, params) -pointKinematics(inputs.mexModel); -inverseDynamics(inputs.mexModel); -inputs = getStateDerivatives(inputs); -inputs = setupGroundContact(inputs); -inputs = getSplines(inputs); -inputs = checkStateGuess(inputs); -inputs = checkControlGuess(inputs); -inputs = checkParameterGuess(inputs); -inputs = getIntegralBounds(inputs); -inputs = getPathConstraintBounds(inputs); -inputs = getTerminalConstraintBounds(inputs); -inputs = getDesignVariableInputBounds(inputs); -if strcmp(inputs.controllerType, 'synergy_driven') +inputs = makeTreatmentOptimizationInputs(inputs, params); +initializeMexOrMatlabParallelFunctions(inputs.mexModel); +if strcmp(inputs.controllerType, 'synergy_driven') inputs = setupMuscleSynergies(inputs); end output = computeVerificationOptimizationMainFunction(inputs, params); @@ -47,4 +37,4 @@ inputs.splineSynergyActivations = spaps(inputs.initialGuess.time, ... inputs.initialGuess.control(:, inputs.numCoordinates + 1:end)', 0.0000001); inputs.synergyLabels = inputs.initialGuess.controlLabels(:, inputs.numCoordinates + 1:end); -end \ No newline at end of file +end diff --git a/src/VerificationOptimization/VerificationOptimizationTool.m b/src/VerificationOptimization/VerificationOptimizationTool.m index 96bb357aa..7c8935c8d 100644 --- a/src/VerificationOptimization/VerificationOptimizationTool.m +++ b/src/VerificationOptimization/VerificationOptimizationTool.m @@ -33,6 +33,6 @@ function VerificationOptimizationTool(settingsFileName) settingsTree = xml2struct(settingsFileName); [inputs, params] = parseVerificationOptimizationSettingsTree(settingsTree); [outputs, inputs] = VerificationOptimization(inputs, params); -reportVerificationOptimizationResults(outputs, inputs); +reportTreatmentOptimizationResults(outputs, inputs); saveVerificationOptimizationResults(outputs, inputs); -end \ No newline at end of file +end diff --git a/src/VerificationOptimization/calcVerificationOptimizationIntegrand.m b/src/VerificationOptimization/calcVerificationOptimizationIntegrand.m index 78499ea88..4853284b3 100644 --- a/src/VerificationOptimization/calcVerificationOptimizationIntegrand.m +++ b/src/VerificationOptimization/calcVerificationOptimizationIntegrand.m @@ -25,42 +25,12 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function integrand = calcVerificationOptimizationIntegrand(values, params, ... - phaseout) -integrand = []; -for i = 1:length(params.integral.tracking) - costTerm = params.integral.tracking{i}; - if costTerm.isEnabled - switch costTerm.type - case "coordinate" - integrand = cat(2, integrand, ... - calcTrackingCoordinateIntegrand(params, ... - values.time, values.statePositions, ... - costTerm.coordinate)); - case "controller" - integrand = cat(2, integrand, ... - calcTrackingControllerIntegrand(params, values, ... - costTerm.controller)); - otherwise - throw(MException('', ['Cost term type ' costTerm.type ... - ' does not exist for this tool.'])) - end - end -end -for i = 1:length(params.integral.minimizing) - costTerm = params.integral.minimizing{i}; - if costTerm.isEnabled - switch costTerm.type - case "joint_jerk" - integrand = cat(2, integrand, ... - calcMinimizingJointJerkIntegrand(values.controlJerks, ... - params, costTerm.coordinate)); - otherwise - throw(MException('', ['Cost term type ' costTerm.type ... - ' does not exist for this tool.'])) - end - end -end -integrand = scaleToBounds(integrand, params.maxIntegral, params.minIntegral); +function integrand = calcVerificationOptimizationIntegrand(values, ... + modeledValues, auxdata) +[costTermCalculations, allowedTypes] = ... + generateCostTermStruct("continuous", "VerificationOptimization"); +integrand = calcTreatmentOptimizationCost( ... + costTermCalculations, allowedTypes, values, modeledValues, auxdata); +integrand = integrand ./ (auxdata.maxIntegral - auxdata.minIntegral); integrand = integrand .^ 2; end \ No newline at end of file diff --git a/src/VerificationOptimization/calcVerificationOptimizationPathConstraint.m b/src/VerificationOptimization/calcVerificationOptimizationPathConstraint.m index d0bb058b2..be8055847 100644 --- a/src/VerificationOptimization/calcVerificationOptimizationPathConstraint.m +++ b/src/VerificationOptimization/calcVerificationOptimizationPathConstraint.m @@ -25,7 +25,7 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function path = calcVerificationOptimizationPathConstraint(values, phaseout, ... +function path = calcVerificationOptimizationPathConstraint(values, modeledValues, ... params) path = []; for i = 1:length(params.path) @@ -37,15 +37,15 @@ calcRootSegmentResidualsPathConstraints(... constraintTerm.load, ... params.inverseDynamicMomentLabels, ... - phaseout.inverseDynamicMoments)); + modeledValues.inverseDynamicMoments)); case "muscle_model_moment_consistency" path = cat(2, path, ... calcMuscleActuatedMomentsPathConstraints(params, ... - phaseout, constraintTerm.load)); + modeledValues, constraintTerm.load)); case "torque_model_moment_consistency" path = cat(2, path, ... calcTorqueActuatedMomentsPathConstraints(params, ... - phaseout, values.controlTorques, constraintTerm.load)); + modeledValues, values.controlTorques, constraintTerm.load)); otherwise throw(MException('', ['Constraint term type ' ... constraintTerm.type ' does not exist for this tool.'])) diff --git a/src/VerificationOptimization/computeVerificationOptimizationContinuousFunction.m b/src/VerificationOptimization/computeVerificationOptimizationContinuousFunction.m index e53110044..8d7494d44 100644 --- a/src/VerificationOptimization/computeVerificationOptimizationContinuousFunction.m +++ b/src/VerificationOptimization/computeVerificationOptimizationContinuousFunction.m @@ -25,13 +25,13 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function phaseout = computeVerificationOptimizationContinuousFunction(inputs) +function modeledValues = computeVerificationOptimizationContinuousFunction(inputs) values = getVerificationOptimizationValueStruct(inputs.phase, inputs.auxdata); -phaseout = calcTorqueBasedModeledValues(values, inputs.auxdata); -phaseout = calcSynergyBasedModeledValues(values, inputs.auxdata, phaseout); -phaseout.dynamics = calcVerificationOptimizationDynamicsConstraint(values, inputs.auxdata); -phaseout.path = calcVerificationOptimizationPathConstraint(values, phaseout, inputs.auxdata); -phaseout.integrand = calcVerificationOptimizationIntegrand(values, inputs.auxdata, ... - phaseout); +modeledValues = calcTorqueBasedModeledValues(values, inputs.auxdata); +modeledValues = calcSynergyBasedModeledValues(values, inputs.auxdata, modeledValues); +modeledValues.dynamics = calcVerificationOptimizationDynamicsConstraint(values, inputs.auxdata); +modeledValues.path = calcVerificationOptimizationPathConstraint(values, modeledValues, inputs.auxdata); +modeledValues.integrand = calcVerificationOptimizationIntegrand(values, ... + modeledValues, inputs.auxdata); end \ No newline at end of file diff --git a/src/VerificationOptimization/getVerificationOptimizationValueStruct.m b/src/VerificationOptimization/getVerificationOptimizationValueStruct.m index 542e2b562..c3e1ada73 100644 --- a/src/VerificationOptimization/getVerificationOptimizationValueStruct.m +++ b/src/VerificationOptimization/getVerificationOptimizationValueStruct.m @@ -1,7 +1,7 @@ % This function is part of the NMSM Pipeline, see file for full license. % % () -> () -% +% % ----------------------------------------------------------------------- % % The NMSM Pipeline is a toolkit for model personalization and treatment % @@ -11,7 +11,7 @@ % National Institutes of Health (R01 EB030520). % % % % Copyright (c) 2021 Rice University and the Authors % -% Author(s): Marleny Vega % +% Author(s): Marleny Vega, Claire V. Hammond % % % % Licensed under the Apache License, Version 2.0 (the "License"); % % you may not use this file except in compliance with the License. % @@ -26,24 +26,11 @@ % ----------------------------------------------------------------------- % function values = getVerificationOptimizationValueStruct(inputs, params) - -values.time = scaleToOriginal(inputs.time, params.maxTime, ... - params.minTime); -state = scaleToOriginal(inputs.state, ones(size(inputs.state, 1), 1) .* ... - params.maxState, ones(size(inputs.state, 1), 1) .* params.minState); -control = scaleToOriginal(inputs.control, ones(size(inputs.control, 1), 1) .* ... - params.maxControl, ones(size(inputs.control, 1), 1) .* params.minControl); -values.statePositions = getCorrectStates(state, 1, params.numCoordinates); -values.stateVelocities = getCorrectStates(state, 2, params.numCoordinates); -values.stateAccelerations = getCorrectStates(state, 3, params.numCoordinates); -values.controlJerks = control(:, 1 : params.numCoordinates); -if strcmp(params.controllerType, 'synergy_driven') - values.controlSynergyActivations = control(:, params.numCoordinates + 1 : ... +values = getTreatmentOptimizationValueStruct(inputs, params); +if strcmp(params.controllerType, 'synergy_driven') + values.controlSynergyActivations = inputs.control(:, params.numCoordinates + 1 : ... params.numCoordinates + params.numSynergies); values.synergyWeights = getSynergyWeightsFromGroups(... params.synergyWeightsGuess, params); -else - values.controlTorques = control(:, params.numCoordinates + 1 : ... - params.numCoordinates + params.numTorqueControls); end -end \ No newline at end of file +end diff --git a/src/VerificationOptimization/inverseDynamics.mexw64 b/src/VerificationOptimization/inverseDynamics.mexw64 deleted file mode 100644 index b9220e37d..000000000 Binary files a/src/VerificationOptimization/inverseDynamics.mexw64 and /dev/null differ diff --git a/src/VerificationOptimization/parseVerificationOptimizationSettingsTree.m b/src/VerificationOptimization/parseVerificationOptimizationSettingsTree.m index 22654f714..c28e9a8c1 100644 --- a/src/VerificationOptimization/parseVerificationOptimizationSettingsTree.m +++ b/src/VerificationOptimization/parseVerificationOptimizationSettingsTree.m @@ -80,6 +80,7 @@ inputs.maxControlTorquesMultiple = getDoubleFromField(maxControlTorques); end end +inputs.toolName = "VerificationOptimization"; end function params = getParams(tree) diff --git a/src/VerificationOptimization/pointKinematics.mexw64 b/src/VerificationOptimization/pointKinematics.mexw64 deleted file mode 100644 index b8ca6b73f..000000000 Binary files a/src/VerificationOptimization/pointKinematics.mexw64 and /dev/null differ diff --git a/src/VerificationOptimization/reportVerificationOptimizationResults.m b/src/VerificationOptimization/reportVerificationOptimizationResults.m deleted file mode 100644 index 1e873e93e..000000000 --- a/src/VerificationOptimization/reportVerificationOptimizationResults.m +++ /dev/null @@ -1,118 +0,0 @@ -% This function is part of the NMSM Pipeline, see file for full license. -% -% () -> () -% - -% ----------------------------------------------------------------------- % -% The NMSM Pipeline is a toolkit for model personalization and treatment % -% optimization of neuromusculoskeletal models through OpenSim. See % -% nmsm.rice.edu and the NOTICE file for more information. The % -% NMSM Pipeline is developed at Rice University and supported by the US % -% National Institutes of Health (R01 EB030520). % -% % -% Copyright (c) 2021 Rice University and the Authors % -% Author(s): Marleny Vega % -% % -% Licensed under the Apache License, Version 2.0 (the "License"); % -% you may not use this file except in compliance with the License. % -% You may obtain a copy of the License at % -% http://www.apache.org/licenses/LICENSE-2.0. % -% % -% Unless required by applicable law or agreed to in writing, software % -% distributed under the License is distributed on an "AS IS" BASIS, % -% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or % -% implied. See the License for the specific language governing % -% permissions and limitations under the License. % -% ----------------------------------------------------------------------- % - -function reportVerificationOptimizationResults(solution, inputs) - -values = getVerificationOptimizationValueStruct(solution.solution.phase, inputs); -if strcmp(inputs.controllerType, 'synergy_driven') -% plot Muscle Activations -plotMuscleActivations(solution.muscleActivations, values.time, ... - inputs.experimentalMuscleActivations, inputs.experimentalTime, ... - inputs.muscleLabels); -% plot synergy activations -synergyTitles = {}; -for i = 1 : inputs.numSynergies - synergyTitles{end + 1} = strcat('synergy activation', num2str(i)); -end -plotResultsWithOutComparison(values.controlSynergyActivations, values.time, ... - synergyTitles, ["Synergy" "Activations"]); -end -% plot joint angles -plotResultsWithComparison(values.statePositions, values.time, ... - inputs.experimentalJointAngles, inputs.experimentalTime, ... - strcat(inputs.coordinateNames, ' [rad]'), ["Joint" "Angles [rad]"]); -% plot joint moments -plotResultsWithComparison(solution.inverseDynamicMoments, values.time, ... - inputs.experimentalJointMoments, inputs.experimentalTime, ... - strcat(inputs.inverseDynamicMomentLabels, ' [Nm]'), ["Joint" "Moments"]); -% plot ground reactions -for i = 1:length(inputs.contactSurfaces) -plotResultsWithComparison(solution.groundReactionsLab.forces{i}, values.time, ... - inputs.contactSurfaces{i}.experimentalGroundReactionForces, inputs.experimentalTime, ... - ["GRFx", "GRFy", "GRFz"], ["Ground" "Reaction Forces"]); -plotResultsWithComparison(solution.groundReactionsLab.moments{i}, values.time, ... - inputs.contactSurfaces{i}.experimentalGroundReactionMoments, inputs.experimentalTime, ... - ["GRTx", "GRTy", "GRTz"], ["Ground" "Reaction Moments"]); -end -end -function plotMuscleActivations(muscleActivations, time, ... - experimentalMuscleActivations, experimentalTime, muscleLabels) - -figure('name', 'Muscle Activations') -nplots = ceil(sqrt(size(muscleActivations, 2))); -for i = 1 : size(muscleActivations,2) -subplot(nplots, nplots, i) -plot(time, muscleActivations(:, i)); hold on -plot(experimentalTime, experimentalMuscleActivations(:, i), 'r') -axis([0 1 0 experimentalTime(end)]) -title(muscleLabels(i)) -figureXLabels(numel(muscleLabels), nplots, i, "Time") -figureYLabels(numel(muscleLabels), nplots, i, ["Muscle" "Activation"]) -end -end -function plotResultsWithComparison(solutionData, solutionTime, ... - experimentalData, experimentalTime, labels, figureTitle) - -figure('name', strjoin(figureTitle)) -nplots = ceil(sqrt(numel(labels))); -for i = 1 : size(solutionData,2) -subplot(nplots, nplots, i) -plot(solutionTime, solutionData(:, i)); hold on -plot(experimentalTime, experimentalData(:, i), 'r') -xlim([0 experimentalTime(end)]) -title(labels(i)) -figureXLabels(numel(labels), nplots, i, "Time") -end -end -function plotResultsWithOutComparison(solutionData, solutionTime, ... - labels, figureTitle) - -figure('name', strjoin(figureTitle)) -for i = 1 : size(solutionData,2) -subplot(numel(labels), 1, i) -plot(solutionTime, solutionData(:, i)); hold on -xlim([0 solutionTime(end)]) -title(labels(i)) -figureXLabels(numel(labels), 1, i, "Time") -end -end -function figureXLabels(numTotalPlots, numColumnPlots, plotIndex, xLabel) - -if plotIndex > numTotalPlots - numColumnPlots - xlabel(xLabel); -else - xticklabels(''); -end -end -function figureYLabels(numTotalPlots, numColumnPlots, plotIndex, yLabel) - -if ismember(plotIndex, 1 : numColumnPlots : numTotalPlots) - ylabel({yLabel(1); yLabel(2)}); -else - yticklabels(''); -end -end \ No newline at end of file diff --git a/src/core/MuscleCalculations/calcMetabolicCost.m b/src/core/MuscleCalculations/calcMetabolicCost.m new file mode 100644 index 000000000..5a458f3f0 --- /dev/null +++ b/src/core/MuscleCalculations/calcMetabolicCost.m @@ -0,0 +1,64 @@ +% This function is part of the NMSM Pipeline, see file for full license. +% +% () -> () +% + +% ----------------------------------------------------------------------- % +% The NMSM Pipeline is a toolkit for model personalization and treatment % +% optimization of neuromusculoskeletal models through OpenSim. See % +% nmsm.rice.edu and the NOTICE file for more information. The % +% NMSM Pipeline is developed at Rice University and supported by the US % +% National Institutes of Health (R01 EB030520). % +% % +% Copyright (c) 2021 Rice University and the Authors % +% Author(s): Marleny Vega % +% % +% Licensed under the Apache License, Version 2.0 (the "License"); % +% you may not use this file except in compliance with the License. % +% You may obtain a copy of the License at % +% http://www.apache.org/licenses/LICENSE-2.0. % +% % +% Unless required by applicable law or agreed to in writing, software % +% distributed under the License is distributed on an "AS IS" BASIS, % +% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or % +% implied. See the License for the specific language governing % +% permissions and limitations under the License. % +% ----------------------------------------------------------------------- % + +function metabolicCost = calcMetabolicCost(time, statePositions, ... + muscleActivations, params) +metabolicCost = []; +for indx = 1 : numel(params.costTerms) + if strcmpi(params.costTerms{indx}.type, 'metabolic_cost') + import org.opensim.modeling.* + model = Model(params.model); + for i = 1 : params.numMuscles + controller = PrescribedController(); + controller.addActuator(model.getMuscles().get(params.muscleNames{i})); + controlFunction = PiecewiseLinearFunction(); + for j = 1:size(muscleActivations, 1) + controlFunction.addPoint(time(j), muscleActivations(j, i)); + end + controller.prescribeControlForActuator(params.muscleNames{i}, ... + controlFunction); + model.addComponent(controller); + end + + state = model.initSystem(); + for i = 1:size(muscleActivations, 1) + for j = 1 : size(params.coordinateNames, 2) + if ~model.getCoordinateSet.get(params.coordinateNames(j)). .... + get_locked + model.getCoordinateSet.get(params.coordinateNames(j)). ... + setValue(state, statePositions(i, j)); + end + end + state.setTime(time(i)); + model.realizeDynamics(state); + model.equilibrateMuscles(state); + tempTotalCost = model.getProbeSet().get(0).getProbeOutputs(state); + metabolicCost(i, :) = tempTotalCost.get(0); + end + end +end +end \ No newline at end of file diff --git a/src/core/TreatmentOptimization/getDiscreteCostTerms.m b/src/core/TreatmentOptimization/IntegrandTerms/calcMinimizingMetabolicCost.m similarity index 57% rename from src/core/TreatmentOptimization/getDiscreteCostTerms.m rename to src/core/TreatmentOptimization/IntegrandTerms/calcMinimizingMetabolicCost.m index fe2786d9c..aba62da00 100644 --- a/src/core/TreatmentOptimization/getDiscreteCostTerms.m +++ b/src/core/TreatmentOptimization/IntegrandTerms/calcMinimizingMetabolicCost.m @@ -25,34 +25,8 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function inputs = getDiscreteCostTerms(tree, inputs) -trackingDiscreteTermsTree = getFieldByName(tree, 'RCNLTrackingCostTerms'); -if isstruct(trackingDiscreteTermsTree) - if isfield(trackingDiscreteTermsTree.RCNLCostTermSet.objects, 'RCNLCostTerm') - rcnlCostTermTree = ... - trackingDiscreteTermsTree.RCNLCostTermSet.objects.RCNLCostTerm; - if length(rcnlCostTermTree) > 1 - inputs.discrete.tracking = ... - parseRcnlCostTermSet(rcnlCostTermTree); - else - inputs.discrete.tracking = ... - parseRcnlCostTermSet({rcnlCostTermTree}); - end - end -end +function cost = calcMinimizingMetabolicCost(metabolicCost) -minimizingDiscreteTermsTree = getFieldByName(tree, 'RCNLMinimizationCostTerms'); -if isstruct(minimizingDiscreteTermsTree) - if isfield(minimizingDiscreteTermsTree.RCNLCostTermSet.objects, 'RCNLCostTerm') - rcnlCostTermTree = ... - minimizingDiscreteTermsTree.RCNLCostTermSet.objects.RCNLCostTerm; - if length(rcnlCostTermTree) > 1 - inputs.discrete.minimizing = ... - parseRcnlCostTermSet(rcnlCostTermTree); - else - inputs.discrete.minimizing = ... - parseRcnlCostTermSet({rcnlCostTermTree}); - end - end +cost = calcMinimizingCostArrayTerm(metabolicCost); end -end \ No newline at end of file + diff --git a/src/core/TreatmentOptimization/IntegrandTerms/calcTrackingControllerIntegrand.m b/src/core/TreatmentOptimization/IntegrandTerms/calcTrackingControllerIntegrand.m index ec694bded..fda1c9d5d 100644 --- a/src/core/TreatmentOptimization/IntegrandTerms/calcTrackingControllerIntegrand.m +++ b/src/core/TreatmentOptimization/IntegrandTerms/calcTrackingControllerIntegrand.m @@ -25,20 +25,28 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function cost = calcTrackingControllerIntegrand(params, values, ... +function cost = calcTrackingControllerIntegrand(auxdata, values, time, ... controllerName) -switch params.controllerType +switch auxdata.controllerType case 'synergy_driven' indx = find(strcmp(convertCharsToStrings( ... - params.synergyLabels), controllerName)); - synergyActivations = fnval(params.splineSynergyActivations, values.time)'; + auxdata.synergyLabels), controllerName)); + if auxdata.splineJointMoments.dim > 1 + synergyActivations = fnval(auxdata.splineSynergyActivations, time)'; + else + synergyActivations = fnval(auxdata.splineSynergyActivations, time); + end cost = calcTrackingCostArrayTerm(synergyActivations, values.controlSynergyActivations, indx); case 'torque_driven' indx1 = find(strcmp(convertCharsToStrings( ... - params.inverseDynamicMomentLabels), controllerName)); + auxdata.inverseDynamicMomentLabels), controllerName)); indx2 = find(strcmp(convertCharsToStrings( ... - strcat(params.controlTorqueNames, '_moment')), controllerName)); - experimentalJointMoments = fnval(params.splineJointMoments, values.time)'; + strcat(auxdata.controlTorqueNames, '_moment')), controllerName)); + if auxdata.splineJointMoments.dim > 1 + experimentalJointMoments = fnval(auxdata.splineJointMoments, time)'; + else + experimentalJointMoments = fnval(auxdata.splineJointMoments, time); + end cost = experimentalJointMoments(:, indx1) - values.controlTorques(:, indx2); end \ No newline at end of file diff --git a/src/core/TreatmentOptimization/IntegrandTerms/calcTrackingCoordinateIntegrand.m b/src/core/TreatmentOptimization/IntegrandTerms/calcTrackingCoordinateIntegrand.m index 649c59fbe..f681ec3a0 100644 --- a/src/core/TreatmentOptimization/IntegrandTerms/calcTrackingCoordinateIntegrand.m +++ b/src/core/TreatmentOptimization/IntegrandTerms/calcTrackingCoordinateIntegrand.m @@ -25,15 +25,15 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function cost = calcTrackingCoordinateIntegrand(params, time, ... +function cost = calcTrackingCoordinateIntegrand(auxdata, time, ... statePositions, coordinateName) -indx = find(strcmp(convertCharsToStrings(params.coordinateNames), ... +indx = find(strcmp(convertCharsToStrings(auxdata.coordinateNames), ... coordinateName)); -if params.splineJointAngles.dim > 1 - experimentalJointAngles = fnval(params.splineJointAngles, time)'; +if auxdata.splineJointAngles.dim > 1 + experimentalJointAngles = fnval(auxdata.splineJointAngles, time)'; else - experimentalJointAngles = fnval(params.splineJointAngles, time); + experimentalJointAngles = fnval(auxdata.splineJointAngles, time); end cost = calcTrackingCostArrayTerm(experimentalJointAngles, ... diff --git a/src/core/TreatmentOptimization/IntegrandTerms/calcTrackingInverseDynamicLoadsIntegrand.m b/src/core/TreatmentOptimization/IntegrandTerms/calcTrackingInverseDynamicLoadsIntegrand.m index 0a9fd79dd..24a90028d 100644 --- a/src/core/TreatmentOptimization/IntegrandTerms/calcTrackingInverseDynamicLoadsIntegrand.m +++ b/src/core/TreatmentOptimization/IntegrandTerms/calcTrackingInverseDynamicLoadsIntegrand.m @@ -28,7 +28,9 @@ function cost = calcTrackingInverseDynamicLoadsIntegrand(params, time, ... inverseDynamicMoments, loadName) -indx = find(strcmp(convertCharsToStrings(params.inverseDynamicMomentLabels), ... +loadName = erase(loadName, '_moment'); +loadName = erase(loadName, '_force'); +indx = find(strcmp(convertCharsToStrings(params.coordinateNames), ... loadName)); if params.splineJointMoments.dim > 1 @@ -36,6 +38,13 @@ else experimentalJointMoments = fnval(params.splineJointMoments, time); end + +momentLabelsNoSuffix = erase(params.inverseDynamicMomentLabels, '_moment'); +momentLabelsNoSuffix = erase(momentLabelsNoSuffix, '_force'); +includedJointMomentCols = ismember(momentLabelsNoSuffix, convertCharsToStrings(params.coordinateNames)); +if ~isequal(mexext, 'mexw64') + experimentalJointMoments = experimentalJointMoments(:, includedJointMomentCols); +end cost = calcTrackingCostArrayTerm(experimentalJointMoments, ... inverseDynamicMoments, indx); end \ No newline at end of file diff --git a/src/core/TreatmentOptimization/PathTerms/calcTorqueActuatedMomentsPathConstraints.m b/src/core/TreatmentOptimization/PathTerms/calcTorqueActuatedMomentsPathConstraints.m index dc640f3a1..dbc14a437 100644 --- a/src/core/TreatmentOptimization/PathTerms/calcTorqueActuatedMomentsPathConstraints.m +++ b/src/core/TreatmentOptimization/PathTerms/calcTorqueActuatedMomentsPathConstraints.m @@ -28,9 +28,10 @@ function pathTerm = calcTorqueActuatedMomentsPathConstraints(params, ... phaseout, controlTorques, loadName) -indx1 = find(strcmp(convertCharsToStrings(params.inverseDynamicMomentLabels), ... - loadName)); -indx2 = find(strcmp(strcat(params.controlTorqueNames, ... - '_moment'), loadName)); +loadName = erase(loadName, '_moment'); +loadName = erase(loadName, '_force'); +indx1 = find(cellfun(@isequal, params.coordinateNames, ... + repmat({loadName}, 1, length(params.coordinateNames)))); +indx2 = find(strcmp(params.controlTorqueNames, loadName)); pathTerm = phaseout.inverseDynamicMoments(:, indx1) - controlTorques(:, indx2); end \ No newline at end of file diff --git a/src/core/TreatmentOptimization/SetupBounds/getDesignVariableInputBounds.m b/src/core/TreatmentOptimization/SetupBounds/getDesignVariableInputBounds.m index 764772061..dd407e304 100644 --- a/src/core/TreatmentOptimization/SetupBounds/getDesignVariableInputBounds.m +++ b/src/core/TreatmentOptimization/SetupBounds/getDesignVariableInputBounds.m @@ -26,7 +26,11 @@ % ----------------------------------------------------------------------- % function inputs = getDesignVariableInputBounds(inputs) -inputs.maxTime = max(inputs.experimentalTime); +if isfield(inputs, "finalTimeRange") + inputs.maxTime = max(inputs.experimentalTime) + inputs.finalTimeRange; +else + inputs.maxTime = max(inputs.experimentalTime); +end inputs.minTime = min(inputs.experimentalTime); maxStatePositions = max(inputs.experimentalJointAngles) + ... diff --git a/src/core/TreatmentOptimization/SetupBounds/getIntegralBounds.m b/src/core/TreatmentOptimization/SetupBounds/getIntegralBounds.m index d186e8fc7..37747ede6 100644 --- a/src/core/TreatmentOptimization/SetupBounds/getIntegralBounds.m +++ b/src/core/TreatmentOptimization/SetupBounds/getIntegralBounds.m @@ -1,7 +1,7 @@ % This function is part of the NMSM Pipeline, see file for full license. % % () -> () -% +% % ----------------------------------------------------------------------- % % The NMSM Pipeline is a toolkit for model personalization and treatment % @@ -26,21 +26,26 @@ % ----------------------------------------------------------------------- % function inputs = getIntegralBounds(inputs) +[~, allowedTypes] = generateCostTermStruct("continuous", inputs.toolName); inputs.maxIntegral = []; -for i = 1:length(inputs.integral.tracking) - costTerm = inputs.integral.tracking{i}; +inputs.minIntegral = []; +for i = 1:length(inputs.costTerms) + costTerm = inputs.costTerms{i}; if costTerm.isEnabled - inputs.maxIntegral = cat(2, inputs.maxIntegral, ... - costTerm.maxAllowableError); + if any(ismember(costTerm.type, allowedTypes)) && ... + ~strcmp(costTerm.type, "user_defined") + inputs.maxIntegral = cat(2, inputs.maxIntegral, ... + costTerm.maxAllowableError); + elseif strcmp(costTerm.type, "user_defined") + if strcmp(costTerm.cost_term_type, "continuous") + inputs.maxIntegral = cat(2, inputs.maxIntegral, ... + costTerm.maxAllowableError); + end + else + throw(MException('', ['Cost term type ' costTerm.type ... + ' does not exist for this tool.'])) + end end end -for i = 1:length(inputs.integral.minimizing) - costTerm = inputs.integral.minimizing{i}; - if costTerm.isEnabled - inputs.maxIntegral = cat(2, inputs.maxIntegral, ... - costTerm.maxAllowableError); - end - -end inputs.minIntegral = zeros(1, length(inputs.maxIntegral)); end \ No newline at end of file diff --git a/src/core/TreatmentOptimization/getContinuousCostTerms.m b/src/core/TreatmentOptimization/calcTreatmentOptimizationCost.m similarity index 63% rename from src/core/TreatmentOptimization/getContinuousCostTerms.m rename to src/core/TreatmentOptimization/calcTreatmentOptimizationCost.m index 42832f5c4..c7d20ff9e 100644 --- a/src/core/TreatmentOptimization/getContinuousCostTerms.m +++ b/src/core/TreatmentOptimization/calcTreatmentOptimizationCost.m @@ -25,28 +25,24 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function inputs = getContinuousCostTerms(tree, inputs) -trackingIntegralTermsTree = getFieldByNameOrError(tree, ... - 'RCNLTrackingCostTerms'); -if isfield(trackingIntegralTermsTree.RCNLCostTermSet.objects, 'RCNLCostTerm') -rcnlCostTermTree = ... - trackingIntegralTermsTree.RCNLCostTermSet.objects.RCNLCostTerm; -if length(rcnlCostTermTree) > 1 - inputs.integral.tracking = parseRcnlCostTermSet(rcnlCostTermTree); -else - inputs.integral.tracking = parseRcnlCostTermSet({rcnlCostTermTree}); +function cost = calcTreatmentOptimizationCost( ... + costTermCalculations, allowedTypes, values, modeledValues, auxdata) +cost = []; +for i = 1:length(auxdata.costTerms) + costTerm = auxdata.costTerms{i}; + if costTerm.isEnabled + if isfield(costTermCalculations, costTerm.type) && ... + any(ismember(allowedTypes, costTerm.type)) + fn = costTermCalculations.(costTerm.type); + cost = cat(2, ... + cost, ... + fn(values, modeledValues, auxdata, costTerm)); +% else +% costTerm +% throw(MException('', ['Cost term type ' costTerm.type ... +% ' does not exist for this tool.'])) + end + end end end -minimizingIntegralTermsTree = getFieldByNameOrError(tree, ... - 'RCNLMinimizationCostTerms'); -if isfield(minimizingIntegralTermsTree.RCNLCostTermSet.objects, 'RCNLCostTerm') -rcnlCostTermTree = ... - minimizingIntegralTermsTree.RCNLCostTermSet.objects.RCNLCostTerm; -if length(rcnlCostTermTree) > 1 - inputs.integral.minimizing = parseRcnlCostTermSet(rcnlCostTermTree); -else - inputs.integral.minimizing = parseRcnlCostTermSet({rcnlCostTermTree}); -end -end -end \ No newline at end of file diff --git a/src/core/TreatmentOptimization/generateCostTermStruct.m b/src/core/TreatmentOptimization/generateCostTermStruct.m new file mode 100644 index 000000000..d469d2b53 --- /dev/null +++ b/src/core/TreatmentOptimization/generateCostTermStruct.m @@ -0,0 +1,166 @@ +% This function is part of the NMSM Pipeline, see file for full license. +% +% This function returns all of the cost term calculation methods including +% user_defined and existing cost term values. Tools use this function for +% the discrete and continuous cost calculations. +% +% inputs: +% costTermType - one of ["discrete", "continuous"] +% toolName - one of ["TrackingOptimization", "TreatmentOptimization", ... +% "DesignOptimization"] +% +% (string, string) -> (struct of function handles, Array of string) +% + +% ----------------------------------------------------------------------- % +% The NMSM Pipeline is a toolkit for model personalization and treatment % +% optimization of neuromusculoskeletal models through OpenSim. See % +% nmsm.rice.edu and the NOTICE file for more information. The % +% NMSM Pipeline is developed at Rice University and supported by the US % +% National Institutes of Health (R01 EB030520). % +% % +% Copyright (c) 2021 Rice University and the Authors % +% Author(s): Marleny Vega, Claire V. Hammond % +% % +% Licensed under the Apache License, Version 2.0 (the "License"); % +% you may not use this file except in compliance with the License. % +% You may obtain a copy of the License at % +% http://www.apache.org/licenses/LICENSE-2.0. % +% % +% Unless required by applicable law or agreed to in writing, software % +% distributed under the License is distributed on an "AS IS" BASIS, % +% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or % +% implied. See the License for the specific language governing % +% permissions and limitations under the License. % +% ----------------------------------------------------------------------- % + +function [costTermCalculations, allowedTypes] = ... + generateCostTermStruct(costTermType, toolName) +allowedTypes = getAllowedTypes(costTermType, toolName); +costTermCalculations = getCostTermCalculations(costTermType); +end + +function allowedTypes = getAllowedTypes(costTermType, toolName) +if strcmp(costTermType, "continuous") + switch toolName + case "TrackingOptimization" + allowedTypes = [ ... + "coordinate_tracking", ... + "inverse_dynamics_load_tracking", ... + "external_force_tracking", ... + "external_moment_tracking", ... + "muscle_activation_tracking", ... + "joint_jerk_minimization", ... + ]; + case "VerificationOptimization" + allowedTypes = [ ... + "coordinate_tracking", ... + "controller_tracking", ... + "joint_jerk_minimization", ... + ]; + case "DesignOptimization" + allowedTypes = [ ... + "coordinate_tracking", ... + "controller_tracking", ... + "joint_jerk_minimization", ... + "user_defined", ... + ]; + otherwise + throw(MException('', ['Tool name' toolName 'is not valid'])) + end +elseif strcmp(costTermType, "discrete") + switch toolName + case "TrackingOptimization" + allowedTypes = [ ... + + ]; + case "VerificationOptimization" + allowedTypes = [ ... + + ]; + case "DesignOptimization" + allowedTypes = [ ... + "synergy_vectors", ... + "user_defined", ... + ]; + otherwise + throw(MException('', ['Tool name' toolName 'is not valid'])) + end +else + throw(MException('', ['Cost term type ' costTermType ... + ' is not valid, must be continuous or discrete'])) +end +end + +function costTermCalculations = getCostTermCalculations(costTermType) + +costTermCalculations.coordinate_tracking = @(values, modeledValues, auxdata, costTerm) ... + calcTrackingCoordinateIntegrand( ... + auxdata, ... + values.time/values.time(end), ... + values.statePositions, ... + costTerm.coordinate ... + ); + +costTermCalculations.controller_tracking = @(values, modeledValues, auxdata, costTerm) ... + calcTrackingControllerIntegrand( ... + auxdata, ... + values, ... + values.time/values.time(end), ... + costTerm.controller ... + ); + +costTermCalculations.joint_jerk_minimization = @(values, modeledValues, auxdata, costTerm) ... + calcMinimizingJointJerkIntegrand( ... + values.controlJerks, ... + auxdata, ... + costTerm.coordinate ... + ); + +costTermCalculations.metabolic_cost = @(values, modeledValues, auxdata, costTerm) ... + calcMinimizingMetabolicCost(modeledValues.metabolicCost); + +costTermCalculations.synergy_vectors = @(values, modeledValues, auxdata, costTerm) ... + calcTrackingCoordinateIntegrand(... + auxdata, ... + values.time, ... + values.statePositions, ... + costTerm.coordinate ... + ); + +costTermCalculations.inverse_dynamics_load_tracking = @(values, modeledValues, auxdata, costTerm) ... + calcTrackingInverseDynamicLoadsIntegrand( ... + auxdata, ... + values.time, ... + modeledValues.inverseDynamicMoments, ... + costTerm.load ... + ); + +costTermCalculations.external_force_tracking = @(values, modeledValues, auxdata, costTerm) ... + calcTrackingExternalForcesIntegrand(auxdata, ... + modeledValues.groundReactionsLab.forces, values.time, ... + costTerm.force); + +costTermCalculations.external_moment_tracking = @(values, modeledValues, auxdata, costTerm) ... + calcTrackingExternalMomentsIntegrand(auxdata, ... + modeledValues.groundReactionsLab.moments, values.time, ... + costTerm.moment); + +costTermCalculations.muscle_activation_tracking = @(values, modeledValues, auxdata, costTerm) ... + calcTrackingMuscleActivationIntegrand( ... + modeledValues.muscleActivations, ... + values.time, auxdata, costTerm.muscle); + +costTermCalculations.user_defined = @(values, modeledValues, auxdata, costTerm) ... + userDefinedFunction(values, modeledValues, auxdata, costTerm, costTermType); + +end + +function output = ... + userDefinedFunction(values, modeledValues, auxdata, costTerm, costTermType) +output = []; +if strcmp(costTerm.cost_term_type, costTermType) + fn = str2func(costTerm.function_name); + output = fn(values, modeledValues, auxdata, costTerm); +end +end diff --git a/src/core/TreatmentOptimization/getTreatmentOptimizationInputs.m b/src/core/TreatmentOptimization/getTreatmentOptimizationInputs.m index be1e19175..5d422a4c8 100644 --- a/src/core/TreatmentOptimization/getTreatmentOptimizationInputs.m +++ b/src/core/TreatmentOptimization/getTreatmentOptimizationInputs.m @@ -60,10 +60,10 @@ parseElementTextByNameOrAlternate(tree, "optimizeSynergyVectors", 0)); inputs = parseTreatmentOptimizationDataDirectory(tree, inputs); inputs.initialGuess = getGpopsInitialGuess(tree); -inputs = getContinuousCostTerms(getFieldByNameOrError(tree, ... - 'RCNLContinuousCostTermSet'), inputs); -inputs = getDiscreteCostTerms(getFieldByName(tree, ... - 'RCNLDiscreteCostTermSet'), inputs); +inputs.experimentalTime = inputs.experimentalTime / ... + inputs.experimentalTime(end); +inputs.costTerms = parseRcnlCostTermSet( ... + getFieldByNameOrError(tree, 'RCNLCostTermSet').RCNLCostTerm); inputs.path = getPathConstraintTerms(tree); inputs.terminal = getTerminalConstraintTerms(tree); contactSurfaces = getFieldByName(inputs.osimx, "contactSurface"); @@ -74,4 +74,4 @@ else inputs.contactSurfaces = {}; end -end \ No newline at end of file +end diff --git a/src/core/TreatmentOptimization/getTreatmentOptimizationValueStruct.m b/src/core/TreatmentOptimization/getTreatmentOptimizationValueStruct.m new file mode 100644 index 000000000..254ee91a0 --- /dev/null +++ b/src/core/TreatmentOptimization/getTreatmentOptimizationValueStruct.m @@ -0,0 +1,48 @@ +% This function is part of the NMSM Pipeline, see file for full license. +% +% () -> () +% + +% ----------------------------------------------------------------------- % +% The NMSM Pipeline is a toolkit for model personalization and treatment % +% optimization of neuromusculoskeletal models through OpenSim. See % +% nmsm.rice.edu and the NOTICE file for more information. The % +% NMSM Pipeline is developed at Rice University and supported by the US % +% National Institutes of Health (R01 EB030520). % +% % +% Copyright (c) 2021 Rice University and the Authors % +% Author(s): Marleny Vega, Claire V. Hammond % +% % +% Licensed under the Apache License, Version 2.0 (the "License"); % +% you may not use this file except in compliance with the License. % +% You may obtain a copy of the License at % +% http://www.apache.org/licenses/LICENSE-2.0. % +% % +% Unless required by applicable law or agreed to in writing, software % +% distributed under the License is distributed on an "AS IS" BASIS, % +% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or % +% implied. See the License for the specific language governing % +% permissions and limitations under the License. % +% ----------------------------------------------------------------------- % + +function values = getTreatmentOptimizationValueStruct(inputs, params) + +values.time = scaleToOriginal(inputs.time, params.maxTime, ... + params.minTime); +state = scaleToOriginal(inputs.state, ones(size(inputs.state, 1), 1) .* ... + params.maxState, ones(size(inputs.state, 1), 1) .* params.minState); +control = scaleToOriginal(inputs.control, ones(size(inputs.control, 1), 1) .* ... + params.maxControl, ones(size(inputs.control, 1), 1) .* params.minControl); +values.statePositions = getCorrectStates(state, 1, params.numCoordinates); +values.stateVelocities = getCorrectStates(state, 2, params.numCoordinates); +values.stateAccelerations = getCorrectStates(state, 3, params.numCoordinates); +values.controlJerks = control(:, 1 : params.numCoordinates); + +if ~strcmp(params.controllerType, 'synergy_driven') + values.controlTorques = control(:, params.numCoordinates + 1 : ... + params.numCoordinates + params.numTorqueControls); +else + values.controlSynergyActivations = control(:, ... + params.numCoordinates + 1 : params.numCoordinates + params.numSynergies); +end +end diff --git a/src/core/TreatmentOptimization/initializeMexOrMatlabParallelFunctions.m b/src/core/TreatmentOptimization/initializeMexOrMatlabParallelFunctions.m new file mode 100644 index 000000000..5fc71ad85 --- /dev/null +++ b/src/core/TreatmentOptimization/initializeMexOrMatlabParallelFunctions.m @@ -0,0 +1,35 @@ +% This function is part of the NMSM Pipeline, see file for full license. +% +% () -> () +% + +% ----------------------------------------------------------------------- % +% The NMSM Pipeline is a toolkit for model personalization and treatment % +% optimization of neuromusculoskeletal models through OpenSim. See % +% nmsm.rice.edu and the NOTICE file for more information. The % +% NMSM Pipeline is developed at Rice University and supported by the US % +% National Institutes of Health (R01 EB030520). % +% % +% Copyright (c) 2021 Rice University and the Authors % +% Author(s): Spencer Williams % +% % +% Licensed under the Apache License, Version 2.0 (the "License"); % +% you may not use this file except in compliance with the License. % +% You may obtain a copy of the License at % +% http://www.apache.org/licenses/LICENSE-2.0. % +% % +% Unless required by applicable law or agreed to in writing, software % +% distributed under the License is distributed on an "AS IS" BASIS, % +% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or % +% implied. See the License for the specific language governing % +% permissions and limitations under the License. % +% ----------------------------------------------------------------------- % + +function initializeMexOrMatlabParallelFunctions(modelFile) +if isequal(mexext, 'mexw64') + pointKinematicsMexWindows(modelFile); + inverseDynamicsMexWindows(modelFile); +end +clear inverseDynamicsMatlabParallel +clear pointKinematicsMatlabParallel +end diff --git a/src/core/TreatmentOptimization/inverseDynamics.m b/src/core/TreatmentOptimization/inverseDynamics.m new file mode 100644 index 000000000..beafa074e --- /dev/null +++ b/src/core/TreatmentOptimization/inverseDynamics.m @@ -0,0 +1,40 @@ +% This function is part of the NMSM Pipeline, see file for full license. +% +% () -> () +% + +% ----------------------------------------------------------------------- % +% The NMSM Pipeline is a toolkit for model personalization and treatment % +% optimization of neuromusculoskeletal models through OpenSim. See % +% nmsm.rice.edu and the NOTICE file for more information. The % +% NMSM Pipeline is developed at Rice University and supported by the US % +% National Institutes of Health (R01 EB030520). % +% % +% Copyright (c) 2021 Rice University and the Authors % +% Author(s): Spencer Williams, Marleny Vega % +% % +% Licensed under the Apache License, Version 2.0 (the "License"); % +% you may not use this file except in compliance with the License. % +% You may obtain a copy of the License at % +% http://www.apache.org/licenses/LICENSE-2.0. % +% % +% Unless required by applicable law or agreed to in writing, software % +% distributed under the License is distributed on an "AS IS" BASIS, % +% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or % +% implied. See the License for the specific language governing % +% permissions and limitations under the License. % +% ----------------------------------------------------------------------- % + +function inverseDynamicMoments = inverseDynamics(time, jointAngles, ... + jointVelocities, jointAccelerations, coordinateLabels, appliedLoads, ... + modelName) +if isequal(mexext, 'mexw64') + inverseDynamicMoments = inverseDynamicsMexWindows(time, jointAngles, ... + jointVelocities, jointAccelerations, coordinateLabels, ... + appliedLoads); +else + inverseDynamicMoments = inverseDynamicsMatlabParallel(time, ... + jointAngles, jointVelocities, jointAccelerations, coordinateLabels, ... + appliedLoads, modelName); +end +end diff --git a/src/core/TreatmentOptimization/inverseDynamicsMatlab.m b/src/core/TreatmentOptimization/inverseDynamicsMatlab.m new file mode 100644 index 000000000..425951a4c --- /dev/null +++ b/src/core/TreatmentOptimization/inverseDynamicsMatlab.m @@ -0,0 +1,65 @@ +function IDLoads = inverseDynamicsMatlab(time,q,qp,qpp,IKLabels,AppliedLoads,modelFile) + % Load Library + import org.opensim.modeling.*; + + % Open a Model by name + osimModel = Model(modelFile); + + % Initialize the system and get the initial state + osimState = osimModel.initSystem(); + idSolver = InverseDynamicsSolver(osimModel); + + % Get the number of coords and markers + numPts = size(time,1); + numControls = size(AppliedLoads,2); + numCoords = osimState.getNQ; + + AccelsTempVec = zeros(numPts,numCoords); + for j=1:numPts + osimState.setTime(time(j,1)); + + for k=1:size(IKLabels,2) + if ~osimModel.getCoordinateSet.get(IKLabels{k}).get_locked + osimModel.getCoordinateSet.get(IKLabels{k}).setValue(osimState,q(j,k)); + osimModel.getCoordinateSet.get(IKLabels{k}).setSpeedValue(osimState,qp(j,k)); + end + end + osimModel.realizeVelocity(osimState); + + for i=1:osimState.getNQ + StateQ = osimState.getQ.get(i-1); + + for ii = 1:size(q,2) + if abs(q(j,ii)-StateQ) <= 1e-6 + AccelsTempVec(j,i) = qpp(j,ii); + end + end + end + + tempMarkerGlobalPos=Vec3; + + newControls = Vector(numControls,0); + + for i=0:numControls-1 + newControls.set(i,AppliedLoads(j,i+1)); + end + + osimModel.setControls(osimState,newControls); + osimModel.markControlsAsValid(osimState); + osimModel.realizeDynamics(osimState); + + AccelsVec = Vector(osimState.getNQ,0); + + for i=0:osimState.getNQ-1 + AccelsVec.set(i,AccelsTempVec(j,i+1)); + end + + IDLoadsVec = Vector; + IDLoadsVec = idSolver.solve(osimState,AccelsVec); + + for i=0:numCoords-1 + IDLoads(j,i+1) = IDLoadsVec.get(i); + end + + end +end \ No newline at end of file diff --git a/src/core/TreatmentOptimization/inverseDynamicsMatlabParallel.m b/src/core/TreatmentOptimization/inverseDynamicsMatlabParallel.m new file mode 100644 index 000000000..eab94a3fd --- /dev/null +++ b/src/core/TreatmentOptimization/inverseDynamicsMatlabParallel.m @@ -0,0 +1,109 @@ +% This function is part of the NMSM Pipeline, see file for full license. +% +% () -> () +% + +% ----------------------------------------------------------------------- % +% The NMSM Pipeline is a toolkit for model personalization and treatment % +% optimization of neuromusculoskeletal models through OpenSim. See % +% nmsm.rice.edu and the NOTICE file for more information. The % +% NMSM Pipeline is developed at Rice University and supported by the US % +% National Institutes of Health (R01 EB030520). % +% % +% Copyright (c) 2021 Rice University and the Authors % +% Author(s): Spencer Williams, Marleny Vega % +% % +% Licensed under the Apache License, Version 2.0 (the "License"); % +% you may not use this file except in compliance with the License. % +% You may obtain a copy of the License at % +% http://www.apache.org/licenses/LICENSE-2.0. % +% % +% Unless required by applicable law or agreed to in writing, software % +% distributed under the License is distributed on an "AS IS" BASIS, % +% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or % +% implied. See the License for the specific language governing % +% permissions and limitations under the License. % +% ----------------------------------------------------------------------- % + +function inverseDynamicMoments = inverseDynamicsMatlabParallel(time, ... + jointAngles, jointVelocities, jointAccelerations, coordinateLabels, ... + appliedLoads, modelName) + +% Get the number of coords and markers +numPts = size(time,1); +numControls = size(appliedLoads,2); +numCoords = length(coordinateLabels); +% Split time points into parallel problems +numWorkers = gcp().NumWorkers; +inverseDynamicJobs = cell(1, numWorkers); +accelsTempVec = cell(1, numWorkers); +parfor worker = 1:numWorkers + inverseDynamicJobs{worker} = idWorkerHelper(modelName, numPts, ... + numControls, numCoords, numWorkers, accelsTempVec{worker}, ... + time, jointAngles, jointVelocities, jointAccelerations, ... + coordinateLabels,appliedLoads,worker); +end +inverseDynamicMoments = inverseDynamicJobs{1}; +for job = 2 : numWorkers + inverseDynamicMoments = cat(1, inverseDynamicMoments, ... + inverseDynamicJobs{job}); +end +end +function inverseDynamicJobs = idWorkerHelper(modelName, numPts, ... + numControls, numCoords, numWorkers, accelsTempVec, time, jointAngles, ... + jointVelocities, jointAccelerations, coordinateLabels, appliedLoads, ... + worker) + +import org.opensim.modeling.*; +persistent osimModel; +persistent osimState; +persistent inverseDynamicsSolver; +if isempty(osimModel) + osimModel = Model(modelName); + osimState = osimModel.initSystem(); + inverseDynamicsSolver = InverseDynamicsSolver(osimModel); +end +inverseDynamicJobs = zeros(length(1 + (worker - 1) * ceil(numPts / numWorkers) : ... + min(worker * ceil(numPts / numWorkers), numPts)), numCoords); +indexOffset = (worker - 1) * ceil(numPts / numWorkers); +for j = 1 + (worker - 1) * ceil(numPts / numWorkers) : min(worker * ceil(numPts / numWorkers), numPts) + osimState.setTime(time(j,1)); + for k=1:size(coordinateLabels,2) + if ~osimModel.getCoordinateSet.get(coordinateLabels{k}).get_locked + osimModel.getCoordinateSet.get(coordinateLabels{k}). ... + setValue(osimState,jointAngles(j, k)); + osimModel.getCoordinateSet.get(coordinateLabels{k}). ... + setSpeedValue(osimState,jointVelocities(j, k)); + end + end + osimModel.realizeVelocity(osimState); + for i=1:osimState.getNQ + statePositions = osimState.getQ.get(i-1); + for ii = 1:size(jointAngles,2) + if abs(jointAngles(j, ii) - statePositions) <= 1e-6 + accelsTempVec(j - indexOffset, i) = jointAccelerations(j, ii); + end + end + end + newControls = Vector(numControls,0); + for i=0 : numControls - 1 + newControls.set(i, appliedLoads(j, i + 1)); + end + osimModel.setControls(osimState, newControls); + osimModel.markControlsAsValid(osimState); + osimModel.realizeDynamics(osimState); + accelsVec = Vector(osimState.getNQ, 0); + includedQIndex = 1; + for i=0:osimState.getNQ-1 + currentCoordinate = osimModel.getCoordinateSet().get(i).getName().toCharArray'; + if any(cellfun(@isequal, coordinateLabels, repmat({currentCoordinate}, 1, length(coordinateLabels)))) + accelsVec.set(i, accelsTempVec(j - indexOffset, includedQIndex)); + includedQIndex = includedQIndex + 1; + end + end + IDLoadsVec = inverseDynamicsSolver.solve(osimState,accelsVec); + for i=0 : numCoords - 1 + inverseDynamicJobs(j - indexOffset, i + 1) = IDLoadsVec.get(i); + end +end +end diff --git a/src/DesignOptimization/inverseDynamics.mexw64 b/src/core/TreatmentOptimization/inverseDynamicsMexWindows.mexw64 similarity index 100% rename from src/DesignOptimization/inverseDynamics.mexw64 rename to src/core/TreatmentOptimization/inverseDynamicsMexWindows.mexw64 diff --git a/src/core/TreatmentOptimization/makeTreatmentOptimizationInputs.m b/src/core/TreatmentOptimization/makeTreatmentOptimizationInputs.m new file mode 100644 index 000000000..52c82f08d --- /dev/null +++ b/src/core/TreatmentOptimization/makeTreatmentOptimizationInputs.m @@ -0,0 +1,43 @@ +% This function is part of the NMSM Pipeline, see file for full license. +% +% () -> () +% + +% ----------------------------------------------------------------------- % +% The NMSM Pipeline is a toolkit for model personalization and treatment % +% optimization of neuromusculoskeletal models through OpenSim. See % +% nmsm.rice.edu and the NOTICE file for more information. The % +% NMSM Pipeline is developed at Rice University and supported by the US % +% National Institutes of Health (R01 EB030520). % +% % +% Copyright (c) 2021 Rice University and the Authors % +% Author(s): Marleny Vega, Claire V. Hammond % +% % +% Licensed under the Apache License, Version 2.0 (the "License"); % +% you may not use this file except in compliance with the License. % +% You may obtain a copy of the License at % +% http://www.apache.org/licenses/LICENSE-2.0. % +% % +% Unless required by applicable law or agreed to in writing, software % +% distributed under the License is distributed on an "AS IS" BASIS, % +% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or % +% implied. See the License for the specific language governing % +% permissions and limitations under the License. % +% ----------------------------------------------------------------------- % + +function inputs = makeTreatmentOptimizationInputs(inputs, params) +if isequal(mexext, 'mexw64') + pointKinematicsMexWindows(inputs.mexModel); + inverseDynamicsMexWindows(inputs.mexModel); +end +inputs = getStateDerivatives(inputs); +inputs = setupGroundContact(inputs); +inputs = getSplines(inputs); +inputs = checkStateGuess(inputs); +inputs = checkControlGuess(inputs); +inputs = checkParameterGuess(inputs); +inputs = getIntegralBounds(inputs); +inputs = getPathConstraintBounds(inputs); +inputs = getTerminalConstraintBounds(inputs); +inputs = getDesignVariableInputBounds(inputs); +end diff --git a/src/core/TreatmentOptimization/pointKinematics.m b/src/core/TreatmentOptimization/pointKinematics.m new file mode 100644 index 000000000..e0c77bd21 --- /dev/null +++ b/src/core/TreatmentOptimization/pointKinematics.m @@ -0,0 +1,40 @@ +% This function is part of the NMSM Pipeline, see file for full license. +% +% () -> () +% + +% ----------------------------------------------------------------------- % +% The NMSM Pipeline is a toolkit for model personalization and treatment % +% optimization of neuromusculoskeletal models through OpenSim. See % +% nmsm.rice.edu and the NOTICE file for more information. The % +% NMSM Pipeline is developed at Rice University and supported by the US % +% National Institutes of Health (R01 EB030520). % +% % +% Copyright (c) 2021 Rice University and the Authors % +% Author(s): Spencer Williams, Marleny Vega % +% % +% Licensed under the Apache License, Version 2.0 (the "License"); % +% you may not use this file except in compliance with the License. % +% You may obtain a copy of the License at % +% http://www.apache.org/licenses/LICENSE-2.0. % +% % +% Unless required by applicable law or agreed to in writing, software % +% distributed under the License is distributed on an "AS IS" BASIS, % +% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or % +% implied. See the License for the specific language governing % +% permissions and limitations under the License. % +% ----------------------------------------------------------------------- % + +function [pointPositions, pointVelocities] = pointKinematics(time, ... + jointAngles,jointVelocities, pointLocationOnBody, body, modelName, ... + coordinateLabels) +if isequal(mexext, 'mexw64') + [pointPositions, pointVelocities] = pointKinematicsMexWindows(time, ... + jointAngles, jointVelocities, pointLocationOnBody', body, ... + coordinateLabels); +else + [pointPositions, pointVelocities] = pointKinematicsMatlabParallel(time, ... + jointAngles, jointVelocities, pointLocationOnBody, body, modelName, ... + coordinateLabels); +end +end diff --git a/src/core/TreatmentOptimization/pointKinematicsMatlabParallel.m b/src/core/TreatmentOptimization/pointKinematicsMatlabParallel.m new file mode 100644 index 000000000..737f13c33 --- /dev/null +++ b/src/core/TreatmentOptimization/pointKinematicsMatlabParallel.m @@ -0,0 +1,103 @@ +% This function is part of the NMSM Pipeline, see file for full license. +% +% () -> () +% + +% ----------------------------------------------------------------------- % +% The NMSM Pipeline is a toolkit for model personalization and treatment % +% optimization of neuromusculoskeletal models through OpenSim. See % +% nmsm.rice.edu and the NOTICE file for more information. The % +% NMSM Pipeline is developed at Rice University and supported by the US % +% National Institutes of Health (R01 EB030520). % +% % +% Copyright (c) 2021 Rice University and the Authors % +% Author(s): Spencer Williams, Marleny Vega % +% % +% Licensed under the Apache License, Version 2.0 (the "License"); % +% you may not use this file except in compliance with the License. % +% You may obtain a copy of the License at % +% http://www.apache.org/licenses/LICENSE-2.0. % +% % +% Unless required by applicable law or agreed to in writing, software % +% distributed under the License is distributed on an "AS IS" BASIS, % +% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or % +% implied. See the License for the specific language governing % +% permissions and limitations under the License. % +% ----------------------------------------------------------------------- % + +function [pointPositions, pointVelocities] = ... + pointKinematicsMatlabParallel(time, jointAngles, jointVelocities, ... + pointLocationOnBody, body, modelName, coordinateNames) + +% Get the number of coords and markers +numPts = size(time,1); +numSprings = size(pointLocationOnBody,1); +% Split time points into parallel problems +numWorkers = gcp().NumWorkers; +pointPositionsJobs = cell(1, numWorkers); +pointVelocitiesJobs = cell(1, numWorkers); +parfor worker = 1:numWorkers + [pointPositionsJobs{worker}, pointVelocitiesJobs{worker}] = ... + pointKinematicsWorkerHelper(modelName, numPts, numSprings, ... + numWorkers, time, jointAngles, jointVelocities, ... + pointLocationOnBody, body, coordinateNames, worker); +end +pointPositions = pointPositionsJobs{1}; +pointVelocities = pointVelocitiesJobs{1}; +for job = 2 : numWorkers + pointPositions = cat(1, pointPositions, pointPositionsJobs{job}); + pointVelocities = cat(1, pointVelocities, pointVelocitiesJobs{job}); +end +end + +function [SpringPosJob, SpringVelJob] = ... + pointKinematicsWorkerHelper(modelFile, numPts, numSprings, ... + numWorkers, time, jointAngles, jointVelocities, pointLocationOnBody, ... + body, coordinateNames, worker) + +import org.opensim.modeling.*; +persistent osimModel; +persistent osimState; +persistent refBodySet; +if isempty(osimModel) + osimModel = Model(modelFile); + osimState = osimModel.initSystem(); + refBodySet = osimModel.getBodySet; +end +SpringPosJob = zeros(length(1 + (worker - 1) * ceil(numPts / numWorkers) : ... + min(worker * ceil(numPts / numWorkers), numPts)), numSprings * 3); +SpringVelJob = zeros(length(1 + (worker - 1) * ceil(numPts / numWorkers) : ... + min(worker * ceil(numPts / numWorkers), numPts)), numSprings * 3); +indexOffset = (worker - 1) * ceil(numPts / numWorkers); +for j = 1 + (worker - 1) * ceil(numPts / numWorkers) : min(worker * ceil(numPts / numWorkers), numPts) + osimState.setTime(time(j,1)); + for k=1:size(coordinateNames,2) + if ~osimModel.getCoordinateSet.get(coordinateNames{k}).get_locked + osimModel.getCoordinateSet.get(coordinateNames{k}). ... + setValue(osimState,jointAngles(j,k)); + osimModel.getCoordinateSet.get(coordinateNames{k}). ... + setSpeedValue(osimState,jointVelocities(j,k)); + end + end + osimModel.realizeVelocity(osimState); + for i=1:numSprings + tempRefParentBody = refBodySet.get(body(i)); + tempLocalPos = Vec3(3, 0); + tempGlobalPos = Vec3(3, 0); + tempGlobalVel = Vec3(3, 0); + tempLocalPos.set(0,pointLocationOnBody(i, 1)); + tempLocalPos.set(1,pointLocationOnBody(i, 2)); + tempLocalPos.set(2,pointLocationOnBody(i, 3)); + osimModel.getSimbodyEngine.getPosition(osimState, ... + tempRefParentBody, tempLocalPos, tempGlobalPos); + osimModel.getSimbodyEngine.getVelocity(osimState, ... + tempRefParentBody, tempLocalPos, tempGlobalVel); + for k=0:2 + SpringPosJob(j - indexOffset, (i - 1) * 3 + k + 1) = ... + tempGlobalPos.get(k); + SpringVelJob(j - indexOffset, (i - 1) * 3 + k + 1) = ... + tempGlobalVel.get(k); + end + end +end +end diff --git a/src/DesignOptimization/pointKinematics.mexw64 b/src/core/TreatmentOptimization/pointKinematicsMexWindows.mexw64 similarity index 100% rename from src/DesignOptimization/pointKinematics.mexw64 rename to src/core/TreatmentOptimization/pointKinematicsMexWindows.mexw64 diff --git a/src/DesignOptimization/reportDesignOptimizationResults.m b/src/core/TreatmentOptimization/reportTreatmentOptimizationResults.m similarity index 90% rename from src/DesignOptimization/reportDesignOptimizationResults.m rename to src/core/TreatmentOptimization/reportTreatmentOptimizationResults.m index 87e16a6da..ca02c6ac3 100644 --- a/src/DesignOptimization/reportDesignOptimizationResults.m +++ b/src/core/TreatmentOptimization/reportTreatmentOptimizationResults.m @@ -1,7 +1,7 @@ % This function is part of the NMSM Pipeline, see file for full license. % % () -> () -% +% % ----------------------------------------------------------------------- % % The NMSM Pipeline is a toolkit for model personalization and treatment % @@ -25,10 +25,17 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function reportDesignOptimizationResults(solution, inputs) - +function reportTreatmentOptimizationResults(solution, inputs) +if isfield(inputs, 'userDefinedVariables') + for i = 1:length(inputs.userDefinedVariables) + parameterResults = scaleToOriginal( ... + solution.solution.phase.parameter(i, 1), ... + inputs.userDefinedVariables{i}.upper_bounds, ... + inputs.userDefinedVariables{i}.lower_bounds) + end +end values = getDesignOptimizationValueStruct(solution.solution.phase, inputs); -if strcmp(inputs.controllerType, 'synergy_driven') +if strcmp(inputs.controllerType, 'synergy_driven') % plot Muscle Activations plotMuscleActivations(solution.muscleActivations, values.time, ... inputs.experimentalMuscleActivations, inputs.experimentalTime, ... @@ -103,16 +110,16 @@ function plotResultsWithOutComparison(solutionData, solutionTime, ... function figureXLabels(numTotalPlots, numColumnPlots, plotIndex, xLabel) if plotIndex > numTotalPlots - numColumnPlots - xlabel(xLabel); -else - xticklabels(''); + xlabel(xLabel); +else + xticklabels(''); end end function figureYLabels(numTotalPlots, numColumnPlots, plotIndex, yLabel) if ismember(plotIndex, 1 : numColumnPlots : numTotalPlots) ylabel({yLabel(1); yLabel(2)}); -else - yticklabels(''); +else + yticklabels(''); +end end -end \ No newline at end of file diff --git a/src/core/TreatmentOptimization/scaleToBounds.m b/src/core/TreatmentOptimization/scaleToBounds.m index ab1615015..15f36aad8 100644 --- a/src/core/TreatmentOptimization/scaleToBounds.m +++ b/src/core/TreatmentOptimization/scaleToBounds.m @@ -26,6 +26,5 @@ % ----------------------------------------------------------------------- % function scaledValue = scaleToBounds(value, maximum, minimum) - scaledValue = (value - (maximum + minimum) / 2) ./ (maximum - minimum); end \ No newline at end of file diff --git a/src/core/TreatmentOptimization/setupGroundContact.m b/src/core/TreatmentOptimization/setupGroundContact.m index a145119f6..06da6f396 100644 --- a/src/core/TreatmentOptimization/setupGroundContact.m +++ b/src/core/TreatmentOptimization/setupGroundContact.m @@ -29,8 +29,9 @@ for i = 1:length(inputs.contactSurfaces) midfootSuperiorLocation = pointKinematics(inputs.experimentalTime, ... inputs.experimentalJointAngles, inputs.experimentalJointVelocities, ... - inputs.contactSurfaces{i}.midfootSuperiorPointOnBody', ... - inputs.contactSurfaces{i}.midfootSuperiorBody, inputs.coordinateNames); + inputs.contactSurfaces{i}.midfootSuperiorPointOnBody, ... + inputs.contactSurfaces{i}.midfootSuperiorBody, inputs.mexModel, ... + inputs.coordinateNames); midfootSuperiorLocation(:, 2) = 0; inputs.contactSurfaces{i}.experimentalGroundReactionMoments = ... transferMoments(inputs.contactSurfaces{i}.electricalCenter, ... diff --git a/src/core/emg/processEmg.m b/src/core/emg/processEmg.m index d9f84a91c..a431d20b2 100644 --- a/src/core/emg/processEmg.m +++ b/src/core/emg/processEmg.m @@ -40,9 +40,9 @@ % High pass filter the data order = valueOrAlternate(params, "filterOrder", 4); -highPassCutoff = valueOrAlternate(params, "highPassCutoff", 10); +highPassCutoff = valueOrAlternate(params, "highPassCutoff", 40); [b,a] = butter(order, 2 * highPassCutoff/sampleRate, 'high'); -emgData = filtfilt(b, a, emgData); +emgData = filtfilt(b, a, emgData')'; % Demean emgData = emgData-ones(size(emgData, 1), 1) * mean(emgData); @@ -51,13 +51,16 @@ emgData = abs(emgData); % Low pass filter -lowPassCutoff = valueOrAlternate(params, "lowPassCutoff", 40); -[b,a] = butter(order, 2 * lowPassCutoff / sampleRate); -emgData = filtfilt(b, a, emgData); +lowPassCutoff = valueOrAlternate(params, "lowPassCutoff", 10); +[b,a] = butter(order, 2 * lowPassCutoff / sampleRate, 'low'); +emgData = filtfilt(b, a, emgData')'; % Remove any negative EMG values that may still exist emgData(emgData < 0) = 0; +% Normalize by maximum value for each channel +emgData = emgData ./ max(emgData, [], 2); + processedEmgData = emgData'; end diff --git a/src/core/osimx/addGcpContactSurface.m b/src/core/osimx/addGcpContactSurface.m deleted file mode 100644 index 2f241e01a..000000000 --- a/src/core/osimx/addGcpContactSurface.m +++ /dev/null @@ -1,67 +0,0 @@ -% This function is part of the NMSM Pipeline, see file for full license. -% -% This function adds a GcpContactSurface to an existing osimx struct -% created by buildGcpOsimxTemplate() or buildOsimxTemplate() -% -% (string, string, number, number, number) -> (struct) -% Prints a generic template for an osimx file - -% ----------------------------------------------------------------------- % -% The NMSM Pipeline is a toolkit for model personalization and treatment % -% optimization of neuromusculoskeletal models through OpenSim. See % -% nmsm.rice.edu and the NOTICE file for more information. The % -% NMSM Pipeline is developed at Rice University and supported by the US % -% National Institutes of Health (R01 EB030520). % -% % -% Copyright (c) 2021 Rice University and the Authors % -% Author(s): Claire V. Hammond % -% % -% Licensed under the Apache License, Version 2.0 (the "License"); % -% you may not use this file except in compliance with the License. % -% You may obtain a copy of the License at % -% http://www.apache.org/licenses/LICENSE-2.0. % -% % -% Unless required by applicable law or agreed to in writing, software % -% distributed under the License is distributed on an "AS IS" BASIS, % -% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or % -% implied. See the License for the specific language governing % -% permissions and limitations under the License. % -% ----------------------------------------------------------------------- % - -function osimx = addGcpContactSurface(osimx, surface, springConstants) - -osimx.NMSMPipelineDocument.OsimxModel.RCNLGroundContact.Comment = ... - 'The modeled ground contact data'; -groundContact = osimx.NMSMPipelineDocument.OsimxModel.RCNLGroundContact; - -groundContact.GCPContactSurfaceSet.GCPContactSurface.is_left_foot.Comment = ... - 'Flag indicating whether foot model should be mirrored'; -isLeftFoot = "false"; if surface.isLeftFoot; isLeftFoot = "true"; end -groundContact.GCPContactSurfaceSet.GCPContactSurface.is_left_foot.Text = ... - convertStringsToChars(isLeftFoot); -groundContact.GCPContactSurfaceSet.GCPContactSurface.toes_coordinate.Comment = ... - 'Name of the toe angle coordinate in the model file'; -groundContact.GCPContactSurfaceSet.GCPContactSurface.toes_coordinate.Text = ... - surface.toesCoordinateName; -groundContact.GCPContactSurfaceSet.GCPContactSurface.toes_joint.Comment = ... - 'Name of the toe joint in the model file'; -groundContact.GCPContactSurfaceSet.GCPContactSurface.toes_joint.Text = ... - surface.toesJointName; - -groundContact.GCPContactSurfaceSet.Comment = 'The set of contact surfaces modeled'; - -groundContact.GCPContactSurfaceSet.GCPContactSurface.Comment = ... - 'The set of contact surfaces modeled'; -contactSurface = groundContact.GCPContactSurfaceSet.GCPContactSurface; -contactSurface.GCPSpringSet.Comment = 'The set of springs for the contact surface'; - -model = Model(surface.model); -for marker = 1:surface.numSpringMarkers - contactSurface = addGcpSpring(contactSurface, model, marker, ... - springConstants(marker)); -end -contactSurface.GCPSpringSet.groups = ''; -groundContact.GCPContactSurfaceSet.GCPContactSurface = contactSurface; -osimx.NMSMPipelineDocument.OsimxModel.RCNLGroundContact = groundContact; -end - diff --git a/src/core/osimx/buildGcpContactSurface.m b/src/core/osimx/buildGcpContactSurface.m index 834225d83..5a4bf4c01 100644 --- a/src/core/osimx/buildGcpContactSurface.m +++ b/src/core/osimx/buildGcpContactSurface.m @@ -37,6 +37,10 @@ groundContact.RCNLContactSurface = {}; else i = length(groundContact.RCNLContactSurface) + 1; + if length(groundContact.RCNLContactSurface) == 1 + groundContact.RCNLContactSurface = ... + {groundContact.RCNLContactSurface}; + end end contact = groundContact.RCNLContactSurface; @@ -103,12 +107,12 @@ newContactSurface = contact{i}; newContactSurface.GCPSpringSet.Comment = ... 'The set of springs for the contact surface'; -for i = 1:length(contactSurface.springs) +for j = 1:length(contactSurface.springs) newContactSurface = buildGcpSpring(newContactSurface, ... - contactSurface.springs{i}); + contactSurface.springs{j}); end newContactSurface.GCPSpringSet.groups = ''; osimx.NMSMPipelineDocument.OsimxModel.RCNLContactSurfaceSet.objects. ... - RCNLContactSurface = newContactSurface; + RCNLContactSurface{i} = newContactSurface; end diff --git a/src/core/osimx/buildGcpOsimxTemplate.m b/src/core/osimx/buildGcpOsimxTemplate.m index a08a7d434..ba9d009e9 100644 --- a/src/core/osimx/buildGcpOsimxTemplate.m +++ b/src/core/osimx/buildGcpOsimxTemplate.m @@ -15,7 +15,7 @@ % National Institutes of Health (R01 EB030520). % % % % Copyright (c) 2021 Rice University and the Authors % -% Author(s): Claire V. Hammond % +% Author(s): Claire V. Hammond, Spencer Williams % % % % Licensed under the Apache License, Version 2.0 (the "License"); % % you may not use this file except in compliance with the License. % @@ -29,25 +29,14 @@ % permissions and limitations under the License. % % ----------------------------------------------------------------------- % -function osimx = buildGcpOsimxTemplate(modelName, ... - osimModelFileName, restingSpringLength, ... - dynamicFrictionCoefficient, dampingFactor) +function osimx = buildGcpOsimxTemplate(modelName, osimModelFileName) osimx = buildOsimxTemplate(modelName, osimModelFileName); -body = osimx.NMSMPipelineDocument.OsimxModel; - -body.RCNLGroundContact.resting_spring_length.Comment = ... - 'The resting spring length of the surface'; -body.RCNLGroundContact.resting_spring_length.Text = convertStringsToChars(num2str(restingSpringLength)); -body.RCNLGroundContact.dynamic_friction_coefficient.Comment = ... - 'The dynamic friction coefficient of the surface'; -body.RCNLGroundContact.dynamic_friction_coefficient.Text = ... - convertStringsToChars(num2str(dynamicFrictionCoefficient)); -body.RCNLGroundContact.damping_factor.Comment = 'The damping factor of the surface'; -body.RCNLGroundContact.damping_factor.Text = convertStringsToChars(num2str(dampingFactor)); - -osimx.NMSMPipelineDocument.OsimxModel = body; +osimx.NMSMPipelineDocument.OsimxModel.RCNLContactSurfaceSet.Comment = ... + 'Optimized contact surface parameters'; +osimx.NMSMPipelineDocument.OsimxModel.RCNLContactSurfaceSet.objects = ''; +osimx.NMSMPipelineDocument.OsimxModel.RCNLContactSurfaceSet.groups = ''; end diff --git a/src/core/osimx/buildRcnlMuscle.m b/src/core/osimx/buildRcnlMuscle.m index c1e00a861..7d432acdc 100644 --- a/src/core/osimx/buildRcnlMuscle.m +++ b/src/core/osimx/buildRcnlMuscle.m @@ -15,7 +15,7 @@ % National Institutes of Health (R01 EB030520). % % % % Copyright (c) 2021 Rice University and the Authors % -% Author(s): Claire V. Hammond, Marleny Vega % +% Author(s): Claire V. Hammond, Marleny Vega, Spencer Williams % % % % Licensed under the Apache License, Version 2.0 (the "License"); % % you may not use this file except in compliance with the License. % @@ -42,34 +42,47 @@ muscles = muscleObjects.RCNLMuscle; muscles{i}.Attributes.name = convertStringsToChars(muscleName); -muscles{i}.electromechanical_delay.Comment = 'Optimized electromechanical delay'; -muscles{i}.electromechanical_delay.Text = convertStringsToChars( ... - num2str(muscleParameters.electromechanicalDelay, 15)); +if isfield(muscleParameters, 'electromechanicalDelay') + muscles{i}.electromechanical_delay.Comment = 'Optimized electromechanical delay'; + muscles{i}.electromechanical_delay.Text = convertStringsToChars( ... + num2str(muscleParameters.electromechanicalDelay, 15)); +end -muscles{i}.activation_time_constant.Comment = 'Optimized activation time constant'; -muscles{i}.activation_time_constant.Text = convertStringsToChars( ... - num2str(muscleParameters.activationTimeConstant, 15)); +if isfield(muscleParameters, 'activationTimeConstant') + muscles{i}.activation_time_constant.Comment = 'Optimized activation time constant'; + muscles{i}.activation_time_constant.Text = convertStringsToChars( ... + num2str(muscleParameters.activationTimeConstant, 15)); +end -muscles{i}.activation_nonlinearity_constant.Comment = 'Optimized activation nonlinearity constant'; -muscles{i}.activation_nonlinearity_constant.Text = convertStringsToChars( ... - num2str(muscleParameters.activationNonlinearityConstant, 15)); +if isfield(muscleParameters, 'activationNonlinearityConstant') + muscles{i}.activation_nonlinearity_constant.Comment = 'Optimized activation nonlinearity constant'; + muscles{i}.activation_nonlinearity_constant.Text = convertStringsToChars( ... + num2str(muscleParameters.activationNonlinearityConstant, 15)); +end -muscles{i}.emg_scale_factor.Comment = 'Optimized EMG scale factor'; -muscles{i}.emg_scale_factor.Text = convertStringsToChars( ... - num2str(muscleParameters.emgScaleFactor, 15)); +if isfield(muscleParameters, 'emgScaleFactor') + muscles{i}.emg_scale_factor.Comment = 'Optimized EMG scale factor'; + muscles{i}.emg_scale_factor.Text = convertStringsToChars( ... + num2str(muscleParameters.emgScaleFactor, 15)); +end -muscles{i}.optimal_fiber_length.Comment = 'Optimized optimal fiber length'; -muscles{i}.optimal_fiber_length.Text = convertStringsToChars( ... - num2str(muscleParameters.optimalFiberLength, 15)); +if isfield(muscleParameters, 'optimalFiberLength') + muscles{i}.optimal_fiber_length.Comment = 'Optimized optimal fiber length'; + muscles{i}.optimal_fiber_length.Text = convertStringsToChars( ... + num2str(muscleParameters.optimalFiberLength, 15)); +end -muscles{i}.tendon_slack_length.Comment = 'Optimized tendon slack length'; -muscles{i}.tendon_slack_length.Text = convertStringsToChars( ... - num2str(muscleParameters.tendonSlackLength, 15)); +if isfield(muscleParameters, 'tendonSlackLength') + muscles{i}.tendon_slack_length.Comment = 'Optimized tendon slack length'; + muscles{i}.tendon_slack_length.Text = convertStringsToChars( ... + num2str(muscleParameters.tendonSlackLength, 15)); +end -muscles{i}.max_isometric_force.Comment = 'Optimized max isometric force'; -muscles{i}.max_isometric_force.Text = convertStringsToChars( ... - num2str(muscleParameters.maxIsometricForce, 15)); +if isfield(muscleParameters, 'maxIsometricForce') + muscles{i}.max_isometric_force.Comment = 'Optimized max isometric force'; + muscles{i}.max_isometric_force.Text = convertStringsToChars( ... + num2str(muscleParameters.maxIsometricForce, 15)); +end osimx.NMSMPipelineDocument.OsimxModel.RCNLMuscleSet.objects.RCNLMuscle = muscles; end - diff --git a/src/core/osimx/parseOsimxFile.m b/src/core/osimx/parseOsimxFile.m index 78bf8b202..a5c9cb598 100644 --- a/src/core/osimx/parseOsimxFile.m +++ b/src/core/osimx/parseOsimxFile.m @@ -13,7 +13,7 @@ % National Institutes of Health (R01 EB030520). % % % % Copyright (c) 2021 Rice University and the Authors % -% Author(s): Claire V. Hammond, Marleny Vega % +% Author(s): Claire V. Hammond, Marleny Vega, Spencer Williams % % % % Licensed under the Apache License, Version 2.0 (the "License"); % % you may not use this file except in compliance with the License. % @@ -43,13 +43,12 @@ if(isstruct(rcnlGroundContactTree)) contactSurfaceTree = getFieldByNameOrError(rcnlGroundContactTree, "objects").RCNLContactSurface; + if isstruct(contactSurfaceTree) + contactSurfaceTree = {contactSurfaceTree}; + end for i = 1:length(contactSurfaceTree) - if length(contactSurfaceTree) == 1 - contactSurface = contactSurfaceTree; - else - contactSurface = contactSurfaceTree{i}; - end + contactSurface = contactSurfaceTree{i}; osimx.groundContact.contactSurface{i} = parseContactSurface(contactSurface); end end @@ -64,13 +63,27 @@ else muscle = musclesTree{i}; end - osimx.muscles.(muscle.Attributes.name).electromechanicalDelay = str2double(muscle.electromechanical_delay.Text); - osimx.muscles.(muscle.Attributes.name).activationTimeConstant = str2double(muscle.activation_time_constant.Text); - osimx.muscles.(muscle.Attributes.name).activationNonlinearityConstant = str2double(muscle.activation_nonlinearity_constant.Text); - osimx.muscles.(muscle.Attributes.name).emgScaleFactor = str2double(muscle.emg_scale_factor.Text); - osimx.muscles.(muscle.Attributes.name).optimalFiberLength = str2double(muscle.optimal_fiber_length.Text); - osimx.muscles.(muscle.Attributes.name).tendonSlackLength = str2double(muscle.tendon_slack_length.Text); - osimx.muscles.(muscle.Attributes.name).maxIsometricForce = str2double(muscle.max_isometric_force.Text); + if isstruct(getFieldByName(muscle, 'electromechanical_delay')) + osimx.muscles.(muscle.Attributes.name).electromechanicalDelay = str2double(muscle.electromechanical_delay.Text); + end + if isstruct(getFieldByName(muscle, 'activation_time_constant')) + osimx.muscles.(muscle.Attributes.name).activationTimeConstant = str2double(muscle.activation_time_constant.Text); + end + if isstruct(getFieldByName(muscle, 'activation_nonlinearity_constant')) + osimx.muscles.(muscle.Attributes.name).activationNonlinearityConstant = str2double(muscle.activation_nonlinearity_constant.Text); + end + if isstruct(getFieldByName(muscle, 'emg_scale_factor')) + osimx.muscles.(muscle.Attributes.name).emgScaleFactor = str2double(muscle.emg_scale_factor.Text); + end + if isstruct(getFieldByName(muscle, 'optimal_fiber_length')) + osimx.muscles.(muscle.Attributes.name).optimalFiberLength = str2double(muscle.optimal_fiber_length.Text); + end + if isstruct(getFieldByName(muscle, 'tendon_slack_length')) + osimx.muscles.(muscle.Attributes.name).tendonSlackLength = str2double(muscle.tendon_slack_length.Text); + end + if isstruct(getFieldByName(muscle, 'max_isometric_force')) + osimx.muscles.(muscle.Attributes.name).maxIsometricForce = str2double(muscle.max_isometric_force.Text); + end end end end diff --git a/src/core/osimx/writeMuscleTendonPersonalizationOsimxFile.m b/src/core/osimx/writeMuscleTendonPersonalizationOsimxFile.m index 516fea45d..7bacaccd2 100644 --- a/src/core/osimx/writeMuscleTendonPersonalizationOsimxFile.m +++ b/src/core/osimx/writeMuscleTendonPersonalizationOsimxFile.m @@ -43,7 +43,8 @@ function writeMuscleTendonPersonalizationOsimxFile(modelFileName, ... [~, name, ~] = fileparts(modelFileName); outfile = fullfile(results_directory, strcat(name, "_mtp.xml")); end - +osimx.modelName = name; +osimx.model = modelFileName; for i = 1:length(muscleNames) muscleParams = makeMuscleParams(model, muscleNames(i), optimizedParams, i); osimx.muscles.(muscleNames(i)) = muscleParams; @@ -53,16 +54,29 @@ function writeMuscleTendonPersonalizationOsimxFile(modelFileName, ... end function params = makeMuscleParams(model, muscleName, optimizedParams, index) -params.electromechanicalDelay = optimizedParams.electromechanicalDelays(index); -params.activationTimeConstant = optimizedParams.activationTimeConstants(index); -params.activationNonlinearityConstant = ... - optimizedParams.activationNonlinearityConstants(index); - +if isfield(optimizedParams, 'electromechanicalDelays') + params.electromechanicalDelay = optimizedParams.electromechanicalDelays(index); +end +if isfield(optimizedParams, 'activationTimeConstants') + params.activationTimeConstant = optimizedParams.activationTimeConstants(index); +end +if isfield(optimizedParams, 'activationNonlinearityConstants') + params.activationNonlinearityConstant = ... + optimizedParams.activationNonlinearityConstants(index); +end muscle = model.getForceSet().getMuscles().get(muscleName); - -params.emgScaleFactor = optimizedParams.emgScaleFactors(index); -params.optimalFiberLength = muscle.get_optimal_fiber_length() * ... - optimizedParams.optimalFiberLengthScaleFactors(index); -params.tendonSlackLength = muscle.get_tendon_slack_length() * ... - optimizedParams.tendonSlackLengthScaleFactors(index); +if isfield(optimizedParams, 'emgScaleFactors') + params.emgScaleFactor = optimizedParams.emgScaleFactors(index); +end +if isfield(optimizedParams, 'optimalFiberLengthScaleFactors') + params.optimalFiberLength = muscle.get_optimal_fiber_length() * ... + optimizedParams.optimalFiberLengthScaleFactors(index); +end +if isfield(optimizedParams, 'tendonSlackLengthScaleFactors') + params.tendonSlackLength = muscle.get_tendon_slack_length() * ... + optimizedParams.tendonSlackLengthScaleFactors(index); +end +if isfield(optimizedParams, 'maxIsometricForce') + params.maxIsometricForce = optimizedParams.maxIsometricForce(index); +end end \ No newline at end of file diff --git a/src/core/osimx/writeNeuralControlPersonalizationOsimxFile.m b/src/core/osimx/writeNeuralControlPersonalizationOsimxFile.m new file mode 100644 index 000000000..0abf5b7df --- /dev/null +++ b/src/core/osimx/writeNeuralControlPersonalizationOsimxFile.m @@ -0,0 +1,74 @@ +% This function is part of the NMSM Pipeline, see file for full license. +% +% This function prints out the optimized muscle tendon parameters from +% Neural Control Personalization in an osimx file +% +% (string, 2D matrix, string) -> (None) +% Prints Neural Control Personalization results in osimx file + +% ----------------------------------------------------------------------- % +% The NMSM Pipeline is a toolkit for model personalization and treatment % +% optimization of neuromusculoskeletal models through OpenSim. See % +% nmsm.rice.edu and the NOTICE file for more information. The % +% NMSM Pipeline is developed at Rice University and supported by the US % +% National Institutes of Health (R01 EB030520). % +% % +% Copyright (c) 2021 Rice University and the Authors % +% Author(s): Spencer Williams, Marleny Vega % +% % +% Licensed under the Apache License, Version 2.0 (the "License"); % +% you may not use this file except in compliance with the License. % +% You may obtain a copy of the License at % +% http://www.apache.org/licenses/LICENSE-2.0. % +% % +% Unless required by applicable law or agreed to in writing, software % +% distributed under the License is distributed on an "AS IS" BASIS, % +% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or % +% implied. See the License for the specific language governing % +% permissions and limitations under the License. % +% ----------------------------------------------------------------------- % + +function writeNeuralControlPersonalizationOsimxFile(inputs, ... + resultsDirectory, precalInputs) +modelFileName = inputs.model; +model = Model(modelFileName); + +buildFromExisting = false; +if isfield(inputs, 'osimxFileName') + if isfile(inputs.osimxFileName) + osimx = parseOsimxFile(inputs.osimxFileName); + [~, name, ~] = fileparts(inputs.osimxFileName); + outfile = fullfile(resultsDirectory, strcat(name, "_ncp.xml")); + buildFromExisting = true; + end +end +if ~buildFromExisting + % As only muscle parameters are included, the MtpOsimxTemplate can be + % reused + osimx = buildMtpOsimxTemplate(... + replace(model.getName().toCharArray',".","_dot_"), ... + modelFileName); + [~, name, ~] = fileparts(modelFileName); + outfile = fullfile(resultsDirectory, strcat(name, "_ncp.xml")); +end +osimx.modelName = name; +osimx.model = modelFileName; +if ~isfield(osimx, 'muscles') + osimx.muscles = []; +end +for i = 1:length(inputs.muscleTendonColumnNames) + if ~isfield(osimx.muscles, inputs.muscleTendonColumnNames(i)) + osimx.muscles.(inputs.muscleTendonColumnNames(i)) ... + .optimalFiberLength = inputs.optimalFiberLength(i); + osimx.muscles.(inputs.muscleTendonColumnNames(i)) ... + .tendonSlackLength = inputs.tendonSlackLength(i); + if precalInputs.optimizeIsometricMaxForce + osimx.muscles.(inputs.muscleTendonColumnNames(i)) ... + .maxIsometricForce = inputs.maxIsometricForce(i); + end + end +end + +writeOsimxFile(buildOsimxFromOsimxStruct(osimx), outfile) +end + diff --git a/src/core/parse/parseMtpNcpSharedInputs.m b/src/core/parse/parseMtpNcpSharedInputs.m index 0aa5d1559..9823ad79a 100644 --- a/src/core/parse/parseMtpNcpSharedInputs.m +++ b/src/core/parse/parseMtpNcpSharedInputs.m @@ -40,6 +40,12 @@ [inputs.inverseDynamicsMoments, ... inputs.inverseDynamicsMomentsColumnNames] = ... parseMtpStandard(inverseDynamicsFileNames); +for i = 1:length(inputs.inverseDynamicsMomentsColumnNames) + if endsWith(inputs.inverseDynamicsMomentsColumnNames(i), "_moment") + inputs.inverseDynamicsMomentsColumnNames(i) = extractBefore( ... + inputs.inverseDynamicsMomentsColumnNames(i), "_moment"); + end +end directories = findFirstLevelSubDirectoriesFromPrefixes(fullfile( ... dataDirectory, "MAData"), inputs.prefixes); [inputs.muscleTendonLength, inputs.muscleTendonColumnNames] = ... diff --git a/src/core/parse/parseRcnlCostTermSet.m b/src/core/parse/parseRcnlCostTermSet.m index 5c0c2befa..51f57e0e7 100644 --- a/src/core/parse/parseRcnlCostTermSet.m +++ b/src/core/parse/parseRcnlCostTermSet.m @@ -31,7 +31,11 @@ function costTerms = parseRcnlCostTermSet(tree) costTerms = cell(1, length(tree)); for term = 1:length(tree) - currentTerm = tree{term}; + if length(tree) == 1 + currentTerm = tree; + else + currentTerm = tree{term}; + end % Find general cost term elements costTerms{term}.type = getTextFromField(getFieldByNameOrError( ... currentTerm, 'type')); diff --git a/src/core/parse/parseSpaceSeparatedList.m b/src/core/parse/parseSpaceSeparatedList.m index de3df8454..053d58533 100644 --- a/src/core/parse/parseSpaceSeparatedList.m +++ b/src/core/parse/parseSpaceSeparatedList.m @@ -31,7 +31,7 @@ function prefixes = parseSpaceSeparatedList(tree, elementName) prefixField = getFieldByName(tree, elementName); -if(length(prefixField.Text) > 0) +if ~isempty(prefixField.Text) if(strcmp(prefixField.Text(1), ' ')) prefixField.Text = prefixField.Text(2:end); end