Mujoco environment is reset while using position command interface

[sangteak601]

Experienced resetting issue with position control. It seems that directly manipulating position is not stable and causes the issue but not sure and need to investigate this further

[dyackzan]

I was able to reproduce and spent some time investigating. It seems like there is some sort of a race condition we're encountering when we attempt to change the mj_data_->qpos array directly that causes the position values to explode sometimes so I don't think it's really supported. I also recorded the joint_state.position values that come across from the command interface and verified that they are reasonable so it doesn't appear that the issue is from your code commanding any bad values here.

But I think it does make sense why MuJoCo wouldn't support directly setting the joint positions: Setting the position value of a joint directly ignores physics and negates the whole purpose of MuJoCo. If we want to simulate a trajectory without physics then we don't need to incorporate a physics simulator.

Looking at the MuJoCo Simulation loop docs, it looks like in between mj_step1() and mj_step2() the only things we should be setting directly in the mjData struct are ctrl, qfrc_applied, or xfrc_applied.

So I think a good solution here is probably just to remove this direct position control functionality and replace it with the PID position control method you've implemented.

[dyackzan]

I'll paste some details here from my investigation:

You can see here that values coming across the position command interface from panda_joint1 are reasonable ~[-.002, 0.055]:

But when I include qpos on the plot, you can see the values shoot off at some point beyond the limits of the actuator ~[-8.25, 6.25]:

I'll also attach the mcap file of the recorded data if you want to take a look. You can unzip it and drag and drop it into Plotjuggler if you want to view. (You can install the version of plotjuggler that supports mcaps from the snap store snap install plotjuggler)
mujoco_system_debugging.zip

[sangteak601]

@dyackzan Thanks for the investigation. It looks insightful. I will also take another look with the data you shared.