System support for running costs and constraint monitoring

[RussTedrake]

This proposal would replace the old RequirementsMonitorSystem proposal in #4998 (which I never landed).

Goal:

• Want to be able to list costs / constraints on Systems, run a simulation, and to be able to evaluate the Context at the end of a simulation to see my results. Specifically I would like to be able to see the accumulated cost and the worst-case value of each constraint being monitored).
• Should be able to get the autodiff of the results for systems that support autodiff.
• Want to leverage the SystemConstraint workflows to author these constraints (and possibly the costs).

A few options I discussed with @sammy-tri : 1) Like the RequirementsMonitorSystem, we could have a separate system that gets access to the values it is monitoring only through output ports. If i want to monitor a state variable of some System, then I have to push that state through an output port and wire it up.
Pros: fits neatly into the systems framework. Cons: wiring is a pain. can't leverage SystemConstraint, or in general any cost/constraint that needs to call System methods in order to do it's evaluation. It would all have to come through output ports.

2) A MonitorSystem wrapper around an existing System that behaves a lot like a Diagram. It would own the system internally -- therefore having access to it's Context directly and be able to call methods on it. And it would augment the Context with a few state variables to track the running cost and the constraint deviation. Pros: still fits in the existing systems framework. could have access to all update/integration events and accrue cost / monitor constraints accordingly. Cons: don't want yet another Diagram, DiagramContext, etc to exist. and we would have to pass through basically everything for this to work.

3) Teach System and Constraint about running costs and constraint violations. This is the one that I recommend after today's discussion.

• add members to Context

  Context<T> {  
  private:
  T running_cost;
  vector<VectorX<T>> minimum_constraint_value, maximum_constraint_value;
  }

  there is a precedent for this (e.g. step_info and accuracy). While these ARE state, I feel that they need to be separate from the existing discrete and continuous state that are the main system model. I don't want these to interfere with control design, analysis, etc. They are also special and general enough that every system should be able to have them easily.

• add methods to System, e.g.

  System<T> {
   DeclareContinuousCost(...)   // for e.g. \int_0^t g(x(t),u(t))
   DeclareDiscretePeriodicCost(...)  // for e.g. \sum_n g(x[n], u[n])
   MonitorContinuousConstraint(SystemConstraint ...)  
   MonitorDiscretePeriodicConstraint(SystemConstraint ...)
  }

  as in #10339, the cost could be a function pointer if it was only a protected method. But since we probably want this for e.g. MultiBodyPlant, we probably want to use a SystemConstraint (now SystemCost? via a SystemEvaluatorBase?) so that the running costs can survive scalar conversion.

  • The discrete costs/constraints should work with the existing event mechanisms, but unfortunately i think we'll need to teach Simulator/IntegratorBase and friends about the new continuous elements.

Option 3 is particularly nice/scalable when it comes to Diagrams, too. By inspecting the final Context, I can see the total cost for the diagram and/or dig down into a subsystem cost. I'd expect running_cost to always be a scalar and always the sum of all costs from the subsystems, whereas the constraint accessors would make available a list of constraints from all the subsystems. Major Con: Not in the main system's framework. Have to touch Simulator. And based on a quick look of the implementation of IntegratorBase, it looks like we'll have to touch a bunch of lines.

[jwnimmer-tri]

There is also an Option (4) where you keep the same kind of side information (running cost, min observed, max observed), and wire it through the Simulator/Integrator the same way, but don't store it in Context. Store it in a Simulator-specific output side structure, which perhaps the user could opt-into (or if not enabled, the Declare / Monitor details could be ignored). Since these new fields don't affect the input/output of the simulation, they don't strictly have to be in the Context to dependency-tracking, etc. The major con is that you don't get hierarchical subsystem running costs for free (though I guess we could offer a flat map of those as well, at the top level, if it was important).

[jwnimmer-tri]

For the cons mentioned for option (1), isn't there an easy patch where we have new a Diagram method to add running costs and constraints, using the SystemConstraint framework? It could snarf any subsystem state that it wanted (with sugar so the user only gets the slice(s) they want).

[RussTedrake]

Re: storing values in the simulator instead of the context... the distinction I would make is that for info stored in the simulator, there should always be a correct behavior that emerges if one calls simulator.Initialize(). It might make you less efficient (e.g. your tuned step size gets reset to defaults), but should still give the same simulation results up to a tolerance. In this case, I think that if I stop and restart my simulator at time 5, I would not want initialize to reset the running cost, etc. It should definitely be part of the context, I think.

Re: option 1 and diagram sugar -- that's a good idea that I hadn't considered. then there is another question about whether the runtime_cost, etc is in DiagramContext.runtime_cost or added as a continuous state variable to the Diagram's context. I still prefer it being treated differently -- if I want to analyze the state space of a closed-loop system (in a Diagram), I don't want these states appearing. They do not effect any of the original dynamics nor outputs.

[sherm1]

My preference for anything state-like would be that we use the state mechanism, so that (for example) if it needs to be integrated the Simulator can discover that easily. Reworking the state abstraction as in

9171 will make it easy to keep that hidden when it should be -- the new states would be a separate numerical state group easily excluded from state-space analysis.

[RussTedrake]

I haven't forgotten #9171 -- but I don't think we can afford to wait for it right now?

[sherm1]

That's at the top of my "on deck" list in ZenHub if I can ever get the event sugar stuff to land. But if you don't want to wait for #9171 (and I don't yet have a good sense for how big a mangle that is going to be), please consider using the existing state and event system if possible so we can easily upgrade later, rather than making big changes to Context and Simulator that we'd have to pull out. Would it be too difficult to work around the extra continuous and discrete state for analysis? I think in general there are likely to be uninteresting state variables present anyway -- for example, some of the friction models we're interested in use auxiliary state variables, as do PID controllers, calculations of work done, etc. Maybe all the new stuff could be put in the "z" continuous variables or their own discrete state group and ignored?

[RussTedrake]

all of those other examples are real state -- they have to be included in the analysis (cannot be ignored). the only "proper" way to ignore them is to build another system that is a simpler (but complete) model/abstraction of the original system, but with the reduced state. i think it is easy to clutter up the state space, but i want to avoid it whenever possible.

Going the #9171 route would at very least require another attribute of the state group that somehow labels it as e.g. running_cost (so the relevant methods can ignore it).

i need a little more thinking to 100% convince myself that it cannot/should not be in State. But every system optimization everywhere takes the form, e.g.

min_{something}  \sum g(x[n], u[n])
s.t. x[n+1] = f(x[n], u[n]), y[n] = h(x[n], u[n])

x[n] is the state in that model. putting \sum_{0}^n g into the state would break (or at least complicate) almost every algorithm we write. And even this (our entire discussion here) assumes that we have an additive cost structure, which is not fully general.

I think the most elegant (but likely not practical) solution would be to have the history of Contexts available after the simulation, so one could compute the costs/constraints post-hoc. But we don't have that -- and the SignalLogger workflow suffers from the same pains that i've articulated above (you have to wire up everything you want to log through an output port). I think fundamentally, this is the same problem.

[RussTedrake]

I wanted to sleep on it, but I have to admit that I’ve now talked myself out of the part of Option 3 where we change the Context. It happened when I wrote

even this (our entire discussion here) assumes that we have an additive cost structure, which is not fully general.

Because it’s not fully general — and we may very well want other measures to summarize the incremental costs — I’m back to thinking it does not deserve such special treatment to own it’s own variables in everybody’s context.

I think the mindset that I need to rectify my objection above (that e.g. the accumulated cost is not part of the model state) is that we need to think about having multiple versions of the diagram for multiple purposes. The model that I’m going to do control design on should not have cost accumulators nor requirements monitors. But there is a chance that the algorithm might create a new diagram that does have a cost accumulator system/state inside it in order to perform its work. Similarly, a requirements monitor is a real thing — a system that you could add to a Diagram that adds new capabilities. And it should be added only when you intend to monitor.

I still don’t see my way through the challenges of building up these capabilities incrementally by writing the leaf systems versions. But some amount of sugar must be possible.

Sorry for being slow.

[jwnimmer-tri]

Is there a revised statement of the problem that you could summarize again, given the newer understanding of the needs?

Is it something like this?

(1) Make it palatable to construct Diagrams that capture a given scenario with different modeling choices. Different applications would use different formulations, but code that creates some elements that are common to several formulations is (ideally) expressed only once.

(2) Make it palatable to add a Monitor system to a Diagram that can compute using the Diagram State (or substates), without having to manually connect all subsystems' output ports as inputs to the monitor, nor add y=x output ports where a state output does not already exist.

[RussTedrake]

Yes. Your (1) and (2) look good to me, though I think (2) is the more important and more difficult one.

[RussTedrake]

In particular, I think we would like to make it palatable to add a Monitor to a Diagram that uses a list of SystemConstraint objects (on the another subsystem of the Diagram) to perform it's work. Integrated running cost and worst-case constraint violation are two simple implementations that I think will get us a long way.