added baumgarte gains

python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py

@@ -185,7 +185,7 @@ class OCP:
         for i in self.pd.allContactIds:
             name = self.pd.model.frames[i].name + "_contact"
             contact = crocoddyl.ContactModel3D(
-                self.state, i, np.zeros(3), nu, np.zeros(2)
+                self.state, i, np.zeros(3), nu, self.pd.baumgarte_gains
             )
             contacts.addContact(name, contact)
             if i not in support_feet:

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -49,6 +49,8 @@ class problemDataAbstract:
         self.x0 = np.concatenate([self.q0, self.v0])
         self.u0 = np.zeros(self.nu)
 
+        self.baumgarte_gains = np.array([0, 50])
+
         self.xref = self.x0
         self.uref = self.u0