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?
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++).