Skip to content
Snippets Groups Projects
Commit 85e1a253 authored by Ale's avatar Ale
Browse files

added force reg, imporved behaviour

parent c33a1c8e
No related branches found
No related tags found
No related merge requests found
Pipeline #21362 failed
Stage: test
Hide whitespace changes
Inline Side-by-side
config/walk_parameters.yaml
+ 6
− 6
View file @ 85e1a253
...@@ -11,7 +11,7 @@ robot: ...@@ -11,7 +11,7 @@ robot:
envID: 0 # Identifier of the environment to choose in which one the simulation will happen envID: 0 # Identifier of the environment to choose in which one the simulation will happen
use_flat_plane: true # If True the ground is flat, otherwise it has bumps use_flat_plane: true # If True the ground is flat, otherwise it has bumps
predefined_vel: true # If we are using a predefined reference velocity (True) or a joystick (False) predefined_vel: true # If we are using a predefined reference velocity (True) or a joystick (False)
N_SIMULATION: 10000 # Number of simulated wbc time steps N_SIMULATION: 750 # Number of simulated wbc time steps
enable_corba_viewer: false # Enable/disable Corba Viewer enable_corba_viewer: false # Enable/disable Corba Viewer
enable_multiprocessing: false # Enable/disable running the MPC in another process in parallel of the main loop enable_multiprocessing: false # Enable/disable running the MPC in another process in parallel of the main loop
perfect_estimator: true # Enable/disable perfect estimator by using data directly from PyBullet perfect_estimator: true # Enable/disable perfect estimator by using data directly from PyBullet
...@@ -24,9 +24,9 @@ robot: ...@@ -24,9 +24,9 @@ robot:
dt_wbc: 0.001 # Time step of the whole body control dt_wbc: 0.001 # Time step of the whole body control
dt_mpc: 0.015 # Time step of the model predictive control dt_mpc: 0.015 # Time step of the model predictive control
type_MPC: 3 # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs type_MPC: 3 # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
save_guess: true # true to interpolate the impedance quantities between nodes of the MPC save_guess: false # true to interpolate the impedance quantities between nodes of the MPC
movement: "circle" # name of the movement to perform movement: "circle" # name of the movement to perform
interpolate_mpc: true # true to interpolate the impedance quantities between nodes of the MPC interpolate_mpc: false # true to interpolate the impedance quantities between nodes of the MPC
interpolation_type: 3 # 0,1,2,3 decide which kind of interpolation is used interpolation_type: 3 # 0,1,2,3 decide which kind of interpolation is used
# Kp_main: [0.0, 0.0, 0.0] # Proportional gains for the PD+ # Kp_main: [0.0, 0.0, 0.0] # Proportional gains for the PD+
# Kd_main: [0., 0., 0.] # Derivative gains for the PD+ # Kd_main: [0., 0., 0.] # Derivative gains for the PD+
...@@ -37,9 +37,9 @@ robot: ...@@ -37,9 +37,9 @@ robot:
# Parameters of Gait # Parameters of Gait
N_periods: 1 N_periods: 1
gait: [20, 1, 0, 0, 1, gait: [20, 1, 1, 1, 1,
20, 0, 1, 1, 0, 10, 1, 0, 0, 1,
1, 1, 1, 1, 1] # Initial gait matrix 20, 1, 1, 1, 1] # Initial gait matrix
# Parameters of Joystick # Parameters of Joystick
gp_alpha_vel: 0.003 # Coefficient of the low pass filter applied to gamepad velocity gp_alpha_vel: 0.003 # Coefficient of the low pass filter applied to gamepad velocity
......
python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+ 68
− 41
View file @ 85e1a253
...@@ -13,7 +13,7 @@ from time import time ...@@ -13,7 +13,7 @@ from time import time
class OCP: class OCP:
def __init__(self, pd: ProblemData, tasks, gait): def __init__(self, pd: ProblemData, tasks, gait):
self.pd = pd self.pd = pd
self.max_iter = 100 self.max_iter = 1
self.state = crocoddyl.StateMultibody(self.pd.model) self.state = crocoddyl.StateMultibody(self.pd.model)
self.initialized = False self.initialized = False
...@@ -67,7 +67,7 @@ class OCP: ...@@ -67,7 +67,7 @@ class OCP:
t_warm_start = time() t_warm_start = time()
self.t_warm_start = t_warm_start - t_update 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) self.ddp.solve(xs, us, self.max_iter, False)
t_ddp = time() t_ddp = time()
...@@ -86,8 +86,9 @@ class OCP: ...@@ -86,8 +86,9 @@ class OCP:
pin.updateFramePlacements(self.pd.model, self.pd.rdata) pin.updateFramePlacements(self.pd.model, self.pd.rdata)
if self.initialized: if self.initialized:
self.update_command_costs( if self.pd.base_velocity_tracking_w > 0:
self.problem.runningModels[0].differential.costs, tasks) self.update_command_costs(
self.problem.runningModels[0].differential.costs, tasks)
self.problem.circularAppend( self.problem.circularAppend(
self.problem.runningModels[0], self.problem.runningModels[0],
...@@ -160,10 +161,6 @@ class OCP: ...@@ -160,10 +161,6 @@ class OCP:
self.make_running_model(model, support_feet, tasks) self.make_running_model(model, support_feet, tasks)
return model return model
if isTerminal:
self.make_terminal_model()
else:
self.make_running_model()
def create_standard_model(self, support_feet, switching): def create_standard_model(self, support_feet, switching):
""" """
...@@ -200,34 +197,36 @@ class OCP: ...@@ -200,34 +197,36 @@ class OCP:
np.array([0, 0, 1])) np.array([0, 0, 1]))
impactCost = crocoddyl.CostModelResidual( impactCost = crocoddyl.CostModelResidual(
self.state, impactAct, impactResidual) self.state, impactAct, impactResidual)
costs.addCost( if self.pd.impact_altitude_w > 0:
"%s_altitudeImpact" % self.pd.model.frames[i].name, costs.addCost(
impactCost, "%s_altitudeImpact" % self.pd.model.frames[i].name,
self.pd.impact_altitude_w / self.pd.dt, impactCost,
) self.pd.impact_altitude_w / self.pd.dt
)
impactVelResidual = crocoddyl.ResidualModelFrameVelocity( impactVelResidual = crocoddyl.ResidualModelFrameVelocity(
self.state, self.state,
i, i,
pin.Motion.Zero(), pin.Motion.Zero(),
pin.ReferenceFrame.LOCAL, pin.ReferenceFrame.WORLD,
actuation.nu, actuation.nu,
) )
if self.pd.impact_velocity_w > 0:
impactVelCost = crocoddyl.CostModelResidual( impactVelCost = crocoddyl.CostModelResidual(
self.state, impactVelResidual) self.state, impactVelResidual)
costs.addCost( costs.addCost(
"%s_velimpact" % self.pd.model.frames[i].name, "%s_velimpact" % self.pd.model.frames[i].name,
impactVelCost, impactVelCost,
self.pd.impact_velocity_w / self.pd.dt, self.pd.impact_velocity_w / self.pd.dt
) )
residual = crocoddyl.ResidualModelState(self.state, self.pd.xref, nu) residual = crocoddyl.ResidualModelState(self.state, self.pd.xref, nu)
activation = crocoddyl.ActivationModelWeightedQuad( activation = crocoddyl.ActivationModelWeightedQuad(
self.pd.state_reg_w**2) self.pd.state_reg_w**2)
state_cost = crocoddyl.CostModelResidual( state_cost = crocoddyl.CostModelResidual(
self.state, activation, residual) self.state, activation, residual)
costs.addCost("state_reg", state_cost, 1) if self.pd.state_reg_w.any() > 0:
costs.addCost("state_reg", state_cost, 1)
differential = crocoddyl.DifferentialActionModelContactFwdDynamics( differential = crocoddyl.DifferentialActionModelContactFwdDynamics(
self.state, actuation, contacts, costs, 0.0, True self.state, actuation, contacts, costs, 0.0, True
...@@ -248,7 +247,9 @@ class OCP: ...@@ -248,7 +247,9 @@ class OCP:
) )
state_cost = crocoddyl.CostModelResidual( state_cost = crocoddyl.CostModelResidual(
self.state, activation, residual) self.state, activation, residual)
model.differential.costs.addCost("terminal_velocity", state_cost, 1) if self.pd.terminal_velocity_w.any() > 0:
model.differential.costs.addCost(
"terminal_velocity", state_cost, 1)
def make_running_model(self, model, support_feet, tasks): def make_running_model(self, model, support_feet, tasks):
""" """
...@@ -273,9 +274,22 @@ class OCP: ...@@ -273,9 +274,22 @@ class OCP:
self.state, activation, residual self.state, activation, residual
) )
friction_name = self.pd.model.frames[i].name + "_friction_cone" friction_name = self.pd.model.frames[i].name + "_friction_cone"
costs.addCost(friction_name, friction_cone,
self.pd.friction_cone_w) if self.pd.friction_cone_w > 0:
costs.changeCostStatus(friction_name, i in support_feet) costs.addCost(friction_name, friction_cone,
self.pd.friction_cone_w)
costs.changeCostStatus(friction_name, i in support_feet)
name = "contact_force_tracking"
nc = len(model.differential.contacts.active.tolist())
ref_force = np.array([0, 0, self.pd.robot_weight / nc])
ref_Force = pin.Force(ref_force, ref_force * 0)
forceRegResidual = crocoddyl.ResidualModelContactForce(
self.state, i, ref_Force, 3, nu)
forceRegCost = crocoddyl.CostModelResidual(
self.state, forceRegResidual)
costs.addCost(
"%s_forceReg" % self.pd.model.frames[i].name, forceRegCost, self.pd.force_reg_w)
else: else:
groundColRes = crocoddyl.ResidualModelFrameTranslation( groundColRes = crocoddyl.ResidualModelFrameTranslation(
...@@ -293,22 +307,26 @@ class OCP: ...@@ -293,22 +307,26 @@ class OCP:
groundColBounds) groundColBounds)
groundColCost = crocoddyl.CostModelResidual( groundColCost = crocoddyl.CostModelResidual(
self.state, groundColAct, groundColRes) self.state, groundColAct, groundColRes)
costs.addCost(
"%s_groundCol" % self.pd.model.frames[i].name, if self.pd.ground_collision_w > 0:
groundColCost, costs.addCost(
self.pd.ground_collision_w, "%s_groundCol" % self.pd.model.frames[i].name,
) groundColCost,
self.pd.ground_collision_w,
)
flyHighResidual = sobec.ResidualModelFlyHigh( flyHighResidual = sobec.ResidualModelFlyHigh(
self.state, i, self.pd.fly_high_slope / 2.0, nu self.state, i, self.pd.fly_high_slope / 2.0, nu
) )
flyHighCost = crocoddyl.CostModelResidual( flyHighCost = crocoddyl.CostModelResidual(
self.state, flyHighResidual) self.state, flyHighResidual)
costs.addCost(
"%s_flyhigh" % self.pd.model.frames[i].name, if self.pd.fly_high_w > 0:
flyHighCost, costs.addCost(
self.pd.fly_high_w, "%s_flyhigh" % self.pd.model.frames[i].name,
) flyHighCost,
self.pd.fly_high_w,
)
name = "base_velocity_tracking" name = "base_velocity_tracking"
ref = pin.Motion(tasks[0], tasks[1]) ref = pin.Motion(tasks[0], tasks[1])
...@@ -316,12 +334,17 @@ class OCP: ...@@ -316,12 +334,17 @@ class OCP:
self.state, self.pd.baseId, ref, pin.LOCAL, nu) self.state, self.pd.baseId, ref, pin.LOCAL, nu)
base_velocity = crocoddyl.CostModelResidual( base_velocity = crocoddyl.CostModelResidual(
self.state, residual_base_velocity) self.state, residual_base_velocity)
costs.addCost(name, base_velocity, self.pd.base_velocity_tracking_w)
if self.pd.base_velocity_tracking_w > 0:
costs.addCost(name, base_velocity,
self.pd.base_velocity_tracking_w)
control_residual = crocoddyl.ResidualModelControl( control_residual = crocoddyl.ResidualModelControl(
self.state, self.pd.uref) self.state, self.pd.uref)
control_reg = crocoddyl.CostModelResidual(self.state, control_residual) control_reg = crocoddyl.CostModelResidual(self.state, control_residual)
costs.addCost("control_reg", control_reg, self.pd.control_reg_w)
if self.pd.control_reg_w > 0:
costs.addCost("control_reg", control_reg, self.pd.control_reg_w)
control_bound_residual = crocoddyl.ResidualModelControl(self.state, nu) control_bound_residual = crocoddyl.ResidualModelControl(self.state, nu)
control_bound_activation = crocoddyl.ActivationModelQuadraticBarrier( control_bound_activation = crocoddyl.ActivationModelQuadraticBarrier(
...@@ -331,9 +354,13 @@ class OCP: ...@@ -331,9 +354,13 @@ class OCP:
control_bound = crocoddyl.CostModelResidual( control_bound = crocoddyl.CostModelResidual(
self.state, control_bound_activation, control_bound_residual self.state, control_bound_activation, control_bound_residual
) )
costs.addCost("control_bound", control_bound, self.pd.control_bound_w)
self.update_command_costs(costs, tasks) if self.pd.control_bound_w > 0:
costs.addCost("control_bound", control_bound,
self.pd.control_bound_w)
if self.pd.base_velocity_tracking_w > 0:
self.update_command_costs(costs, tasks)
def update_command_costs(self, costs, tasks): def update_command_costs(self, costs, tasks):
name = "base_velocity_tracking" name = "base_velocity_tracking"
......
python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+ 5
− 3
View file @ 85e1a253
...@@ -28,6 +28,8 @@ class problemDataAbstract: ...@@ -28,6 +28,8 @@ class problemDataAbstract:
self.collision_model = self.robot.collision_model self.collision_model = self.robot.collision_model
self.visual_model = self.robot.visual_model self.visual_model = self.robot.visual_model
self.robot_weight = -sum([Y.mass for Y in self.model.inertias]) * self.model.gravity.linear[2]
self.frozen_names = frozen_names self.frozen_names = frozen_names
if frozen_names != []: if frozen_names != []:
self.frozen_idxs = [self.model.getJointId( self.frozen_idxs = [self.model.getJointId(
...@@ -49,7 +51,7 @@ class problemDataAbstract: ...@@ -49,7 +51,7 @@ class problemDataAbstract:
self.x0 = np.concatenate([self.q0, self.v0]) self.x0 = np.concatenate([self.q0, self.v0])
self.u0 = np.zeros(self.nu) self.u0 = np.zeros(self.nu)
self.baumgarte_gains = np.array([0, 100]) self.baumgarte_gains = np.array([0, 50])
self.xref = self.x0 self.xref = self.x0
self.uref = self.u0 self.uref = self.u0
...@@ -92,12 +94,12 @@ class ProblemData(problemDataAbstract): ...@@ -92,12 +94,12 @@ class ProblemData(problemDataAbstract):
# Cost function weights # Cost function weights
# Cost function weights # Cost function weights
ref_foot_flying_altitude = 7e-2 ref_foot_flying_altitude = 7e-2
self.fly_high_slope = 3 / ref_foot_flying_altitude self.fly_high_slope = 100 #3 / ref_foot_flying_altitude
self.mu = 0.7 self.mu = 0.7
self.base_velocity_tracking_w = 1e2 self.base_velocity_tracking_w = 1e2
self.ground_collision_w = 1e3 self.ground_collision_w = 1e3
self.fly_high_w = 1e1 self.fly_high_w = 1e3
self.friction_cone_w = 1e4 self.friction_cone_w = 1e4
self.impact_altitude_w= 1e3 self.impact_altitude_w= 1e3
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment