I am interfacing Matlab with Opensim to perform optimizations in Matlab. I am using the neck model available on the OpenSim website. Given a static posture q, I pull the terms for the equations of motion using the SimbodyMatterSubsystem class with calcMinv() and calcStationJacobian().
I have 2 questions:
1. Is there a way to calculate the vector of gravitational forces (nx1 matrix where n is the number of DOFs)? I have looked through all documentation that is available and have not found a way.
2. I want to find the torque that the bushing forces are applying at each DOF. The code (below) returns an empty vector. Do you know what I am doing wrong? They are defined as type: BushingForce and have non-zero values for rotational stiffness in each coordinate direction.
Code: Select all
%import libraries
import org.opensim.modeling.*;
% Read in osim model
osimModel = Model('neck_model.osim');
%initialize state
state = osimModel.initSystem();
%Get terms for equations of motion
%Get Minv: (n x n) inverse of the mass matrix at current state with generalized coordinates q
%void = calcMinv(org.opensim.modeling.state,org.opensim.modeling.Matrix)
tempM = Matrix();
smss = osimModel.getMatterSubsystem();
smss.calcMInv(state,tempM); %returns inverse mass matrix tempM
%convert to Matlab array
Model.Minv = osimMatrixToArray(tempM); %(30 x 30)
% HELP!
%Get G: (n x 1) joint torque due to gravity at q
% HELP!
%Get tau_p: (n x 1) joint torques due to joint stiffness at current state with generalized coordinates q
spine_c7_bushing=osimModel.getForceSet().get('Spine_C7_Bushing'); %example bushing, will use for-loop to build tau_p
spine_c7_bushtorque = spine_c7_bushing.getStateVariableValues(state);
% returns empty vec3: ~[] despite there being a non-zero rotation at that joint in the current state
%Get Jacobian (3 x n)
%void = calcStationJacobian(org.opensim.modeling.State, int, org.opensim.modeling.Vec3, org.opensim.modeling.Matrix)
tempJ = Matrix();
skull_index = osimModel.getBodySet().get('skull').getMobilizedBodyIndex();
station_vector = Vec3(0.1, 0.1, 0.0); %forehead station in skull body frame
smss.calcStationJacobian(state, skull_index, station_vector, tempJ);
%convert to Matlab array
Model.Jacobian = osimMatrixToArray(tempJ);
Maximum acceleration found by formulating the affine problem (QUASI-STATIC)
n: num of DOFs (n = 30)
m: num of muscles (m = 98)
qdotdot = L a + g , where
L (nxm)= Minv(R F_a)
L (n x 1): terms dependent on force and states
Minv (n x n): inverse mass matrix
R (n x m): moment arms
F_a (m x m): scaling factor for active muscle force from f-l rel
a (m x 1): muscle activation vector,
g = Minv(R F_p + tau_p + G)
g (n x 1): terms that are dependent on coordinate states but not force
F_p (m x 1): passive muscle forces
G (n x 1): torques due to gravity
tau_p (n x 1): torques due to joint stiffness, bushing forces
qdotdot (n x 1): joint angular accelerations
Linear acceleration at the end effector (forehead) can be computed using the acceleration Jacobian, J mapping joint angular accelerations qdotdot to end effector linear acceleration xdotdot:
xdotdot = J qdotdot = J (L a + g)
This is formulated into an affine non-linear problem of
xdotdot = J (L x + g)
Thank you for any help!
Rebecca