23 lines
497 B
Python
23 lines
497 B
Python
"""可信本地脚本示例：用 PD 控制让倒立摆保持竖直。"""
|
|
|
|
NAME = "倒立摆 PD 平衡控制"
|
|
CONTROL_HZ = 100
|
|
|
|
|
|
def init(api):
|
|
return {
|
|
"joint": api.joint("balance_hinge"),
|
|
"actuator": api.actuator("balance_motor"),
|
|
}
|
|
|
|
|
|
def step(ctx, state):
|
|
angle = ctx.qpos(state["joint"])
|
|
angular_velocity = ctx.qvel(state["joint"])
|
|
torque = -80.0 * angle - 12.0 * angular_velocity
|
|
ctx.set_control(state["actuator"], torque)
|
|
|
|
|
|
def reset(state):
|
|
pass
|