正确实现带有向量输出端口的LeafSystem的困惑

Question

我是一名自学 Drake 的学生，特别是 pydrake 与 Russ Tedrake 博士的优秀欠驱动机器人课程。我正在尝试编写一个组合的能量整形和 lqr 控制器来保持 cartpole 系统平衡直立。我的图表基于 Underactuated Robotics [http://underactuated.mit.edu/acrobot.html] 第 3 章中的 cartpole 示例，以及第 2 章中的 SwingUpAndBalanceController：[http:/ /underactuated.mit.edu/pend.html].

我发现由于使用 cart_pole.sdf model，我必须创建一个抽象输入端口，以便从 cart_pole.get_output_port(0) 接收 FramePoseVector。从那里我知道我必须创建 BasicVector 类型的控制信号输出以在馈入 cartpole 的驱动端口之前馈入饱和块。

我现在遇到的问题是，我不知道如何在DeclareVectorOutputPort的回调函数中获取系统的当前状态数据。我假设我会在回调函数 OutputControlSignal 中使用 LeafContext 参数，获取 BasicVector 连续状态向量。然而，这个结果向量 x_bar 总是 NaN。出于绝望（并测试以确保我的程序的其余部分正常工作），我将 x_bar 设置为控制器的初始化 cart_pole_context 并发现模拟运行时控制信号为 0.0（如预期）。我还可以将 output 设置为 100，然后 cartpole 模拟就会飞向无穷无尽的 space（正如预期的那样）。

TL;DR：在使用 DeclareVectorOutputPort 扩展 LeafSystem 的自定义控制器中获取连续状态向量的正确方法是什么？

感谢您的帮助！真的很感激:)我一直在自学所以有点辛苦哈哈。

# Combined Energy Shaping (SwingUp) and LQR (Balance) Controller
# with a simple state machine
class SwingUpAndBalanceController(LeafSystem):

    def __init__(self, cart_pole, cart_pole_context, input_i, ouput_i, Q, R, x_star):
        LeafSystem.__init__(self)
        self.DeclareAbstractInputPort("state_input", AbstractValue.Make(FramePoseVector()))
        self.DeclareVectorOutputPort("control_signal", BasicVector(1),
                                     self.OutputControlSignal)
        (self.K, self.S) = BalancingLQRCtrlr(cart_pole, cart_pole_context,
                                             input_i, ouput_i, Q, R, x_star).get_LQR_matrices()
        (self.A, self.B, self.C, self.D) = BalancingLQRCtrlr(cart_pole, cart_pole_context,
                                                             input_i, ouput_i,
                                                             Q, R, x_star).get_lin_matrices()
        self.energy_shaping = EnergyShapingCtrlr(cart_pole, x_star)
        self.energy_shaping_context = self.energy_shaping.CreateDefaultContext()
        self.cart_pole_context = cart_pole_context

    def OutputControlSignal(self, context, output):
        #xbar = copy(self.cart_pole_context.get_continuous_state_vector())
        xbar = copy(context.get_continuous_state_vector())
        xbar_ = np.array([xbar[0], xbar[1], xbar[2], xbar[3]])
        xbar_[1] = wrap_to(xbar_[1], 0, 2.0*np.pi) - np.pi

        # If x'Sx <= 2, then use LQR ctrlr. Cost-to-go J_star = x^T * S * x
        threshold = np.array([2.0])
        if (xbar_.dot(self.S.dot(xbar_)) < 2.0):
            #output[:] = -self.K.dot(xbar_) # u = -Kx
            output.set_value(-self.K.dot(xbar_))
        else:
            self.energy_shaping.get_input_port(0).FixValue(self.energy_shaping_context,
                                                          self.cart_pole_context.get_continuous_state_vector())
            output_val = self.energy_shaping.get_output_port(0).Eval(self.energy_shaping_context)
            output.set_value(output_val)

        print(output)

Answer 1

以下两点可能会有所帮助：

如果您想从 MultibodyPlant 获取车杆的状态，您可能想要连接到 continuous_state 输出端口，它给你一个法向量而不是抽象类型 FramePoseVector。在这种情况下，您对 get_input_port().Eval(context) 的调用应该可以正常工作。
如果你做真的想读FramePoseVector，那么你必须稍微不同地评估输入端口。您可以找到 here.

正确实现带有向量输出端口的LeafSystem的困惑

Confusion About Implementing LeafSystem With Vector Output Port Correctly

python-3.x

control-theory

drake