From 9facdbc9f9810a1923b16316b5d99db3e852a14b Mon Sep 17 00:00:00 2001
From: Fanny Risbourg <frisbourg@laas.fr>
Date: Fri, 12 Aug 2022 11:48:43 +0200
Subject: [PATCH] create all costs
---
.../WB_MPC/CrocoddylOCP.py | 56 ++++++++++---------
.../WB_MPC/ProblemData.py | 2 +-
2 files changed, 31 insertions(+), 27 deletions(-)
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 0192d066..01ce59a1 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -91,10 +91,11 @@ class OCP:
self.initialized = True
def make_task(self, footstep):
- task = []
+ task = [[], []]
for foot in range(4):
if footstep[:, foot].any():
- task += [[self.pd.allContactIds[foot], footstep[:, foot]]]
+ task[0].append(self.pd.allContactIds[foot])
+ task[1].append(footstep[:, foot])
return task
def get_results(self):
@@ -138,24 +139,13 @@ class OCP:
return acc
def update_model(self, model, tasks, support_feet):
- if tasks is not None:
- for (id, pose) in tasks:
- name = self.pd.model.frames[id].name + "_foot_tracking"
- if name in model.differential.costs.active.tolist():
- model.differential.costs.costs[name].cost.residual.reference = pose
- else:
- residual = crocoddyl.ResidualModelFrameTranslation(
- self.state, id, pose, nu
- )
- cost = crocoddyl.CostModelResidual(self.state, residual)
- model.differential.costs.addCost(
- name, cost, self.pd.foot_tracking_w
- )
-
for i in self.pd.allContactIds:
name = self.pd.model.frames[i].name + "_contact"
model.differential.contacts.changeContactStatus(name, i in support_feet)
+ self.update_tracking_costs(model.differential.costs, tasks)
+
+
def create_model(self, support_feet=[], tasks=[], is_terminal=False):
"""
Create the action model
@@ -231,7 +221,7 @@ class OCP:
"""
nu = model.differential.actuation.nu
costs = model.differential.costs
- for i in support_feet:
+ for i in self.pd.allContactIds:
cone = crocoddyl.FrictionCone(self.pd.Rsurf, self.pd.mu, 4, False)
residual = crocoddyl.ResidualModelContactFrictionCone(
self.state, i, cone, nu
@@ -242,8 +232,18 @@ class OCP:
friction_cone = crocoddyl.CostModelResidual(
self.state, activation, residual
)
- friction_name = self.pd.model.frames[id].name + "_friction_cone"
+ friction_name = self.pd.model.frames[i].name + "_friction_cone"
costs.addCost(friction_name, friction_cone, self.pd.friction_cone_w)
+ costs.changeCostStatus(friction_name, i in support_feet)
+
+ name = self.pd.model.frames[i].name + "_foot_tracking"
+ residual = crocoddyl.ResidualModelFrameTranslation(
+ self.state, i, np.zeros(3), nu
+ )
+ foot_tracking = crocoddyl.CostModelResidual(self.state, residual)
+ costs.addCost(name, foot_tracking, self.pd.foot_tracking_w)
+
+ costs.changeCostStatus(name, False)
control_residual = crocoddyl.ResidualModelControl(self.state, self.pd.uref)
control_reg = crocoddyl.CostModelResidual(self.state, control_residual)
@@ -258,11 +258,15 @@ class OCP:
)
costs.addCost("control_bound", control_bound, self.pd.control_bound_w)
- if tasks is not None:
- for (id, pose) in tasks:
- name = self.pd.model.frames[id].name + "_foot_tracking"
- residual = crocoddyl.ResidualModelFrameTranslation(
- self.state, id, pose, nu
- )
- foot_tracking = crocoddyl.CostModelResidual(self.state, residual)
- costs.addCost(name, foot_tracking, self.pd.foot_tracking_w)
+ self.update_tracking_costs(costs, tasks)
+
+ def update_tracking_costs(self, costs, tasks):
+ for i in self.pd.allContactIds:
+ name = self.pd.model.frames[i].name + "_foot_tracking"
+ index = 0
+ if i in tasks[0]:
+ costs.changeCostStatus(name, True)
+ costs.costs[name].cost.residual.reference = tasks[1][index]
+ index += 1
+ else:
+ costs.changeCostStatus(name, False)
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 46e520d8..855e3d52 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -182,7 +182,7 @@ class ProblemDataFull(problemDataAbstract):
# Cost function weights
self.mu = 0.7
self.foot_tracking_w = 1e3
- # self.friction_cone_w = 1e3 * 0
+ self.friction_cone_w = 1e3 * 0
self.control_bound_w = 1e3
self.control_reg_w = 1e0
self.state_reg_w = np.array([1e-5] * 3 + [1e0] * 3)
--
GitLab