PyDrake：从机器人中提取 AutoDiff 梯度（通过微分逆运动学控制）

Question

我正在使用 PyDrake 构建 Franka Emika Panda 机器人手臂的简单模型，它可以捡起和放置一块砖。

我想观察砖块初始选择起始位置的变化如何影响自定义目标损失函数。因此，我想使用 Drake 内置的 AutoDiffXd 功能，在模拟结束时自动提取损失函数相对于我的初始输入的导数。

我像往常一样构建我的系统，然后运行 ToAutoDiffXd() 将相应的系统转换为 autodiff 版本。但是，我收到以下错误：

The object named [Inverse Kinematics] of type

    drake::manipulation::planner::DifferentialInverseKinematicsIntegrator

does not support ToAutoDiffXd

运气不好，我的控制器 class（利用 DifferentialInverseKinematicsIntegrator）似乎不支持自动差异转换。由于这个系统本质上是 DoDifferentialInverseKinematics class 的一个方便的包装器 class，我尝试手动创建一个 IK 控制器，并将 autodiff 变量直接提供给 DoDifferentialInverseKinematics。但是，这似乎也不支持autodiff：

DoDifferentialInverseKinematics(example_auto, v_current, desired_spatial_velocity, jac_wrt_v, DifferentialInverseKinematicsParameters(num_positions=2, num_velocities=2))

TypeError: DoDifferentialInverseKinematics(): incompatible function arguments. The following argument types are supported:
    1. (q_current: numpy.ndarray[numpy.float64[m, 1]], v_current: numpy.ndarray[numpy.float64[m, 1]], V: numpy.ndarray[numpy.float64[m, 1]], J: numpy.ndarray[numpy.float64[m, n]], parameters: pydrake.manipulation.planner.DifferentialInverseKinematicsParameters) -> pydrake.manipulation.planner.DifferentialInverseKinematicsResult
    2. (robot: drake::multibody::MultibodyPlant<double>, context: pydrake.systems.framework.Context_[float], V_WE_desired: numpy.ndarray[numpy.float64[6, 1]], frame_E: drake::multibody::Frame<double>, parameters: pydrake.manipulation.planner.DifferentialInverseKinematicsParameters) -> pydrake.manipulation.planner.DifferentialInverseKinematicsResult
    3. (robot: drake::multibody::MultibodyPlant<double>, context: pydrake.systems.framework.Context_[float], X_WE_desired: pydrake.common.eigen_geometry.Isometry3_[float], frame_E: drake::multibody::Frame<double>, parameters: pydrake.manipulation.planner.DifferentialInverseKinematicsParameters) -> pydrake.manipulation.planner.DifferentialInverseKinematicsResult

Invoked with: array([[<AutoDiffXd 0.5 nderiv=2>],
       [<AutoDiffXd 0.3 nderiv=2>]], dtype=object), array([0., 0.]), array([0., 0., 0., 1., 0., 0.]), array([[0. , 0. ],
       [0. , 0. ],
       [0. , 0. ],
       [0.3, 0. ],
       [0. , 0. ],
       [0. , 0. ]]), <pydrake.manipulation.planner.DifferentialInverseKinematicsParameters object at 0x7f6f5061c330>

我试着在 C++ documentation for DoDifferentialKinematics 中查找线索。看起来这个函数确实只接受双标量类型。但是，关于 DoDifferentialKinematics 的实现的注释指出，在幕后发生的所有事情基本上都是这个函数运行是 MathematicalProgram。我的理解是，通过 MathematicalProgram 编织 AutoDiff 是 德雷克支持的。

所以我的问题是：实现目标的最佳方式是什么？我是否应该使用 MathematicalProgram API 手动重新创建 DifferentialInverseKinematics 的自定义自动差异版本？这甚至会成功吗？另外，有没有更简单的选择？

Answer 1

你的推论在我看来是正确的，除了关于 MathematicalProgram 的最后评论。 MathematicalProgram 知道如何消耗 AutoDiffXd，但是要获取 MathematicalProgram 优化解的梯度，需要获取最优条件 (KKT) 的梯度。我们在这里有一个问题：https://github.com/RobotLocomotion/drake/issues/4267。我会交叉post这个问题，看看有没有更新。

根据您尝试使用逆运动学进行的操作，可能更简单的方法（采用雅可比行列式的伪逆）对您来说效果很好。在该工作流程中，您将像 http://manipulation.csail.mit.edu/pick.html 中那样编写自己的 DifferentialInverseKinematics 系统并使其支持 AutoDiffXd。（这可能发生在 python 或 c++ 中）。

Answer 2

计算一般优化问题的梯度的挑战在于它需要 constraint/costs 的 Hessian。这需要我们使用嵌套的 AutoDiffScalar 类型来计算 Hessian，目前我们不支持。

在 DifferentialInverseKinematics 中，它求解二次规划。 QP cost/constraints 的 Hessian 是固定的。因此，我们可以编写一个特殊的函数，来区分 QP 的解决方案。

PyDrake：从机器人中提取 AutoDiff 梯度（通过微分逆运动学控制）

PyDrake: Extracting AutoDiff gradients from a robot (controlled via Differential Inverse Kinematics)

python

c++

drake

automatic-differentiation

inverse-kinematics