From 6c9e3d066f0a7f8f40777f986646d2cc653525d0 Mon Sep 17 00:00:00 2001
From: Fanny Risbourg <frisbourg@laas.fr>
Date: Fri, 12 Aug 2022 11:48:43 +0200
Subject: [PATCH] pick create all costs commit
---
.../WB_MPC/CrocoddylOCP.py | 56 ++++++++++---------
1 file changed, 30 insertions(+), 26 deletions(-)
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 571dde8e..c3df055d 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -92,10 +92,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):
@@ -139,24 +140,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
@@ -232,7 +222,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
@@ -243,8 +233,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)
@@ -259,11 +259,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)
--
GitLab