Nonsensical articulated inertia/mass matrix

[mentisn4b]

I want to find the "effective mass matrix" JM⁻¹Jᵀ to compute constraint impulses (Jv/JM⁻¹Jᵀ), using for J the Jacobians from a ContactConstraint. But it seems the mass matrices returned by getArticulatedInertia() are unsuitable for this.

For a single rigid body, all is OK. But when using a weld joint to combine two spheres with size 0.1 and mass 10, the one sphere is transformed (0.5, 0.5, 0) from the other, calling getArticulatedInertia() on the first sphere yields a matrix that is a somehow a function of both of the spheres, and even has off-block-diagonal elements (nonsensical?!):

2.52 -2.5 0 0 0 5 -2.5 2.52 0 0 0 -5 0 0 5.02 -5 5 0 0 0 -5 20 0 0 0 0 5 0 20 0 5 -5 0 0 0 20

And getArticulatedInertia() on the second sphere just gives the mass matrix of the rigid body in isolation:

0.01 0 0 0 -0 0 0 0.01 0 0 0 -0 0 0 0.01 -0 0 0 0 0 -0 10 0 0 -0 0 0 0 10 0 0 -0 0 0 0 10

Is there any further transformation that needs to be done on the result from getArticulatedInertia() to obtain the "effective mass" JM⁻¹Jᵀ, or is it necessary to use the unit impulse method to compute JM⁻¹Jᵀ?

Thanks!

[jslee02]

Try with Skeleton::getMassMatrix() instead of getArticulatedInertia(). An articulated inertia is a cache data that is internally used for the forward dynamics. getMassMatrix() returns the joint space inertia of the skeleton.

[mxgrey]

At the risk of bikeshedding a little bit, I wouldn't characterize getArticulatedInertia() as internal cache data. It's always kept up-to-date through lazy evaluation, so what you get out of it is always valid, and there's nothing wrong with asking for it.

The important factor here is that "articulated inertia" is a very specific dynamics quantity that's used in Featherstone's Articulated Body Algorithm. It is not the same thing as the mass matrix. If you have an algorithm (like a controller, for instance) that uses the "articulated inertia", there is absolutely nothing wrong with retrieving it using getArticulatedInertia().

[mentisn4b]

Thanks for the feedback, that makes sense if the "articulated inertia" is another quantity. Just to check then for the right way to get the "effective mass" matrix. Let's say there is a sphere2 body which is welded to a sphere1 body which is attached to the world by a free joint (as described in the original post). It seems that only sphere1 will return a mass matrix; the matrix returned for sphere2 is of size 0. Why do mass matrices appear to be a property of Skeletons rather than BodyNodes?

Code I'm using:

dart::dynamics::Skeleton *sk = myWorld->getSkeleton("sphere skeleton");
for (std::size_t i = 0; i < sk->getNumBodyNodes(); ++i) {
    dart::dynamics::BodyNode *bn = sk->getBodyNode(i);
    std::cout << "Body node " << i << ": mass matrix " << sk->getMassMatrix(sk->getIndexOf(bn)) << std::endl;
}

Result:

Body node 0: mass matrix 2.52 -2.5 0 0 0 5 -2.5 2.52 0 0 0 -5 0 0 5.02 -5 5 0 0 0 -5 20 0 0 0 0 5 0 20 0 5 -5 0 0 0 20 Body node 1: mass matrix

Obviously I could use a unit impulse test to find the effective mass, as the PGS LCP solver does, but I was expecting it would be possible to use the JM⁻¹Jᵀ formulation (which should be equivalent).

Cheers!

[mxgrey]

Maybe we need to clarify something:

Are you looking for the n-body mass matrix of the system, or are you looking for the moment of inertia? Those are also two different quantities. The term "mass matrix" should not be used to refer to the moment of inertia, because that would be incorrect terminology.

The dimensionality of a mass matrix is dependent on the number of degrees of freedom in a system. Therefore, when you attach a body with a weld joint, the mass matrix does not get changed because the degrees of freedom have not changed. Each body will retain its own moment of inertia which you can retrieve using bodyNode->getInertia()->getMoment() where bodyNode is a pointer to the BodyNode instance that you are interested in.

[mxgrey]

Also, to further clarify, the code snippet you have provided is not using the Skeleton::getMassMatrix(~) function correctly. The index that you may pass into Skeleton::getMassMatrix(std::size_t) must be the index of a tree in the Skeleton, not a BodyNode index. The number of trees in a Skeleton is determined by the number of root BodyNodes in the Skeleton (a Skeleton is allowed to contain arbitrarily many root BodyNodes). A root BodyNode is any BodyNode which is attached to the "world", usually by a FreeJoint (but any joint type is allowed).

Your Skeleton only has one tree, and the index of that tree will be 0. Its mass matrix will be a 6x6 matrix, because all of its degrees of freedom come from a 6-dof FreeJoint. When you then ask for the mass matrix of tree 1, the result will have to be empty because you do not have a second tree.

[mentisn4b]

Hi mxgrey, thanks for your responses. Please let me clarify. I will assume spatial algebra notation, so "velocity" actually refers to linear + angular velocity, same for positions etc.

The problem I'm trying to solve is to measure the change in velocity of one particular BodyNode in an articulated skeleton, when an impulse is applied at a particular point on that BodyNode. As far as I understand, there are two ways to do this:

• Apply a unit (magnitude 1) impulse and measure the change in velocities (extant method in PGSLCPSolver.cpp)
• Compute using linear algebra what the change in velocity would be, i.e. (JM⁻¹Jᵀ)⁻¹, where J is the constraint Jacobian and M is the sought-after "mass matrix".

I thought it would be easy to obtain M (and J is just contactConstraint.mJacobians1 or 2 in the case of a contact constraint). N.B. when a unitImpulse is applied, it appears that getInvProjArtInertia() is used in calculating the velocity change:

void GenericJoint<ConfigSpaceT>::updateVelocityChangeDynamic(...
...
  mVelocityChanges
      = getInvProjArtInertia()
      * (mTotalImpulse - getRelativeJacobianStatic().transpose()
         *artInertia*math::AdInvT(this->getRelativeTransform(), velocityChange));

I'm not sure if the second method (computing JM⁻¹Jᵀ) is feasible at all at this point, since it is not clear what M should be in the case of an articulated skeleton. Perhaps it is easier to just continue to use the unit impulse test method?

[mxgrey]

Keep in mind that a WeldJoint effectively turns two bodies into one. DART uses generalized coordinates, so joint constraints are enforced implicitly. That means that a WeldJoint cannot have any flexibility, and any impulse imparted on one body will be fully transferred to the other.

If you want to investigate the impulse responses of the bodies independently of each other, perhaps you really want to make a FreeJoint between them, perhaps with very high spring stiffness and very high damping coefficients (see dart::dynamics::Joint::setSpringStiffness(~,~) and Joint::setDampingCoefficient(~,~)). Stiffness and damping effects are computed using implicit Euler integration, so they should produce stable dynamics even for fairly large coefficients.

Using a FreeJoint between the bodies will produce a 12x12 mass matrix where the upper-right and lower-left 6x6 should be all zeros. Therefore the upper-left 6x6 and lower-right 6x6 will likely be the mass matrices you're looking for.

[mentisn4b]

Hi, many thanks for your detailed explanation. Applying unit impulses and measuring the change in velocity does the trick. My custom velocity-level sequential impulse solver is now working perfectly. Cheers!