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

问题描述

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

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

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

The object named [Inverse Kinematics] of type

    drake::manipulation::planner::DifferentialInverseKinematicsIntegrator

does not support ToautodiffXd

运气不好，我的控制器类（利用 DifferentialInverseKinematicsIntegrator）似乎不支持 autodiff 转换。由于该系统本质上是 DoDifferentialInverseKinematics 类的方便包装类，我尝试改为手动创建 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,V: numpy.ndarray[numpy.float64[m,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,frame_E: drake::multibody::Frame<double>,parameters: pydrake.manipulation.planner.DifferentialInverseKinematicsParameters) -> pydrake.manipulation.planner.DifferentialInverseKinematicsResult
    3. (robot: drake::multibody::MultibodyPlant<double>,X_WE_desired: pydrake.common.eigen_geometry.Isometry3_[float],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.]),0.,1.,array([[0.,0. ],[0.,[0.3,0. ]]),<pydrake.manipulation.planner.DifferentialInverseKinematicsParameters object at 0x7f6f5061c330>

我尝试查找 C++ documentation for DoDifferentialKinematics 以寻找线索。这个函数似乎确实只接受双标量类型。然而，关于 DoDifferentialKinematics 的实现的注释指出，本质上发生在幕后的所有事情都是该函数运行一个 MathematicalProgram。我的理解是 Drake 支持通过 MathematicalProgram 编织 autodiff。

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

解决方法

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

在 DifferentialInverseKinematics 中，它求解二次程序。 QP 成本/约束的 Hessian 是固定的。因此，我们可以编写一个特殊的函数来对 QP 的解进行微分。