RobotLocomotion / drake

Model-based design and verification for robotics.
https://drake.mit.edu
Other
3.33k stars 1.26k forks source link

Return the status of taking a potential step in integrator #17788

Open hongkai-dai opened 2 years ago

hongkai-dai commented 2 years ago

When we determine the step size of the integrator, I would like to have some function to return the status of taking a candidate step, and then adjust the step size based on that returned status.

The use case is that I have a forward ODE xdot = f(x, u), and a controller. Inside this controller it solves an optimization problem. This problem is feasible if there is no integration error, but if there is a large integration error then this optimization will be infeasible. So if this optimization problem is infeasible, I would like to return some status so that we should take a smaller time step.

cc @sherm1 @amcastro-tri

sherm1 commented 2 years ago

This is a common integrator need that we anticipated but didn't hook up the wiring for -- see also #13799 and #9171. We should be able to return status from CalcDerivatives() so that it can communicate to the integrator that some necessary condition was not satisfied. Variable step integrators then respond to that bad status by reducing the step size and retrying.

jwnimmer-tri commented 2 years ago

@hongkai-dai can you cite which specific API functions you're talking about? I can't quite tell which part of our code this relates to.

hongkai-dai commented 2 years ago

I don't know the specific API as I am not familiar with the integrator's code. I think @sherm1 knows about this better. My use case is that when the integrator guesses the time step, it tries a candidate time step, do some computation (like compute the time derivatives at that step), and then either reject or accept that candidate time step. In my use case, I have a controller like this

class MyController {
  MyController () {
    this->DeclareVecterOutput(u_dim, &MyController::EvalOutput);
  }
  void EvalOutput(const Context<double>& context, BasicVector<double>* output) const {
    // Do some computation
  }
}

In this MyController::EvalOutput function, it tries to solve an optimization problem. If this optimization is infeasible, then I want to tell the integrator that this candidate step is not good.

jwnimmer-tri commented 2 years ago

In my mental model, the only thing that affects the size of variable time steps are the derivatives used for continuous integration, not any output ports. Is that right @sherm1?

In any case, I thought the way to indicate to the integrator that the step was too large was to return NaNs for the derivatives. If some system's derivatives are based on an input port fed by your controller's output port, maybe that would work.

sherm1 commented 2 years ago

the only thing that affects the size of variable time steps are the derivatives used for continuous integration, not any output ports?

That's right. But if the derivative method reads an input port and doesn't like what it sees it could report an error that would cause the integrator to take a smaller step.

As best I can tell the integrators do not currently respond to NaN derivatives in any reasonable way. Treating NaN as a non-fatal error causing step size reduction (until a minimum step size is reached) would be a good thing to do and wouldn't require any API changes.