Merge pull request #207 from rcnl-org/FreeFinalTime

Added free final time formulation

src/DesignOptimization/calcDesignOptimizationIntegrand.m

@@ -31,8 +31,6 @@
     generateCostTermStruct("continuous", "DesignOptimization");
 integrand = calcTreatmentOptimizationCost( ...
     costTermCalculations, allowedTypes, values, modeledValues, auxdata);
-integrand = scaleToBounds(integrand, auxdata.maxIntegral, auxdata.minIntegral);
+integrand = integrand ./ (auxdata.maxIntegral - auxdata.minIntegral);
 integrand = integrand .^ 2;
-end
-
-
+end

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;

src/DesignOptimization/computeDesignOptimizationEndpointFunction.m

@@ -47,7 +47,7 @@
     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)

src/DesignOptimization/computeDesignOptimizationMainFunction.m

@@ -26,8 +26,8 @@
 % ----------------------------------------------------------------------- %
 
 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, ...
@@ -41,7 +41,7 @@
 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')
@@ -63,6 +63,10 @@
             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)
@@ -78,4 +82,4 @@
         variable.upper_bounds, ...
         variable.lower_bounds)];
 end
-end
+end

src/DesignOptimization/getDesignOptimizationValueStruct.m

@@ -45,10 +45,12 @@
             fnval(params.splineSynergyActivations, values.time);
     end
 end
-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);
+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

src/DesignOptimization/parseDesignOptimizationSettingsTree.m

@@ -87,6 +87,11 @@
 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

src/NeuralControlPersonalization/parseNeuralControlPersonalizationSettingsTree.m

@@ -68,7 +68,7 @@
     parseMtpStandard(findFileListFromPrefixList( ...
     fullfile(mtpResultsDirectory, "muscleActivations"), inputs.prefixes));
 osimxFileName = getFieldByName(tree, "input_osimx_file");
-if ~isstruct(osimxFileName)
+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);

src/TrackingOptimization/TrackingOptimizationTool.m

@@ -32,13 +32,7 @@
 function TrackingOptimizationTool(settingsFileName)
 settingsTree = xml2struct(settingsFileName);
 [inputs, params] = parseTrackingOptimizationSettingsTree(settingsTree);
-
-tic
-
 [outputs, inputs] = TrackingOptimization(inputs, params);
-
-toc
-
 reportTreatmentOptimizationResults(outputs, inputs);
 saveTrackingOptimizationResults(outputs, inputs);
 end

src/TrackingOptimization/getTrackingOptimizationValueStruct.m

@@ -37,7 +37,5 @@
         values.synergyWeights = getSynergyWeightsFromGroups(...
             params.synergyWeightsGuess, params);
     end
-    values.controlSynergyActivations = inputs.control(:, ...
-        params.numCoordinates + 1 : params.numCoordinates + params.numSynergies);
 end
 end

src/VerificationOptimization/calcVerificationOptimizationIntegrand.m

@@ -31,6 +31,6 @@
     generateCostTermStruct("continuous", "VerificationOptimization");
 integrand = calcTreatmentOptimizationCost( ...
     costTermCalculations, allowedTypes, values, modeledValues, auxdata);
-integrand = scaleToBounds(integrand, auxdata.maxIntegral, auxdata.minIntegral);
+integrand = integrand ./ (auxdata.maxIntegral - auxdata.minIntegral);
 integrand = integrand .^ 2;
 end

src/core/TreatmentOptimization/IntegrandTerms/calcTrackingControllerIntegrand.m

@@ -25,20 +25,28 @@
 % permissions and limitations under the License.                          %
 % ----------------------------------------------------------------------- %
 
-function cost = calcTrackingControllerIntegrand(auxdata, values, ...
+function cost = calcTrackingControllerIntegrand(auxdata, values, time, ...
     controllerName)
 
 switch auxdata.controllerType
     case 'synergy_driven'
         indx = find(strcmp(convertCharsToStrings( ...
             auxdata.synergyLabels), controllerName));
-        synergyActivations = fnval(auxdata.splineSynergyActivations, values.time)';
+        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( ...
             auxdata.inverseDynamicMomentLabels), controllerName));
         indx2 = find(strcmp(convertCharsToStrings( ...
             strcat(auxdata.controlTorqueNames, '_moment')), controllerName));
-        experimentalJointMoments = fnval(auxdata.splineJointMoments, values.time)';
+        if auxdata.splineJointMoments.dim > 1
+            experimentalJointMoments = fnval(auxdata.splineJointMoments, time)';
+        else
+            experimentalJointMoments = fnval(auxdata.splineJointMoments, time);
+        end
         cost = experimentalJointMoments(:, indx1) - values.controlTorques(:, indx2);
 end

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) + ...

src/core/TreatmentOptimization/generateCostTermStruct.m

@@ -97,7 +97,7 @@
 costTermCalculations.coordinate_tracking = @(values, modeledValues, auxdata, costTerm) ...
     calcTrackingCoordinateIntegrand( ...
     auxdata, ...
-    values.time, ...
+    values.time/values.time(end), ...
     values.statePositions, ...
     costTerm.coordinate ...
     );
@@ -106,6 +106,7 @@
     calcTrackingControllerIntegrand( ...
     auxdata, ...
     values, ...
+    values.time/values.time(end), ...
     costTerm.controller ...
     );
 

src/core/TreatmentOptimization/getTreatmentOptimizationInputs.m

@@ -60,6 +60,8 @@
     parseElementTextByNameOrAlternate(tree, "optimizeSynergyVectors", 0));
 inputs = parseTreatmentOptimizationDataDirectory(tree, inputs);
 inputs.initialGuess = getGpopsInitialGuess(tree);
+inputs.experimentalTime = inputs.experimentalTime / ...
+    inputs.experimentalTime(end);
 inputs.costTerms = parseRcnlCostTermSet( ...
     getFieldByNameOrError(tree, 'RCNLCostTermSet').RCNLCostTerm);
 inputs.path = getPathConstraintTerms(tree);

src/core/TreatmentOptimization/getTreatmentOptimizationValueStruct.m

@@ -41,6 +41,8 @@
 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

src/core/TreatmentOptimization/inverseDynamics.m

@@ -11,7 +11,7 @@
 % National Institutes of Health (R01 EB030520).                           %
 %                                                                         %
 % Copyright (c) 2021 Rice University and the Authors                      %
-% Author(s): Spencer Williams                                             %
+% 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.        %
@@ -25,13 +25,16 @@
 % permissions and limitations under the License.                          %
 % ----------------------------------------------------------------------- %
 
-function IDLoads = inverseDynamics(time,q,qp,qpp,IKLabels,AppliedLoads, ...
-    modelFile)
+function inverseDynamicMoments = inverseDynamics(time, jointAngles, ...
+    jointVelocities, jointAccelerations, coordinateLabels, appliedLoads, ...
+    modelName)
 if isequal(mexext, 'mexw64')
-    IDLoads = inverseDynamicsMexWindows(time,q,qp,qpp,IKLabels, ...
-        AppliedLoads);
+    inverseDynamicMoments = inverseDynamicsMexWindows(time, jointAngles, ...
+        jointVelocities, jointAccelerations, coordinateLabels, ...
+        appliedLoads);
 else
-    IDLoads = inverseDynamicsMatlabParallel(time,q,qp,qpp,IKLabels, ...
-        AppliedLoads,modelFile);
+    inverseDynamicMoments = inverseDynamicsMatlabParallel(time, ...
+        jointAngles, jointVelocities, jointAccelerations, coordinateLabels, ...
+        appliedLoads, modelName);
 end
 end