trajopt | Trajectory optimization algorithms for robotic control | Robotics library

I'm trying to port trajectory optimization code originally written for TrajOpt into Drake. One of the nice things about TrajOpt is that it could solve SQP trajectory optimization problems with a constraint enforcing a minimum distance between the robot and surrounding obstacles, and it supported a pretty broad range of geometries (all the standard convex primitives plus simple convex meshes). For a number of reasons, TrajOpt is no longer the right choice for my project, so I'm porting my trajectory optimization code over to Drake. I think MinimumDistanceConstraint is what I want to replicate this functionality, but it seems that Drake allows AutoDiffXd signed distance queries only for spheres and half-spaces, not for more general convex shapes (like boxes or cylinders).

All of my other constraints support AutoDiff (I have some custom constraints for "probability of collision," but those provide an analytical derivative that can be used in an AutoDiff). In order to add a MinimumDistanceConstraint that supports more general geometry, would I have to formulate the MathematicalProgram entirely with doubles? Would that slow down the performance of the solver (e.g. by having to do finite differences instead of using the gradient information in AutoDiffXd)?

In an ideal world, I'd like to avoid resorting to "bubble-wrapping" my robot and environment (replacing all the collision geometry with spheres), since the runtime of the custom constraints I'm using scales with the number of collide-able pairs in the scene (I'm currently using convex geometry to keep this number relatively low).

Any help would be appreciated!