Matching the InverseDynamicsSolver with the GUI/InverseDynamicsTool

[Owen Pearl]

Hello all!

I been all over the forums trying to figure out (1) why the inverse dynamics solver doesn't match the IDTool when there are not ground reaction forces applied to either, and (2) how to successfully incorporate experimental GRF data into the InverseDynamicsSolver approach in MATLAB.

While many users have asked about this before, I am struggling to find a working example on here or the github where the inversedynamicssolver is used correctly to match results from ID in the GUI or the tool. Even following the implementation in the InverseDynamicsTool.cpp file doesn't seem to yield consistent results in the MATLAB api. Can anyone help point me in the right direction?

Here is the code that I have been working with (with and without the external loads part which does seem to work as intended):

Code: Select all

% Load the osim model and initialize the state
osimModel = Model(fullfile(inputDirectory, modelfile));
modelProcessor = ModelProcessor(osimModel);
modelProcessor.append(ModOpRemoveMuscles());
osimModel = modelProcessor.process();

% This method (1) works for loading external loads the same as the next
% lGRF = ExternalForce(Storage(grffile), 'lF', 'lCOP','lM', 'calcn_l', 'ground', 'ground')
% rGRF = ExternalForce(Storage(grffile), 'rF', 'rCOP','rM', 'calcn_r', 'ground', 'ground')
% osimModel.addForce(lGRF)
% osimModel.addForce(rGRF)

% This method (2) works for loading external loads the same as the previous
idTool = InverseDynamicsTool();
idTool.createExternalLoads(fullfile(inputDirectory, 'OpenSim_ExternalLoads.xml'), osimModel);

state =  osimModel.initSystem();
idSolver = InverseDynamicsSolver(osimModel);

Results = [];
numFrames = length(time_mat);
qdd_simvec = Vector(numQ,0.0);
for i = 1:numFrames
    clc
    fprintf('Processing frame %d out of %d...', i, numFrames)
    for j = 1:numQ
        state.setTime(time_mat(i)); % do this so that the solver knows when to query the GRF file
        osimModel.updCoordinateSet().get(j-1).setValue(state, q_mat(i,j));
        osimModel.updCoordinateSet().get(j-1).setSpeedValue(state, qd_mat(i,j));
        qdd_simvec.set(j-1,qdd_mat(i,j));
    end
    
    % Solve for this frame
    jointTorques = idSolver.solve(state,qdd_simvec);
    
    % Unpack
    for j=1:numQ
        Results(i,j) = jointTorques.get(j-1);
    end
end

When I do this for the same inputs between the GUI and IDSolver without the grf's applied, I get the following (larger residuals and worse results for the IDSolver implementation):

Example.PNG (35.37 KiB) Viewed 1269 times

When I do the GRF implementation, this is what I get for the residuals (makes it seem like the GRFs are not the problem, but something in the implementation of the state or acceleration tracking).

Example2.PNG (53.22 KiB) Viewed 1269 times

However, I am also not totally sure how the solver should be told what time to query the GRFs at to apply to the model with this setup, since time is never passed in or updated as far as the API documentation instructs us. How are we supposed to update the time of the state of the model so that it queries the GRF external loads at the right time? I tried doing that myself by updating the state time each step since it is then realized to dynamics in the solver, and it does seem to help make the ankle torque line up at least (see below), but it would be great to have some explicit instruction as to how this is best done.

Here are the joint torque comparisons when the GRFs are applied:

Example3.PNG (49.03 KiB) Viewed 1269 times

Since the ankle torques are so close, that makes me think I am doing the GRFs right but something about how the acceleration's are being passed in is making things go sideways. When I looked at the inversekinematicssolver.cpp, it has comments that seem to suggest everything just goes into the solver in the order of the generalized coordinates, which I tried, even though the outputs come out in the order of the multibodytree. Some clarity here would very appreciated as well.

Thanks so much!

[Owen Pearl]

As a quick follow up, I also actually found that without the GRFs, things match better when you provide the accelerations:

idSolver.solve(state,qdd);

But when you include the GRFs, my best attempt so far was without any accelerations provided (so I am guessing now that the acceleration info and the grf's were redundant):

idSolver.solve(state);

Example4.PNG (43.48 KiB) Viewed 1206 times

[Mohammadreza Rezaie]

Hi, call realizeAcceleration() before idSolver.solve() and see how it works.

Code: Select all

osimModel.realizeAcceleration(state)

[Owen Pearl]

Thank you for the suggestion! I tried this and it seems like this produced identical results to the last plots I posted.

I believe the reason for this is because idSolver.solve(state) realizes the state to the dynamics stage inside (you can see this is you view InverseDynamicsSolver.cpp on the opensim-core github).

[Ayman Habib]

Hi Owen,

Few comments on the posted code:
1. Please do not use "updCoordinateSet()", and use "getCoordinateSet()" instead. The value of the coordinate is not kept with the coordinate but in the state, as such the Coordinate object is const, this may not make a difference here but a general rule to be aware of, the side effects of using "upd" methods that access properties.
2. The order of the coordinates and speeds is a big concern, our code jumps through hoops to find the right index for coordinate and speed in the state vector. If you want to avoid that you should get the coordinate object and set its value and speed directly rather than through a vector that has an implicit order.
3. There're an option to enforce constraints when setting coordinate values, typically we set all values of coordinates and speeds then call assemble/realize once at the end. If the model has constraints, the assembly could change the values you set. As a part of troubleshooting you should verify the coordinate values and speeds are still there after assembly/realization.
4. The time is maintained by the state and thus used to query the GRF for values.
5. Tools can do padding/filtering, the solver(s) assume any filtering was done already.
Please let us know what you find out so we can help you get to the bottom of this issue.

Best regards,
-Ayman

[Mohammadreza Rezaie]

Hi, as Ayman said, the order of coordinates in the acceleration vector matters for the solver. It requires coordinates in multibody tree order not CoordinateSet order. Now, I could get identical results compared to InverseDynamics tool:

ID_tool_solver.png (70.11 KiB) Viewed 996 times