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
get coriolis and gyroscopic forces
- Michael Sherman
- Posts: 806
- Joined: Fri Apr 01, 2005 6:05 pm
Re: get coriolis and gyroscopic forces
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
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
- Posts: 806
- Joined: Fri Apr 01, 2005 6:05 pm
Re: get coriolis and gyroscopic forces
BTW, getTotalCentrifugalForces() was buggy prior to OpenSim 3.3 -- be sure to upgrade if you haven't already.
Sherm
Sherm
- Kevin Tanghe
- Posts: 36
- Joined: Mon Sep 22, 2014 6:54 am
Re: get coriolis and gyroscopic forces
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:
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:
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.
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;
}
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);
}
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
- Posts: 806
- Joined: Fri Apr 01, 2005 6:05 pm
Re: get coriolis and gyroscopic forces
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
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
- Posts: 36
- Joined: Mon Sep 22, 2014 6:54 am
Re: get coriolis and gyroscopic forces
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() ?
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
- Posts: 806
- Joined: Fri Apr 01, 2005 6:05 pm
Re: get coriolis and gyroscopic forces
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.why does osimModel.getBodySet().get(i).getInertia(rInertia) does not return the same result as osimModel.getMatterSubsystem().getMobilizedBody(i).getBodyMassProperties(s).calcInertia() ?