DIRCON rework

[mposa]

Much overdue, given the poor documentation (and poor code from yours truly). This will lean heavily on newly pushed #155 and some pending changes in #157 . Before I jump in, I want to get feedback on the external API (the internal-use functions, all currently marked public, will also be overhauled). For @yangwill and @yminchen who have used it, what would you like to see changed? The current constructor is

HybridDircon(plant, vec<n_dt>, vec<min_dt>, vec<max dt>, vec<constraints>, vec<options>)
options = (constraint scaling, relative constraint, start/end type, force regularization)

• First off, I'd suggest replacing min/max_dt with min/max_T (using total time of a mode instead of dt time). It's more natural to think that way.
• Second, all those vectors need to go. One vector of a DirconMode struct which captures everything.

DirconMode:
 KinematicEvaluatorSet
 num_dt
 min_T
 max_T
 is_relative vector
 start/end type
 constraint scaling
 force regularization

And then Dircon (no need for the "Hybrid" bit) can take either a single DirconMode or a vector of modes.

How does this look? Are there methods you'd like to see in an API? Features that would help debug? Get your requests in now!

[yangwill]

Some small things that come to mind:

• add a getter for mode_lengths_
• rename the quaternion slack variable to anything but gamma (the velocity correction in the paper is called gamma). I've gotten this confused many times and still am not positive that I'm not making a mistake.
• be able to compute a KinematicEvaluatorSet for a particular knot point for the solution. I'm not sure exactly how referencing a particular evaluator should work, maybe by an index?

A bigger feature

• saving/loading decision variables. This is a tough one because for it to be very useful it is not as easy as saving and loading a vector. This is because constraints/number of knots points/or even the number of modes might not be constant. At the very least, it should be possible to recover some settings of the optimization problem from the saved solution.

I'll continue this list if anything else comes to mind.

[mposa]

• be able to compute a KinematicEvaluatorSet for a particular knot point for the solution. I'm not sure exactly how referencing a particular evaluator should work, maybe by an index?
  I'm not totally sure that there's a ton of room for extra sugar to make this much easier, given that the evaluator can produce a number of outputs (phi, J, etc). Here's what the code might look like with just a basic getter for the set.

auto evaluator = dircon.GetEvaluatorSet(mode_index);
auto x = result.GetSolution(dircon.state_by_mode(mode_index, state_index));
plant.SetPositionsAndVelocities(context, x);
auto result = evaluator.EvalXXX(context)

• saving/loading decision variables. This is a tough one because for it to be very useful it is not as easy as saving and loading a vector. This is because constraints/number of knots points/or even the number of modes might not be constant. At the very least, it should be possible to recover some settings of the optimization problem from the saved solution.

I might defer to @yminchen here, since he's been doing a lot of trajectory saving and loading, and to your experience, to actually implement such a feature. What is your use case for reusing decision variables when the underlying problem has significantly changed? For instance, we could save as a vector, and be able to re-constitute either as a vector (for the same problem) or as a set of trajectory objects (when the number of knot points changes, or for visualization). I'm not sure we'd need to provide an API to do more than that. If you want to change the number of constraints, you could adapt the force trajectory manually.

[mposa]

Thoughts on the constructor/options configuration?

[yminchen]

I think the proposed constructor is more transparent about what the arguments are if we compare it to the current implement where {is_relative, start/endtype, ...} hide inside DirconOption. One the flip side, the new constructor could be a bit daunting for the people who see it for the first time. I personally like the new constructor, since I already know what they are...

For saving/loading trajectory, I'm not very sure what specific feature Will needs, but I think it might be nice to have a function that returns std::vector<DirconSolution> where each DirconSolution contains the solution of each mode such as {VectorXd time, MatrixXd state, MatrixXd input, MatrixXd force, ...}?

A feature that I need:

• Dircon automatically uses KinematicEvaluatorSet::FindUnion() to disable redundant kinematic constraints (at position level) at the knot points where modes overlap

A modification we need:

• (Sorry for my bad implementation...) When we set the indices for is_relative, the current Dircon uses the ones of "active" constraints instead of the "full" constraints. I think every adjustment to the constraints (such as is_relative and constraint_scaling) should refer to the indices of the full constraints.

[yangwill]

The main issue with organizing the parameters by the mode is that it makes getting and setting min/max dt and num_dt more verbose.

Instead of using a single vector that holds all of these values for all of the modes, each DirconMode parameters must be set independently and requires more lines of code.

On the other hand, using DirconModes does help with readability and I can see it being helpful down the line.

For saving and loading trajectories, what @yminchen suggested about being able to retrieve the decision variables and their type should be enough. Any use case beyond that we can write separate functions for.

Saving and loading trajectories is a nice feature to have because I want to separate the solving part of the trajectory optimization problem (which is slow and should not change often) from the analysis or visualization part (which is quick and requires frequent adjustments). Also there might be cases were we have an old solution, and since then we've changed the original code used to generate that solution. It would be nice to still be able to recover the meaning of each decision variable.

[mposa]

Given that the time parameters almost always must be set (knot points must be set), and are never changed, I was thinking of including them in the constructor

DirconMode(
      const multibody::KinematicEvaluatorSet<T>& evaluators, int num_knotpoints,
      double min_T = 0, double max_T = std::numeric_limits<double>::infinity(),
      double force_regularization_cost_ = 1.0e-4);

min_T, max_T, and the regularization cost can get default parameters. I think this will make setting the options relatively straightforward without a lot of extraneous lines.

[mposa]

I think the proposed constructor is more transparent about what the arguments are if we compare it to the current implement where {is_relative, start/endtype, ...} hide inside DirconOption. One the flip side, the new constructor could be a bit daunting for the people who see it for the first time. I personally like the new constructor, since I already know what they are...

My hope is that the new constructor will be easy to use (if you do not need all of the flags and options). The I'll also probably tweak the constraint scaling quite a bit, since we have more introspective capabilities into the constraint indexing. Forcing the user to memorize index numbers is on it's way out...

For saving/loading trajectory, I'm not very sure what specific feature Will needs, but I think it might be nice to have a function that returns std::vector<DirconSolution> where each DirconSolution contains the solution of each mode such as {VectorXd time, MatrixXd state, MatrixXd input, MatrixXd force, ...}?

I'm a little hesitant to create a new "solution" class, why would this be needed vs. extracting the individual elements?

A feature that I need:

• Dircon automatically uses KinematicEvaluatorSet::FindUnion() to disable redundant kinematic constraints (at position level) at the knot points where modes overlap

Will do.

A modification we need:

• (Sorry for my bad implementation...) When we set the indices for is_relative, the current Dircon uses the ones of "active" constraints instead of the "full" constraints. I think every adjustment to the constraints (such as is_relative and constraint_scaling) should refer to the indices of the full constraints.

Good point. Will do.

[yminchen]

For saving/loading trajectory, I'm not very sure what specific feature Will needs, but I think it might be nice to have a function that returns std::vector<DirconSolution> where each DirconSolution contains the solution of each mode such as {VectorXd time, MatrixXd state, MatrixXd input, MatrixXd force, ...}?

I'm a little hesitant to create a new "solution" class, why would this be needed vs. extracting the individual elements?

It might not be necessary to have a "solution" class. What I think we need is a method (or methods) that extracts state/input/force solutions, similar to MultipleShooting::GetStateSamples() and MultipleShooting::GetInputSamples(). I imagine many Dircon users will need this method(s)