Page 1 of 1
get coriolis and gyroscopic forces
Posted: Tue Nov 17, 2015 8:51 am
by 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
Re: get coriolis and gyroscopic forces
Posted: Tue Nov 17, 2015 11:09 am
by sherm
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
Re: get coriolis and gyroscopic forces
Posted: Tue Nov 17, 2015 11:33 am
by sherm
BTW, getTotalCentrifugalForces() was buggy prior to OpenSim 3.3 -- be sure to upgrade if you haven't already.
Sherm
Re: get coriolis and gyroscopic forces
Posted: Wed Nov 18, 2015 6:49 am
by 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.
Re: get coriolis and gyroscopic forces
Posted: Wed Nov 18, 2015 12:11 pm
by sherm
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
Re: get coriolis and gyroscopic forces
Posted: Thu Nov 19, 2015 3:09 am
by 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() ?
Re: get coriolis and gyroscopic forces
Posted: Thu Nov 19, 2015 10:43 am
by sherm
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.