Switch beack from xs optimization to dxs optimization

python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py

@@ -163,15 +163,17 @@ class OCP:
         print("-- End of solve() --")
         """from IPython import embed
         embed()"""
-    
+
     def my_debug(self, i):
         print("---" + str(i) + "---")
         for j in range(4):
             print(self.opti.debug.value(self.datas[0].f[j]))
 
         for cost in self.datas[0].costs:
-            self.log_costs[cost].append([self.opti.value(data.costs[cost]) for data in self.datas])
-    
+            self.log_costs[cost].append(
+                [self.opti.value(data.costs[cost]) for data in self.datas]
+            )
+
         """for data in self.datas:
             for cost in data.costs:
                 self.log_costs[cost].append(self.opti.value(data.costs[cost]))"""
@@ -210,7 +212,10 @@ class OCP:
         ]
         self.acs = [opti.variable(self.pd.nv) for _ in self.runningModels]
         self.us = [opti.variable(self.pd.nu) for _ in self.runningModels]
-        self.xs = [opti.variable(self.pd.nx) for _ in (self.runningModels + [self.terminalModel])]
+        self.xs = [
+            m.integrate(x0, dx)
+            for m, dx in zip(self.runningModels + [self.terminalModel], self.dxs)
+        ]
         self.fs = []
         for m in self.runningModels:
             f_tmp = [opti.variable(3) for _ in range(len(m.contactIds))]
@@ -234,10 +239,11 @@ class OCP:
         eq = []
         # First running node is in initial state of the robot
         eq.append(self.runningModels[0].difference(self.xs[0], x0))
+        # Could also be eq.append(self.dxs[0]) to force first dxs to 0?
 
         # Set targets in xref of ProblemData
-        self.pd.xref[self.pd.nq:(self.pd.nq + 3)] = targets[0]
-        self.pd.xref[(self.pd.nq + 3):(self.pd.nq + 6)] = targets[1]
+        self.pd.xref[self.pd.nq : (self.pd.nq + 3)] = targets[0]
+        self.pd.xref[(self.pd.nq + 3) : (self.pd.nq + 6)] = targets[1]
 
         # Gather costs and equality constraints for each running node
         for t in range(self.pd.T):
@@ -310,8 +316,8 @@ class OCP:
                     ]
                 )
 
-            for x, xg in zip(self.xs, xs_g):
-                self.opti.set_initial(x, xg)
+            for x, xg in zip(self.dxs, xs_g):
+                self.opti.set_initial(x, xdiff(x0, xg))
             for a, ag in zip(self.acs, acs_g):
                 self.opti.set_initial(a, ag)
             for u, ug in zip(self.us, us_g):
@@ -324,8 +330,6 @@ 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]