simtk.org

Code: Select all

filename = 'Walk 2.xlsx';                                                   % Name of the file containing joint trajectories and GRF data
sheet = 1;                                                                  % Sheet number of the excel file, is where the trajectory data is.
[NUM,TXT,RAW] = xlsread(filename,sheet);                                    % Read the file and access data
[nRows, nCols] = size(RAW);                                                 % Calculate the number of rows and columns in the file. Number of columns is equal to degrees of freedom and one column for time. Number of rows is number of instances.

time = NUM(:,1);                                                            % First colmun is time data.
gencood_rad = NUM*2*pi/360;                                                 % All the data is in degrees, so converted them into radians
gencood_rad(:,5:7)= NUM(:,5:7);                                         % PelvisTx,Ty,Tz are not angles but translation
gencood_rad = lowpass(gencood_rad,6,60);

v = (gencood_rad(2:(nRows-1),:) - gencood_rad(1:(nRows-2),:))/0.016666;     
a = (v(2:(nRows-2),:)-v(1:(nRows-3),:))/0.016666;                          

%% Inverse Dynamics

import org.opensim.modeling.*                                               % Import the OpenSim library

model = Model('subject01_simbody.osim');                                    % Import the lower body model
path='E:\Opensim\OpenSim 4.1\Geometry';                                     % Path to the folder where all the geometry files of the model is stored
ModelVisualizer.addDirToGeometrySearchPaths(path);                          % This path is given to the Model visualizer so that visualization of the model can be done

model.setUseVisualizer(true);                                               % Visulaizer is true for the model
state = model.initSystem();                                                 % Initialize the state of the model

% Setting the muscle property in ID_model such that no force is applied by them
for mn = 1:54
    model.getMuscles().get(mn-1).setAppliesForce(state, 0);
end

% Following code snippet is necessary to run the visualizer
viz = model.updVisualizer().updSimbodyVisualizer();               % Start the visualizer for the given model
viz.setBackgroundColor(Vec3(1));                                            % Set the background colour of the model using RBG.
viz.setGroundHeight(0);                                                           % Set the height at which model will function

ID = InverseDynamicsSolver(model);                                     % Inverse dynamics solver needs the model 
u_dot = Vector(model.getNumCoordinates(),0);                    % desired acceleration of all the joints

for i = 1:size(a)
    timed = time(i+1)-time(i);                                              % Calculate the time step between two instances
    for j = 2:nCols                                                         % For every instance, desired angle and speed
        model.getCoordinateSet().get(j-2).setValue(state, gencood_rad(i,j));
        model.getCoordinateSet().get(j-2).setSpeedValue(state, v(i,j));
        u_dot.set(j-2,a(i,j));                                              % Desired acceleration of all the joints
    end
    
    tau = ID.solve(state, u_dot);                                           % Solves the inverse dynamics 
    for k = 1 : model.getNumCoordinates()
        obtained_tau(i,k) = tau.get(k-1);                                   % 'tau' contains the joint moments 
    end
    newT(i) = time(i);
    
    manager = Manager(model);
    state.setTime(0);
    manager.initialize(state);
    state = manager.integrate(timed);
end