getting the vector of gravitational forces from SimbodyMatterSubsystem

[Rebecca Abbott]

In case it is helpful to anyone, I was able to create the mex interface for Matlab to get the gravity term in the equations of motion. I have attached my c++ code "GravityEngine.cpp".

Thank you for your help, Dimitar. I was able to base the structure off of your example code.

It requires the mexplus macros found here: https://github.com/kyamagu/mexplus

You create a class in Matlab to interface with mex, which I have included here:

Code: Select all

classdef gravity_engine < handle
    
properties (Access = private)
    id_ % ID of the session
end

methods
    function this = gravity_engine()
        %GravityEngine Create a new
        this.id_ = GravityEngine('new');
    end
    
    function delete(this)
        %delete Destructor
        GravityEngine('delete', this.id_);
    end
    
    function setup(this, fileName)
        assert(isscalar(this));
        assert(ischar(fileName));
        GravityEngine('setup', this.id_, fileName)
    end
    
    function updateState(this, stateVec)
        assert(isscalar(this));
        GravityEngine('updateState', this.id_, stateVec);
    end

    function newQ = getStateCoords(this)
        assert(isscalar(this));
        newQ = GravityEngine('getStateCoords', this.id_);
    end
    
    function gravForces = calcGravForces(this)
        assert(isscalar(this));
        gravForces = GravityEngine('calcGravForces', this.id_);
    end
    
end
end

To compile, you can use the Matlab mex command, which looked like this for me:

Code: Select all

    mex -I"C:/Simbody/include" -I"C:/OpenSim 4.0/sdk/include" -L"C:/Simbody/lib" -lSimTKcommon -lSimTKsimbody -lSimTKmath -L"C:/OpenSim 4.0/sdk/lib" -losimSimulation -losimTools -losimCommon  -losimAnalyses -losimActuators GravityEngine.cpp

With the compiled GravityEngine.mexw64 file in your working directory, you can use your wrapper class like this:

Code: Select all

% get handle to mex object
obj = gravity_engine();

%load and initialize your model
obj.setup(filePath);

%update the independent coordinates
obj.updateState(qVec);

%get the current coordinates (joint angles)
qVecNew = obj.getStateCoords();

%get the vector of generalized forces/torques due to gravity in ground frame
gravF = obj.calcGravForces();

% releases the object instance
obj.delete 
clear obj;

[Dimitar Stanev]

Thank you for sharing. I am glad that you managed to solve the problem. It is unfortunate though that you had to get into so much trouble because these functions were not exposed from the begging.

[Aravind Sundararajan]

In case it is helpful to anyone, I was able to create the mex interface for Matlab to get the gravity term in the equations of motion. I have attached my c++ code "GravityEngine.cpp".

This and Dimitar's posts/example were very helpful for understanding OpenSim mex interface, thank you both!

[Rebecca Abbott]

For a few reasons, I ended up writing code in Matlab to get the gravity vectors without using the MEX interface. This way I don't need to have separate parallel instantiations of the model in C++ and Matlab. I have included the Matlab code below. I compared it to the output of my MEX interface code and it gives me the same generalized gravity force vectors. Hopefully this is helpful to someone.

Code: Select all

function Fgrav = getGravityForces(osimModel, state)
    % Pass in reference to model and state
    %osimModel = Model(model_path);
    %state = osimModel.initSystem();

    % import OpenSim Libraries
    import org.opensim.modeling.*; 
    
    % get references to OpenSim Objects
    bodies = osimModel.getBodySet();    
    ground = osimModel.getGround();    
    grav = osimModel.getGravity();

    % number of bodies and coordinates
    nbods = osimModel.getNumBodies();
    ncoords = osimModel.getNumCoordinates();

    %start GravF vector with zeros for ground mobodIndex = 0;
    GravF_G = zeros(6,1); 

    % iterate through each body; get gravity vector in ground frame
    for bb = 1 : nbods
        
        %get reference to current body
        body = bodies.get(bb-1);
        
        % get current body parameters
        mass = body.getMass();
        CoM = osimVec3ToArray(body.getMassCenter())';

        % Rotation Matrix from body to ground frame
        R_GB = osimMatrixToArray(body.getTransformInGround(state).R());

        % gravity vector expressed in own body frame 
        grav_B = osimVec3ToArray(ground.expressVectorInAnotherFrame(state,grav,body))';

        % Force of gravity applied at body origin in ground frame
        F_G = R_GB * mass * grav_B;
        % Moment of gravity about body origin in ground frame
        M_G = R_GB * cross(CoM, mass * grav_B);

        %append gravity moments and torques into GravF vector
        %spatial forces due to gravity at each body origin 
        %indexed by mobod_index * 6 (where mobod_index 0 (first 6 elements) is ground)
        GravF_G = [GravF_G; M_G; F_G]; %((nbods+1) * 6) x 1 vector)
    end

    % System Jacobian relating spatial forces to generalized forces
    sysJ = Matrix(6*nbods, ncoords);
    osimModel.getMatterSubsystem().calcSystemJacobian(state, sysJ);
    sysJmat = osimMatrixToArray(sysJ);

    % vector of generalized forces indexed by mobility (same as Q or state vector)
    Fgrav = sysJmat' * GravF_G; % (nu x 1)
end

A couple notes, you need to be using OpenSim 4.0 to access the Matter Subsystem Jacobian classes. The 'osimVec3ToArray()' and 'osimMatrixToArray()' functions were included in the OpenSim 4.0 installation and just convert between OpenSim Matrix and Vec3 types to Matlab Arrays that we can work with in Matlab.

[ZAKARIA BOUZID]

For a few reasons, I ended up writing code in Matlab to get the gravity vectors without using the MEX interface. This way I don't need to have separate parallel instantiations of the model in C++ and Matlab. I have included the Matlab code below. I compared it to the output of my MEX interface code and it gives me the same generalized gravity force vectors. Hopefully this is helpful to someone.

Code: Select all

function Fgrav = getGravityForces(osimModel, state)
    % Pass in reference to model and state
    %osimModel = Model(model_path);
    %state = osimModel.initSystem();

    % import OpenSim Libraries
    import org.opensim.modeling.*; 
    
    % get references to OpenSim Objects
    bodies = osimModel.getBodySet();    
    ground = osimModel.getGround();    
    grav = osimModel.getGravity();

    % number of bodies and coordinates
    nbods = osimModel.getNumBodies();
    ncoords = osimModel.getNumCoordinates();

    %start GravF vector with zeros for ground mobodIndex = 0;
    GravF_G = zeros(6,1); 

    % iterate through each body; get gravity vector in ground frame
    for bb = 1 : nbods
        
        %get reference to current body
        body = bodies.get(bb-1);
        
        % get current body parameters
        mass = body.getMass();
        CoM = osimVec3ToArray(body.getMassCenter())';

        % Rotation Matrix from body to ground frame
        R_GB = osimMatrixToArray(body.getTransformInGround(state).R());

        % gravity vector expressed in own body frame 
        grav_B = osimVec3ToArray(ground.expressVectorInAnotherFrame(state,grav,body))';

        % Force of gravity applied at body origin in ground frame
        F_G = R_GB * mass * grav_B;
        % Moment of gravity about body origin in ground frame
        M_G = R_GB * cross(CoM, mass * grav_B);

        %append gravity moments and torques into GravF vector
        %spatial forces due to gravity at each body origin 
        %indexed by mobod_index * 6 (where mobod_index 0 (first 6 elements) is ground)
        GravF_G = [GravF_G; M_G; F_G]; %((nbods+1) * 6) x 1 vector)
    end

    % System Jacobian relating spatial forces to generalized forces
    sysJ = Matrix(6*nbods, ncoords);
    osimModel.getMatterSubsystem().calcSystemJacobian(state, sysJ);
    sysJmat = osimMatrixToArray(sysJ);

    % vector of generalized forces indexed by mobility (same as Q or state vector)
    Fgrav = sysJmat' * GravF_G; % (nu x 1)
end

A couple notes, you need to be using OpenSim 4.0 to access the Matter Subsystem Jacobian classes. The 'osimVec3ToArray()' and 'osimMatrixToArray()' functions were included in the OpenSim 4.0 installation and just convert between OpenSim Matrix and Vec3 types to Matlab Arrays that we can work with in Matlab.

Hello Friend,

I need that function to use in designing pd with compensator controller but I got issue with the function osimMatrixToArray which not included in OpenSim 4.2 can u send me that function.

and Thank u so much for sharing the code.

[ZAKARIA BOUZID]

I created my script to change to Matlab array.
thank you so much for the code.