Obtaining body rotation matrices in a loop

[Mohsen Alizadeh Noghani]

Hi.

I am trying to obtain the body rotation matrix (e.g., with respect to the ground) in a loop in MATLAB. But the body rotation/translation never gets updated. What am I doing wrong?

Thanks!

Code: Select all

import org.opensim.modeling.*

model = Model('Generated_Model.osim');
storage = Storage('stride_1_IKOut.mot');

state = model.initSystem();
nRows = storage.getSize();

for i = 0:nRows-1

    stateVector = storage.getStateVector(i);
    state.setTime(stateVector.getTime);
    
    model.realizePosition(state);

    body1 = model.getBodySet().get('pelvis');
    body1Transform = body1.getTransformInGround(state);
    body1RotationMatrix = body1Transform.R()
    body1TranslationVec = body1Transform.T()

end

[Nicos Haralabidis]

Hey Mohsen,

Before realizing to the position stage you need to set the states (coordinates in this case) for each time frame, currently you are only extracting them from your *.mot file. Take a look at the following methods:

https://simtk.org/api_docs/opensim/api_ ... 1cd7df6e71

https://simtk.org/api_docs/opensim/api_ ... 689cccbc02

https://simtk.org/api_docs/opensim/api_ ... 27ee7e54f1

Hope that helps!

Nicos

[Mohsen Alizadeh Noghani]

Hi Nicos.

That provided a really helpful starting point, thanks!

I suspected that I needed to assign state value and just setting time wouldn't do it. I also realized that the name of the states in my model were in a different format than the .mot file (e.g., 'pelvis_tilt' vs. '/jointset/ground_pelvis/pelvis_tilt/value'), which required some processing.

This is the code that works for me, written with help from ChatGPT. I am sharing it here in case people look for a similar example of this in the future.

Code: Select all

import org.opensim.modeling.*

% Set to 1 to show which state value is being updated.
disp_assignments = 1;

% 1. Load the model
model = Model('Generated_Model.osim');

% 2. Load the storage file (e.g., IK or states data)
storage = Storage('stride_1_IKOut.mot');

% 3. Initialize the system and get a default State
state = model.initSystem();

% Check the name of the state variables of the model.
stateVariableNames = model.getStateVariableNames();
for i = 0 : stateVariableNames.getSize()-1
    disp(char(stateVariableNames.get(i)));
end

filteredNames = {};  % cell array to store matches

% Find the variables we are interested in (joint values).
for i = 0 : stateVariableNames.getSize()-1
    varName = char(stateVariableNames.get(i));  % Convert Java string to MATLAB char
    
    % Check for "jointset" AND "value", and ensure "speed" or "forceset" are NOT in the name.
    if contains(varName, 'jointset') && contains(varName, 'value') ...
            && ~contains(varName, 'speed') && ~contains(varName, 'forceset')
        
        % If it meets our criteria, add to the list.
        filteredNames{end+1} = varName; 
    end
end

% Display the filtered names
disp('Filtered state variable names:');
disp(filteredNames);

% 4. Get the number of time frames (rows)
nRows = storage.getSize();

% 5. Get the column labels from the storage
labels = storage.getColumnLabels();  
nColumns = labels.getSize();
% Typically, labels.get(0) is "time", and the rest are coordinate (or state) names.

for i = 0 : nRows-1
    % 6. Get the StateVector for the i-th row
    stateVector = storage.getStateVector(i);
    
    % 7. Extract time from the StateVector and set the State's time
    timeVal = stateVector.getTime();
    state.setTime(timeVal);
    
    % 8. Get the data array for this time frame
    dataArray = stateVector.getData();  % This is an ArrayDouble of length nColumns-1
    
    % 9. Assign each column value to the corresponding state variable
    %    Column 0 in the .mot is time, so its label is "time".
    %    Actual state data typically start at column 1.
    for j = 1 : (nColumns-1)
        varName  = char(labels.get(j));   
        if (disp_assignments == 1)
            disp(strcat("Assigning '", varName,"' to '", filteredNames{j}, "'."));
        end
        varValue = dataArray.get(j-1);        % dataArray is 0-based, so j-1
        % Set that variable's value in the State
        model.setStateVariableValue(state, filteredNames{j}, varValue);
    end
    
    % 10. Realize the state to at least Position stage 
    %     (or higher, depending on your needs)
    model.realizePosition(state);
    
    % 11. Now you can use the updated State for further calculations
    %     For example, get a transform from a body:
    body1 = model.getBodySet().get('pelvis');
    body1Transform = body1.getTransformInGround(state);
    body1RotationMatrix = body1Transform.R();
    body1TranslationVec = body1Transform.T();
    
    % ... do whatever else you need here ...
end

[Mohammadreza Rezaie]

Hi, there is already a method for updating the column labels TabOpUseAbsoluteStateNames:

Code: Select all

table = osim.TableProcessor(IK_path)
table.append(osim.TabOpConvertDegreesToRadians()) # if in degrees
table.append(osim.TabOpUseAbsoluteStateNames())
table = table.process(model)

Alternatively, you can update the coordinate values directly using Coordinate::setValue

Hope this helps.