In Drake, what is the best way to implement a cost function that involves signed distances when the signed distances are not auto-differentiable?
We would like to solve an optimization problem that minimizes something like this for its cost function:
minimize sum_i phi_i^2(q) subject to (some constraints)
where the phi_i s are signed distance functions between collision geometries that are in the shapes of spheres and cylinders. Unfortunately, as of today, drake's sdf calculation (query_object.ComputeSignedDistancePairClosestPoints) between spheres and cylinders do not support AutoDiffXd return type. This implies no gradients on the cost can be obtained directly, without which SNOPT cannot really solve the problem in my understanding.
What is the best way to approach this? Should we manually define a gradient, for example?
Thanks!
We tried using ik.AddMinimumDistanceUpperBoundConstraint as an alternative, but this does not really capture the problem we want to solve. We tried using the float query_object.ComputeSignedDistancePairClosestPoints returns to define the cost, but did not manage to get SNOPT to solve the resulting problem, likely due to the lack of a gradient.
If you just need the gradient of the signed distance function, you do not need it to support automatic differentiation. Instead, you an compute the gradient of the signed distance function w.r.t robot configuration using MultibodyPlant<double>. For the math, you could refer to https://github.com/RobotLocomotion/drake/blob/bdcd5ff96a1dfed77791302fb499ec2aa831a03e/multibody/inverse_kinematics/distance_constraint_utilities.cc#L11-L45, which compute the gradient ∂ϕ/∂q using contact normal and contact Jacobian.