Evaluate the joint PD inside mj_step via MuJoCo actuators

[T-K-233]

Problem

MujocoSystem::write() computed the MIT PD itself and injected it as qfrc_applied:

const double cmd = stiffness_command * (position_command - position)
                 + damping_command  * (velocity_command - velocity) + effort_command;
mj_data_->qfrc_applied[joint_state.mj_vel_adr] = clamp(cmd, -max_effort, max_effort);

Two consequences:

1. The CM rate became the PD rate in sim. On real hardware the RobStride firmware closes its own ~1 kHz loop, so the host rate only sets how often the setpoint is refreshed. In sim the PD was resampled once per controller_manager tick, so lowering the CM rate destabilised joints that are perfectly stable on the robot.
2. MuJoCo's actuator machinery was bypassed entirely — no ctrl, no gainprm, and critically no implicit integration of the damping term, which is what keeps a stiff PD stable at a coarse timestep.

Change

Each joint's actuator is converted into a position servo at init — biastype=affine, since a plain MJCF <motor> is gaintype=fixed/gainprm=1 with no bias, and biasprm=(0,−kp,−kv) makes it behave exactly like <position kp kv>. write() then updates gainprm/biasprm from the stiffness/damping command interfaces and ctrl from the position command, and MuJoCo re-evaluates the feedback every physics step.

Only the terms that are constant over a tick remain a generalized applied force — the velocity target and torque feedforward, both zero on the RL and standby paths. Force clamping now comes from the MJCF's own forcerange, applied inside mj_step. read() reports qfrc_actuator + qfrc_applied so effort still accounts for both paths.

This is the architecture gz_ros2_control already uses: write() deposits a setpoint and the simulator's own PID runs in PreUpdate, every physics iteration. It is also what mjlab trains against, so it closes a sim/training gap as well as a sim/real one.

Measured

Lite arms sim, worst-joint peak-to-peak after standby settles:

	p2p	behaviour
200 Hz, PD in mj_step	0.00065 rad	quiescent
200 Hz, PD in write()	0.0371 rad	25–37 Hz wrist chatter
400 Hz, PD in write()	0.0024 rad	quiescent

Not only chatter-free at the lower rate but 3.6× quieter than the previous best arrangement — and sim and real can now share one CM rate.

Compatibility

A joint whose MJCF declares no actuator falls back to the previous qfrc_applied behaviour with a warning, so models without actuators keep working unchanged.

Depends on MJCFs whose actuators are position servos: Berkeley-Humanoids/Lite-Description#13. Companion CM-rate change: Berkeley-Humanoids/humanoid_control_ros2#24.

🤖 Generated with Claude Code

https://claude.ai/code/session_012QKJQ9hze1hMWXJpdPcjp3