Skip to content

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

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
IsaacZH wants to merge 1 commit into
base: main
Choose a base branch
from
Open

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions simulate_python/unitree_mujoco.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,9 @@ def SimulationThread():

locker.acquire()

# Update control torques from latest command
unitree.UpdateControl()

Comment on lines 52 to +56

Copilot AI Apr 24, 2026

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copilot uses AI. Check for mistakes.
if config.ENABLE_ELASTIC_BAND:
if elastic_band.enable:
mj_data.xfrc_applied[band_attached_link, :3] = elastic_band.Advance(
Expand Down
37 changes: 26 additions & 11 deletions simulate_python/unitree_sdk2py_bridge.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,9 @@ def __init__(self, mj_model, mj_data):
self.low_cmd_suber = ChannelSubscriber(TOPIC_LOWCMD, LowCmd_)
self.low_cmd_suber.Init(self.LowCmdHandler, 10)

# Cache latest motor commands (updated by LowCmdHandler)
self.low_cmd_latest = None

# joystick
self.key_map = {
"R1": 0,
Expand All @@ -109,18 +112,30 @@ def __init__(self, mj_model, mj_data):
}

def LowCmdHandler(self, msg: LowCmd_):
if self.mj_data != None:
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
- self.mj_data.sensordata[i + self.num_motor]
)
"""Cache the latest low command. Control update happens in UpdateControl()."""
self.low_cmd_latest = msg

def UpdateControl(self):
"""
Recompute motor control torques on every simulation step.
This is called from the simulation thread before mj_step().
τ = tau_cmd + kp*(q_des-q_act) + kd*(dq_des-dq_act)
"""
if self.mj_data is None or self.low_cmd_latest is None:
return

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
Comment on lines +126 to +135

Copilot AI Apr 24, 2026

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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 uses AI. Check for mistakes.
- self.mj_data.sensordata[i + self.num_motor]
)
)

def PublishLowState(self):
if self.mj_data != None:
Expand Down
Loading