PyDrake: Extracting AutoDiff gradients from a robot (controlled via Differential Inverse Kinematics)

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?

Source: Windows Questions C++

LEAVE A COMMENT