working on walking

it's floating in the air

python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py

@@ -58,10 +58,11 @@ class CasadiOCP:
                 self.datas += [NodeData()]
 
         self.make_opti_variables(x0)
-        self.cost, eq_constraints = self.make_ocp()
+        self.cost, eq_constraints, ineq_constraints = self.make_ocp()
 
         self.opti.minimize(self.cost)
         self.opti.subject_to(eq_constraints == 0)
+        self.opti.subject_to(ineq_constraints <= 0)
 
         p_opts = {"expand": False}
         s_opts = {"tol": 1e-8,
@@ -104,6 +105,7 @@ class CasadiOCP:
     def make_ocp(self):
         totalcost = 0
         eq = []
+        ineq = []
 
         eq.append(self.dxs[0])
         for t in range(self.pd.T):
@@ -111,25 +113,35 @@ class CasadiOCP:
             self.runningModels[t].init(self.datas[t], self.xs[t], self.acs[t], self.us[t], self.fs[t], False)
 
             # If it is landing
-            """ if (self.target.contactSequence[t] != self.target.contactSequence[t-1] and t >= 1):
-                print('Contact on ', str(self.runningModels[t].contactIds)) """
+            if (self.target.contactSequence[t] != self.target.contactSequence[t-1] and t >= 1):
+                print('Contact on ', str(self.runningModels[t].contactIds))
+                self.runningModels[t].constraint_landing_feet_eq(self.pd.xref)
 
             xnext, rcost = self.runningModels[t].calc(x_ref=self.pd.xref,
                                                       u_ref=self.pd.uref,
-                                                      target=self.target.evaluate_in_t(t))
+                                                      target=self.target.target)
 
             # 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))
 
-            totalcost += rcost
-        eq_constraints = casadi.vertcat(*eq)
+            ineq.append(self.runningModels[t].constraint_swing_feet_ineq(self.pd.xref, k = self.pd.k)) 
+            ineq.append(self.runningModels[t].constraint_standing_feet_ineq())
 
+            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)
-        totalcost += self.terminalModel.calc(x_ref=self.pd.xref)[1]
+        _, rcost = self.terminalModel.calc(x_ref=self.pd.xref)
+        #totalcost += self.terminalModel.calc(x_ref=self.pd.xref)[1]
+        totalcost += rcost
+
+        eq_constraints = casadi.vertcat(*eq)
+        ineq_constraints = casadi.vertcat(*ineq)
+        
 
-        return totalcost, eq_constraints
+        return totalcost, eq_constraints, ineq_constraints
 
     def warmstart(self, x0, guess=None):
         for g in guess:

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -9,9 +9,9 @@ class problemDataAbstract:
         self.dt_bldc = 0.0005
         self.r1 = int(self.dt / self.dt_sim)
         self.r2 = int(self.dt_sim / self.dt_bldc)
-        self.init_steps = 10 # full stand phase
-        self.target_steps = 30 # manipulation steps
-        self.T = self.init_steps + self.target_steps -1
+        self.t_step = 12 # timestep for each contact phase
+        self.repetitions = 3
+        self.T = (2*self.t_step) * self.repetitions -1
 
         self.robot = erd.load("solo12")
         self.q0 = self.robot.q0
@@ -50,6 +50,7 @@ class problemDataAbstract:
         self.rfFootId = self.model.getFrameId(self.rfFoot)
         self.lhFootId = self.model.getFrameId(self.lhFoot)
         self.rhFootId = self.model.getFrameId(self.rhFoot)
+        self.baseId = self.model.getFrameId("base_link")
 
         self.Rsurf = np.eye(3)
 
@@ -77,15 +78,14 @@ class ProblemData(problemDataAbstract):
         self.control_reg_w = 1e1
         self.state_reg_w = np.array([0] * 3 \
                             + [1e1] * 3 \
-                            + [1e0] * 3 \
-                            + [1e-3] * 3\
-                            + [1e0] * 6
+                            + [1e0] * 12 \
                             + [0] * 6 \
-                            + [1e1] * 3 \
-                            + [1e-1] * 3\
-                            + [1e1] * 6 ) 
+                            + [1e1] * 12 ) 
         self.terminal_velocity_w = np.array([0] * 18 + [1e3] * 18 )
         self.control_bound_w = 1e3
+        self.k = np.array([2,2])
+        self.linear_velocity_weight = np.array([10, 10, 10])
+        self.angular_velocity_weight = np.array([10, 10, 10])
 
         self.x0 = np.array([ 0.0, 0.0, 0.2607495, 0, 0, 0, 1,
                              0,  0.7, -1.4,  

python/quadruped_reactive_walking/WB_MPC/ShootingNode.py

@@ -164,6 +164,21 @@ class ShootingNode():
 
         return(casadi.vertcat(*eq))
 
+    def constraint_standing_feet_ineq(self):
+        # Friction cone
+        ineq = []
+        for i, stFoot in enumerate(self.contactIds):
+            R = self.Rfeet[stFoot](self.x)
+            f_ = self.fs[i]
+            fw = R @ f_
+            ineq.append(-fw[2])
+            """ ineq.append( fw[0] - conf.mu*fw[2] )
+            ineq.append( -fw[0] - conf.mu*fw[2] )
+            ineq.append(  fw[1] - conf.mu*fw[2] )
+            ineq.append( -fw[1] - conf.mu*fw[2] ) """
+
+        return (casadi.vertcat(*ineq))
+
     def constraint_standing_feet_cost(self):
         # Friction cone
         for i, stFoot in enumerate(self.contactIds):
@@ -188,11 +203,11 @@ class ShootingNode():
         eq.append(self.acc(self.x, self.tau, *self.fs) - self.a)
         return(casadi.vertcat(*eq))
 
-    def constraint_swing_feet_ineq(self, x_ref):
+    def constraint_swing_feet_ineq(self, x_ref, k):
         ineq = []
         for sw_foot in self.freeIds:
-            ineq.append(-self.feet[sw_foot](self.x)
-                        [2] + self.feet[sw_foot](x_ref)[2])
+            ineq.append(-self.feet[sw_foot](self.x)[2] + self.feet[sw_foot](x_ref)[2])
+            ineq.append(self.vfeet[sw_foot](self.x)[0:2] - k[0]* self.feet[sw_foot](self.x)[2])
         return(casadi.vertcat(*ineq))
 
     def force_reg_cost(self):
@@ -222,16 +237,15 @@ class ShootingNode():
 
     def target_cost(self, target):
         # I am Assuming just FR FOOt to be free
-        for sw_foot in self.freeIds:
-
-            self.cost += 1/2 * self.pd.foot_tracking_w * casadi.sumsqr(self.feet[sw_foot](self.x) - target[sw_foot]) * self.pd.dt
+        for obj in target:
+            self.cost += casadi.sumsqr(self.pd.linear_velocity_weight*(self.baseVelocityLin(self.x) - target[obj]["linear_velocity"])) * self.pd.dt
+            self.cost += casadi.sumsqr(self.pd.angular_velocity_weight*(self.baseVelocityAng(self.x) - target[obj]["angular_velocity"])) * self.pd.dt
 
     def compute_cost(self, x_ref, u_ref, target):
         self.cost = 0
         if not self.isTerminal:
-            self.constraint_standing_feet_cost()
             self.control_cost(u_ref)
-            self.constraint_control_cost()
+            #self.constraint_control_cost()
             self.target_cost(target)
 
         self.body_reg_cost(x_ref=x_ref)

python/quadruped_reactive_walking/WB_MPC/Target.py

@@ -7,45 +7,28 @@ class Target:
         self.pd = pd
         self.dt = pd.dt
 
-        if pd.useFixedBase == 0:
-            self.gait = [] \
-                + [ [ 1,1,1,1 ] ] * pd.init_steps \
-                + [ [ 1,0,1,1 ] ] * pd.target_steps
-        elif pd.useFixedBase == 1:
-            self.gait = [] \
-                + [ [ 0,0,0,0 ] ] * pd.init_steps \
-                + [ [ 0,0,0,0 ] ] * pd.target_steps
-        else:
-            print("'useFixedBase' can be either 0 or 1 !")
+        self.gait = [] \
+            + [ [ 0,1,1,0 ] ] * pd.t_step \
+            + [ [ 1,0,0,1 ] ] * pd.t_step
+
+        self.gait = self.gait * pd.repetitions
 
         self.T = pd.T
         self.contactSequence = [ self.patternToId(p) for p in self.gait]
 
-        self.target = {pd.rfFootId: []}
-        q = pd.x0[: pd.nq]
-        v = pd.x0[pd.nq :]
-        pin.forwardKinematics(pd.model, pd.rdata, q, v)
-        pin.updateFramePlacements(pd.model, pd.rdata)
-        self.FR_foot0 = pd.rdata.oMf[pd.rfFootId].translation.copy()
-        self.A = np.array([0, 0.05, 0.05])
-        self.offset = np.array([0.05, 0., 0.06])
-        self.freq = np.array([0, 2.5, 2.5])
-        self.phase = np.array([0, 0, np.pi/2])
+        self.target = {pd.baseId: {"linear_velocity" :[1,0,0], "angular_velocity": [0,0,0]}}
+
 
     def patternToId(self, gait):
             return tuple(self.pd.allContactIds[i] for i,c in enumerate(gait) if c==1 )
         
     def shift_gait(self):
-        self.gait.pop(0)
-        self.gait += [self.gait[-1]]
+        g0 = self.gait.pop(0)
+        self.gait += [g0]
         self.contactSequence = [ self.patternToId(p) for p in self.gait]    
 
     def update(self, t):
-        target = []
-        for n in range(self.T+1):
-            target += [self.FR_foot0 + self.offset + self.A *
-                       np.sin(2*np.pi*self.freq * (n+t) * self.dt + self.phase)]
-        self.target[self.pd.rfFootId] = np.array(target)
+        pass
     
     def evaluate_in_t(self, t):
-        return {e: self.target[e][t] for e in self.target}
+        pass
\ No newline at end of file