In the tutorial (https://commonroad.in.tum.de/tutorial-optimization-solution) there is an error causing cvxpy to not finding a proper solution.
As stated in the introduction of the tutorial, ego initial velocity should be 22. However, in section “Formulation of the Optimization Problem” the ego state is initialized as
x_0 = npy.array([initial_state.position[0],0,0,0]).reshape([n,]) # initial state
resulting in an initial velocity of zero.
This should be changed to
x_0 = np.array([initial_state.position[0],initial_state.velocity,0,0]).reshape([n,]) # initial state.