相对于各个关节角度的雅可比导数的 Autodiff

Question

我正在尝试在 drake C++ 中为操纵器计算 $\partial{J}{q_i}$，根据我的搜索，最好的方法似乎是使用 autodiff 函数。我无法从我找到的资源中完全理解 autodiff 方法，所以如果我的方法不够清楚，我深表歉意。我已经从论坛上提到的一些已经提出的关于自动差异的问题以及 https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html 作为参考使用了我的理解。

因为我想计算 $\partial{J}{q_i}$，return 类型将是张量，即 3 * 7 * 7（或 6 * 7 * 7，具体取决于在空间雅可比矩阵上）。我可以考虑使用 std::vectorEigen::MatrixXd 来分配输出，或者一次只做一个 $q_i$ 并为自动差异计算相应的雅可比。无论哪种情况，我都在努力在初始化 jacobian 函数时传递它。我做了以下初始化 autodiff

std::unique_ptr<multibody::MultibodyPlant<AutoDiffXd>> mplant_autodiff = systems::System<double>::ToAutoDiffXd(mplant);
std::unique_ptr<systems::Context<AutoDiffXd>> mContext_autodiff = mplant_autodiff->CreateDefaultContext();

mContext_autodiff->SetTimeStateAndParametersFrom(*mContext);
const multibody::Frame<AutoDiffXd>* mFrame_EE_autodiff = &mplant_autodiff->GetBodyByName(mEE_link).body_frame();
const multibody::Frame<AutoDiffXd>* mWorld_Frame_autodiff = &(mplant_autodiff->world_frame());

//Initialize the q as autodiff vector
drake::AutoDiffVecXd q_autodiff = drake::math::InitializeAutoDiff(mq_robot);
MatrixX<AutoDiffXd> mJacobian_autodiff; // Linear Jacobian matrix.

mplant_autodiff->SetPositions(context_autodiff.get(), q_autodiff);

mplant_autodiff->CalcJacobianTranslationalVelocity(*mContext_autodiff,
                                      multibody::JacobianWrtVariable::kQDot,
                                      *mFrame_EE_autodiff,
                                      Eigen::Vector3d::Zero(),
                                      *mWorld_Frame_autodiff,
                                      *mWorld_Frame_autodiff,
                                      &mJacobian_autodiff 
                                      );

然而，据我了解，InitializeAutoDiff 初始化为单位矩阵，而我想要 $\partial{J}{q_i}$，所以有没有更好的方法来做到这一点。此外，当我尝试调用雅可比矩阵时会收到错误消息。有没有办法为每个 q_i 的 $\partial{J}{q_i}$ 和在 for 循环中更改 q_i 或直接在张量中获取结果来解决这个问题.如果我做的事情完全与正确的方法相悖，我深表歉意。谢谢你的期待。

Answer 1

However, as far as I understand, InitializeAutoDiff initializes to the identity matrix, whereas I want to $\partial{J}{q_i}$, so is there is a better way to do it

没错。当您调用 InitializeAutoDiff 并计算 mJacobian_autodiff 时，您会得到一个 AutoDiffXd 矩阵。每个 AutoDiffXd 都有一个存储双精度值的 value() 函数，以及一个将梯度存储为 Eigen::VectorXd 的 derivatives() 函数。我们有

mJacobian(i, j).value() = J(i, j)
mJacobian_autodiff(i, j).derivatives()(k) = ∂J(i, j)/∂q(k)

所以如果你想创建一个std::vecot<Eigen::MatrixXd>使得这个向量的第k个条目存储矩阵∂J/∂q(k)，那么这里有一个代码

std::vector<Eigen::MatrixXd> dJdq(q_autodiff.rows());
for (int i = 0; i < q_autodiff.rows(); ++i) {
  dJdq[i].resize(mJacobian_autodiff.rows(), mJacobian_autodiff.cols());
}
for (int i = 0; i < q_autodiff.rows(); ++i) {
  // dJidq stores the gradient of the ∂J.col(i)/∂q, namely dJidq(j, k) = ∂J(j, i)/∂q(k)
  auto dJidq = ExtractGradient(mJacobian_autodiff.col(i));
  for (int j = 0; j < static_cast<int>(dJdq.size()); ++j) {
    dJdq[j].col(i) = dJidq.col(j);
  }
}

计算单个 i 的 ∂J/∂q(i)

如果您不想为所有 i 计算 ∂J/∂q(i)，而只想为一个特定的 i 计算，您可以将 q_autodiff 的初始化从 InitializeAutoDiff 更改为

AutoDiffVecXd q_autodiff(q.rows());
for (int k = 0; k < q_autodiff.rows(); ++k) {
  q_autodiff(k).value() = q(k)
  q_autodiff(k).derivatives() = Vector1d::Zero();
  if (k == i) {
    q_autodiff(k).derivatives()(0) = 1;
  }
}

即q_autodiff存储梯度∂q/∂q(i)，对于所有k!=i为0，当k==i时为1。然后您可以使用当前代码计算 mJacobian_autodiff 。现在 mJacobian_autodiff(m, n).derivatives() 存储特定 i 的 ∂J(m, m)/∂q(i) 的梯度。您可以将此渐变提取为

Eigen::Matrix dJdqi(mJacobian_autodiff.rows(), mJacobian_autodiff.cols());
for (int m = 0; m < dJdqi.rows(); ++m) {
  for (int n = 0; n < dJdqi.cols(); ++n) {
    dJdqi(m, n) = mJacobian_autodiff(m, n).derivatives()(0);
  }
}

相对于各个关节角度的雅可比导数的 Autodiff

Autodiff for Jacobian derivative with respect to individual joint angles

drake