Подход к линеаризации нелинейной системы вокруг переменных решения (x, u) из MathematicalProgram в каждый дискретный момент времени - PullRequest
Новая
       56

Подход к линеаризации нелинейной системы вокруг переменных решения (x, u) из MathematicalProgram в каждый дискретный момент времени

2 голосов
/ 14 июля 2020

Это дополнительный вопрос по той же системе из следующего сообщения (для дополнительного контекста). Я описываю нелинейную систему как LeafSystem_ [T] (с использованием шаблонов) с двумя входными портами и одним выходным портом. Затем я, по сути, хотел бы выполнить прямую транскрипцию с помощью MathematicalProgram с дополнительной функцией стоимости, которая зависит от линеаризованной динамики на каждом временном шаге (и, следовательно, линеаризуется вокруг переменных решения). Я использую два входных порта, поскольку это казалось наиболее простым способом получения линеаризованной динамики формы из этой статьи на DIRTREL (если я могу взять якобиан в отношении входных портов)

δx i + 1 ≈ A i δx + B i δu + G i w

, где i - временной шаг, x - состояние, первый входной порт может инкапсулировать u, а второй входной порт может моделировать w, что может быть помехой, неопределенностью и т.д. c.

Мой главный вопрос заключается в том, какой набор методов был бы наиболее подходящим для получения линеаризованной динамики вокруг переменных решения на каждом временном шаге с использованием c автоматического дифференцирования? Мне рекомендовали попробовать автоматическое c дифференцирование после попытки использования символьного c подхода в предыдущем посте, но я не знаком с настройкой для этого. Я экспериментировал с

  1. , используя примитивы. Linearize () (вызывая его дважды, один раз для каждого входного порта), что кажется довольно неуклюжим, и я не уверен, можно ли принять решение переменные в контекст

  2. возможно преобразование моей системы в многотельную и использование multibody.tree.JacobianWrtVariable ()

  3. или форматирование моей системной динамики так что я могу передать их в качестве аргумента функции для forwarddiff.jacobian

, но добился ограниченного успеха.

Ответы [ 2 ]

3 голосов
/ 14 июля 2020

Самый простой способ получить Ai, Bi - создать экземпляр вашей системы с помощью AutoDiffXd, а именно LeafSystem<AutoDiffXd>. Следующий код даст вам Ai, Bi

MyLeafSystem<AutoDiffXd> my_system;
Eigen::VectorXd x_val = ...
Eigen::VectorXd u_val = ...
Eigen::VectorXd w_val = ...
// xuw_val concantenate x_val, u_val and w_val
Eigen::VectorXd xuw_val(x_val.rows() + u_val.rows() + w_val.rows());
xuw_val.head(x_val.rows()) = x_val;
xuw_val.segment(x_val.rows(), u_val.rows()) = u_val;
xuw_val.segment(w_val.rows()) = w_val;
// xuw_autodiff stores xuw_val in its value(), and an identity matrix in its gradient()
AutoDiffVecXd xuw_autodiff = math::initializeAutoDiff(xuw_val);

AutoDiffVecXd x_autodiff = xuw_autodiff.head(x_val.rows());
AutoDiffVecXd u_autodiff = xuw_autodiff.segment(x_val.rows(), u_val.rows());
AutoDiffVecXd w_autodiff = xuw_autodiff.tail(u_val.rows());

// I suppose you have a function x[n+1] = dynamics(system, x[n], u[n], w[n]). This dynamics function could be a wrapper of CalcUnrestrictedUpdate function.
AutoDiffVecXd x_next_autodiff = dynamics(my_system, x_autodiff, u_autodiff, w_autodiff);
Eigen::MatrixXd x_next_gradient = math::autoDiffToGradientMatrix(x_next_autodiff);
Eigen::MatrixXd Ai = x_next_gradient.block(0, 0, x_val.rows(), x_val.rows());
Eigen::MatrixXd Bi = x_next_gradient.block(0, x_val.rows(), x_val.rows(), u_val.rows());
Eigen::MatrixXd Gi = x_next_gradient.block(0, x_val.rows() + u_val.rows(), x_val.rows(), w_val.rows());

Таким образом, вы получите значение Ai, Bi, Gi в конце.

Если вам нужно написать функцию стоимости, вам нужно будет создать подкласс решателей :: Стоимость . Внутри функции Eval этого производного класса вы реализуете свой код, чтобы сначала вычислить Ai, Bi, Gi, а затем интегрировать уравнение Риккати.

Но я думаю, поскольку ваша функция стоимости зависит от Ai, Bi, Gi , градиент вашей функции затрат будет зависеть от градиента Ai, Bi, Gi. В настоящее время мы не предоставляем функцию для вычисления градиента второго порядка динамики.

Насколько сложна ваша динамическая система? Можно ли записать динамику вручную? Если да, то мы можем сделать несколько сокращений, чтобы сгенерировать градиент второго порядка вашей динамики.

@ sherm или другие специалисты по динамике Дрейка, было бы здорово узнать ваше мнение о том, как получить градиент второго порядка (при условии, что Фил сможет подтвердить, что ему действительно нужен градиент второго порядка.)

1 голос
/ 16 июля 2020

Извините за мой запоздалый ответ.

Поскольку ваша динамика может быть написана вручную, я бы предложил создать шаблонную функцию для вычисления Ai, Bi, Gi как 

template <typename T>
void ComputeLinearizedDynamics(
 const LeafSystem<T>& my_system,
 const Eigen::Ref<const drake::VectorX<T>>& x,
 const Eigen::Ref<const drake::VectorX<T>>& u,
 drake::MatrixX<T>* Ai,
 drake::MatrixX<T>* Bi,
 drake::MatrixX<T>* Gi) const;

. в этой функции необходимо вручную записать матрицу Ai, Bi, Gi. Затем, когда вы создаете экземпляр своего LeafSystem с помощью T=AutoDiffXd, эта функция вычислит Ai, Bi, Gi с его градиентом, учитывая состояние x, ввод u и возмущение w.

Затем в функции стоимости, вы можете рассмотреть возможность создания подкласса класса Cost как 

class MyCost {
 public:
  MyCost(const LeafSystem<AutoDiffXd>& my_system) : my_system_{&my_system} {}

 protected:
  void DoEval(const Eigen::Ref<const Eigen::VectorXd>& x_input, Eigen::VectorXd* y) const {
  // The computation here is inefficient, as we need to cast
  // x_input to Eigen vector of AutoDiffXd, and then call
  // DoEval with AutoDiffXd version, and then convert the 
  // result back to double. But it is easy to implement.
    const AutoDiffVecXd x_autodiff = math::initializeAutoDiff(x_input);
    AutoDiffVecXd y_autodiff;
    this->DoEval(x_autodiff, &y_autodiff);
    *y = math::autodiffToValueMatrix(y_autodiff);
  }

  void DoEval(const Eigen::Ref<const drake::AutoDiffVecXd>& x_input, drake::AutoDiffVecXd* y) const {
    // x_input here contains all the state and control sequence The authors need to first partition x_input into x, u
    drake::VectorX<T> x_all = x_input.head(num_x_ * nT_);
    drake::VectorX<T> u_all = x_input.tail(num_u_ * nT_);
    y->resize(1);
    y(0) = 0;
    // I assume S_final_ is stored in this class.
    Eigen::MatrixXd S = S_final_;
    for (int i = nT-1; i >= 0; --i) {
      drake::MatrixX<AutoDiffXd> Ai, Bi, Gi;
   
      ComputeLinearizedDynamics(
        *my_system_,
         x_all.segment(num_x_ * i, num_x_),
         u_all.segment(num_u_ * i, num_u_),
         &Ai, &B_i, &Gi);
      S = Ai.T*S + S*Ai + ... // This is the Riccati equation.
      // Now compute your cost with this S
      ...
    }
  }

  void DoEval(const Eigen::Ref<const VectorX<symbolic::Variable>& x, VectorX<symbolic::Expression>* y) const {
    // You don't need the symbolic version of the cost in nonlinear optimization.
    throw std::runtime_error("Not implemented yet");
  }
 private:
  LeafSystem<AutoDiffXd>* my_system_;
};

Функция DoEval с версией autodiff автоматически вычислит градиент стоимости. Затем вам нужно будет вызвать функцию AddCost в MathematicalProgram, чтобы добавить эту стоимость вместе со всеми x, u в качестве связанной переменной этой стоимости.

...