关于正确实现矢量输出端口LeafSystem的困惑

Question

我是一个学生，教自己德雷克，特别是比德雷克博士与罗斯泰德拉克博士的出色的欠驱动机器人课程。我试图写一个组合的能量整形和lqr控制器，以保持一个Cart极系统的平衡直立。我基于“欠驱动机器人http://underactuated.mit.edu/acrobot.html，”第三章中的图和第二章: http://underactuated.mit.edu/pend.html.中的SwingUpAndBalanceController图。

我发现，由于我使用了cart_pole.sdf model，我必须创建一个抽象的输入端口，以便从cart_pole.get_output_port(0)接收FramePoseVector。从这里我知道，我必须创建一个BasicVector类型的控制信号输出，以输入饱和块，然后再输入到Cart极的驱动端口。

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

TL;DR:在扩展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

这里有两件可能有帮助的事情：

1. 如果您想从MultibodyPlant获取购物车杆的状态，您可能希望连接到continuous_state输出端口，这将给出一个法线向量，而不是抽象类型的FramePoseVector。在这种情况下，您对get_input_port().Eval(context)的调用应该工作得很好。
2. 如果您确实希望读取FramePoseVector，则必须对输入端口进行稍微不同的计算。您可以找到这个这里的一个例子。