Problems using InverseDynamicsSolver function

[Roger Pallarès López]

Hello,

I am using the OpenSim API in MATLAB to solve an Inverse Dynamics analysis for a skeletal model of 10 DOF. Since I am implementing an optimal control algorithm I need to introduce generalized coordinates, velocities and accelerations to the solver. Thus, I cannot use the ‘InverseDynamicsTool’ that only needs generalized coordinates to compute torques (I know that inside de function velocities and accelerations are obtained by differentiating coordinates, but this does not work for the optimal control algorithm). On the other hand, I am using the ‘InverseDynamicsSolver', in which generalized coordinates and their time derivatives can be introduced.

As a first test, I am computing torques without introducing ground reaction forces. I introduce coordinates, velocities, accelerations and the model into the function. Also, I set the masses to each body of the model. But, when I calculate the resulting forces and torques, I get wrong results. As it can be seen in the attached figure, it seems that the function does not take into account the masses I set, since vertical force (Residual Fy) is closer to zero.

I am new on this and I do not know very much where I can find extended information or some examples of this kind of functions. I would be very grateful if someone could tell me what I am doing wrong or what I am missing. Thank you very much!

I attach the figure, the model and the code. Also, I copy the code here below:


% q, qdot and qdot2 are matrices with the evolution of generalized coordinates, velocities and accelerations.
% FPL1 and FPL2 are matrices with body forces (3 forces and 3 moments). In this case are matrices of zeros, since I am not introducing ground reaction forces.
% modelFile is the 10 DOF skeletal model I am using.

function [Res, Tau] = IDSolver_GRF_propi(q,qdot,qdot2,FPL1,FPL2,modelFile)

import org.opensim.modeling.*

osimModel = Model(modelFile);
osimState = osimModel.initSystem;
idSolver = InverseDynamicsSolver(osimModel);

nb = osimModel.getNumBodies();
nq = osimModel.getNumCoordinates();

g = osimModel.getGravity();

n = length(q);
Results = zeros(n,nq);
xdot2 = Vector(nq,0.0);

BodyForce_0 = SpatialVec(0.0); % methodsview(SpatialVec)
BodyForce_5 = SpatialVec(0.0);
BodyForce_10 = SpatialVec(0.0);

appliedBodyForces = VectorOfSpatialVec(nb,BodyForce_0); % methodsview(VectorOfSpatialVec)

weight = Vec3(0.0);
Force_5 = Vec3(0.0);
Moment_5 = Vec3(0.0);

Force_10 = Vec3(0.0);
Moment_10 = Vec3(0.0);

appliedMobilityForces = Vector(nq,0.0);
bodies = osimModel.getBodySet(); % methodsview(bodies)

for i=1:n

x = q(i,:);
xdot = qdot(i,:);

for j=1:nq
osimModel.getCoordinateSet().get(j-1).setValue(osimState,x(j));
osimModel.getCoordinateSet().get(j-1).setSpeedValue(osimState,xdot(j));
xdot2.set(j-1,qdot2(i,j));
end

for k=1:nb-1 % ground és el 0, però està comptat a nb
body = bodies.get(k);
m = body.getMass();
weight.set(1,m*g.get(1));
BodyForce_0.set(0,weight); % vaig sobreescrivint
appliedBodyForces.set(k,BodyForce_0);
end

body = bodies.get(5);
m = body.getMass();
weight.set(1,m*g.get(1));
Force_5.set(0,FPL1(i,1));
Force_5.set(1,FPL1(i,2)+weight.get(1));
Force_5.set(2,FPL1(i,3));
Moment_5.set(0,FPL1(i,4));
Moment_5.set(1,FPL1(i,5));
Moment_5.set(2,FPL1(i,6));
BodyForce_5.set(0,Force_5);
BodyForce_5.set(1,Moment_5);
appliedBodyForces.set(5,BodyForce_5);

body = bodies.get(10);
m = body.getMass();
weight.set(1,m*g.get(1));
Force_10.set(0,FPL2(i,1));
Force_10.set(1,FPL2(i,2)+weight.get(1));
Force_10.set(2,FPL2(i,3));
Moment_10.set(0,FPL2(i,4));
Moment_10.set(1,FPL2(i,5));
Moment_10.set(2,FPL2(i,6));
BodyForce_10.set(0,Force_10);
BodyForce_10.set(1,Moment_10);
appliedBodyForces.set(10,BodyForce_10);

jointTorques = idSolver.solve(osimState,xdot2,appliedMobilityForces,appliedBodyForces);

for j=1:nq
Results(i,j) = jointTorques.get(j-1);
end

end

Res = Results(:,1:3);
Tau = Results(:,4:nq);

end

[Christopher Dembia]

It looks like you mostly know what you are doing! It is great that you are trying to use the implicit form of the dynamics for your optimal control problem.

It seems that you are using GPOPS. Consider looking at the code from Andrew Meyer and B.J. Fregly for using OpenSim with GPOPS-II: https://simtk.org/projects/synwalkingpred/; they also use the implicit form of the multibody dynamics.

You are attempting to use the `solve()` function that takes user-specified forces. Consider using the simpler function that takes only the state and the acceleration and uses the model to compute the forces (e.g., gravity) for you: https://simtk.org/api_docs/opensim/api_ ... a3b6d0e400
This should help avoid any issues with manually specifying the forces.

Also, consider first getting your code to work properly for a simpler model (e.g., single pendulum).

[Roger Pallarès López]

It looks like you mostly know what you are doing! It is great that you are trying to use the implicit form of the dynamics for your optimal control problem.

It seems that you are using GPOPS. Consider looking at the code from Andrew Meyer and B.J. Fregly for using OpenSim with GPOPS-II: https://simtk.org/projects/synwalkingpred/; they also use the implicit form of the multibody dynamics.

You are attempting to use the `solve()` function that takes user-specified forces. Consider using the simpler function that takes only the state and the acceleration and uses the model to compute the forces (e.g., gravity) for you: https://simtk.org/api_docs/opensim/api_ ... a3b6d0e400
This should help avoid any issues with manually specifying the forces.

Also, consider first getting your code to work properly for a simpler model (e.g., single pendulum).

Hello again!

Thank you very much for your answer and advice. Yes, you are right, I am using the implicit form in GPOPS!

As you suggested me, I tried to implement the function using a simpler model (a double pendulum). I implemented the function only giving states and accelerations and worked perfectly.

However, I need to introduce foot ground forces to my model. Doing some tests introducing body forces, I found out what was the problem: I was writing wrong the external forces to the spatial vector 'appliedBodyForces'. First moments and then forces must be introduced, i.e., [[Mx, My, Mz], [Fx, Fy-mg, Fz]]. And I had been doing the other way around.

I still have some issues as I get different torques for ankle angles and lumbar extension angle (same plot, but shifted 5-10 Nm downwards). However, the other torques are almost the same. Thank you very much for your help!!

[Sujash Bhattacharya]

Hi Roger,

I am also using the implicit form of the dynamic solver. I am stuck with it as I am not being able to introduce ground reaction forces as an input to inverseDynamicsSolver for gait2354 model. Can you please post your code or give some clues how you added grfs?

Regards,
Sujash Bhattacharya