Time optimal formulation

Just to update this topic, an example with time scaled-dynamics and a time optimal formulation was added here: