1

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

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

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

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

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

# 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)
4

1 回答 1

0

这里有两件事可能会有所帮助：

  1. 如果您想从 获取cart-pole 的状态MultibodyPlant，您可能希望连接到continuous_state输出端口，这将为您提供法线向量而不是 abstract-type FramePoseVector。在这种情况下，您的呼叫get_input_port().Eval(context)应该可以正常工作。
  2. 如果您确实想阅读FramePoseVector，那么您必须稍微不同地评估输入端口。你可以在这里找到一个例子。
于 2021-08-29T09:02:45.857 回答