在每个离散时间点从 MathematicalProgram 围绕决策变量 (x, u) 线性化非线性系统的方法

Question

这是来自以下 (for additional context). I describe a nonlinear system as a LeafSystem_[T] (using templates) with two input ports and one output port. Then, I essentially would like to perform direct transcription using MathematicalProgram with an additional cost function that is dependent on the linearized dynamics at each time step (and hence linearized around the decision variables). I use two input ports as it seemed the most straightforward way for obtaining the linearized dynamics of the form from this paper on DIRTREL 的同一系统的后续问题（如果我可以采用关于输入端口的雅可比行列式）

δx_i+1≈A_iδx + B_iδu + G_iw

其中i是时间步长，x是状态，第一个输入端口可以封装u，第二个输入端口可以对w进行建模，w可能是干扰，不确定性等

我的主要问题是，使用自动微分在每个时间步获得围绕决策变量的线性化动态的最合适的方法集是什么？在之前 post 中尝试符号方法后，有人建议我尝试自动微分，但我不熟悉这样做的设置。我已经尝试过

使用 primitives.Linearize() （调用它两次，每个输入端口调用一次）感觉很笨拙，我不确定是否可以将决策变量传递到上下文中
也许将我的系统转换为多体并利用 multibody.tree.JacobianWrtVariable()
或格式化我的系统动态，以便我可以将它们作为函数参数传递给 forwarddiff.jacobian

但取得的成功有限。

Answer 1

获得 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的值

如果需要编写成本函数，则需要创建 solvers::Cost 的子class。在这个派生的 class 的 Eval 函数中，您将实现代码以首先计算 Ai、Bi、Gi，然后对 Riccati 方程求积分。

但我认为既然你的代价函数取决于Ai, Bi, Gi，你的代价函数的梯度将取决于Ai, Bi, Gi的梯度。目前我们没有提供计算动力学二阶梯度的函数。

你的动力系统有多复杂？是否可以手写动态？如果是这样，我们可以使用一些快捷方式来生成动力学的二阶梯度。

@sherm 或其他 Drake 动力学人员，如果能就如何获得二阶梯度征求你的意见会很棒（假设 Phil 可以确认他确实需要二阶梯度。）

Answer 2

抱歉我迟来的回复。

既然你的动态可以手写，那么我建议创建一个模板函数来计算 Ai, Bi, Gi as

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。然后，当您使用 T=AutoDiffXd 实例化 LeafSystem 时，此函数将计算 Ai, Bi, Gi 及其梯度，给定状态 x、输入 u 和扰动 [=18] =].

然后在成本函数中，你可以考虑创建一个子class of Cost class as

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_;
};

autodiff版本的DoEval函数会自动为你计算成本的梯度。然后您需要调用 MathematicalProgram 中的 AddCost 函数将此成本与所有 x, u 相加作为此成本的关联变量。

在每个离散时间点从 MathematicalProgram 围绕决策变量 (x, u) 线性化非线性系统的方法

Approach for Linearizing Nonlinear System around Decision Variables (x, u) from MathematicalProgram at each discrete point in time

drake