pydrake实现带向量输出端口的LeafSystem并获取连续状态的疑问

问题根因 #

你自定义的SwingUpAndBalanceController是无状态的LeafSystem，本身没有声明任何连续状态变量，所以你从控制器自身的context中读取连续状态向量自然会得到空值/NaN。你需要的车杆系统实时状态，应该从你声明的state_input抽象输入端口读取，而不是控制器自身的context。

正确实现步骤 #

1. 在输出端口回调中读取输入端口的实时状态
   你在__init__中已经声明了接收FramePoseVector的抽象输入端口，在回调中直接调用该端口的Eval方法即可拿到实时的系统状态数据，不需要操作控制器自身的context状态。
2. （可选优化）如果你的cart_pole是Drake的MultibodyPlant对象，不需要走FramePoseVector端口解析，可以直接对接其状态输出端口：将输入端口声明为接收BasicVector类型，直接连接cart_pole.get_state_output_port()，省去解析姿态的步骤。

修正后的核心代码片段 #

def OutputControlSignal(self, context, output):
    # 从输入端口读取实时的车杆状态，不要读控制器自身的context状态
    # 推荐直接对接全状态端口的写法：
    # __init__中先修改输入端口声明为 self.DeclareVectorInputPort("state_input", BasicVector(4))
    # 构建仿真图时用 builder.Connect(cart_pole.get_state_output_port(), controller.get_input_port(0))
    xbar = self.get_input_port(0).Eval(context).get_value()
    xbar[1] = wrap_to(xbar[1], 0, 2.0*np.pi) - np.pi

    # 后续控制逻辑不变
    if (xbar.dot(self.S.dot(xbar)) < 2.0):
        output.set_value(-self.K.dot(xbar))
    else:
        self.energy_shaping.get_input_port(0).FixValue(self.energy_shaping_context, xbar)
        output_val = self.energy_shaping.get_output_port(0).Eval(self.energy_shaping_context)
        output.set_value(output_val)

额外说明 #

你之前代码里固定用初始化传入的cart_pole_context是错误的，这个context保存的是初始化时刻的车杆状态，不会随仿真推进更新，所以只有固定输出的测试场景能用，实际控制必须用输入端口传入的实时状态。

内容的提问来源于stack exchange，提问作者Roy Xing