diff --git a/src/DesignOptimization/calcDesignOptimizationIntegrand.m b/src/DesignOptimization/calcDesignOptimizationIntegrand.m
index fb9de4dc..66a4be42 100644
--- a/src/DesignOptimization/calcDesignOptimizationIntegrand.m
+++ b/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
\ No newline at end of file
diff --git a/src/DesignOptimization/calcDesignOptimizationObjective.m b/src/DesignOptimization/calcDesignOptimizationObjective.m
index 5856be4c..55d0c888 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/computeDesignOptimizationEndpointFunction.m b/src/DesignOptimization/computeDesignOptimizationEndpointFunction.m
index 4eb27a30..1fcf3e28 100644
--- a/src/DesignOptimization/computeDesignOptimizationEndpointFunction.m
+++ b/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)
diff --git a/src/DesignOptimization/computeDesignOptimizationMainFunction.m b/src/DesignOptimization/computeDesignOptimizationMainFunction.m
index b2b3a8ba..990a459f 100644
--- a/src/DesignOptimization/computeDesignOptimizationMainFunction.m
+++ b/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
\ No newline at end of file
diff --git a/src/DesignOptimization/getDesignOptimizationValueStruct.m b/src/DesignOptimization/getDesignOptimizationValueStruct.m
index ada0f970..78358164 100644
--- a/src/DesignOptimization/getDesignOptimizationValueStruct.m
+++ b/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
diff --git a/src/DesignOptimization/parseDesignOptimizationSettingsTree.m b/src/DesignOptimization/parseDesignOptimizationSettingsTree.m
index 7ad17c1a..84cce51f 100644
--- a/src/DesignOptimization/parseDesignOptimizationSettingsTree.m
+++ b/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
diff --git a/src/NeuralControlPersonalization/parseNeuralControlPersonalizationSettingsTree.m b/src/NeuralControlPersonalization/parseNeuralControlPersonalizationSettingsTree.m
index 6aa28099..6efe6a87 100644
--- a/src/NeuralControlPersonalization/parseNeuralControlPersonalizationSettingsTree.m
+++ b/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);
diff --git a/src/TrackingOptimization/TrackingOptimizationTool.m b/src/TrackingOptimization/TrackingOptimizationTool.m
index cd6f928d..d8d9592f 100644
--- a/src/TrackingOptimization/TrackingOptimizationTool.m
+++ b/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
diff --git a/src/TrackingOptimization/getTrackingOptimizationValueStruct.m b/src/TrackingOptimization/getTrackingOptimizationValueStruct.m
index 520f586f..80d2cb2f 100644
--- a/src/TrackingOptimization/getTrackingOptimizationValueStruct.m
+++ b/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
diff --git a/src/VerificationOptimization/calcVerificationOptimizationIntegrand.m b/src/VerificationOptimization/calcVerificationOptimizationIntegrand.m
index 7ead230d..4853284b 100644
--- a/src/VerificationOptimization/calcVerificationOptimizationIntegrand.m
+++ b/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
\ No newline at end of file
diff --git a/src/core/TreatmentOptimization/IntegrandTerms/calcTrackingControllerIntegrand.m b/src/core/TreatmentOptimization/IntegrandTerms/calcTrackingControllerIntegrand.m
index b4ecbf8c..fda1c9d5 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(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
\ No newline at end of file
diff --git a/src/core/TreatmentOptimization/SetupBounds/getDesignVariableInputBounds.m b/src/core/TreatmentOptimization/SetupBounds/getDesignVariableInputBounds.m
index 76477206..dd407e30 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/generateCostTermStruct.m b/src/core/TreatmentOptimization/generateCostTermStruct.m
index 3c310f01..d469d2b5 100644
--- a/src/core/TreatmentOptimization/generateCostTermStruct.m
+++ b/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 ...
     );
 
diff --git a/src/core/TreatmentOptimization/getTreatmentOptimizationInputs.m b/src/core/TreatmentOptimization/getTreatmentOptimizationInputs.m
index ba90bab8..5d422a4c 100644
--- a/src/core/TreatmentOptimization/getTreatmentOptimizationInputs.m
+++ b/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);
diff --git a/src/core/TreatmentOptimization/getTreatmentOptimizationValueStruct.m b/src/core/TreatmentOptimization/getTreatmentOptimizationValueStruct.m
index 1a601ed5..254ee91a 100644
--- a/src/core/TreatmentOptimization/getTreatmentOptimizationValueStruct.m
+++ b/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
diff --git a/src/core/TreatmentOptimization/inverseDynamics.m b/src/core/TreatmentOptimization/inverseDynamics.m
index 8f204234..beafa074 100644
--- a/src/core/TreatmentOptimization/inverseDynamics.m
+++ b/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
diff --git a/src/core/TreatmentOptimization/inverseDynamicsMatlabParallel.m b/src/core/TreatmentOptimization/inverseDynamicsMatlabParallel.m
index 8ced4df1..eab94a3f 100644
--- a/src/core/TreatmentOptimization/inverseDynamicsMatlabParallel.m
+++ b/src/core/TreatmentOptimization/inverseDynamicsMatlabParallel.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,91 +25,85 @@
 % permissions and limitations under the License.                          %
 % ----------------------------------------------------------------------- %
 
-function IDLoads = inverseDynamicsMatlabParallel(time,q,qp,qpp,IKLabels,AppliedLoads,modelFile)
+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(IKLabels);
-
+numControls = size(appliedLoads,2);
+numCoords = length(coordinateLabels);
 % Split time points into parallel problems
 numWorkers = gcp().NumWorkers;
-IDLoadsJobs = cell(1, numWorkers);
-AccelsTempVec = cell(1, numWorkers);
-
+inverseDynamicJobs = cell(1, numWorkers);
+accelsTempVec = cell(1, numWorkers);
 parfor worker = 1:numWorkers
-    IDLoadsJobs{worker} = idWorkerHelper(modelFile, numPts, numControls, numCoords, numWorkers, AccelsTempVec{worker},time,q,qp,qpp,IKLabels,AppliedLoads,worker);
+    inverseDynamicJobs{worker} = idWorkerHelper(modelName, numPts, ...
+        numControls, numCoords, numWorkers, accelsTempVec{worker}, ...
+        time, jointAngles, jointVelocities, jointAccelerations, ...
+        coordinateLabels,appliedLoads,worker);
 end
-
-IDLoads = IDLoadsJobs{1};
+inverseDynamicMoments = inverseDynamicJobs{1};
 for job = 2 : numWorkers
-    IDLoads = cat(1, IDLoads, IDLoadsJobs{job});
+    inverseDynamicMoments = cat(1, inverseDynamicMoments, ...
+        inverseDynamicJobs{job});
 end
-
 end
-
-function IDLoadsJob = idWorkerHelper(modelFile, numPts, numControls, numCoords, numWorkers, AccelsTempVec,time,q,qp,qpp,IKLabels,AppliedLoads,worker)
+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 idSolver;
-    if isempty(osimModel)
-        osimModel = Model(modelFile);
-        osimState = osimModel.initSystem();
-        idSolver = InverseDynamicsSolver(osimModel);
-    end
-
-    IDLoadsJob = 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(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-indexOffset,i) = qpp(j,ii);
-                end
-            end
-        end
-
-        newControls = Vector(numControls,0);
-
-        for i=0:numControls-1
-            newControls.set(i,AppliedLoads(j,i+1));
+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
-
-        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, IKLabels, repmat({currentCoordinate}, 1, length(IKLabels))))
-                AccelsVec.set(i,AccelsTempVec(j-indexOffset,includedQIndex));
-                includedQIndex = includedQIndex + 1;
+    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
-
-        IDLoadsVec = idSolver.solve(osimState,AccelsVec);
-
-        for i=0:numCoords-1
-            IDLoadsJob(j-indexOffset,i+1) = IDLoadsVec.get(i);
+    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/core/TreatmentOptimization/pointKinematics.m b/src/core/TreatmentOptimization/pointKinematics.m
index 848b7745..e0c77bd2 100644
--- a/src/core/TreatmentOptimization/pointKinematics.m
+++ b/src/core/TreatmentOptimization/pointKinematics.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 [SpringPos, SpringVel] = pointKinematics(time,q,qp,SpringMat,SpringBodyMat,...
-    modelFile,IKLabels)
+function [pointPositions, pointVelocities] = pointKinematics(time, ...
+    jointAngles,jointVelocities, pointLocationOnBody, body, modelName, ...
+    coordinateLabels)
 if isequal(mexext, 'mexw64')
-    [SpringPos, SpringVel] = pointKinematicsMexWindows(time,q,qp, ...
-        SpringMat',SpringBodyMat,IKLabels);
+    [pointPositions, pointVelocities] = pointKinematicsMexWindows(time, ...
+        jointAngles, jointVelocities, pointLocationOnBody', body, ...
+        coordinateLabels);
 else
-    [SpringPos, SpringVel] = pointKinematicsMatlabParallel(time,q,qp, ...
-        SpringMat,SpringBodyMat,modelFile,IKLabels);
+    [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
index 1a33263a..737f13c3 100644
--- a/src/core/TreatmentOptimization/pointKinematicsMatlabParallel.m
+++ b/src/core/TreatmentOptimization/pointKinematicsMatlabParallel.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,75 +25,79 @@
 % permissions and limitations under the License.                          %
 % ----------------------------------------------------------------------- %
 
-function [SpringPos, SpringVel] = pointKinematicsMatlabParallel(time,q,qp,SpringMat,SpringBodyMat,...
-    modelFile,IKLabels)
+function [pointPositions, pointVelocities] = ...
+    pointKinematicsMatlabParallel(time, jointAngles, jointVelocities, ...
+    pointLocationOnBody, body, modelName, coordinateNames)
 
 % Get the number of coords and markers
 numPts = size(time,1);
-numSprings = size(SpringMat,1);
-
+numSprings = size(pointLocationOnBody,1);
 % Split time points into parallel problems
 numWorkers = gcp().NumWorkers;
-SpringPosJobs = cell(1, numWorkers);
-SpringVelJobs = cell(1, numWorkers);
-
+pointPositionsJobs = cell(1, numWorkers);
+pointVelocitiesJobs = cell(1, numWorkers);
 parfor worker = 1:numWorkers
-    [SpringPosJobs{worker}, SpringVelJobs{worker}] = pointKinematicsWorkerHelper(modelFile, numPts, numSprings, numWorkers, time,q,qp,SpringMat,SpringBodyMat,IKLabels,worker);
+    [pointPositionsJobs{worker}, pointVelocitiesJobs{worker}] = ...
+        pointKinematicsWorkerHelper(modelName, numPts, numSprings, ...
+        numWorkers, time, jointAngles, jointVelocities, ...
+        pointLocationOnBody, body, coordinateNames, worker);
 end
-
-SpringPos = SpringPosJobs{1};
-SpringVel = SpringVelJobs{1};
+pointPositions = pointPositionsJobs{1};
+pointVelocities = pointVelocitiesJobs{1};
 for job = 2 : numWorkers
-    SpringPos = cat(1, SpringPos, SpringPosJobs{job});
-    SpringVel = cat(1, SpringVel, SpringVelJobs{job});
+    pointPositions = cat(1, pointPositions, pointPositionsJobs{job});
+    pointVelocities = cat(1, pointVelocities, pointVelocitiesJobs{job});
 end
-
 end
 
-function [SpringPosJob, SpringVelJob] = pointKinematicsWorkerHelper(modelFile, numPts, numSprings, numWorkers, time,q,qp,SpringMat,SpringBodyMat,IKLabels,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(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
+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
-        osimModel.realizeVelocity(osimState);
-
-        for i=1:numSprings
-            tempRefParentBody = refBodySet.get(SpringBodyMat(i));
-            tempLocalPos = Vec3(3,0);
-            tempGlobalPos = Vec3(3,0);
-            tempGlobalVel = Vec3(3,0);
-            tempLocalPos.set(0,SpringMat(i,1));
-            tempLocalPos.set(1,SpringMat(i,2));
-            tempLocalPos.set(2,SpringMat(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
+    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/core/TreatmentOptimization/reportTreatmentOptimizationResults.m b/src/core/TreatmentOptimization/reportTreatmentOptimizationResults.m
index 50782ad8..ca02c6ac 100644
--- a/src/core/TreatmentOptimization/reportTreatmentOptimizationResults.m
+++ b/src/core/TreatmentOptimization/reportTreatmentOptimizationResults.m
@@ -26,11 +26,15 @@
 % ----------------------------------------------------------------------- %
 
 function reportTreatmentOptimizationResults(solution, inputs)
-
-if isfield(solution.solution, "parameter")
-    parameterResults = solution.solution.parameter
+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 = getTrackingOptimizationValueStruct(solution.solution.phase, inputs);
+values = getDesignOptimizationValueStruct(solution.solution.phase, inputs);
 if strcmp(inputs.controllerType, 'synergy_driven')
 % plot Muscle Activations
 plotMuscleActivations(solution.muscleActivations, values.time, ...