Drake 从数学程序在每个离散时间点线性化决策变量（x，u）周围的非线性系统的方法

这是一个关于同一系统的后续问题，来自以下内容（用于其他上下文）。我将非线性系统描述为具有两个输入端口和一个输出端口的LeafSystem_ut（使用模板）。然后，我基本上希望使用MathematicalProgram执行直接转录，并使用额外的成本函数，该函数依赖于每个时间步的线性化动力学（因此，围绕决策变量进行线性化）。我使用两个输入端口，因为这似乎是获得形式的线性化动力学的最直接的方法（如果我可以采用关于输入端口的雅可比矩阵）

δxi+1≈ Aiδx+Biδu+Giw

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

我的主要问题是，使用自动微分法在每个时间步获得决策变量周围的线性化动力学，最合适的方法是什么？在上一篇文章中，我尝试了一种符号化的方法之后，有人建议我尝试自动区分，但我不熟悉这样做的设置。我已经试过了

使用primitives.Linearize（）（调用两次，每个输入端口调用一次），感觉相当笨拙，我不确定是否可以将决策变量传递到上下文中

也许可以将我的系统转换成一个多体，并利用multibody.tree.JacobianWrtVariable（）

或者格式化系统动力学，以便将它们作为forwarddiff.jacobian的函数参数传入

---

但是成功有限。

获得

Ai，Bi

的最简单方法是使用AutoDiffXd实例化您的系统，即

LeafSystem。在这个派生类的
Eval
函数中，您将实现代码，首先计算Ai、Bi、Gi，然后积分Riccati方程

但我认为，由于成本函数依赖于
Ai，Bi，Gi
，因此成本函数的梯度将依赖于
Ai，Bi，Gi
的梯度。目前我们没有提供计算动力学二阶梯度的函数

你的动力系统有多复杂？有没有可能用手记下动态？如果是这样，我们可以通过一些捷径来生成动力学的二阶梯度

@sherm或其他Drake dynamics的人，如果能得到你关于如何获得二阶梯度的意见（假设Phil能够确认他确实需要二阶梯度），那就太好了。
对不起，我的回复太晚了

由于您的动态可以手工编写，因此我建议创建一个模板函数来计算
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;

带有autodiff版本的
DoEval
函数将自动为您计算成本梯度。然后，您需要调用MathematicalProgramm中的
AddCost
函数，将该成本与所有
x，u
相加，作为该成本的关联变量。
为了让我澄清，这里的二阶梯度是超矩阵∂A.ᵢ∂x、 u，w？我猜这将采取矩阵列表的形式∂A.ᵢ/∂xⱼ ∀j等等。？我很难想象它是如何在实践中使用的。我目前正在使用简单的系统，我可以手工编写动力学，所以最好的方法可能是手工获取Ai、Bi、Gi？此外，我在pydrake文档中看到了成本类，但是，我可以用python中相同的方式使用DoEval函数创建子类吗？在python中，创建自定义开销的最简单方法如中所示，对于LeafSystem中的事件处理，我建议在单独的stackoverflow问题中问这个问题。
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;

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