Dear Forum, There have been a few posts dealing with a similar issue: Inputting kinematics (usually from experimental recording) to a model, calculating torques with inverse dynamics then using those data to actuate the model to replay the kinematics. This post includes my thoughts on the issue as well as a test model/code for other users to try. i.e. set qpos, qvel, qacc mj_inverse(m, d); .... save the data ... set xfrc_applied = ctrl = 0 set qfrc_applied = qfrc_inverse from the earlier step mj_step(m, d); Long story short, the sampling of the input kinematics data must be fine enough, otherwise the model diverges and eventually blows up. In theory (I think) as long as the dt and the numerical precision of qacc and qvel calculations are appropriate, the replay should work. In practice, you can add servos to do very small corrections to keep the model on the desired trajectory. This is helpful if dt becomes too small to be practical. I made a simple test model complex enough to be non-trivial, but simple enough to get working easily. It is a 2-segment limb stuck to the ground + ball joints. To make this post easier to read, I put most details at the bottom so more interested readers can see/replicate for themselves. This is a long post (sorry!) in 4 sections so skip to the one that interests you: 1) Observations 2) My "solution" 3) Additional thoughts that may or may not be interesting 4) Notes on how to try this yourself with my test_model attached to this post. 1) Observations Observation 1: My test model replays the kinematics "perfectly" (i.e. by eyeball) in a few test cases as described below. E.g. the "wrist" segment rotates along a fixed axis with a fixed "shoulder". The match is best for short simulations (e.g. moving the wrist up), but begins to diverge over longer simulations (wrist moving up/down several times). Observation 2: The equations do not balance exactly. See the post by Radu http://www.mujoco.org/forum/index.p...matics-on-motion-capture-data.3572/#post-4494 qfrc_inverse + qfrc_passive + qfrc_constraint "=" M * v' + qfrc_bias where "=" is almost, but not exact. According to Emo, this is probably due to errors from the RNE method. The post was left hanging and unresolved, so I cannot confirm this information. Observation 3: Watching exploding simulations is amusing. But seriously... Observation 4: If I note when the simulation begins to diverge, let's say t = 20, I can then restart the simulator at t=20 by setting the state (i.e. qpos, qvel, qacc at t=20) and the simulation will run stably until it again diverges at a later point. In a loop, I compare error at each time step, then stop/reset the simulator when qpos (or qvel or qacc) drifts beyond a given tolerance. This is cheating but, it suggests to me that my qfrc_inverse is basically correct, but tiny errors somewhere are accumulating over the integration. Perhaps this is due to Observation #2?? Or perhaps from my poor coding. Nevertheless, my experimentally-recorded ground reaction forces and joint torques are close to those inferred from mj_inverse. 2) A solution: I used splines to get perfectly smooth kinematics. I then up-sampled my experimental data to a very fine sample interval and adjusted the simulation timestep accordingly. This improved the forward simulation a little. Finally, I used a simple PD controller (D was not needed, in the end) to make tiny adjustments. I made my own quaternion PD controller because that was easier for me than working out mujoco's (message me if you'd like my code for that). With this ultra-fine timestep + ultra-smooth kinematics I did not need much fine-tuning of the controller gains. The kinematics from mj_forward played back "perfectly" within a small tolerance that I set. 3) More thoughts (skip this if you like): Using numerical differentiation may cause a "glitch" at the beginning of the simulation which grows over time. To get values for derivatives at qvel and qacc at t=start one can obtain fits from the kinematics data (e.g. spline, polynomial, etc) and algebraically differentiate them and substitute for a value at t=0. I have yet to try this Additionally, I have not tried up-sampling my experimental data to get ultra-tiny dt to see if I could eliminate the need for a PD controller. Finally, perhaps I am having these problems because I deal with small objects (inertias on the order of 10-7 kgm2) that move quickly. 4) You can try my model yourself using the attached test model: Please reply to this thread if you do try this, and let me know your results (i.e. did you get similar behaviour?). In practice, I load experimental kinematics from a file. For this it's simpler as an exercise not to worry about numerical precision of the file import/export, so the kinematics will be set using a sine function. 1. Set up the kinematics parameters using a sin function acting at the most distal joints to produce motion. Set the sim timestep. ...case 1: mjtNum ncycles = 0.5; // number of movement cycles to execute int samplespercycle = 100;//we will change this to see the effect on time sampling int cyclespersec = 100; int nsamples = ncycles * samplespercycle; // number of time steps for the duration of the simulation mjtNum dt = 1/(mjtNum)samplespercycle/(mjtNum)cyclespersec; //timestep m->opt.timestep = dt; 2. Calculate qpos, qvel and qacc. It calls qpos2qvel and qvel2qacc using calculate_qvelqacc.cpp attached (kindly written by a colleague of mine - feel free to improve upon it). // initialize state vectors for each time row mjtNum q[nsamples*m->nq];//position mjtNum v[nsamples*m->nv];//velocity mjtNum a[nsamples*m->nv];//acceleration mjtNum tact[nsamples*m->nv];//actuator torques I will calculate from mj_inverse. //calculate q for every time point. I will only modulate the final two ball joints for (int i = 0; i < nsamples; i ++) { mjtNum q1[4] = {1, 0, 0, 0}; mjtNum q2[4] = {1, 0, 0, 0}; mjtNum q3[4] = {1, 0, 0, 0}; mjtNum q4[4] = {1, 0, 0, 0}; mjtNum a3[3] = {0.4444, 0.8888, 0.1111};//axis for the 3rd quaternion just an arbitrary axis of rotation mjtNum a4[3] = {1, 0, 0};//axis for the 4th quat mjtNum time_value = ((mjtNum)i) / ((mjtNum) nsamples) * (mjtNum) (ncycles * 3.141); mjtNum ang3 = 0.3 * sin(time_value);//angle for the 3rd joint mjtNum ang4 = 3.141 / 2 - (3.141 / 2) * sin(time_value);//angle for the 4th joint //addresses of the ball joint quaternions int q1_adr = (m->jnt_qposadr)[0] + 3; int q2_adr = (m->jnt_qposadr)[1]; int q3_adr = (m->jnt_qposadr)[2]; int q4_adr = (m->jnt_qposadr)[3]; //mju_axisAngle2Quat(q3, a3, ang3); leave this commented out for now to leave 3rd joint stationary mju_axisAngle2Quat(q4, a4, ang4); //rotation for the most distal quat for (int j = 0; j < 4; j ++) { q[i*m->nq + j + q1_adr] = q1[j]; q[i*m->nq + j + q2_adr] = q2[j]; q[i*m->nq + j + q3_adr] = q3[j]; q[i*m->nq + j + q4_adr] = q4[j]; } //make sure the free joint is fixed on the floor q[i*m->nq + 0] = 0; q[i*m->nq + 1] = 0; q[i*m->nq + 2] = 0; } //calculate v and a. qpos2qvel(m, q, v, nsamples, m->opt.timestep); qvel2qacc(m, v, a, nsamples, m->opt.timestep); 3. Loop through q and perform inverse dynamics: for (int r = 0; r < nsamples; r ++) ... mju_copy(d->qpos,&q[r*m->nq],m->nq); mju_copy(d->qvel,&v[r*m->nv],m->nv); mju_copy(d->qacc,&a[r*m->nv],m->nv); mj_fwdPosition(m,d); mj_inverse(m,d); //save inverse data for (int i = 0; i < m->nv; i++) { tact[r*m->nv + i] = d->qfrc_inverse; } 4. Reset the state (set qpos, qvel, qacc to their values at r = 0). 5. Loop through tact to perform forward dynamics, compare kinematics with input kinematics for (int r = 0; r < nsamples; r ++) ... //set control and xfrc_applied to 0, just to be sure for (int i = 0; i < m->nu; i ++) { d->ctrl = 0; } for (int i = 0; i < (6* m->nbody); i ++) { d->xfrc_applied = 0; } //apply the force to all dof for (int i = 0; i < m->nv; i ++) { d->qfrc_applied = tact[r*m->nv + i]; } mj_step(m, d); 6. Check the error at each time step mjtNum qErrorTot; //total error for all dof for (int i = 0; i < m->nq; i ++) { qErrorTot += abs(d->qpos - q[r*m->nq + i]); } printf("total error %f r = %i\n", totErr, r); Using these conditions, the simulation "looks" good, but there is some slight error that accumulates. Things to try out: case 2: Same as above, but try 4 cycles of motion (ncycles = 4). You will see the sim starting to diverge. case 3: Change back to ncycles = 0.5; uncomment the following line to impose more complex motion: mju_axisAngle2Quat(q3, a3, ang3); Now, the sim blows up. case 4: Same as case 4, but with with more time steps (samplespercycle = 1000); The model improves, but still becomes unstable at the end. You can use this to play with the model and see the effect of sampling. I hope this is useful, sorry if my code is sloppy I'm not a programmer. Please reply to the thread with any thoughts, etc.
A few things to keep in mind: Inverse dynamics are exact and analytical. Forward dynamics rely on an iterative solver, which does not converge exactly to the optimal solution assumed in inverse dynamics. So if you compute inverse dynamics on an empirical trajectory, and apply the resulting forces to forward dynamics, the numerical nature of the forward dynamics is likely to be the main cause for divergence. There is a trick to bypass the above issue. If you warm-start the forward dynamics solver with the optimal acceleration (by setting qacc_warmstart = your data before each mj_step) the numerical solver will immediately discover that the gradient is zero and exit, so now the forward dynamics will be consistent with inverse dynamics. In fact the discrepancy between the analytical inverse solution and the numerical forward solution is used internally by MuJoCo to assess the convergence of the numerical solver. If you enable the option flag FwdInv, it will compare the forward and inverse dynamics at each step and print log10 of the difference in the simulation window. Coming back to your questions, numerical integration could also be an issue. What you are doing here is running a long open-loop simulation. If you started with kinematic data consistent with MuJoCo physics, that will work (assuming the finite difference scheme you use is identical to the Euler integrator). But if you obtained data from elsewhere (especially from actual experiments) there will be estimation errors or other discrepancies, and the trajectories will usually be inconsistent with the physics model. A physics model with hard constraints would just throw an error and give up. MuJoCo uses soft constraints so anything is consistent with it, in the sense that any behavior can be generated given large enough applied forces. But these forces may indeed have to be large to compensate for underlying model-data mismatch (e.g. when the kinematics say that there is a contact penetration). So now you are integrating large and possibly noisy forces in open-loop. This is a recipe for disaster. The resetting trick you discovered is essentially a way to introduce feedback, albeit intermittently. An even better way to do feedback is to design a PD controller that pushes the simulation towards the data trajectory. The PD gains can be small; just having feedback in the loop will have a significant stabilizing effect. Finally, the problem that you should be solving here is somewhat different from what inverse dynamics alone can do for you. The real problem is this: given kinematics obtained from somewhere, find similar but not necessarily identical kinematics consistent with the physics model, and only then obtain forces via inverse dynamics. This is sometimes called dynamic filtering, and is a form of dynamic trajectory optimization. The new product I am developing (Optico) will solve this among other things, so it will become standard functionality. Until then you are on your own in terms of cleaning up the kinematic data in a physically-consistent way. The RNE point I made in another thread is not an inaccuracy in RNE. I was simply saying that the algorithm used by RNE is different, so mathematically they are identical but the floating-point round-off errors accumulate differently, thus one can expect very small differences. I don't believe this has anything to with the issues you are having though.