Skip to content
Snippets Groups Projects
Commit 9facdbc9 authored by Fanny Risbourg's avatar Fanny Risbourg
Browse files

create all costs

parent a68439bd
Branches costs_creation
No related tags found
No related merge requests found
Pipeline #21083 failed
......@@ -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)
......@@ -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)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment