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

Question

我正在使用PyDrake做一个简单的模型，一个弗兰卡埃米卡熊猫机器人手臂，捡起和放置一块砖头。

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

我正常地构建我的系统，然后运行ToAutoDiffXd()将各自的系统转换成一个自动关闭版本。但是，我得到以下错误：

The object named [Inverse Kinematics] of type

    drake::manipulation::planner::DifferentialInverseKinematicsIntegrator

does not support ToAutoDiffXd

没有运气，看来我的控制器类(它利用DifferentialInverseKinematicsIntegrator)不支持自动does转换。由于这个系统本质上是DoDifferentialInverseKinematics类的一个方便的包装类，所以我尝试手动创建一个IK控制器，并将自动关闭变量直接输入DoDifferentialInverseKinematics。但是，这似乎也不支持自动关闭：

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++的DoDifferentialKinematics文档的线索。实际上，这个函数似乎只接受双标量类型。然而，在DoDifferentialKinematics的实现说明中，实际上，这个函数运行的是一个MathematicalProgram，我的理解是在Drake中支持在MathematicalProgram中编织AutoDiff。

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

Answer 1

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

取决于你试图做什么与逆运动学，它可能是一个更简单的方法(采取伪逆的雅可比)将工作对你很好。在该工作流中，您将编写自己的DifferentialInverseKinematics系统，就像在http://manipulation.csail.mit.edu/pick.html中一样，并使其支持AutoDiffXd。(这可能发生在python或c++中)。

Answer 2

计算一般优化问题的梯度时所面临的挑战是它要求约束/代价的Hessian值。这将需要我们使用嵌套的AutoDiffScalar类型来计算Hessian，而且目前我们不支持这种类型。

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