Hello! I am facing awfully time-wrecking troubles, trying to implement a (fairly high gain) PD - joint controller with gravity compensation. Essentially what i am doing is: 1. call mj_step_1 2. compute input as u = K(q_d - q) + D_d*(dq_d - dq) + Cg //(mj_rne(flg_acc=0) 3. call mj_step_2 I am not using sensors to retrieve the current velocity and position of my joints but I am directly grabbing them from mj_data (after having called mj_step_1). -> Is there a difference between using sensors (assuming no noise/cutoff) and retrieving the values directly after mj_step_1? It seems as if the d-gain(should be aperiodically damped) in simulation causes instabilities very quickly. The velocities quickly evaluate to large values. Decreasing the simulation step-size didn't help either. What frustrates me most is that using a computed torque controller (M^-1*(K(q_d - q) + D_d*(dq_d - dq)) + Cg) allows one to robustly simulate enormously stiff pd controlled manipulators. Are there any suggestions/tips or am I assuming sth. awfully wrong with the way I utilize mj_data?
Accessing mjData directly is fine. Several thing could be causing the problem: 1. Large damping causes numerical instability with Euler integration. Options are to use RK4, or (even better) define damping in the joints instead of the controller. The advantage of joint damping is that the Euler integrator handles it implicitly which is a lot more stable. You can still have D*qvel_desired added to the control, but put the -D*qvel term in the joint damping. 2. If you have free joints or ball joints, you cannot simply subtract the position vectors. The resulting vector has more dimensions than the DOF space and things get misaligned, causing a general mess. Instead you should use mj_differentiatePos. 3. mj_rne computes not only gravity but also centripetal and Coriolis forces, so you are cancelling those forces in addition to gravity. Did you mean to do that? It will not make your simulation unstable either way. 4. Not sure how much you decreased the simulation stepsize. If the numerical integrator is the problem, there is always a small enough stepsize that will make it stable. So it is possible that your feedback gains are to large and are making the real system unstable, in which case the simulator is doing the right thing and telling you that your system is unstable. 5. Are there constraints in this model? If so, check the convergence of the constraint solver; maybe it gets interrupted early and computes incorrect forces. Note also that RK4 works best for smooth systems.
That helped a lot! I was able to simulate a stiff pd controller with gravity compensation by defining the damping in the joint damping. How ever this has risen some (hopefully final) follow-up questions. 1. Just out of pure curiosity. I have seen the " handles it implicitly" (damping in joints) in the mujoco documentation as well. Could you just very briefly elaborate on what that means (could one replicate that "implicit handling" in code and then apply it as input torque)? 2. Since the mass matrix is not stationary and since I want to achieve aperiodic damping, I have to modify the joint damping parameters of the model online. Now I can remember that you mentioned in a thread that mujoco assumes a constant mj_model throughout the phase of one mj_step(). For me it would of course be advantageous to modify the damping after mj_step1 just prior to setting the input torque and executing mj_step2. Can I do that without violating any important mujoco/physics assumptions. 3. You mentioned that RK4 could also be beneficial. Now the documentation states that if using mj_step1 and mj_step2, RK4 will not apply. The alternative I guess would be using mj_step and the control callback. Is this possible if one works in a multithreaded application with many (nota constant number) mj_model and mj_data instances?
Sorry about the long delay. 1. Implicit damping means that a damping term is added to the diagonal of the inertia matrix, which is then re-factorized in the Euler integrator. This does not work with RK4. So ironically, Euler can be more stable/accurate than RK4 when you have substantial joint damping. It cannot be reproduced with a control torque, because the correction is applied after the constraint forces have been computed, and the constraint forces depend on the control torque. 2. Modifying joint damping between step1 and step2 will lead to inconsistent computations. MuJoCo will not check if you did it, but it is better to avoid it. 3. Yes, if you want RK4 you should use a control callback... unless you are able to compute the control before mj_step? Assuming you cannot do that, multi-threading with callbacks works fine. The callback function is the same for all threads, but it is called with different mjModel and mjData pointers -- which you passed to mj_step in each thread. Normally one would use a single mjModel and per-thread mjData, but that is not a requirement. You can also have per-thread mjModel if you want. The only requirement is that two threads do not write to the same memory buffer -- which is guaranteed if each thread has separate mjData. First you should play with the model and make sure that RK4 is actually helping. My observation is that it helps with smooth dynamics but not so much with contact dynamics.