--- a
+++ b/MATLAB/private/realizeMatStructKinematics.m
@@ -0,0 +1,86 @@
+%-------------------------------------------------------------------------%
+% Copyright (c) 2019 Modenese L.                                          %
+%                                                                         %
+% 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                                              %
+%    email:    l.modenese@imperial.ac.uk                                  %
+% ----------------------------------------------------------------------- %
+%
+% A function to realize the kinematic state of a model. It is currently
+% slow but robust.
+%
+% TODO: write comments
+%
+% written: 2014/2015 (Griffith University)
+% last modified: 
+% February 2017 (comments)
+% 29/11/2018 renamed and added to MFD Matlab version.
+% ----------------------------------------------------------------------- %
+function state = realizeMatStructKinematics(osimModel, state, kinStruct, n_frame)
+% TO DO
+% this function is slow. I should find a way of passing in only a row of
+% coordinates! That would eliminate the need of a n_frame as well.
+
+% OpenSim suggested settings
+import org.opensim.modeling.*
+OpenSimObject.setDebugLevel(3);
+
+% getting model coordinates and their number
+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)
+if ~out
+    for n_m = 1:length(missing_coords_list)
+        coordName = missing_coords_list{n_m};
+        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_modelCoords = 0:N_coordsModel-1
+    
+    % 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_modelCoords).get_motion_type())
+        case 'rotational'
+            % transform to radiant for angles
+            coordsModel.get(n_modelCoords).setValue(state,cur_coordValue*pi/180);
+        case 'translational'
+            % not changing linear distances
+            coordsModel.get(n_modelCoords).setValue(state,cur_coordValue);
+        case 'coupled'
+            error('motiontype ''coupled'' is not managed by realizeMatStructKinematics.m');
+        otherwise
+            error('motion type not recognized. Please check OpenSim model (maybe update error?')
+    end
+end
+
+end
\ No newline at end of file