Jacobian导数相对于单个关节角的自差

Question

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

由于我想计算$\partial{J}{q_i}$，返回类型将是张量，即3*7*7(或6*7*7取决于空间jacobian)。我可以考虑使用std::vectorEigen::MatrixXd分配输出，或者一次只执行一个$q_i$，并为自动差异计算各自的jacobian。在这两种情况下，我都很难在初始化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}$，所以有更好的方法来实现它。此外，当我试图调用jacobian矩阵时，我会得到错误消息。对于每个$\partial{J}{ q_i }$和在for循环中更改q_i，还是直接在张量中得到结果，都有解决这个问题的方法吗？如果我所做的事情与正确的方法完全相切，我很抱歉。我期待着感谢你。

Answer 1

然而，据我所知，

初始化为恒等矩阵，而我想要$\partial{J}{q_i}$，所以有更好的方法来实现它

这是正确的。当您调用InitializeAutoDiff并计算mJacobian_autodiff时，您将得到一个AutoDiffXd矩阵。每个AutoDiffXd都有一个存储双值的value()函数，以及一个将梯度存储为特征：：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的q_autodiff J/∂Q(I)，但只计算一个特定的i，则可以将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时，所有k != i和1的梯度值为0，然后可以使用当前代码计算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);
  }
}