What exactly is qLD? #224

JeanElsner · 2022-04-05T17:42:18Z

JeanElsner
Apr 5, 2022

According to the API documentation mjData.qLD is an L'*D*L factorization of M whereas mjData.qLDiagInv is the inverse of the diagonal elements of the factorization. As the diagonal elements of the full matrix of qLD retrieved from mj_fullM are not ones as expected for an LDL decomposition I think qLD is actually L*D. This seems to be true as qLD*qlDiagInv results in a matrix with ones on the diagonal. This matrix however is also not a triangular matrix, which I think may be due to mj_fullM. In any case I am unable to reconstruct the dense representation of qM from these entities. I would expect L'*D*L = qM where L = tril(qLD*qlDiagInv), D = diag(1/qlDiagInv) and the respective dense representations of qLD and qM.

I hope this is understandable, any help would be appreciated.

Answered by yuvaltassa

Apr 6, 2022

Yes, LD storage needs clarification. It is the lower triangle of the LDL factorisation with D replacing the unit diagonal.
Using Python notation, if

M = np.zeros((nv, nv))
LD = np.zeros((nv, nv))
mujoco.mj_fullM(model, M, data.qM)
mujoco.mj_fullM(model, LD, data.qLD)

Then for

L = np.tril(LD, -1) + np.eye(nv)
D = np.diag(np.diag(LD))

we get

L.T @ D @ L == M

to numerical precision. The qLDiagInv and qLDiagSqrtInv are there just to avoid taking roots and inverses during the solve, which is an important operation.

NB we have the upper triangle on the left and the lower triangle on the right, since this is zero-fill-in reverse Cholesky, what Featherstone calls "LTL", rather than the LDL you'll…

View full answer

yuvaltassa · 2022-04-05T20:32:15Z

yuvaltassa
Apr 5, 2022
Maintainer

Both qM and qLD are the non-zero elements of branch-induced sparse matrices, as in Chapter 6.4 of Featherstone (2008).

mj_fullM reconstructs the dense, nv x nv representation. Here is the implementation, it will give you insight into the memory structure:

// convert sparse inertia matrix M into full matrix
void mj_fullM(const mjModel* m, mjtNum* dst, const mjtNum* M) {
  int i, j, adr = 0, nv = m->nv;
  mju_zero(dst, nv*nv);

  for (i=0; i<nv; i++) {
    j = i;
    while (j>=0) {
      dst[i*nv+j] = M[adr];
      dst[j*nv+i] = M[adr];
      j = m->dof_parentid[j];
      adr++;
    }
  }
}

0 replies

JeanElsner · 2022-04-05T23:18:31Z

JeanElsner
Apr 5, 2022
Author

Yes, thank you. What I gather from this piece of code is that mj_fullM will always reconstruct a symmetric matrix. For an LDL decomposition the L matrix however is supposed to be a unit triangular matrix. But even if you make L triangular yourself the main diagonal entries are not 1. They become 1 if you multiply a diagonal matrix with qLDiagInv elements (hence I think qLD is L*D). So far so good, but even then L'*D*L does not equal the dense representation of qM.

Maybe a little more concrete: how do qLD and qLDiagInv relate to qM (concerning dense nv x nv representations).
My interpretation is that the following should hold:

#!/usr/bin/env python3
import mujoco
import numpy as np

XML = """<mujoco>
  <worldbody>
    <body name="test" pos="0 0 .5" euler="45 45 0">
      <geom type="box" size=".1 .1 .1" rgba="1 1 1 .8" />
      <body pos="0 0 .5">
        <joint />
        <geom type="box" size=".1 .1 .1" rgba="1 1 1 .8" />
        <body pos="0 0 .5">
          <joint />
          <geom type="box" size=".1 .1 .1" rgba="1 1 1 .8" />
          <body pos="0 0 .5">
            <joint />
            <geom type="box" size=".1 .1 .1" rgba="1 1 1 .8" />
              <body pos="0 0 .5">
                <joint />
                <geom type="box" size=".1 .1 .1" rgba="1 1 1 .8" />
            </body>
          </body>
        </body>
      </body>
    </body>
  </worldbody>
</mujoco>"""

model = mujoco.MjModel.from_xml_string(XML)
data = mujoco.MjData(model)
mujoco.mj_step(model, data)

LD = np.zeros((model.nv, model.nv))
D = np.diag(1/data.qLDiagInv)
M = np.zeros((model.nv, model.nv))

# Get dense representation of qM and qLD
mujoco.mj_fullM(model, LD, data.qLD)
mujoco.mj_fullM(model, M, data.qM)

# Get unit triangular matrix from LD
L = np.tril([email protected](data.qLDiagInv))

# L'*T*L = M
print(np.allclose(L.T@D@L, M))

0 replies

yuvaltassa · 2022-04-06T00:04:34Z

yuvaltassa
Apr 6, 2022
Maintainer

Yes, LD storage needs clarification. It is the lower triangle of the LDL factorisation with D replacing the unit diagonal.
Using Python notation, if

M = np.zeros((nv, nv))
LD = np.zeros((nv, nv))
mujoco.mj_fullM(model, M, data.qM)
mujoco.mj_fullM(model, LD, data.qLD)

Then for

L = np.tril(LD, -1) + np.eye(nv)
D = np.diag(np.diag(LD))

we get

L.T @ D @ L == M

to numerical precision. The qLDiagInv and qLDiagSqrtInv are there just to avoid taking roots and inverses during the solve, which is an important operation.

NB we have the upper triangle on the left and the lower triangle on the right, since this is zero-fill-in reverse Cholesky, what Featherstone calls "LTL", rather than the LDL you'll find in standard textbooks.

1 reply

JeanElsner Apr 6, 2022
Author

Ah, thank you so much, this clarifies it completely. A simple and compact scheme, but I didn't think of this.

kevinzakka · 2022-04-09T20:36:44Z

kevinzakka
Apr 9, 2022
Collaborator

This is super informative, thanks @yuvaltassa !

0 replies

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

What exactly is qLD? #224

Uh oh!

{{title}}

Uh oh!

Replies: 4 comments 1 reply

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{editor}}'s edit

{{editor}}'s edit

Uh oh!

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{editor}}'s edit

{{editor}}'s edit

Uh oh!

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{title}}

Uh oh!

Select a reply

Uh oh!

What exactly is qLD? #224

Uh oh!

JeanElsner Apr 5, 2022

Replies: 4 comments · 1 reply

Uh oh!

yuvaltassa Apr 5, 2022 Maintainer

Uh oh!

Uh oh!

JeanElsner Apr 5, 2022 Author

Uh oh!

Uh oh!

yuvaltassa Apr 6, 2022 Maintainer

Uh oh!

JeanElsner Apr 6, 2022 Author

Uh oh!

kevinzakka Apr 9, 2022 Collaborator

JeanElsner
Apr 5, 2022

Replies: 4 comments 1 reply

yuvaltassa
Apr 5, 2022
Maintainer

JeanElsner
Apr 5, 2022
Author

yuvaltassa
Apr 6, 2022
Maintainer

JeanElsner Apr 6, 2022
Author

kevinzakka
Apr 9, 2022
Collaborator