Update control and state interfaces for non PID actuators

[eholum]

When bypassing PID control we are writing to the state variables in mjData, rather than the control inputs, as noted in the documentation for the actuation model. We should be passing position and velocity commands to mjData.ctrl. Further, joint efforts are being read from the command input, whereas we should be reading from mjData.actuator_force.

This PR updates those values accordingly.

[sangteak601]

@eholum Actually, the idea behind position and velocity controllers is to bypass the physics and directly manipulate the states. That’s why we have a separate PID control interface.

If I understand correctly, this will write commands to the actuators, which will function as a PID controller internally. Additionally, actuator indices differ from position or velocity indices, so I don't think this code will work as intended.

[eholum]

Thanks @sangteak601! Just for background I was running into issues with the reported efforts from /joint_states being way off, and I do believe that that we should be reporting actuator_force instead of qfrc_applied, that got me digging a little more.

If I understand correctly, this will write commands to the actuators, which will function as a PID controller internally.

Yes that is exactly the goal. Was that the intention of this conditional? I think it's a great feature to allow users to be able to use Mujoco's PD controller directly (from digesting the model docs I don't think they support an i term), because it's handy to be able to move the MJCF around and not have to worry about tuning controllers in two different places. If that is something we want then I do think we want to write to the actuator's ctrl interface rather than its state. Which brings us to your next comment...

Additionally, actuator indices differ from position or velocity indices, so I don't think this code will work as intended.

Thank you so much for pointing that out! I get lost in Mujoco docs. This just happened to randomly match up in my model so it worked... I'm testing this against different models (and the panda) just to see if it does what I think it should do now.

[eholum]

@sangteak601 will leave this as an option for you. We'll probably use it because many of our robots have actuators tuned directly in the models using mujoco's Simulate app.

I think that the default behavior might be unintuitive to people as it essentially makes the default position controller an "unstoppable force", whereas I think it might be nice to let mujoco handle the joint level control if users don't specify a _pid interface. I added some basic docs to explain the two options and updated the demos accordingly. Thanks!