Interpolate derivatives

[thowell]

Based on discussion of prior work with @hartikainen, adding an option to skip derivative computation at each time step in the planning horizon and instead linearly interpolate values between computed points.

Currently in progress, but already seeing large speed improvements for the total iLQG plan time with this feature.

• [x] Clean up code
• [x] GUI option to set number of skips

[erez-tom]

@DMackRus -- how easy would it be for you to add some heuristic that's better than a naive, regular skip?

[DMackRus]

Shouldn't be too difficult! I started on this this week, just familiarizing myself with the codebase. Its the main thing I'm working on this week so should get some decent progress on it!

[thowell]

@DMackRus you should just need to modify a few lines of code.

Modify this section:

// evaluate indices
int s = skip + 1;
evaluate_.push_back(0);
for (int t = s; t < T - s; t += s) {
  evaluate_.push_back(t);
}
evaluate_.push_back(T - 2);
evaluate_.push_back(T - 1);

To something like:

// evaluate indices
evaluate_.push_back(0);
// ---------------
// Your code here
//----------------
evaluate_.push_back(T - 2);
evaluate_.push_back(T - 1);

We definitely need to evaluate the last time step T-1 derivatives because this is a special case. To retain all the code below we also need derivative evaluations at 0 and T-2. Otherwise, this is where your algorithm to select better evaluation points comes in.