find method in source code

[Kevin Tanghe]

Hi,

I would like to know how the method getBodyAngularVelocity of the class MobilizedBody is implemented. In the source code I found the following:

Code: Select all

const Vec3& getBodyAngularVelocity(const State& state) const {      // w_GB
    return getBodyVelocity(state)[0];
}

Then I found :

Code: Select all

const SpatialVec& MobilizedBody::getBodyVelocity(const State& s) const {
    return getImpl().getBodyVelocity(s);
}

And now I'm stuck. Where can I find what happens in getImpl().getBodyVelocity?

[Michael Sherman]

I would like to know how the method getBodyAngularVelocity of the class MobilizedBody is implemented.

Hi, Kevin. I'm not sure exactly what you're asking. Are you trying to look around with only the header files and a binary library? Or do you have access to Simbody's source code? If you don't have a Simbody source build you won't be able to look at implementations. If you do have the source code and you can't just right click on a method name to look at its implementation then you need to get a better IDE. For example, in Visual Studio I simply right-clicked on the line getImpl().getBodyVelocity() and it showed me

Code: Select all

const SpatialVec& getBodyVelocity(const State& s) const {
  const SBTreeVelocityCache& vc = getMyMatterSubsystemRep().getTreeVelocityCache(s);
  return getMyRigidBodyNode().getV_GB(vc);
}

You can get similar behavior with Eclipse, Xcode, CLion, and even vim+ctags.

For this particular method you are likely to be disappointed because it is simply extracting the angular velocity that has already been computed previously. I'm guessing you really want to know how it computes the angular velocity, not just how it extracts already-computed numbers.

Regards,
Sherm

[Kevin Tanghe]

Hi Michael. I am using Visual Studio, but a right click didn't give me the code that you posted. This was probably because I just opened the MobilizedBody.cpp file, instead of the .sln file of the Simbody build.

I am indeed interested to find out how the angular velocities are computed. Here's why: I want to compute the angular and linear accelerations of the different bodies in the model knowing the generalized coordinates q and their first 2 derivatives (q_dot and q_ddot). If I'm correct, I can't realize the state to the acceleration stage, since I don't know the applied external forces on the body. The use of SimTK::MobilizedBody::getBodyAngularAcceleration is thus not possible.

I can compute the linear accelerations, using Jacobians:
a = J * q_ddot + Jdot * q_dot
However, I didn't find a way to compute the angular accelerations. I was hoping to get some inspiration from the source code, by inspecting how the angular velocities are be computed.
I also tried to find out how calculations are done starting from the SimTK::System::realize method, but again I didn't find actual computations.

Of course I can compute the angular accelerations manually, starting from the floating base and going down to child bodies, each time calculating the angular acceleration of the body. But then I cannot apply my code to other models.

Can you give me some advice on how to solve this?

Regards,
Kevin

[Michael Sherman]

Hi, Kevin. Simbody can calculate the angular acceleration for you. You are right that realize() is not the right method because that is generally used for forward dynamics. If you look in the doxygen for SimbodyMatterSubsystem and scroll down to "Miscellaneous Operators" there is an assortment of inverse dynamics and related methods for problems in which you know the accelerations. You might just need calcBodyAccelerationFromUDot().

A few notes:

• When Simbody documentation says "body acceleration" or "spatial acceleration" it generally means both linear and angular acceleration. These are typically returned in a "SpatialVec" which is a pair of Vec3 objects (rotation, then translation). Internally, Simbody uses spatial algebra methods that do not distinguish between translations and rotations so both kinds of quantities are always available.
• You can calculate both linear and angular acceleration using a Jacobian as you wrote, and in fact that is exactly how calcBodyAccelerationFromUDot() works as described in doxygen. However, you need to use the right Jacobian, one that includes both linear and angular effects. Simbody calls that a "Frame Jacobian"; see the doxygen section "System and Task Space Kinematic Jacobian Operators" on the SimbodyMatterSubsytem page.
• You should be careful about q_ddot versus u_dot. They are often the same but not for all joint types since sometimes q_dot != u (Ball joints, for example). Simbody has operators to convert; post if you need assistance with that.

Regards,
Sherm

[Kevin Tanghe]

Thanks Sherm. calcBodyAccelerationFromUDot() indeed seems the solution to my problem. Just to be sure: are all calculated accelerations expressed in the ground frame or in the body frame?

Could you explain why q_dot != u?

Kind regards,
Kevin

[Michael Sherman]

The doxygen documentation for calcBodyAccelerationFromUDot() says for parameter A_GB:

Spatial accelerations of all the body frames measured and expressed in the Ground frame.

So it is measuring the acceleration of the body frames with respect to Ground, and expressing the resulting pair of vectors in the Ground basis. The linear acceleration is of the body frame origin; angular acceleration doesn't depend on a point.

Could you explain why q_dot != u?

The state variables are the generalized coordinates q and generalized speeds u. Those can be chosen independently as long as there is an instantaneously-linear relationship between qdot and u: qdot=N(q)*u. Generalized speeds u are physical in the sense that their dimensionality is the same as the number of degrees of freedom in the multibody tree. Generalized coordinates q are commonly chosen instead for numerical stability or convenience. Most commonly, a quaternion (four q's) is chosen to represent unrestricted orientation, even though there are only 3 dofs and hence 3 u's. In that case there are four qdots and three u's so they clearly can't be the same thing! But there is a simple 4x3 matrix N such that qdot=N(q)*u when q is a quaternion.

If you don't have numerical concerns regarding orientation, it is no problem in Simbody to construct systems in which qdot=u; i.e. N is an identity matrix. Simple joints like pins, sliders, and u-joints already have that property, and the Gimbal joint can be used instead of a Ball to provide arbitrary orientations with 3 q's and 3 u's=qdots. But that necessarily has a singular configuration.

You might find this paper helpful in understanding the parameterization of multibody systems in Simbody, but also any basic discussion of rigid body mechanics will include at least quaternions and thus have the qdot != u problem to deal with.

Sherm

[Kevin Tanghe]

Thank you very much! That made it all clear.

Regards,
Kevin