working on walking (very stiff)

python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py

@@ -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

@@ -74,7 +74,7 @@ class ProblemData(problemDataAbstract):
         self.mu = 0.7
         self.friction_cone_w = 1e3
         #self.control_bound_w = 1e3
-        self.control_reg_w = 1e0 * 0
+        self.control_reg_w = 1e1
         self.state_reg_w = np.array([0] * 3 \
                             + [1e0] * 3 \
                             + [1e1] * 12 \