Dear Forum, I am attempting to use MuJoCo inverse dynamics to calculate joint torques from experimental data. I have a few questions about how this is done: I understand that the inverse (instead of forward) function should be called, but where are the experimental data input? 1. Regarding kinematics, should I use joint position actuators (ball joints) that are updated at each time step? If so, I'm unclear on the following statement from documentation (under actuation/general/joint) "...The actuator length is defined as the dot-product between this gear axis and the angle-axis representation of the joint quaternion position." If the rotation axis is defined by the gear, wouldn't that make the unit quaternion redundant? What is meant by the angle-axis representation of a quaternion - is it simply the axis vector and angle extracted from a unit quaternion? 2. Regarding ground reaction forces, should I include those as a point force applied to the foot (i.e. substituting those for the contact model)? Sorry for the long question, Thanks in advance for the help, Best, Chris
The inputs to inverse dynamics are qpos, qvel, qacc. The main outputs are: qfrc_inverse: applied force in joint coordinates qfrc_constraint: constraint force in joint coordinates efc_force: constraint force in constraint coordinates mj_inverse() does not currently attempt to infer actuator forces. It just computes qfrc_inverse which is the sum of all external and actuator forces. On the bright side, the constraint forces are the output of the computation and not the input. So all you need to provide is the current position, velocity and acceleration (qpos, qvel, qacc). This may seem magic, but the reason it is doable is because the constraint model is soft -- so the kinematics uniquely determine both the applied force and the constraint force.
Hi, Great, thanks for the quick response, as usual. Very impressed with the software so far! Best, Chris
Hello again, I now happily have my simulation replaying my experimental kinematics. I've used the following: d -> qpos = quat_data; where quat_data are my imported quaternion values at a point in time. I have two questions (probably related): 1. Why is it necessary to input qpos, qvel and qacc? Shouldn't the derivatives follow from qpos implicitly? I tried inputting my qvel and qacc data using: d -> qvel = quatdot_data, etc, where quatdot_data are the velocities expressed as quaternions (likewise for accelerations). But, the simulation basically blew up. So, I removed the qvel and qacc data and the simulation replays the experimental kinematics. 2. qfrc_inverse returns only 0 values for any given time point. So, I must be missing where/how I am to input the kinematics data properly. So far, I've implemented: mj_inverse(m, d); within the simulation loop. Is there something that I'm missing? My model code is below (note the extremely small masses and inertia): Code: <body name="s_pft"><!-- Foot --> <inertial pos="0 0 0" mass="0.00016292" diaginertia="2.575E-09 4.48E-10 2.73E-09"/> <geom type="capsule" mass="0.5" fromto="0. 0. 8.7E-2 0. 0. 0" size="0.02" rgba="0.5 0.5 0 .4"/> <joint name="jhL" type="ball" pos="0. 0. 0"/> <body name="s_tar"><!-- Tarsals --> <inertial pos="0 0 0" mass="0.00015557" diaginertia="1.279E-09 3.99E-10 1.264E-09"/> <geom type="capsule" density="1" fromto="0. 0. 0.1983 0. 0. 0.087" size="0.02" rgba="0.5 0.5 0 .4"/> <joint name="jkL" type="ball" pos="0. 0. 0.087"/> <body name="s_tib"><!-- Tibfib --> <inertial pos="0 0 0" mass="0.00036153" diaginertia="6.383E-09 1.524E-09 6.6E-09"/> <geom type="capsule" density="1" fromto="0. 0. 0.4811 0. 0. 0.1983" size="0.02" rgba="0.7 0.7 0 .4"/> <joint name="jaL" type="ball" pos="0. 0. 0.1983"/> <body name="s_fem"><!-- Femur --> <inertial pos="0 0 0" mass="0.00077613" diaginertia="1.57E-08 6.71E-09 1.594E-08"/> <geom type="capsule" density="1" fromto="0. 0. 0.7266 0. 0. 0.4811" size="0.02" rgba="0.5 0.5 0 .4"/> <joint name="jtL" type="ball" pos="0. 0. 0.4811"/> <body name="s_bod"><!-- Torso --> <inertial pos="0 0 0" mass="0.0072" diaginertia="4.952E-07 4.042E-07 7.105E-07"/> <geom type="capsule" density="1" fromto="0. 0. 1.0 0. 0. 0.7266" size="0.02" rgba="0.5 0.5 0 .4"/> <joint name="jpL" type="ball" pos="0. 0. 0.7266"/> </body> </body> </body> </body> Thank you again, Best, Chris
MuJoCo does not attempt to compute derivatives via finite differences. This is up to the user. It couldn't do that anyway because each call is independent from the previous calls, so it can only see the current qpos and does not assume anything about the previous or next qpos. So you have to set qpos, qvel, qacc in order to run inverse dynamics. Once all of them are in place, mj_inverse() should work. You mentioned that you set qvel and qacc. Note that when you have quaternion joints, the position of the quaternion is 4D while its velocity and acceleration are 3D - did you take this into account? If not, you ended up with the wrong data in qvel and qacc. Given two qpos vectors possibly containing quaternion joints, you can use the function mj_differentiatePos() to "subtract" them properly, and compute a velocity vector in the format of qvel.
Yes, I had qvel and qacc expressed mistakenly as quaternions. It works now! Thanks for the help! Best, Chris
Actually, it doesn't really work ... there is something wrong with the magnitudes of qfrc_inverse - the values are extremely large (by ~2 to 3 orders of magnitude). The problem seems to be the values I'm using for qvel. Are these values supposed to be angular velocity vectors? I.e. 3D angular velocity = scalar dtheta/dt * rotation axis vector? Similarly for accelerations? Thank you, Best, Chris
qvel for rotational joint is in radians/sec (and similarly for qacc). The compiled model always uses radians, even if the XML was defined in degrees.
Sorry, I was not clear about my question, let me clarify a couple of points then restate the question: 1. I use radians always. 2. I pre-process in Mathematica, so I don't want to use mj_differentiatePos(). Happy to explain later, but I'd like to keep this post short. 3. In Mathematica, I compute quaternions, Q, in local joint coordinates (works fine). Then, I calculate velocities (w) for a given joint by w = rotation axis * dtheta/dt, where w is what I import into MujoCo for qvel (concatenated for all joints) and dtheta/dt is the "3d" scalar angular velocity magnitude about the instantaneous rotation axis in local coordinates (i.e. transformed by Q). I use a similar calculation for qacc. Is this incorrect? If so, could you please tell me how to calculate the 3D velocity vectors from the quaternions? Can you please help with this? Best wishes, Chris
Sorry about the delay. Suppose you have two quaterions q1 and q2. First you compute the quaternion difference between them as: mju_negQuat(qneg, q1); mju_mulQuat(qdif, qneg, q2); Then you convert this difference quaternion into an 3D velocity vector as: mju_quat2Vel(vel, qdif, 1); The "1" has the meaning of dt. You can specify the actual dt.
OK, thanks, that's helpful. Just two small further questions: 1. Given that inverse dynamics computes mjData.qfrc_inverse = mjData.qfrc_applied + Jacobian'*mjData.xfrc_applied + mjData.qfrc_actuator ... and I have no actuators nor xfrc point forces in my model, can I use the output of qfrc_input (saved to file) to use as input torques to the same model, but run forward (i.e. set qfrc_applied using prior results from inverse dynamics)? I want to run the forward dynamics to check the inverse dynamics results to see if I get the same kinematics. 2. I suspect something is wrong with my inverse dynamics code - the result doesn't seem to depend at all on qvel (i.e. if I make those values = 0, the result is the same). Am I advancing the simulation properly? I've modified my code based on the simulate.cpp file from the documentation. I've already verified that qpos, qvel and qacc are being set correctly. Is the code below OK? Code: void advance(void) { quatpos();// function to input kinematics data into qpos, qvel, qacc //mj_step(m, d); mj_inverse(m, d); usleep(10000); counter += 1; } Thank you
1. Yes you can do that sanity check. In fact it is built in. If you enable the flag "fwdinv", MuJoCo will compute the inverse dynamics after each forward dynamics step, and save the norm of the discrepancy in mjData.sovler_fwdinv. But you can also do it yourself. 2. If you are setting qpos at every time step, there is no simulation, you are advancing the state manually. Or did you mean that qvel does not affect the output of inverse dynamics, namely mjData.qfrc_inverse. If it is the latter, something weird is indeed going on.
Yes, qvel seems to have no impact on the output of inverse dynamics - I'm quite confused by this. I will double check to see if I am not accidentally overwriting that variable...
Hi Emo, I am interested in obtaining inverse dynamics in a form of inferred actuator forces, as you described in post #2. Basically, I have a system trajectory in a form of a sequence of time-points, positions and velocities, and I want to obtain applied torques that would guide the system over that trajectory (in presence of all gravitational, interaction forces, etc.). I was wondering if that capability is available in MuJoCo Pro now, or you can suggest an approach or an example code that would help to solve this problem? In my understanding, the problem can be generally solved via an iterative optimization approach, but there should be faster ways to do that using existing MuJoCo architecture. Best, Anton
If you set qpos, qvel and qacc to the corresponding values at one point along the trajectory, and call mj_inverse, it will compute the applied torque in qfrc_inverse. Now if you want to further decompose this torque into a torque that can generated by the actuators and a torque in the unactuated space, that is up to you. You need the field actuator_moment, which is the matrix mapping actuator forces to joint torques. The pseudo-inverse and null space of that matrix can be used to do the decomposition.
Thank you a lot for the answer, I was able to extract torques from qpos, qvel and qacc, and then replay them through MuJoCo HAPTIX precisely repeating the trajectory. I think it will be enough for now, I will analyze the unactuated space outside of MuJoCo, but thanks for the advice on decomposition.
Hi Chris, I encounter the same problem as you. Did you solve it? The magnitude of qfrc_inverse doesn't make sense for the joint force. Thanks!
Hi Cat, So sorry for my delay, I've just returned from traveling over the past two weeks. I don't have much time to report on this now, but in short, yes I did end up with qfrc_inverse values that make sense (i.e. forces/torques that nearly match forceplate data). But I wouldn't say that I've completely solved the issues with this approach. There were a number of problems with my earlier models which I have since solved, but a few remaining. I will write a longer response later today or tomorrow describing my status with this in case it might be helpful to you. Best wishes, Chris