Inverse Dynamics - Unable to calculate ctrl from qfrc

[SiddheshKanawade]

I am learning how to compute ctrl from qpos for the myo-arm model using the inverse dynamics tutorial. I get qpos from predefined trajectory. Following is the myo-arm model configuration(just for reference):

Configuration:

• model.nu = 63 # Number of actuators
• model.nv = 38 # Number of DOFs
• model.nq = 38 # Number of generalized coordinates
• model.na = 63 # Number of activation states
• steps = 300

I am stuck at obtaining ctrl after computing qfrc using inverse dynamics. I believe qfrc calculation is correct, since I was able to replicate the similar trajectory using the generated qfrc_inverse(by updating qfrc_applied with qfrc_inverse before calling mj_step). I think the issue might be related to how I’m approaching the computation of ctrl from qpos(I am using same approach as mentioned in tutorial) for the myo-arm model.

In a LQR tutorial with the humanoid model, they used the formula to get ctrl from qfrc

ctrl0 = np.atleast_2d(qfrc0) @ np.linalg.pinv(actuator_moment)

Thus, I was wondering if the approach to calculate ctrl from qfrc varies model to model(I am only considering musculoskeletal models). I have also gone through issue #263, which discusses calculating acceleration when nq ≠ nv, but I’m still stuck

Also, I have recently started working with biomechanics, so I am still learning the nuances of musculoskeletal models. Any help in figuring out this issue would be greatly appreciated. Thanks!

[andreh1111]

hey @SiddheshKanawade, the main difference between the LQR tutorial from MuJoCo and the ID tutorial of MyoSuite is that in the former you only have motor actuators, while here we have muscle actuators. You cannot handle the muscle actuators using the code line you posted. Think about the muscle dynamics for example (see here): you need to do the inverse of the whole process if you want to estimate the controls required to achieve the required torques. The tutorial from myosuite that you mentioned explains exactly a strategy to do this inversion in an efficient way. Let me know if something is not clear there :)

[SiddheshKanawade]

Thanks for the response.

The tutorial is explanatory and quite clear about how to approach the problem of getting control from torque (qpos). I tried running the same MyoHand model with a different trajectory, and I noticed comparatively higher error in that case. On debugging, I found that the sigmoid approximation function we are using ( y = 1.875x − 0.4375 ) works well when x is close to 0.5. In my case, the trajectory range is quite large (0.3–0.7), and I believe that’s why I’m seeing higher error between qpos_reference and qpos_eval (please let me know if I’m going in the wrong direction here).

I’m not sure how to fix this. Current attempts involve tinkering with the QP so that the range of x — in our case, ((ctrl − act)/tausmooth + 0.5) — stays closer to 0.5. Do let me know if there’s any other approach to address this problem.

What I’ve already tried:

• Changing the value of tausmooth to scale down (ctrl − act)/tausmooth. By increasing tausmooth, I see the error between qpos_ref and qpos_eval decrease initially, but beyond a certain point, (ctrl − act)/tausmooth becomes insignificant.
• Adding Tikhonov regularization to avoid near-singularities in P (in QP).
• Trying a smaller model (1-DOF elbow). The results are better here, but the error is still larger than what’s shown in the tutorial.

[andreh1111]

I tested the tutorial with multiple trajectories in the past and I never faced this specific issue. Maybe you wanna share your trajectory to allow me to replicate the issue? The first guess I have in mind is that maybe your trajectory's timestep is too big. Consider that with this tutorial, the timestep has to be the same of the model, i.e., 0.002s, so an interpolation of the target qpos is most probably required.

[SiddheshKanawade]

Apologies for the delayed reply.

I have replicated the notebook I’m working on, which you can find here. For simplicity, I generated a manual trajectory using elbow/myoelbow_2dof6muscles.xml. The trajectory is quite simple: r_elbow_flex moves from 0 to 90 degrees over 300 steps. You can find the trajectory here.

What I’ve noticed is a problem resembling oscillations commonly seen in control systems. Please let me know if I’m going wrong somewhere. Let me know if you need anything else. Thanks for your help.

[SiddheshKanawade]

@andreh1111 were you able to replicate the issue?

[andreh1111]

hey @SiddheshKanawade, I apologize, I haven't tried it yet. I'll test it asap, most probably over the weekend, sorry for the delay

[andreh1111]

in the meantime @SiddheshKanawade if you are hurry, I suggest you to try what I suggested in this thread in the past: #274 (comment)
I'm talking about the qacc being set before calling mj_inverse like: $qacc = (qpos_{target} - qpos) * kp - qvel * kv$

[SiddheshKanawade]

Thanks. Will take a look.

[andreh1111]

Hey @SiddheshKanawade, I just tried your trajectory and it works. I used the tutorial's code and set qacc as mentioned in my previous message, with kp=1000 and kv=100 (there could be better values; I quickly tried these because they work most of the time for these models), and removed the qfrc_scaler. Let me know.