For an application I am writing I need MuJoCo to function as the dynamics back-end. This is going so far so good, however, I'm running into a problem tying stuff together. I need the system dynamics in terms of the state: Where the state typically contains positons and velocities: This notation is nice because it allows you to tackle Eulrer angles vs. Angular velocity. (See this post.) In this case qpos has one more dimension than qvel. The second part of the state derivative is easy: But what to do with ? Is there a built-in function to get the derivative of d->qpos? I know it is not (always) equal to d->qvel, which is obvious when the dimensions are different. In the documentation this is glanced over without giving a real answer: http://www.mujoco.org/book/programming.html#siStateControl I could find the derivative of the quaternion using qvel, however, for e.g. the Humanoid model with nq = 28 it is not trival to find which variables make up the freejoint rotation. Thank you!
The time derivative of qpos is qvel. The difference between the dimensions stems from the fact that quaternions have 4 parameters and angular velocity has three parameters. https://en.wikipedia.org/wiki/Rotat...ee_dimensions#Quaternion_↔_angular_velocities
@florianw thank you for your reply. A question, how can you figure out wich values from qpos make up a quaternion? Since it is a combination of the degrees-of-freedeom (not exactly, but that's roughly the idea) it is not obvious to me to find specific parts of it.
Quaternions occur for freejoints and ball joints. Also the joints occur in the same sequence as specified in the mjcf file. You can get everything from the api, like joint type and the corresponding starting adress in qpos (qposadr if i remember correctly). As a quick yet hack check you could calculate the norm of the four values. it should equal 1.
I wanted to come to this to complete it. You can use mju_derivQuat() to compute the quaternion derivative based on the angular velocity. You could analyse your model to find the indices of quaternions in qpos and use it to convert qvel to qpos_dot. I also needed partial derivatives, so I needed a more explicit conversion. I read up on quaternions a little bit and found: Here is an example with my implementation.