如何用Drake计算机器人雅可比矩阵J以求解末端执行器空间力

要在Drake中获取机器人末端执行器的雅可比矩阵J（用于τ = JᵀF的力映射），核心是利用MultibodyPlant提供的雅可比计算接口，步骤如下：

• 准备上下文与末端执行器对象
  先确保你已经加载了机器人模型并创建了MultibodyPlant实例。接着创建上下文并设置当前广义位置q：

  # 假设plant是已配置好的MultibodyPlant实例
  context = plant.CreateDefaultContext()
  plant.SetPositions(context, q)
  

  通过模型里的连杆名称获取末端执行器对应的Body对象：

  end_effector = plant.GetBodyByName("你的末端连杆名称")
  

• 计算空间速度雅可比矩阵
  Drake的CalcJacobianSpatialVelocity函数直接返回空间速度雅可比（对应V = J·q_dot，V是末端的空间速度），这正是公式里需要的J：

  from drake.multibody.math import JacobianWrtVariable
  import numpy as np
  
  J = plant.CalcJacobianSpatialVelocity(
      context,
      JacobianWrtVariable.kQDot,
      end_effector.body_frame(),  # 末端执行器的坐标系
      np.zeros(3),  # 计算末端坐标系原点的雅可比
      plant.world_frame(),  # 参考坐标系（世界系）
      plant.world_frame()   # 雅可比的输出坐标系
  )
  

  这个J是6×N的矩阵（N为机器人关节自由度），完全匹配τ = JᵀF的需求——这里的J转置后和关节力、末端空间力的映射关系直接对应。

• 可选：拆分线性/角速度雅可比
  如果需要单独获取末端的线速度雅可比（3×N）或角速度雅可比（3×N），可以用两个专用函数：

  # 线速度雅可比（末端原点线速度与q_dot的映射）
  Jv = plant.CalcJacobianTranslationalVelocity(
      context,
      JacobianWrtVariable.kQDot,
      end_effector.body_frame(),
      np.zeros(3),
      plant.world_frame(),
      plant.world_frame()
  )
  
  # 角速度雅可比（末端角速度与q_dot的映射）
  Jw = plant.CalcJacobianAngularVelocity(
      context,
      JacobianWrtVariable.kQDot,
      end_effector.body_frame(),
      plant.world_frame(),
      plant.world_frame()
  )
  

  把两者垂直拼接J = np.vstack([Jw, Jv])，结果和CalcJacobianSpatialVelocity的输出完全一致。

• 后续力计算说明
  得到J后，若已知关节广义力τ，可通过伪逆求解末端空间力F：F = np.linalg.pinv(J.T) @ τ——如果雅可比矩阵满秩，直接用逆矩阵即可。

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