fixed contact switch

python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py

@@ -11,7 +11,7 @@ from time import time
 class OCP:
     def __init__(self, pd: ProblemData, footsteps, gait):
         self.pd = pd
-        self.max_iter = 1
+        self.max_iter = 1000
 
         self.state = crocoddyl.StateMultibody(self.pd.model)
         self.initialized = False
@@ -21,7 +21,7 @@ class OCP:
 
         self.initialize_models(gait, footsteps)
 
-        self.x0 = self.pd.x0_reduced
+        self.x0 = self.pd.x0
 
         self.problem = crocoddyl.ShootingProblem(
             self.x0, self.models, self.terminal_model
@@ -59,7 +59,7 @@ class OCP:
         t_warm_start = time()
         self.t_warm_start = t_warm_start - t_update
 
-        # self.ddp.setCallbacks([crocoddyl.CallbackVerbose()])
+        self.ddp.setCallbacks([crocoddyl.CallbackVerbose()])
         self.ddp.solve(xs, us, self.max_iter, False)
 
         t_ddp = time()
@@ -196,17 +196,6 @@ class Node:
             self.dmodel, self.control, self.pd.dt
         )
 
-    def update_contact_model(self, supportFootIds):
-        self.remove_contacts()
-        self.contactModel = crocoddyl.ContactModelMultiple(self.state, self.nu)
-        for i in supportFootIds:
-            supportContactModel = crocoddyl.ContactModel3D(
-                self.state, i, np.array([0.0, 0.0, 0.0]), self.nu, np.array([0.0, 0.0])
-            )
-            self.dmodel.contacts.addContact(
-                self.pd.model.frames[i].name + "_contact", supportContactModel
-            )
-
     def make_terminal_model(self):
         self.isTerminal = True
         stateResidual = crocoddyl.ResidualModelState(self.state, self.pd.xref, self.nu)
@@ -219,6 +208,39 @@ class Node:
         self.costModel.addCost("terminalVelocity", stateReg, 1)
 
     def make_running_model(self, supportFootIds, swingFootTask):
+        self.add_friction_cone(supportFootIds)
+
+        ctrlResidual = crocoddyl.ResidualModelControl(self.state, self.pd.uref)
+        ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual)
+        self.costModel.addCost("ctrlReg", ctrlReg, self.pd.control_reg_w)
+
+        ctrl_bound_residual = crocoddyl.ResidualModelControl(self.state, self.nu)
+        ctrl_bound_activation = crocoddyl.ActivationModelQuadraticBarrier(
+            crocoddyl.ActivationBounds(-self.pd.effort_limit, self.pd.effort_limit)
+        )
+        ctrl_bound = crocoddyl.CostModelResidual(
+            self.state, ctrl_bound_activation, ctrl_bound_residual
+        )
+        self.costModel.addCost("ctrlBound", ctrl_bound, self.pd.control_bound_w)
+
+        self.tracking_cost(swingFootTask, supportFootIds)
+
+    def update_contact_model(self, supportFootIds):
+        self.remove_contacts()
+        self.remove_friction_cone()
+
+        self.contactModel = crocoddyl.ContactModelMultiple(self.state, self.nu)
+        for i in supportFootIds:
+            supportContactModel = crocoddyl.ContactModel3D(
+                self.state, i, np.array([0.0, 0.0, 0.0]), self.nu, np.array([0.0, 0.0])
+            )
+            self.dmodel.contacts.addContact(
+                self.pd.model.frames[i].name + "_contact", supportContactModel
+            )
+
+        self.add_friction_cone(supportFootIds)
+
+    def add_friction_cone(self, supportFootIds):
         for i in supportFootIds:
             cone = crocoddyl.FrictionCone(self.pd.Rsurf, self.pd.mu, 4, False)
             coneResidual = crocoddyl.ResidualModelContactFrictionCone(
@@ -235,21 +257,29 @@ class Node:
                 frictionCone,
                 self.pd.friction_cone_w,
             )
-
-        ctrlResidual = crocoddyl.ResidualModelControl(self.state, self.pd.uref)
-        ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual)
-        self.costModel.addCost("ctrlReg", ctrlReg, self.pd.control_reg_w)
-
-        ctrl_bound_residual = crocoddyl.ResidualModelControl(self.state, self.nu)
-        ctrl_bound_activation = crocoddyl.ActivationModelQuadraticBarrier(
-            crocoddyl.ActivationBounds(-self.pd.effort_limit, self.pd.effort_limit)
-        )
-        ctrl_bound = crocoddyl.CostModelResidual(
-            self.state, ctrl_bound_activation, ctrl_bound_residual
-        )
-        self.costModel.addCost("ctrlBound", ctrl_bound, self.pd.control_bound_w)
-
-        self.tracking_cost(swingFootTask)
+    
+    def tracking_cost(self, task, supportFootIds):
+        if task is not None:
+            for (id, pose) in task:
+                if id not in supportFootIds:
+                    frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
+                        self.state, id, pose, self.nu
+                    )
+                    footTrack = crocoddyl.CostModelResidual(
+                        self.state, frameTranslationResidual
+                    )
+                    if (
+                        self.pd.model.frames[id].name + "_footTrack"
+                        in self.dmodel.costs.active.tolist()
+                    ):
+                        self.dmodel.costs.removeCost(
+                            self.pd.model.frames[id].name + "_footTrack"
+                        )
+                    self.costModel.addCost(
+                        self.pd.model.frames[id].name + "_footTrack",
+                        footTrack,
+                        self.pd.foot_tracking_w,
+                    )
 
     def remove_running_costs(self):
         runningCosts = self.dmodel.costs.active.tolist()
@@ -268,29 +298,13 @@ class Node:
         for c in allContacts:
             self.dmodel.contacts.removeContact(c)
 
-    def tracking_cost(self, task):
-        if task is not None:
-            for (id, pose) in task:
-                frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
-                    self.state, id, pose, self.nu
-                )
-                footTrack = crocoddyl.CostModelResidual(
-                    self.state, frameTranslationResidual
-                )
-                if (
-                    self.pd.model.frames[id].name + "_footTrack"
-                    in self.dmodel.costs.active.tolist()
-                ):
-                    self.dmodel.costs.removeCost(
-                        self.pd.model.frames[id].name + "_footTrack"
-                    )
-                self.costModel.addCost(
-                    self.pd.model.frames[id].name + "_footTrack",
-                    footTrack,
-                    self.pd.foot_tracking_w,
-                )
+    #TODO Remove just the useless friction cone instead of all
+    def remove_friction_cone(self):
+        fc_names = [c for c in self.dmodel.costs.active.tolist() if "frictionCone" in c]
+        for c in fc_names:
+            self.dmodel.costs.removeCost(c)
 
     def update_model(self, support_feet=[], task=[]):
         self.update_contact_model(support_feet)
         if not self.isTerminal:
-            self.tracking_cost(task)
+            self.tracking_cost(task, support_feet)