Merge branch 'realiseKinematics' - improved realiseKinematics and MFD matlab function

Author: modenaxe

_test_data/Arm26/elbow_flexion.mot

@@ -0,0 +1,2 @@
+*.log
+*.asv

_test_data/gait2392/subject01.osim

@@ -14,10 +14,29 @@
 effective_attachm = 'true';
 test_input = [];
 
-N_frame_test = 5
+N_frame_test = 5;
 muscleLinesOfActionStruct = muscleForceDirection_plugin(osimModel_name,...
                                                                 IK_mot_file,...
                                                                 bodyOfInterest_name,...
                                                                 bodyExpressResultsIn_name,...
                                                                 effective_attachm,...
-                                                                []);
+                                                                'true',...
+                                                                []);
+                                                            
+                                                            
+                                                            
+% osimModel_name = '../_test_data/gait2392/subject01.osim';
+% IK_mot_file = '../_test_data/gait2392/subject01_walk1_ik.mot';
+% bodyOfInterest_name = 'femur_r';
+% bodyExpressResultsIn_name = [];
+% effective_attachm = 'true';
+% test_input = [];
+% 
+% N_frame_test = 5;
+% muscleLinesOfActionStruct = muscleForceDirection_plugin(osimModel_name,...
+%                                                                 IK_mot_file,...
+%                                                                 bodyOfInterest_name,...
+%                                                                 bodyExpressResultsIn_name,...
+%                                                                 effective_attachm,...
+%                                                                 'true',...
+%                                                                 []);

_test_data/gait2392/subject01_walk1_ik.mot

@@ -50,7 +50,7 @@
 
     % point under examination
     curr_point = curr_PathPointSet.get(n_p);
-    attachBody = char(curr_point.getBodyName());
+    attachBody = curr_point.getBodyName();
 
     % if pathpoint is conditional, then check if it is active
     if strcmp(char(curr_point.getConcreteClassName), 'ConditionalPathPoint')
@@ -59,15 +59,15 @@
         if cond_viapoint.isActive(s)
             % if yes add it to point set
             mus_pointset_mat(n_pp, 1:3) = [curr_point.getLocation.get(0), curr_point.getLocation.get(1), curr_point.getLocation.get(2)]; %#ok<*AGROW>
-            mus_bodyset_list(n_pp) = {attachBody};
+            mus_bodyset_list(n_pp) = {char(attachBody)};
             n_pp = n_pp+1;
         else
             continue
         end
     else
         % if not conditional add it
         mus_pointset_mat(n_pp, 1:3) = [curr_point.getLocation.get(0), curr_point.getLocation.get(1), curr_point.getLocation.get(2)];
-        mus_bodyset_list(n_pp) = {attachBody};
+        mus_bodyset_list(n_pp) = {char(attachBody)};
         n_pp = n_pp+1;
     end
 end

matlab_tool/.gitignore

@@ -0,0 +1,43 @@
+function [out, missing_coords_list] = isKinMatchingModelCoords(osimModel, headers_in_struct)
+
+% OpenSim suggested settings
+import org.opensim.modeling.*
+OpenSimObject.setDebugLevel(3);
+
+% getting model coordinates and their number
+coordsModel = osimModel.updCoordinateSet();
+N_coordsModel = coordsModel.getSize();
+
+% check if coordinates are the same (sometimes time is also here)
+if any(strcmp('time', headers_in_struct))
+    coordsNames_in_struct = headers_in_struct(~strcmp('time', headers_in_struct));
+else
+    coordsNames_in_struct = headers_in_struct;
+end
+n_miss = 1;
+n_matched = 1;
+coordNames = {};
+missing_coords_list = {};
+
+for n = 0:N_coordsModel-1
+    %extracting the column for the state variable of interest
+    cur_coordName =  char(coordsModel.get(n).getName());
+    if find(strcmp(cur_coordName, coordsNames_in_struct)) == n+1
+        coordNames{n_matched} = cur_coordName;
+        n_matched = n_matched+1;
+    else
+        missing_coords_list{n_miss} = cur_coordName;
+        n_miss = n_miss+1;
+    end
+end
+
+if length(coordNames) == length(coordsNames_in_struct)
+    disp('Coordinate structure and model are properly matched (coordinates and their order).');
+    out = 1;
+    return
+else
+    out = 0;
+    
+end
+
+end

matlab_tool/TEST_plugin.m

@@ -42,6 +42,11 @@
 % initialise answ
 flag = 0;
 
+% check name
+if isjava(osimBodyName)
+    osimBodyName = char(osimBodyName);
+end
+
 % extract muscle pathpointset
 muspathpointset = osimMuscle.getGeometryPath.getPathPointSet();
 
@@ -51,9 +56,9 @@
     % current attachment body of the muscle point
     curr_attach_body = char(muspathpointset.get(n_p).getBodyName);
     % check if this is the specified body
-    if strcmp(curr_attach_body, char(osimBodyName))
+    if strcmp(curr_attach_body, osimBodyName)
         flag = 1;
-        display(['Muscle ', char(osimMuscle.getName),' is attached to body ', char(osimBodyName)]);
+        display(['Muscle ', char(osimMuscle.getName),' is attached to body ', osimBodyName]);
         return
     end
 end

matlab_tool/getCurrentMusclePathAsMat.m

@@ -37,6 +37,7 @@
                                                                 bodyOfInterest_name,...
                                                                 bodyExpressResultsIn_name,...
                                                                 effective_attachm,...
+                                                                visualise,...
                                                                 test_input)
 
 % Load Library
@@ -67,78 +68,83 @@
     end
 end
 
+% define muscles to include in the analysis
 % get muscles
 muscleSet = osimModel.getMuscles();
-N_muscles = muscleSet.getSize();
+n_keep = 1;
+for n_m = 0:muscleSet.getSize()-1
+    
+    % extract muscle
+    curr_mus = muscleSet.get(n_m);
+    curr_mus_name = char(muscleSet.get(n_m).getName());
+    
+    % check if muscle is attached (1) or not (0) to bone of interest
+    % left in internal loop to handle conditional viapoints, if
+    % necessary. They might be active only at certain states, therefore
+    % it is risky to exclude muscles a priori.
+    % NO! CANNOT CHANGE ORIGIN OR INSERTION
+    musc_attach = isMuscleConnectedToBody(curr_mus, bodyOfInterest_name);
+    
+    if musc_attach == 1
+        target_mus_names{n_keep} = curr_mus_name;
+        
+        % defining headers as well
+        rowheaders{n_keep,1} = curr_mus_name;
+        store_range = 1+3*(n_keep-1):3+3*(n_keep-1);
+        colheaders_MFD_vec(store_range)    = {  [curr_mus_name,'_vx_in_',bodyExpressResultsIn_name],...
+                                                [curr_mus_name,'_vy_in_',bodyExpressResultsIn_name],...
+                                                [curr_mus_name,'_vz_in_',bodyExpressResultsIn_name]};
+        colheaders_MFD_attach(store_range) = {  [curr_mus_name,'_px_in_',bodyExpressResultsIn_name],...
+                                                [curr_mus_name,'_py_in_',bodyExpressResultsIn_name],...
+                                                [curr_mus_name,'_pz_in_',bodyExpressResultsIn_name]};
+        n_keep = n_keep +1;
+    end
+end
 
-% TODO: get subset of muscles if needed
+N_target_muscles = length(target_mus_names);
 
 % current state. It will change according to kinematics
+if strcmp(visualise, 'true')
+    osimModel.setUseVisualizer(true);
+end
 curr_state = osimModel.initSystem();
 
+% number of frames
 N_frames = size(IKStruct.data,1);
-
-% initialize matrix to use as output. Same output as plugin.
-% The idea is to have a matrix with a row for each muscle and three groups
-% of columns: attachment on bone, line of action and transport moment.
-% This matrix will make easy both checking the equilibrium and setting the
-% loads in the FE model
-colheaders = {'bone_attach_X', 'bone_attach_Y', 'bone_attach_Z', 'effect_attach_X', 'effect_attach_Y', 'effect_attach_Z', ...
-                'act_line_X',   'act_line_Y',    'act_line_Z',    'trans_mom_X',   'trans_mom_Y',   'trans_mom_Z'};
-
-
-% Variables could be preallocated for speed, but I didn't like to have all
-% zeros, which might be confused with actual values from the analysis.
-% mus_info_mat = zeros(muscleSet.getSize(),12, N_frames);
-
 % this is a testing option to reduce processed kinematics frames when 
 % developing related functions
 if ~isempty(test_input)
     N_frames = test_input;
 end
 
-
+% Variables could be preallocated for speed, but I didn't like to have all
+% zeros, which might be confused with actual values from the analysis.
+% mus_info_mat = zeros(muscleSet.getSize(),12, N_frames);
 for n_frame = 1:N_frames
 
     % realize kinematics
     state_new = realizeMatStructKinematics(osimModel, curr_state, IKStruct, n_frame);
-    
+    if strcmp(visualise, 'true')
+        osimModel.updVisualizer().show(state_new);
+    end
     % counter for muscles to be kept at each frame
     n_keep = 1;
 
-    for n_m = 0:N_muscles-1
+    % display time progression
+    disp('--------------------');
+    disp(['FRAME: ',num2str(n_frame),'/',num2str(N_frames),'.']); 
+    disp('--------------------');
+    
+    for n_m = 0:N_target_muscles-1
 
         % extract muscle
-        curr_mus = muscleSet.get(n_m);
-        curr_mus_name = muscleSet.get(n_m).getName();
+        curr_mus_name = target_mus_names{n_m+1};
+        curr_mus = muscleSet.get(curr_mus_name);
 
         % loop through the points
-        disp('----------------------')
-        disp(['FRAME: ',num2str(n_frame),'/',num2str(N_frames),'. Processing muscle (',num2str(n_m+1),'/',num2str(N_muscles),'): ',char(curr_mus_name)])
 
-        % check if muscle is attached (1) or not (0) to bone of interest
-        % left in internal loop to handle conditional viapoints, if
-        % necessary. They might be active only at certain states, therefore
-        % it is risky to exclude muscles a priori.
-        musc_attach = isMuscleConnectedToBody(curr_mus, bodyOfInterest_name);
+        disp(['Processing muscle (',num2str(n_m+1),'/',num2str(N_target_muscles),'): ', curr_mus_name]);
 
-        if musc_attach == 0
-            % if not attached then nan for the sake of this investigation
-            continue
-        else
-            % if attached
-            
-            % for final data structure
-            if n_frame == 1
-                rowheaders{n_keep,1} = char(curr_mus_name);
-                store_range = 1+3*(n_keep-1):3+3*(n_keep-1);
-                colheaders_MFD_vec(store_range)    = {[char(curr_mus_name),'_vx_in_',bodyExpressResultsIn_name],...
-                    [char(curr_mus_name),'_vy_in_',bodyExpressResultsIn_name],...
-                    [char(curr_mus_name),'_vz_in_',bodyExpressResultsIn_name]};
-                colheaders_MFD_attach(store_range) = {[char(curr_mus_name),'_px_in_',bodyExpressResultsIn_name],...
-                    [char(curr_mus_name),'_py_in_',bodyExpressResultsIn_name],...
-                    [char(curr_mus_name),'_pz_in_',bodyExpressResultsIn_name]};
-            end
             % make available info about the muscle pointSet (body names 
             % and points coordinates)
             [mus_bodyset_list, mus_pointset_mat] = getCurrentMusclePathAsMat(curr_mus, state_new);
@@ -167,7 +173,7 @@
             if attach_vec(1)==0 && attach_vec(end)==0
                 %  This case is for multi-articular muscles that jump the
                 %  body and can be attached with a viapoint.
-                display(['Muscle has no bone attachments on ',bodyOfInterest_name]);
+                display(['Muscle has only viapoints (no bone attachments) on ',bodyOfInterest_name,'. Skipping...']);
                 continue
             end
 
@@ -230,7 +236,7 @@
             % clear variables to avoid surprises
             clear attachment force_dir_norm transp_mom_norm lastAttach otherBodyAttach_vec force_dir_res
 
-        end
+%         end
     end
 end
 
@@ -257,12 +263,20 @@
 % attachments in the local reference system, the file will contain the 
 % first and last muscle points specified for that muscle in the original model file.
 MFD_attachments.colheaders = colheaders_MFD_attach;
-if (effective_attachm==1) || strcmp(effective_attachm, 'true')
+if strcmp(effective_attachm, 'true')
     MFD_attachments.data = mus_info_mat(:, :, 1);
 else
     MFD_attachments.data = mus_info_mat(:, :, 2);
 end
 
+%--------------------------
+% Advanced MATLAB summary |
+%--------------------------
+% The idea is to have a matrix with frames as rows and in column bone
+% attachments, effective attachments, lines of action and transport
+% moments.
+colheaders = {'bone_attach_X', 'bone_attach_Y', 'bone_attach_Z', 'effect_attach_X', 'effect_attach_Y', 'effect_attach_Z', ...
+                'act_line_X',   'act_line_Y',    'act_line_Z',    'trans_mom_X',   'trans_mom_Y',   'trans_mom_Z'};
 % this is an advanced summary for Matlab use
 MFDStruct.colheaders = colheaders;
 MFDStruct.rowheaders = rowheaders;

matlab_tool/isKinMatchingModelCoords.m

@@ -39,41 +39,47 @@
 coordsModel = osimModel.updCoordinateSet();
 N_coordsModel = coordsModel.getSize();
 
+% checking is kinematics matches coordinates order
+[out, missing_coords_list] = isKinMatchingModelCoords(osimModel, kinStruct.colheaders);
+
+% correct if there are generalised coordinates not computed in IK (e.g.
+% models modified after running IK)
+% NB: not tested!!
+if ~out
+    for n_m = 1:length(missing_coords_list)
+        missing_coords_vals(n_m) =  coordsModel.get(coordName).getDefaultValue;
+    end
+else
+    missing_coords_vals = [];
+end
+% if needed
+% kinStruct.colheader = [kinStruct.colheaders, missing_coords_list];
+
+% account for time column in data structure
+if any(strcmp('time', kinStruct.colheaders))    
+    kin_state_angles = [kinStruct.data(n_frame, 2:end), missing_coords_vals];
+else
+    kin_state_angles = [kinStruct.data(n_frame, :), missing_coords_vals];
+end
+
 % looping thought coordinates to assign them a value
-for n_StateCoord = 0:N_coordsModel-1
-    
-    %%%%%% updating pose of the model %%%%%
-    %extracting the column for the state variable of interest
-    coordName =  char(coordsModel.get(n_StateCoord).getName());
+for n_modelCoords = 0:N_coordsModel-1
 
-    % check if the coordinates as an equivalent in storage.
-    CoordInd_in_kinStorage = getMatStructColumn(kinStruct, coordName);
-    
-    if isempty(CoordInd_in_kinStorage)
-        % this is the case when a coordinate is not in the storage file
-        if n_frame==0
-            warndlg([coordName,' not found in storage. Default value will be assigned.'])
-        end
-        % assign default value
-        currentCoordValue =  coordsModel.get(coordName).getDefaultValue;
-    else
-        coordColumValues = getMatStructColumn(kinStruct, coordName);
-        % Value of the state variable at that frame
-        currentCoordValue = coordColumValues(n_frame);
-    end
+    % Value of the state variable at that frame
+    cur_coordValue = kin_state_angles(n_modelCoords+1);
 
     % assigning the coordinates depending on their motion type
-    switch char(coordsModel.get(n_StateCoord).get_motion_type())
+    switch char(coordsModel.get(n_modelCoords).get_motion_type())
         case 'rotational'
             % transform to radiant for angles
-            coordsModel.get(n_StateCoord).setValue(state,currentCoordValue*pi/180);
+            coordsModel.get(n_modelCoords).setValue(state,cur_coordValue*pi/180);
         case 'translational'
             % not changing linear distances
-            coordsModel.get(n_StateCoord).setValue(state,currentCoordValue);
+            coordsModel.get(n_modelCoords).setValue(state,cur_coordValue);
         case 'coupled'
-            error('motiontype ''coupled'' is not managed by this function');
+            error('motiontype ''coupled'' is not managed by realizeMatStructKinematics.m');
         otherwise
-            error('motion type not recognized. Error due to OpenSim update?')
+            error('motion type not recognized. Please check OpenSim model (maybe update error?')
     end
 end