get coriolis and gyroscopic forces

[Kevin Tanghe]

Hi,

The dynamic equation of a total system can be written as:

M(q) * udot + h(q, u) + g(q) = tau + J^T*Fext

with M the mass matrix, h the coriolis and gyroscopic forces, g the gravitational forces, tau the mobilizer forces, J the jacobian and Fext the external forces.

I'm interested to know the values of h(q, u). How can I get these values? I know they are calculated somewhere in Simbody, but I don't see how I can retrieve them.

(For clarification, I don't know tau, so I can't use the dynamic equation to compute it. Things I do know are q, u, and udot).


Regards,
Kevin

[Michael Sherman]

Hi, Kevin. Assuming you know how to get access to the SimbodyMatterSubsystem through the OpenSim API, you can get the Coriolis and gyroscopic forces using the method getTotalCentrifugalForces(). That is the same as h(q,u) in your equation. There are also methods there for getting the individual terms.

Only q and u are needed to calculate this term, but the State currently has to be realized through Dynamics stage.

Regards,
Sherm

[Michael Sherman]

BTW, getTotalCentrifugalForces() was buggy prior to OpenSim 3.3 -- be sure to upgrade if you haven't already.

Sherm

[Kevin Tanghe]

Thanks for your answer.
Since I want to compute h for the total system I think I still need to multiply with the jacobian of each body:

Code: Select all

mbs->realize(s, SimTK::Stage::Dynamics);
for (SimTK::MobilizedBodyIndex i(1); i < sms.getNumBodies(); ++i) {
	colForce = osimModel.getMatterSubsystem().getTotalCentrifugalForces(s, i);
	osimModel.getMatterSubsystem().calcFrameJacobian(s, i, SimTK::Vec3(0.0), Jf);
	
	for (int j = 0; j < 3; ++j) {
		colForce_vec.set(j, colForce.get(0)[j]);
		colForce_vec.set(j + 3, colForce.get(1)[j]);
	}
	H_vector += (~Jf)*colForce_vec;
}

This seems to work fine. However, I would rather want the stage at velocity level. Following the explanation of the getTotalCentrifugalForces() method, I tried to compute it from the coriolisaccerelation and the gyroscopic force:

Code: Select all

for (SimTK::MobilizedBodyIndex i(1); i < sms.getNumBodies(); ++i) {
	mobod = osimModel.getMatterSubsystem().getMobilizedBody(i);
	gyrForce = osimModel.getMatterSubsystem().getGyroscopicForce(s, i);
	colAcc = osimModel.getMatterSubsystem().getTotalCoriolisAcceleration(s, i);
	osimModel.getMatterSubsystem().calcFrameJacobian(s, i, SimTK::Vec3(0.0), Jf);

	osimModel.getBodySet().get(i).getInertia(I_mat);
  //I = mobod.getBodyMassProperties(s).calcInertia();
  m = mobod.getBodyMassProperties(s).getMass();
	
  for (int j = 0; j < 3; ++j) {
		colAcc_vec.set(j, colAcc.get(0)[j]);
		colAcc_vec.set(j + 3, colAcc.get(1)[j]);

		gyrForce_vec.set(j, gyrForce.get(0)[j]);
		gyrForce_vec.set(j + 3, gyrForce.get(1)[j]);
		for (int k = 0; k < 3; ++k) {
			bodySpatialInertia.set(j, k, I_mat[j][k]);
		}
		bodySpatialInertia.set(j+3, j+3, m);
	}
  H_vector += (~Jf)*(bodySpatialInertia*colAcc_vec + gyrForce_vec);

}

This does not return the same result as the first method. The norm of the difference has a value up to 5, so I guess it's not a numerical isue. Am I doing something wrong here?

I also noticed that osimModel.getBodySet().get(i).getInertia(I_mat) did not return the same result as mobod.getBodyMassProperties(s).calcInertia(). Is this normal?
I use Opensim 3.3 with Simbody 3.5.3.

[Michael Sherman]

Hi, Kevin. Yes, you are right. To produce your term h(q,u) the spatial forces need to be mapped to generalized forces by the system Jacobian. The way you wrote it works; if you need to do it more efficiently you can first collect all the centrifugal forces together in a Vector_<SpatialVec> and then do a single multiplyBySystemJacobian().

In Simbody 4.0 getTotalCentrifugalForces() will only require velocity stage. The workaround you proposed should allow you to compute it at velocity stage in Simbody 3.5.3, but you need to get the body spatial inertia in the Ground frame (you are getting the mass properties in the body frame). There is a MobilizedBody method available for that purpose, see MobilizedBody::getBodySpatialInertiaInGround().

Regards,
Sherm

[Kevin Tanghe]

Thank you very much. Everything works now!

Just out of interest, why does osimModel.getBodySet().get(i).getInertia(rInertia) does not return the same result as osimModel.getMatterSubsystem().getMobilizedBody(i).getBodyMassProperties(s).calcInertia() ?

[Michael Sherman]

why does osimModel.getBodySet().get(i).getInertia(rInertia) does not return the same result as osimModel.getMatterSubsystem().getMobilizedBody(i).getBodyMassProperties(s).calcInertia() ?

OpenSim uses centroidal inertia (that is, inertia about a body's center of mass), while Simbody uses inertia taken about the body origin. These should be the same for bodies where the COM is at the origin. Simbody provides utility methods for shifting the inertia from one place to another.