1: $ and warm start optimization until convergence or collision

2: \State $