Merge pull request #206 from rcnl-org/Merging-parpool-mex-into-dev

Parallel pool alternatives to mex functions

src/DesignOptimization/DesignOptimization.m

@@ -27,6 +27,7 @@
 
 function [output, inputs] = DesignOptimization(inputs, params)
 inputs = makeTreatmentOptimizationInputs(inputs, params);
+initializeMexOrMatlabParallelFunctions(inputs.mexModel);
 if strcmp(inputs.controllerType, 'synergy_driven')
     inputs = setupMuscleSynergies(inputs);
 end

src/TrackingOptimization/TrackingOptimization.m

@@ -27,5 +27,6 @@
 
 function [output, inputs] = TrackingOptimization(inputs, params)
 inputs = makeTreatmentOptimizationInputs(inputs, params);
+initializeMexOrMatlabParallelFunctions(inputs.mexModel);
 output = computeTrackingOptimizationMainFunction(inputs, params);
 end

src/TrackingOptimization/TrackingOptimizationTool.m

@@ -32,7 +32,13 @@
 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/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, :))';

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,40 +42,40 @@
 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)
 
 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, ...
     stateVelocities, params)
 
 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( ...

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, ...

src/VerificationOptimization/VerificationOptimization.m

@@ -27,6 +27,7 @@
 
 function [output, inputs] = VerificationOptimization(inputs, params)
 inputs = makeTreatmentOptimizationInputs(inputs, params);
+initializeMexOrMatlabParallelFunctions(inputs.mexModel);
 if strcmp(inputs.controllerType, 'synergy_driven')
     inputs = setupMuscleSynergies(inputs);
 end

src/core/TreatmentOptimization/IntegrandTerms/calcTrackingInverseDynamicLoadsIntegrand.m

@@ -28,14 +28,23 @@
 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
     experimentalJointMoments = fnval(params.splineJointMoments, time)';
 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

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

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

src/core/TreatmentOptimization/inverseDynamics.m

@@ -0,0 +1,37 @@
+% 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 IDLoads = inverseDynamics(time,q,qp,qpp,IKLabels,AppliedLoads, ...
+    modelFile)
+if isequal(mexext, 'mexw64')
+    IDLoads = inverseDynamicsMexWindows(time,q,qp,qpp,IKLabels, ...
+        AppliedLoads);
+else
+    IDLoads = inverseDynamicsMatlabParallel(time,q,qp,qpp,IKLabels, ...
+        AppliedLoads,modelFile);
+end
+end

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