1

私は、Dr. Russ Tedrake の優秀な Underactuated Robotics コースで Drake、特に pydrake を独学で学んでいる学生です。カートポール システムのバランスを直立に保つために、エネルギー シェーピングと lqr コントローラーを組み合わせて作成しようとしています。この図は、Underactuated Robotics [http://underactuated.mit.edu/acrobot.html] の第 3 章にあるカートポールの例と、第 2 章の SwingUpAndBalanceController: [http://underactuated.mit.edu/pend] に基づいています。 .html]。

を使用しているため、からcart_pole.sdf model受信するための抽象入力ポートを作成する必要があることがわかりました。そこから、カートポールの作動ポートに供給する前に、タイプ BasicVector の制御信号出力を作成して Saturation ブロックに供給する必要があることがわかりました。FramePoseVectorcart_pole.get_output_port(0)

私が今直面している問題は、DeclareVectorOutputPortのコールバック関数でシステムの現在の状態データを取得する方法がわからないことです。LeafContextコールバック関数でパラメータを使用して、連続状態ベクトルOutputControlSignalを取得すると仮定していました。BasicVectorただし、この結果のベクトルx_barは常にNaNです。やむを得ず (およびプログラムの残りの部分が機能することを確認するためのテスト) x_bar、コントローラーの初期化を設定したcart_pole_contextところ、シミュレーションが 0.0 の制御信号で実行されることがわかりました (予想どおり)。100 に設定outputすることもできます。カートポール シミュレーションは無限の空間に飛んでいきます (予想どおり)。

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