Without warm start, set all nodes to initial state

python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py

@@ -280,6 +280,8 @@ class OCP:
             print("Got warm start")
         except:
             print("Can't load warm start")
+            for x in self.xs:
+                self.opti.set_initial(x, x0)
 
     def get_results(self):
         xs_sol = [self.opti.value(x) for x in self.xs]