optimized realisation of kinematics

Author: modenaxe

matlab_tool/isKinMatchingModelCoords.m

@@ -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/isMuscleConnectedToBody.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/realizeMatStructKinematics.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