1

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

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

我像往常一样构建我的系统，然后运行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, 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>

我尝试查找DoDifferentialKinematics 的 C++ 文档以获取线索。确实，这个函数似乎只接受双标量类型。然而，关于DoDifferentialKinematics's implementation 的注释指出，本质上所有发生的事情都是这个函数运行一个MathematicalProgram. 我的理解是 Drake 支持通过 a 编织MathematicalProgram AutoDiff。

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

4

2 回答 2

1

你的推论在我看来是正确的，除了关于MathematicalProgram. MathematicalProgram知道如何消费AutoDiffXd，但是要获取MathematicalProgram优化解的梯度，需要获取最优性条件（KKT）的梯度。我们在这里有一个问题：https ://github.com/RobotLocomotion/drake/issues/4267 。我将在此处交叉发布此问题以查看是否有任何更新。

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

于 2021-01-15T02:30:26.733 回答
0

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

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

于 2021-01-15T18:43:51.450 回答