When inspecting the joint space inertia matrix, e.g. with the line: mju_printMat(d->qM, m->nv, m->nv); the result is not symmetric? Shouldn't it be? For example, using the inverted_double_pendulum.xml model I get something like: 17.63140337 1.42614617 -2.88962607 0.51363217 -0.02552320 0.76069177 10.79701932 1.42487788 -2.00145298 All other models I have tried are also non-symmetric. If it makes a difference, I am calling the printMat function from the control callback. -Mike
It is symmetric, but it uses a custom sparse representation while mju_printMat assumes a dense representation. You can use the function mj_fullM(model, your_buffer, data->qM) to obtain a dense representation, which you can then print with mju_printMat.
What if I want to make use of LTDL factorization which is available at (data->qLD) and calculate (M.inverse * x). I think I need the parent array (i.e. lambda) which I cannot find in the data -- I am using Featherstone's notation. By the way, I am going through all this to calculate the linear dynamics, so if there is a shortcut, it is more to the advantage. Thanks in advance! Regards, Mahan
The function mj_solveM solves M * x = v for given v, using the factorization in data->qLD. There is also mj_solveM2 which solves sqrtm(M) * x = v. As for linerizing the dynamics, you can do it with finite differences, see code sample here: http://www.mujoco.org/book/programming.html#saDerivative I am working on the next product called Optico which is an optimal control toolbox built on top of MuJoCo, and it will have analytical derivatives (or analytical approximations to the derivatives in some cases). But it is not ready yet. In the meantime, finite differences are the only option. They can be computed quickly using multi-threading, as illustrated in the code sample.