使用 PyDrake AutoDiff 计算某个帧中向量的雅可比矩阵

Question

假设我有一个向量 n_F 固定在一个框架 F 中（例如，固定在局部指尖框架中的指尖表面上的法向量）。矢量n_W(q) 取决于表达式n_W = R_WF @ n_F 的配置，其中旋转矩阵R_WF 取决于正向运动学映射的配置。

我的问题是如何使用 Drake 的 AutoDiff 恢复 n_W 的 Jacobian Dn_W 相对于 q （这将是一些 3 x n 矩阵）。我认为必须有一些内部实现，因为在 IK 期间可以强制执行不同帧中两个角度之间的约束，并且我假设 IK 是使用基于梯度的求解器求解的。但是，我在处理这个问题时遇到了麻烦，因为 AutoDiffXd 似乎只适用于标量函数。

或者，如果有一些简单的方法可以使用可用的 Drake 函数来表达这个雅可比行列式，那也足以满足我的应用程序——我无法手动为此计算出一个干净的表达式。

Answer 1

我目前实施的（非 AutoDiff）解决方案不涉及轮换。

让J0 和J1 表示坐标系F 和矢量n_F 的原点的平移速度雅可比行列式均在坐标系F 中表示，其中速度在世界坐标系中测量和表示。然后，所需的 Jacobian Dn_W 就是 J1 - J0。