Time cost in nonlinear optimization

[marcelinomalmeidan]

When reading polynomial_optimization_nonlinear_impl.h, in line 283, we have this:
cost_time = total_time * total_time * optimization_data->optimization_parameters_.time_penalty;
Thus, the time is constrained quadratically, instead of linearly as in the referenced paper. Is there any reason for this to be so?
I feel like that we can obtain better results (and more predictable) if the time is constrained linearly, instead of quadratically.

[rikba]

Hi Marcelino,

the implementation follows Burri et al. where the segment times enter the cost quadratically (Eq. 11).

Feel free to test both cost functions and tell us about the results.

[marcelinomalmeidan]

@rikba, thanks! I guess I was looking into the wrong reference!
I will test different cost functions to see what makes more sense. I'll post here when I get some answers!

[marcelinomalmeidan]

Okay, I got news on this!
First of all, I don't trust the results given by Nlopt... It doesn't seem to be solving for optimal segment times very well. As I said in another issue, I wrote my own gradient descent method, and I am reaching solutions that have costs that are even an order of magnitude lower than Nlopt.

Proceeding on this, I chose a set of 10 waypoints, fixed the final time and computed the minimum snap with optimal segment times. Then, I changed the final time and observed how the cost changed when the final time (tf) changed. The relation between the cost and final time can be seen in the following log-log plot:

We can see a perfect slope in this plot. When I fit this "curve" to a polynomial, I get a perfect linear polynomial with slope -7.
Therefore, the minimum snap optimal cost decreases as a function of tf^7. This number is reproducible by choosing different set of waypoints and polynomial orders N.
Hence, this makes me believe that the cost for shouldn't be linear or quadratic, it should be:
cost_time = pow(total_time ,7) * optimization_data->optimization_parameters_.time_penalty;
This should make it easier to tune k_T.