still working

config/walk_parameters.yaml

@@ -38,8 +38,7 @@ robot:
     # Parameters of Gait
     N_periods: 1
     gait: [20, 1, 0, 0, 1,
-           20, 0, 1, 1, 0,
-           1, 1, 1, 1, 1]  # Initial gait matrix
+           20, 0, 1, 1, 0]  # Initial gait matrix
 
     # Parameters of Joystick
     gp_alpha_vel: 0.003  # Coefficient of the low pass filter applied to gamepad velocity

python/quadruped_reactive_walking/Controller.py

@@ -192,8 +192,7 @@ class Controller:
                           'acs': file['acs'], 'fs': file['fs']}
             print("Initial guess loaded \n")
         except:
-            self.xs_init = None
-            self.us_init = None
+            self.guess = {}
             print("No initial guess\n")
 
         device = DummyDevice()

python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py

@@ -7,6 +7,8 @@ import pinocchio as pin
 from .ProblemData import ProblemData
 from .ShootingNode import ShootingNode
 from .Target import Target
+import pinocchio.casadi as cpin
+
 
 class NodeData:
     def __init__(self, isTerminal=False):
@@ -19,6 +21,7 @@ class NodeData:
             self.f = None
             self.xnext = None
 
+
 class OcpResult:
     def __init__(self):
         self.xs = None
@@ -30,7 +33,6 @@ class OcpResult:
         self.q = None
         self.v = None
         self.solver_time = None
-        
 
 
 class OCP:
@@ -48,16 +50,17 @@ class OCP:
         self.results = OcpResult()
 
         # build the different shooting nodes
-        self.shooting_nodes = set ([tuple(self.pd.allContactIds[i] for i in np.nonzero(step == 1)[0]) for step in gait[:-1] ])
+        self.shooting_nodes = {contacts: ShootingNode(pd, contacts)
+                               for contacts in set([tuple(self.pd.allContactIds[i] for i in np.nonzero(step == 1)[0]) for step in gait[:-1]])}
 
     def solve(self, x0, tasks, gait, guess={}):
         """ Solve the NLP
         :param guess: ditctionary in the form: {'xs':array([T+1, n_states]), 'acs':array([T, nv]),\
             'us':array([T, nu]), 'fs': [array([T, nv]),]}
         """
-        self.runningModels = [self.shooting_nodes[gait[t]]
+        self.runningModels = [self.shooting_nodes[self.pattern_to_id(gait[t])]
                               for t in range(self.pd.T)]
-        self.terminalModel = self.shooting_nodes[gait[self.pd.T]]
+        self.terminalModel = self.shooting_nodes[self.pattern_to_id(gait[self.pd.T])]
         self.datas = []
         for i in range(self.pd.T + 1):
             if i == self.pd.T:
@@ -99,10 +102,12 @@ class OCP:
         opti = casadi.Opti()
         self.opti = opti
         # Optimization variables
-        self.dxs = [opti.variable(self.pd.ndx)for _ in self.runningModels+[self.terminalModel]]
+        self.dxs = [opti.variable(self.pd.ndx)
+                    for _ in self.runningModels+[self.terminalModel]]
         self.acs = [opti.variable(self.pd.nv) for _ in self.runningModels]
         self.us = [opti.variable(self.pd.nu) for _ in self.runningModels]
-        self.xs = [m.integrate(x0, dx) for m, dx in zip(self.runningModels+[self.terminalModel], self.dxs)]
+        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))]
@@ -115,26 +120,30 @@ class OCP:
         eq.append(self.dxs[0])
         for t in range(self.pd.T):
             # These change every time! Do not use runningModels[0].x to get the state!!!! use xs[0]
-            self.runningModels[t].init(self.datas[t], self.xs[t], self.acs[t], self.us[t], self.fs[t], False)
+            self.runningModels[t].init(
+                self.datas[t], self.xs[t], self.acs[t], self.us[t], self.fs[t], False)
 
             xnext, rcost = self.runningModels[t].calc(x_ref=self.pd.xref,
                                                       u_ref=self.pd.uref,
                                                       target=tasks)
 
-             # If it is landing
-            if (gait[t] != gait[t-1]):
+            # If it is landing
+            if (gait[t] != gait[t-1]).any():
                 print('Contact on ', str(self.runningModels[t].contactIds))
-                self.runningModels[t].constraint_landing_feet_cost(self.pd.xref)
+                self.runningModels[t].constraint_landing_feet_cost(
+                    self.pd.xref)
 
             # Constraints
             eq.append(self.runningModels[t].constraint_standing_feet_eq())
             eq.append(self.runningModels[t].constraint_dynamics_eq())
-            eq.append(self.runningModels[t].difference(self.xs[t + 1], xnext) - np.zeros(self.pd.ndx))
+            eq.append(self.runningModels[t].difference(
+                self.xs[t + 1], xnext) - np.zeros(self.pd.ndx))
 
             totalcost += rcost
-        
-        eq.append(self.xs[self.pd.T][self.pd.nq :])
-        self.terminalModel.init(data=self.datas[self.pd.T], x=self.xs[self.pd.T],  isTerminal=True)
+
+        eq.append(self.xs[self.pd.T][self.pd.nq:])
+        self.terminalModel.init(
+            data=self.datas[self.pd.T], x=self.xs[self.pd.T],  isTerminal=True)
         totalcost += self.terminalModel.calc(x_ref=self.pd.xref)[1]
         totalcost += rcost
 
@@ -156,8 +165,9 @@ class OCP:
 
             def xdiff(x1, x2):
                 return np.concatenate([
-                    pin.difference(self.pd.model, x1[:self.pd.nq], x2[:self.pd.nq]),
-                                                  x2[self.pd.nq:]-x1[self.pd.nq:]])
+                    pin.difference(
+                        self.pd.model, x1[:self.pd.nq], x2[:self.pd.nq]),
+                    x2[self.pd.nq:]-x1[self.pd.nq:]])
             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):
@@ -189,8 +199,10 @@ class OCP:
             fsol_to_ws.append(np.concatenate([self.opti.value(
                 self.fs[t][j]) for j in range(len(self.runningModels[t].contactIds))]))
 
-            pin.framesForwardKinematics(self.pd.model, self.pd.rdata, np.array(xs_sol)[t, : self.pd.nq])
-            [fs_world[foot].append(self.pd.rdata.oMf[foot].rotation @ fsol[foot][t]) for foot in fs_world]
+            pin.framesForwardKinematics(
+                self.pd.model, self.pd.rdata, np.array(xs_sol)[t, : self.pd.nq])
+            [fs_world[foot].append(
+                self.pd.rdata.oMf[foot].rotation @ fsol[foot][t]) for foot in fs_world]
 
         for foot in fs_world:
             fs_world[foot] = np.array(fs_world[foot])
@@ -201,7 +213,6 @@ class OCP:
         self.results.fs = fsol_to_ws
         return self.results
 
-
     def get_feet_position(self, xs_sol):
         """ Get the feet positions computed durnig one ocp
 
@@ -243,4 +254,6 @@ class OCP:
             xs_sol[i]).full()[:, 0]) for i in range(len(xs_sol))]
         base_pos_log = np.array(base_pos_log)
         return base_pos_log
-
+    
+    def pattern_to_id(self, gait):
+        return tuple(self.pd.allContactIds[i] for i,c in enumerate(gait) if c==1 )

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -113,8 +113,6 @@ class ProblemData(problemDataAbstract):
             [0] * self.nv + [1e4] * self.nv)
         self.force_reg_w = 1e0
 
-
-
         self.xref = self.x0
         self.uref =self.u0
 

python/quadruped_reactive_walking/WB_MPC/ShootingNode.py

@@ -103,13 +103,13 @@ class ShootingNode():
 
         # vfeet[c](x) =  linear velocity of the foot <c> at configuration q=x[:nq] with vel v=x[nq:]
         self.vfeet = {idf: casadi.Function('vfoot'+cmodel.frames[idf].name,
-                                           [cx], [cpin.getFrameVelocity(cmodel, cdata, idf, pin.LOCAL_WORLD).linear])
+                                           [cx], [cpin.getFrameVelocity(cmodel, cdata, idf, pin.LOCAL).linear])
                       for idf in pd.allContactIds}
 
         # vfeet[c](x,a) =  linear acceleration of the foot <c> at configuration q=x[:nq] with vel v=x[nq:] and acceleration a
         self.afeet = {idf: casadi.Function('afoot'+cmodel.frames[idf].name,
                                            [cx, ca], [cpin.getFrameClassicalAcceleration(cmodel, cdata, idf,
-                                                                                         pin.LOCAL_WORLD).linear])
+                                                                                         pin.LOCAL).linear])
                       for idf in pd.allContactIds}
 
         # wrapper for the inverse of the exponential map
@@ -264,9 +264,9 @@ class ShootingNode():
                               self.difference(self.x, x_ref)) * self.pd.dt
 
     def target_cost(self, task):
-        self.cost += casadi.sumsqr(self.pd.linear_velocity_w*(
+        self.cost += casadi.sumsqr(self.pd.base_velocity_tracking_w*(
             self.baseVelocityLin(self.x) - task[0])) * self.pd.dt
-        self.cost += casadi.sumsqr(self.pd.angular_velocity_w*(
+        self.cost += casadi.sumsqr(self.pd.base_velocity_tracking_w*(
             self.baseVelocityAng(self.x) - task[1])) * self.pd.dt
 
     def compute_cost(self, x_ref, u_ref, target):
@@ -274,7 +274,7 @@ class ShootingNode():
         if not self.isTerminal:
             self.control_cost(u_ref)
             self.constraint_control_cost()
-            self.constraint_standing_feet_cost(x_ref)
+            self.constraint_standing_feet_cost()
             self.constraint_swing_feet_cost(x_ref)
             self.target_cost(target)
 

python/quadruped_reactive_walking/WB_MPC_Wrapper.py

@@ -15,6 +15,8 @@ class Result:
     def __init__(self, pd):
         self.xs = list(np.zeros((pd.T + 1, pd.nx)))
         self.us = list(np.zeros((pd.T, pd.nu)))
+        self.acs = list(np.zeros((pd.T, pd.nv)))
+        self.fs = list(np.zeros((pd.T, pd.nu)))
         self.K = list(np.zeros([pd.T, pd.nu, pd.ndx]))
         self.solving_duration = 0.0
         self.new_result = False