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

Question

I am using PyDrake do build a simple model of a Franka Emika Panda robot arm which picks up and places a brick.

I would like to observe how a change in the initial chosen starting position of my brick affects a custom target loss function. Therefore, I would like to use the AutoDiffXd functionality built into Drake to automatically extract the derivative of my loss function at the end of simulation with respect to my initial inputs.

I build my system with as normal, then run ToAutoDiffXd() to convert the respective systems to an autodiff version. However, I get the following error:

The object named [Inverse Kinematics] of type

    drake::manipulation::planner::DifferentialInverseKinematicsIntegrator

does not support ToAutoDiffXd

No luck, it seems my controller class (which leverages DifferentialInverseKinematicsIntegrator ) does not support autodiff conversion. Since this system is essentially a convenient wrapper class for the DoDifferentialInverseKinematics class, I tried instead manually creating an IK controller, and feeding the autodiff variables to DoDifferentialInverseKinematics directly. However, this also does not support autodiff it seems:

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>

I tried looking up the C++ documentation for DoDifferentialKinematics for clues. It does indeed seem that this function only accepts the double scalar type. However, a note on DoDifferentialKinematics 's implementation remarks that essentially all that happens under the hood is that this function runs a MathematicalProgram . My understanding is that weaving AutoDiff through a MathematicalProgram is supported in Drake.

So my question is: What is the best way for me to accomplish my goal? Should I just manually recreate a custom auto-diff version of DifferentialInverseKinematics using the MathematicalProgram API? Will this even succeed? Additionally, is there an easier alternative?

Answer 1

Your deductions look correct to me, except perhaps the very last comment about MathematicalProgram . MathematicalProgram knows how to consume AutoDiffXd , but to take the gradient of the solution of a MathematicalProgram optimization, one needs to take the gradients of the optimality conditions (KKT). We have an issue on this here: https://github.com/RobotLocomotion/drake/issues/4267 . I will cross-post this issue there to see if there is any update.

Depending on what you are trying to do with inverse kinematics, it might be that a simpler approach (taking the pseudo-inverse of the Jacobian) would work just fine for you. In that workflow, you would write your own DifferentialInverseKinematics system like in http://manipulation.csail.mit.edu/pick.html and make it support AutoDiffXd . (This could happen in either python or c++).

Answer 2

The challenge of computing the gradient of a general optimization problem, is that it requires the Hessian of the constraint/costs. This would require we use nested AutoDiffScalar type to compute the Hessian, and currently we don't support that.

In DifferentialInverseKinematics, it solves a quadratic program. The Hessian of the QP cost/constraints are fixed. Hence it is possible that we can write a special function, to just differentiate the solution of the QP.