"""可信本地脚本示例：用 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