--- a
+++ b/MATLAB_tool/MuscleParOptTool/private/sampleMuscleQuantities.m
@@ -0,0 +1,228 @@
+%-------------------------------------------------------------------------%
+% Copyright (c) 2019 Modenese L., Ceseracciu, E., Reggiani M., Lloyd, D.G.%
+%                                                                         %
+% 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.                          %
+%                                                                         %
+%    Author: Luca Modenese, March 2013                                    %
+%                        revised July 2014 (improved management of joints)%
+%                        modified May 2015 (management of max resolution) %
+%    email:    l.modenese@imperial.ac.uk                                  % 
+% ----------------------------------------------------------------------- %
+%
+% Given as INPUT an OpenSim muscle OSModel, OSMuscle, a muscle variable and a nr
+% of evaluation points this function returns as
+% musOutput a vector of the muscle variable of interest
+% obtained by sampling the ROM of the joint spanned by the muscle in
+% N_EvalPoints evaluation points.
+% For multidof joint the combinations of ROMs are considered.
+% For multiarticular muscles the combination of ROM are considered.
+% The script is totally general because based on generating strings of code 
+% correspondent to the encountered code. The strings are evaluated at the end.
+%
+% IMPORTANT 1
+% The function can decrease the N_EvalPoints if there are too many dof 
+% involved (ASSUMPTION is that a better sampling will be vanified by the 
+% huge amount of data generated). This is an option that can be controlled
+% by the user by deciding if to use it or not (setting limit_discr = 0/1)
+% and, in case the sampling is limited, by setting a lower limit to the
+% discretization.
+%
+% IMPORTANT 2
+% Another check is done on the dofs: only INDEPENDENT coordinate are
+% considered. This is fundamental for patellofemoral joint that both in
+% LLLM and Arnold's model are constrained dof, dependent on the knee
+% flexion angle. This function assumes to be used with Arnold's model.
+% currentState is initialState;
+%
+% IMPORTANT 3
+% At the purposes of the muscle optimizer it is important that here the
+% model is initialize every time. So there are no risk of working with an
+% old state (important for Schutte muscles for instance, where we observed
+% that it was necessary to re-initialize the muscle after updating the
+% muscle parameters).
+
+function musOutput = sampleMuscleQuantities(osimModel,OSMuscle,muscleQuant, N_EvalPoints)  %#ok<STOUT>
+
+%======= SETTINGS ======
+% limit (1) or not (0) the discretization of the joint space sampling
+limit_discr = 0; 
+% minimum angular discretization
+%min_increm_in_deg = 2.5;
+min_increm_in_deg = 1;
+
+
+%=======================
+
+% initialize the model
+currentState = osimModel.initSystem();
+
+% getting the joint crossed by a muscle
+muscleCrossedJointSet = getJointsSpannedByMuscle(osimModel, OSMuscle);
+
+% index f+or effective dofs
+n_dof = 1;
+DOF_Index = [];
+for n_joint = 1:size(muscleCrossedJointSet,2)
+    
+    % current joint
+    curr_joint = muscleCrossedJointSet{n_joint};
+    
+    % get CoordinateSet for the crossed joint
+    %curr_joint_CoordinateSet = osimModel.getJointSet().get(curr_joint).getCoordinateSet();
+    
+    % Initial estimation of the nr of Dof of the CoordinateSet for that
+    % joint before checking for locked and constraint dofs.
+    %nDOF = osimModel.getJointSet().get(curr_joint).getCoordinateSet().getSize();
+    nDOF = osimModel.getJointSet().get(curr_joint).numCoordinates();
+    % skip welded joint and removes welded joint from muscleCrossedJointSet
+    if nDOF == 0;
+        continue;
+    end
+    
+    % calculating effective dof for that joint 
+    effect_DOF = nDOF;
+    for n_coord = 0:nDOF-1
+        
+        % get coordinate
+        %curr_coord = curr_joint_CoordinateSet.get(n_coord);
+        curr_coord = osimModel.getJointSet().get(curr_joint).get_coordinates(n_coord);
+        
+        curr_coord_name = char(curr_coord.getName());
+        
+        % skip dof if locked
+        if curr_coord.getLocked(currentState)
+            continue;
+        end
+        
+        % if coordinate is constrained then the independent coordinate and
+        % associated joint will be listed in the sampling "map"
+        if curr_coord.isConstrained(currentState) && ~curr_coord.getLocked(currentState)
+            constraint_coord_name = curr_coord_name;
+            % finding the independent coordinate
+            [ind_coord_name, ind_coord_joint_name] = getIndipCoordAndJoint(osimModel, constraint_coord_name); %#ok<NASGU>
+            % updating the coordinate name to be saved in the list
+            curr_coord_name = ind_coord_name;
+            effect_DOF = effect_DOF-1;
+            % ignoring constrained dof if they point to an independent
+            % coordinate that has already been stored
+            if sum(ismember(DOF_Index, osimModel.getCoordinateSet.getIndex(curr_coord_name)))>0
+                continue
+            end
+            % skip dof if independent coordinate locked (the coord
+            % correspondent to the name needs to be extracted)
+            if osimModel.getCoordinateSet.get(curr_coord_name).getLocked(currentState)
+                continue;
+            end
+        end
+                
+        % NB: DOF_Index is used later in the string generated code.
+        % CRUCIAL: the index of dof now is model based ("global") and
+        % different from the joint based used until now.
+        DOF_Index(n_dof) = osimModel.getCoordinateSet.getIndex(curr_coord_name);
+
+        % necessary update/reload the curr_coord to avoid problems with
+        % dependent coordinates
+        curr_coord = osimModel.getCoordinateSet.get(DOF_Index(n_dof));
+        
+        % Getting the values defining the range
+        jointRange(1) = curr_coord.getRangeMin();
+        jointRange(2) = curr_coord.getRangeMax();
+        
+        % Storing range of motion conveniently
+        CoordinateBoundaries(n_dof) = {jointRange}; %#ok<NASGU>
+        
+        % increments in the variables when sampling the mtl space. 
+        % Increments are different for each dof and based on N_eval.
+        %Defining the increments
+        degIncrem(n_dof) = (jointRange(2)-jointRange(1))/(N_EvalPoints-1);
+        
+        % limit or not the discretization of the joint space sampling
+        if limit_discr==1
+            % a limit to the increase can be set though
+            if degIncrem(n_dof)<(min_increm_in_deg/180*pi)
+                degIncrem(n_dof)=(min_increm_in_deg/180*pi); %#ok<*AGROW>
+            end
+        end
+        
+        % updating index of list of dof
+        n_dof = n_dof+1;
+    end
+end
+
+
+% initializes the counter to save the results
+iter = 1;  %#ok<NASGU>
+
+% assigns an initial and a final value variable for each dof X
+% calling them setAngleStartDofX and setLimitDof respectively.
+for n_instr = 1:size(DOF_Index,2);
+    % setting up the variables
+    eval(['setAngleStartDof',num2str(n_instr),' = CoordinateBoundaries{',num2str(n_instr),'}(1);'])
+    eval(['setLimitDof',num2str(n_instr),' = CoordinateBoundaries{',num2str(n_instr),'}(2);'])
+end
+
+
+% setting up for loops in order to explore all the possible combination of
+% joint angles (looping on all the dofs of each joint for all the joint
+% crossed by the muscle).
+% The model pose is updated via: " coordToUpd.setValue(currentState,setAngleDof)"
+% The right dof to update is chosen via: "coordToUpd = osimModel.getCoordinateSet.get(n_instr)"
+stringToExcute1 = '';
+for n_instr = 1:size(DOF_Index,2);
+    stringToExcute1 = [stringToExcute1,[' for setAngleDof',num2str(n_instr),'=setAngleStartDof',num2str(n_instr),':degIncrem(',num2str(n_instr),'):setLimitDof',num2str(n_instr),';   coordToUpd = osimModel.getCoordinateSet.get(DOF_Index(',num2str(n_instr),'));    coordToUpd.setValue(currentState,setAngleDof',num2str(n_instr),');']]; %#ok<AGROW>
+end
+
+% calculating muscle length for the muscle
+switch muscleQuant
+    case 'MTL'
+            stringToExcute2 = 'musOutput(iter) = OSMuscle.getGeometryPath.getLength(currentState);';
+    case 'LfibNorm'
+            stringToExcute2 =  ['OSMuscle.setActivation(currentState,1.0);',...
+                                ' osimModel.equilibrateMuscles(currentState);',...
+                                ' musOutput(iter) = OSMuscle.getNormalizedFiberLength(currentState);'];
+    case 'Lten'
+            stringToExcute2 =  ['OSMuscle.setActivation(currentState,1.0);',...
+                                ' osimModel.equilibrateMuscles(currentState);',...
+                                ' musOutput(iter) = OSMuscle.getTendonLength(currentState);'];
+    case 'Ffib'
+            stringToExcute2 =  ['OSMuscle.setActivation(currentState,1.0);',...
+                                ' osimModel.equilibrateMuscles(currentState);',...
+                                ' musOutput(iter) = OSMuscle.getActiveFiberForce(currentState);'];
+    case 'all'
+            stringToExcute2 = [ 'OSMuscle.setActivation(currentState,1.0);',...
+                                ' osimModel.equilibrateMuscles(currentState);',...
+                                'musOutput(iter,1) = OSMuscle.getGeometryPath.getLength(currentState);',...
+                                ' musOutput(iter,2) = OSMuscle.getNormalizedFiberLength(currentState);',...
+                                ' musOutput(iter,3) = OSMuscle.getTendonLength(currentState);',...
+                                ' musOutput(iter,4) = OSMuscle.getActiveFiberForce(currentState);',...
+                                ' musOutput(iter,5) = OSMuscle.getPennationAngle(currentState);'];
+end
+
+% updating iteration
+stringToExcute3 =['iter = iter+1;',...
+%     'angles.colheaders(iter-1) = {char(osimModel.getCoordinateSet.get(DOF_Index(',num2str(n_instr),')).getName)};'...
+%     'angles.data(iter-1,1) = setAngleDof',num2str(n_instr),';'...
+    ''];
+
+% closing the loops started in stringToExcute1 with appropriate nr of end
+stringToExcute4 = '';
+for n_instr = 1:size(DOF_Index,2);
+    stringToExcute4 =[stringToExcute4,'end;']; %#ok<AGROW>
+end
+
+% assembling the code string
+Code = [stringToExcute1,stringToExcute2,stringToExcute3,stringToExcute4];
+
+% evaluate the code
+eval(Code)
+
+end