getting the vector of gravitational forces from SimbodyMatterSubsystem

[Rebecca Abbott]

Hi,

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);

For more context, here is how I am attempting to set up the equations of motion with the terms that I am having trouble extracting highlighted in red. (I extract the muscle parameters elsewhere):

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

[Michael Sherman]

Hi, Rebecca. I don't have a complete answer to your question, but here is a partial one:

Gravity in Simbody is treated like any other force -- there is a force element that calculates and applies the gravity forces to the system. (There are actually different force elements that can be used -- Force::Gravity and Force::UniformGravity.) So SimbodyMatterSubsystem doesn't know anything about gravity; it just knows about body and joint forces in general.

If OpenSim is using Simbody's Force::Gravity element, that element can report the body forces it is applying. Those are returned in a form that can be fed to SimbodyMatterSubsystems' multiplyBySystemJacobianTranspose() method to turn those body forces into generalized forces at the joints.

OpenSim may provide a more-convenient method for accessing gravity forces -- hopefully an OpenSim expert can say for sure. But the above is what's happening under the covers.

Sherm

[Dimitar Stanev]

Hi,

As Sherman correctly pointed out you need a low level access to those quantities. Please look at the following C++ implementation where I have created a wrapper for calculating the quantities of the equations of motion

https://github.com/mitkof6/task-space/b ... icsModel.h
https://github.com/mitkof6/task-space/b ... el.cpp#L12

Then a function that calculates the muscle moment arm

https://github.com/mitkof6/task-space/b ... ol.cpp#L30

Then again you if you are interested in task space projection, please examine the whole repository to see how I calculate the various quantities.

Hope this helps!

[Rebecca Abbott]

Dear Sherm and Dimitar,

Thank you for your helpful responses!

Dimitar - I have looked through your C++ code and it was very helpful. I was hoping to stick with the Matlab and Python APIs because I am more familiar with those languages and already have code implemented in them.

I am now weighing 3 options:
1. Switch over completely to C++, which would mean learning C++ and reimplementing my code
2. Stay with Matlab and calculate the Gravity terms in the equations of motion outside of Opensim using transforms (which is very complex with the large number of joints, offset axes, and coupled coordinate functions in the neck model).
3. Write a MEX C++ wrapper to to call getGravityForce().getBodyForces(state) in Matlab. I saw that you have an example of writing a wrapper and there is abundant documentation on the Mathworks website. However, it looks far more technical than anything I have done before.

Based on your experience, do you have a suggestion for the best option moving forward?

Thank you!
Rebecca

[Dimitar Stanev]

If you cannot do this through the Matlab or Python API then you have to move either to C++ or MEX. You are right C++ is a bit difficult especially if you are not familiar with the building process. The MEX solution would require the implementation of a wrapper on top of the C++ code that you will develop and sometimes it is very difficult to debug. Are you interested in calculating the gravity term only or do you need access to some other quantity?

[Rebecca Abbott]

I am just interested in the gravity term to update the equations of motion based on state (only realized to position stage). All I really need is that getGravityForce().getBodyForces(state) call so that I can put it into model.getMatterSubsystem().multiplyBySystemJacobianTranspose().

I've been playing around with MEX the last few days and finding it pretty confusing, especially since I am not very comfortable with C++ and the building process. I have put together the .cpp files but stuck on the building and including the proper header files.

[Rebecca Abbott]

Hi Dimitar,

I have created a MEX interface to read in the gravity term for the equations of motion. I have some more work to do on it but I am getting a return value into matlab that makes sense.

I currently am loading the model separately into Matlab and the MEX C++ function, so I have to do everything concurrently because I am not referencing the same model instance. I am not running dynamics, I am just realizing the model to the position stage to get muscle parameters and position state information to update my equations of motion.

There is a function available for both the Model and State classes in Matlab called getCPtr(). I am wondering if I can pass this pointer in to the MEX function so that I can work on the same instance of the model in both. I'm wondering if you would recommend against this, or if it seems like a good idea? Can you think of another way?

Thanks for your help! Let me know if including code would help.

[Dimitar Stanev]

Matlab uses the Java API, while in MEX you are using the C++ API and I don't think that there is a function that can make the transformation. Instead you could update the state variables by creating the appropriate interfaces (set[Q|U|Y] and get). I may be better if you could implement the whole thing in the C++ API.

[Rebecca Abbott]

Hi Dimitar,

Thank you for your response. That is helpful and keeps me from wasting time trying to get a pointer to work.

I have another question for you. I am using a model (the neck model available on OpenSim) that has 6 independent coordinates (roll, pitch, yaw of 2 joints) and 15 coordinates that are constrained by coupled coordinate constraints as a function of those 6 independent coordinates.

When I do model.getNumCoordinates() or state.getNQ(), it includes all 21 q's (as I would expect). I have 2 questions.

1. In Matlab, I am using model.updCoordinateSet().get('coordinate').setValue(state, value, bool for each independent variable (where bool = 1 which should enforce the constraints on that coordinate). I am only setting the independent coordinates and would like the dependent ones to update based on the coupled coordinate constraints. Would the appropriate next step be to state.realizePosition()?
2. What I am understanding is that I would then extract that updated state with the .getQ(), .getU(), .getY() for all variables (both independent and dependent) in Matlab and pass those values to my C++ mex function and use setQ(value) etc., to create an identical state there. If I do that, should I be worried that it will try to enforce constraints on the dependent q's?

Thank you for your help with this. Your responses and your example code have been very helpful to me.
Rebecca

[Dimitar Stanev]

1. state.realizePosition() should be called before calculating the gravity (https://simbody.github.io/simbody-3.6-d ... ml#details)
2. It's best to call model.updCoordinateSet().get('coordinate').setValue(state, value, bool) on the MEX model.