trajopt | Trajectory Optimization

Consider an existing Drake System (for example MultibodyPlant). Is there a way to wrap that System inside a Diagram in such a way as to convert some of the states of the internal System to be inputs instead, i.e. set directly from input ports of the outer Diagram?

The motivation would be essentially to make a change in modeling decisions. For example, a quadrotor is sometimes considered to have its angular rates and collective thrust as inputs, instead of the collective thrust and body moments (or similarly, individual rotor commands).

In a more complex system, perhaps I might assume I have instantaneous control over certain velocities (e.g. internal, fully-actuated joints with fast dynamics), but still want to model the entire multibody system's dynamics accounting for the current choice of velocity in terms of Coriolis terms etc.

What I'm getting at is actually very similar to the modeling choice for the elevator in the Flat-Plate Glider Model - but I'd like to avoid manually implementing a LeafSystem because my system has nontrivial multibody dynamics.

My sense is that this may not be possible since I don't know of any way for a Diagram to interfere with the internal dynamics of a System, so "deleting" a state and promoting it to an input seems impossible. But I thought there might be some clever method to do this.

Thanks in advance!

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!