diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 5f15386d796fbc31b6192417e8457c949bfc0e58..481fd70385e196e09c59545dab91a58d20525d2d 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/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)