still working on walking

python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py

@@ -65,8 +65,8 @@ class CasadiOCP:
         self.opti.subject_to(ineq_constraints <= 0)
 
         p_opts = {"expand": False}
-        s_opts = {"tol": 1e-8,
-                  "acceptable_tol": 1e-8,
+        s_opts = {"tol": 1e-4,
+                  "acceptable_tol": 1e-4,
                   # "max_iter": 21,
                   # "compl_inf_tol": 1e-2,
                   # "constr_viol_tol": 1e-2
@@ -115,7 +115,7 @@ class CasadiOCP:
             # 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))
-                self.runningModels[t].constraint_landing_feet_eq(self.pd.xref)
+                eq.append(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,
@@ -126,7 +126,7 @@ class CasadiOCP:
             eq.append(self.runningModels[t].constraint_dynamics_eq())
             eq.append(self.runningModels[t].difference(self.xs[t + 1], xnext) - np.zeros(self.pd.ndx))
 
-            ineq.append(self.runningModels[t].constraint_swing_feet_ineq(self.pd.xref, k = self.pd.k)) 
+            #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

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -72,20 +72,19 @@ class ProblemData(problemDataAbstract):
         self.useFixedBase = 0
         # Cost function weights
         self.mu = 0.7
-        self.foot_tracking_w = 1e2
         self.friction_cone_w = 1e3
-        self.control_bound_w = 1e3
-        self.control_reg_w = 1e1
+        #self.control_bound_w = 1e3
+        self.control_reg_w = 1e0 * 0
         self.state_reg_w = np.array([0] * 3 \
-                            + [1e1] * 3 \
-                            + [1e0] * 12 \
+                            + [1e0] * 3 \
+                            + [1e1] * 12 \
                             + [0] * 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])
+                            + [1e0] * 12 ) 
+        #self.terminal_velocity_w = np.array([0] * 18 + [1e3] * 18 )
+        #self.control_bound_w = 1e3
+        self.k = np.array([10,10])
+        self.linear_velocity_w = np.array([10, 10, 10])
+        self.angular_velocity_w = 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

@@ -172,10 +172,10 @@ class ShootingNode():
             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] ) """
+            """ ineq.append( fw[0] -  self.pd.mu*fw[2] )
+            ineq.append( -fw[0] - self.pd.mu*fw[2] )
+            ineq.append(  fw[1] - self.pd.mu*fw[2] )
+            ineq.append( -fw[1] - self.pd.mu*fw[2] ) """
 
         return (casadi.vertcat(*ineq))
 
@@ -207,7 +207,9 @@ class ShootingNode():
         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.vfeet[sw_foot](self.x)[0:2] - k[0]* self.feet[sw_foot](self.x)[2])
+            ineq.append(self.vfeet[sw_foot](self.x)[0:2] - k* self.feet[sw_foot](self.x)[2])
+            #ineq.append(self.vfeet[sw_foot](self.x)[1] - k[1]* self.feet[sw_foot](self.x)[2])
+
         return(casadi.vertcat(*ineq))
 
     def force_reg_cost(self):
@@ -227,9 +229,9 @@ class ShootingNode():
             self.cost += 1/2 * \
                 casadi.sumsqr(self.pd.state_reg_w *
                               self.difference(self.x, x_ref))
-            self.cost += 1/2 * \
-                casadi.sumsqr(self.pd.terminal_velocity_w *
-                              self.difference(self.x, x_ref))
+            #self.cost += 1/2 * \
+            #    casadi.sumsqr(self.pd.terminal_velocity_w *
+            #                  self.difference(self.x, x_ref))
         else:
             self.cost += 1/2 * \
                 casadi.sumsqr(self.pd.state_reg_w *
@@ -238,8 +240,8 @@ class ShootingNode():
     def target_cost(self, target):
         # I am Assuming just FR FOOt to be free
         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
+            self.cost += casadi.sumsqr(self.pd.linear_velocity_w*(self.baseVelocityLin(self.x) - target[obj]["linear_velocity"])) * self.pd.dt
+            self.cost += casadi.sumsqr(self.pd.angular_velocity_w*(self.baseVelocityAng(self.x) - target[obj]["angular_velocity"])) * self.pd.dt
 
     def compute_cost(self, x_ref, u_ref, target):
         self.cost = 0

python/quadruped_reactive_walking/WB_MPC/Target.py

@@ -8,15 +8,15 @@ class Target:
         self.dt = pd.dt
 
         self.gait = [] \
-            + [ [ 0,1,1,0 ] ] * pd.t_step \
-            + [ [ 1,0,0,1 ] ] * pd.t_step
+            + [ [ 1,0,0,1 ] ] * pd.t_step \
+            + [ [ 0,1,1,0 ] ] * 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.baseId: {"linear_velocity" :[1,0,0], "angular_velocity": [0,0,0]}}
+        self.target = {pd.baseId: {"linear_velocity" :[2,0,0], "angular_velocity": [0,0,0]}}
 
 
     def patternToId(self, gait):

python/quadruped_reactive_walking/main_solo12_control.py

@@ -7,6 +7,7 @@ import quadruped_reactive_walking as qrw
 from .Controller import Controller
 from .tools.LoggerControl import LoggerControl
 from .WB_MPC.ProblemData import ProblemData, ProblemDataFull
+from pinocchio.visualize import GepettoVisualizer
 from .WB_MPC.Target import Target
 
 params = qrw.Params()  # Object that holds all controller parameters
@@ -21,6 +22,15 @@ else:
     from .tools.qualisysClient import QualisysClient
 
 
+try:
+    viz = GepettoVisualizer(pd.model,pd.collision_model,  pd.visual_model)
+    viz.initViewer()
+    viz.loadViewerModel()
+    gv = viz.viewer.gui
+except:
+    print("No viewer"  )
+
+
 
 def get_input():
     """