Блог

Я студент, обучающий себя Дрейку, в частности Пайдрейку, с помощью превосходного курса робототехники доктора Расса Тедрейка, недостаточно разработанного. Я пытаюсь написать комбинированный контроллер формирования энергии и lqr для поддержания вертикального равновесия системы картпола. Я основал диаграмму на примере картополя, приведенном в главе 3 «Недостаточно развитая робототехника» [http://underactuated.mit.edu/acrobot.html], и контролер SwingUpAndBalanceController в главе 2: [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, и симуляция картпола просто улетит в бесконечное пространство (как и ожидалось).

TL;DR: каков правильный способ получения вектора непрерывного состояния в пользовательском контроллере, расширяющемся LeafSystem с помощью a 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)
 

Вот две вещи, которые могут помочь:

1. Если вы хотите узнать состояние столба тележки MultibodyPlant , вы, вероятно, захотите подключиться к continuous_state выходному порту, который дает вам нормальный вектор вместо абстрактного типа FramePoseVector . В этом случае ваш звонок get_input_port().Eval(context) должен сработать просто отлично.
2. Если вы действительно хотите прочитать FramePoseVector , то вам нужно немного по-другому оценить входной порт. Пример этого вы можете найти здесь.