1.Add a SOCP problem to find the initial guess for nonlinear problem in the dynamic planning
2. change the test case to walking on flat ground