使用PyDrake AutoDiff计算某坐标系下向量的雅可比矩阵

假设有一个固定于坐标系F中的向量n_F（比如固定在指尖局部坐标系的指尖表面法向量），向量n_W(q)满足表达式 n_W = R_WF @ n_F，其中旋转矩阵R_WF通过正运动学映射依赖于配置参数q。需要解决：

• 如何用Drake的AutoDiff获取n_W相对于q的雅可比矩阵Dn_W（3×n的矩阵）
• 若有使用Drake现有函数表达该雅可比的简便方法也可，自行推导的表达式不够简洁

先澄清一个误区：Drake的AutoDiffXd完全支持向量值函数的自动微分，并非仅适用于标量函数。下面提供两种实用方案：

方案一：直接用AutoDiffXd计算 #

操作步骤清晰直接：

1. 把配置参数q转成AutoDiffXd类型，给每个参数初始化自身的偏导数（也就是q_i对自己的导数为1，对其他参数为0）
2. 通过正运动学计算带微分信息的旋转矩阵R_WF_autodiff
3. 计算n_W_autodiff = R_WF_autodiff @ n_F，提取它的导数部分就是雅可比矩阵Dn_W

示例代码：

#include "drake/math/autodiff.h"
#include "drake/multibody/plant/multibody_plant.h"

// 假设已经初始化好MultibodyPlant<double> plant和对应的Context<double> context
// 配置参数q的维度为n
Eigen::VectorXd q_double = ...; // 给定的配置参数值

// 将q转换为AutoDiffXd类型，初始化导数矩阵为单位矩阵
Eigen::VectorX<drake::AutoDiffXd> q_autodiff = drake::math::InitializeAutoDiff(q_double);

// 创建AutoDiff版本的Context
std::unique_ptr<drake::systems::Context<drake::AutoDiffXd>> context_autodiff =
    plant.ToAutoDiffXdContext(context);

// 设置AutoDiff版本的配置参数
plant.GetMutablePositions(context_autodiff.get()) = q_autodiff;

// 获取坐标系F相对于世界坐标系W的旋转矩阵（带AutoDiff信息）
const drake::math::RotationMatrix<drake::AutoDiffXd> R_WF_autodiff =
    plant.EvalBodyPoseInWorld(*context_autodiff, body_F).rotation();

// 定义固定于F的向量n_F（转成AutoDiffXd类型，导数为0）
Eigen::Vector3<drake::AutoDiffXd> n_F_autodiff = n_F.cast<drake::AutoDiffXd>();

// 计算n_W_autodiff
Eigen::Vector3<drake::AutoDiffXd> n_W_autodiff = R_WF_autodiff * n_F_autodiff;

// 提取雅可比矩阵Dn_W（3×n）
Eigen::MatrixXd Dn_W = drake::math::ExtractGradient(n_W_autodiff);

方案二：用Drake的雅可比计算工具 #

可以直接利用MultibodyPlant自带的函数，结合旋转向量的导数特性来计算：
n_W = R_WF n_F对q的雅可比Dn_W，等价于ω_WF × n_W对q的雅可比（其中ω_WF是坐标系F相对于W的角速度），所以可以通过计算角速度的雅可比，再结合叉乘得到结果：

示例代码：

// 假设已初始化好plant和context（double类型）
Eigen::VectorXd q_double = ...;
plant.SetPositions(context.get(), q_double);

// 获取坐标系F相对于W的旋转矩阵，计算n_W
const drake::math::RotationMatrix<double> R_WF =
    plant.EvalBodyPoseInWorld(*context, body_F).rotation();
Eigen::Vector3d n_W = R_WF * n_F;

// 计算角速度的雅可比J_omega（3×n）：ω_WF = J_omega * 关节速度v
Eigen::MatrixXd J_omega = plant.CalcJacobianSpatialVelocity(
    *context,
    drake::multibody::JacobianWrtVariable::kQDot,
    body_F,
    drake::math::Vector3d::Zero(), // 取F坐标系原点作为参考点
    drake::math::FrameId::World(),
    drake::math::FrameId::World());
// 取前3行，对应角速度部分
J_omega = J_omega.topRows(3);

// 构造叉乘矩阵：输入向量a，返回矩阵A，满足A*b = a×b
auto skew = [](const Eigen::Vector3d& a) {
    Eigen::Matrix3d mat;
    mat << 0, -a(2), a(1),
           a(2), 0, -a(0),
           -a(1), a(0), 0;
    return mat;
};

// 计算最终的雅可比矩阵Dn_W
Eigen::MatrixXd Dn_W = skew(n_W) * J_omega;

内容的提问来源于stack exchange，提问作者robodobo