修复 Python 仿真控制更新时序：按仿真步重算关节控制量

[IsaacZH]

问题

在 Python 仿真中，关节控制量此前仅在接收到新 LowCmd 回调时计算一次。
当控制端发送固定目标值时，机器人状态（关节位置/速度）仍在持续变化，但控制量未在每个仿真步同步更新，导致控制输出与当前状态不一致。

解决方案

将控制流程拆分为“命令缓存 + 按步计算”两阶段：

1. 回调函数仅缓存最新 LowCmd，不直接依赖回调时机写最终控制量
2. 在每个仿真步（mj_step 前）基于当前状态重算控制量并写入控制数组

[Copilot]

Pull request overview

This PR changes the Unitree MuJoCo simulation control path so motor torques are recomputed every physics step using the most recent received LowCmd, avoiding stale torques when command callbacks arrive slower than the simulation loop.

Changes:

• Cache the latest LowCmd message in the subscriber callback instead of directly writing mj_data.ctrl.
• Add UpdateControl() to recompute PD torques every simulation step from cached LowCmd.
• Call unitree.UpdateControl() each loop iteration before mujoco.mj_step().

Reviewed changes

Copilot reviewed 2 out of 2 changed files in this pull request and generated 2 comments.

File	Description
simulate_python/unitree_sdk2py_bridge.py	Adds low_cmd_latest caching and per-step UpdateControl() PD torque computation.
simulate_python/unitree_mujoco.py	Invokes UpdateControl() each physics tick before stepping MuJoCo.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

UpdateControl() assumes the incoming LowCmd message has at least self.num_motor entries in msg.motor_cmd. If a mismatched/partial command arrives (e.g., different robot IDL, bad publisher), this will raise IndexError in the simulation thread and can halt the sim. Add a guard (validate message length / expected motor count and skip or clamp the loop) so malformed commands can’t crash the simulation loop.

Suggested change

	msg = self.low_cmd_latest
	for i in range(self.num_motor):
	self.mj_data.ctrl[i] = (
	msg.motor_cmd[i].tau
	+ msg.motor_cmd[i].kp
	* (msg.motor_cmd[i].q - self.mj_data.sensordata[i])
	+ msg.motor_cmd[i].kd
	* (
	msg.motor_cmd[i].dq
	msg = self.low_cmd_latest
	motor_cmd = getattr(msg, "motor_cmd", None)
	if motor_cmd is None or len(motor_cmd) < self.num_motor:
	return
	for i in range(self.num_motor):
	self.mj_data.ctrl[i] = (
	motor_cmd[i].tau
	+ motor_cmd[i].kp
	* (motor_cmd[i].q - self.mj_data.sensordata[i])
	+ motor_cmd[i].kd
	* (
	motor_cmd[i].dq

[Copilot]

The simulation loop uses manual locker.acquire()/release() without a try/finally (or context manager). With UpdateControl() now in the critical section, any unexpected exception (e.g., from control update or mj_step) will leave the lock held and freeze the viewer/sim threads. Consider switching to with locker: (or wrap in try/finally) to guarantee release.