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

Question

我正在用PyDrake做一个简单的弗兰卡·埃米卡熊猫机器人手臂模型，它可以拿起并放置一块砖块

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

我用as normal构建系统，然后运行ToAutoDiffXd()将各个系统转换为autodiff版本。但是，我得到以下错误：

The object named [Inverse Kinematics] of type

    drake::manipulation::planner::DifferentialInverseKinematicsIntegrator

does not support ToAutoDiffXd

运气不好，我的控制器类（利用DifferentialInverseKinematicsIntegrator）似乎不支持自动差异转换。因为这个系统本质上是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, 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。我的理解是，Drake支持通过MathematicalProgram编织AutoDiff

所以我的问题是：对我来说，实现目标的最佳方式是什么？我是否应该使用MathematicalProgramm API手动重新创建一个自定义的DifferentialsInverseKinetics自动差异版本？这会成功吗？此外，还有更简单的选择吗

Answer 1

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

在微分学中，它求解一个二次规划。QP成本/约束的Hessian值是固定的。因此，我们可以编写一个特殊的函数来区分QP的解决方案

Answer 2

你的推论在我看来是正确的，除了最后一句关于MathematicalProgram的评论MathematicalProgram知道如何使用AutoDiffXd，但要获得MathematicalProgram优化解的梯度，需要获得最优性条件（KKT）的梯度。我们这里有一个问题：https://github.com/RobotLocomotion/drake/issues/4267。我会交叉张贴这个问题，看看是否有任何更新

根据您尝试对反向运动学所做的操作，可能更简单的方法（采用雅可比矩阵的伪逆）对您来说很合适。在该工作流中，您可以像http://manipulation.csail.mit.edu/pick.html那样编写自己的DifferentialInverseKinematics系统，并使其支持AutoDiffXd。（这可能发生在python或c++中）