diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index cb849cecdcb86bca4dede64780d75124542cfb55..2a1f0634d46dbdde87efbef3a31b6e553225d491 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -24,7 +24,7 @@ robot:
dt_wbc: 0.001 # Time step of the whole body 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
- save_guess: false # true to interpolate the impedance quantities between nodes of the MPC
+ save_guess: true # true to interpolate the impedance quantities between nodes of the MPC
movement: "circle" # name of the movement to perform
interpolate_mpc: true # 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
@@ -37,8 +37,7 @@ robot:
# Parameters of Gait
N_periods: 1
- gait: [8, 1, 0, 0, 1,
- 8, 0, 1, 1, 0] # Initial gait matrix
+ gait: [100, 1, 1, 1, 1] # Initial gait matrix
# Parameters of Joystick
gp_alpha_vel: 0.003 #Â Coefficient of the low pass filter applied to gamepad velocity
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index eaae8014c60848a95f6f945dd1f37fa2079ea759..1f6075d4c04bc066b563458bb26b011a7d9570f0 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -153,8 +153,7 @@ class Controller:
self.pd = pd
self.params = params
- self.gait = np.concatenate([np.repeat(np.array([1, 1, 1, 1]).reshape((1, 4)), self.pd.init_steps, axis=0),
- np.repeat(np.array([1, 0, 1, 1]).reshape((1, 4)), self.pd.target_steps - 1, axis=0)])
+ self.gait = params.gait
self.q_init = pd.q0
init_robot(q_init, params)
@@ -312,8 +311,8 @@ class Controller:
t_send = time.time()
self.t_send = t_send - t_mpc
- self.clamp_result(device)
- self.security_check(m)
+ # self.clamp_result(device)
+ # self.security_check(m)
if self.error:
self.set_null_control()
@@ -476,6 +475,10 @@ class Controller:
return oRh, hRb, oTh
+ def tuple_to_array(self, tup):
+ a = np.array([element for tupl in tup for element in tupl])
+ return a
+
def read_state(self, device):
qj_m = device.joints.positions
vj_m = device.joints.velocities
@@ -499,7 +502,6 @@ class Controller:
m["x_m"][self.pd.nq:] - self.mpc_result.xs[0][self.pd.nq:]
]
)
- # x_diff = self.mpc_result.xs[0] - m["x_m"]
tau = self.mpc_result.us[0] - np.dot(self.mpc_result.K[0], x_diff)
return tau
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index f29fadb0470c385203d8a31ef27505a3e69f99b6..cc90e24274d322b7f30dfb25887ea4d1d675fab8 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -30,7 +30,7 @@ class OCP:
def initialize_models(self, gait, footsteps):
models = []
- for t, step in enumerate(gait):
+ for t, step in enumerate(gait[:-1]):
tasks = self.make_task(footsteps[t])
support_feet = [self.pd.allContactIds[i] for i in np.nonzero(step == 1)[0]]
models.append(self.create_model(support_feet, tasks))
@@ -58,7 +58,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()
@@ -146,7 +146,7 @@ class OCP:
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)
+ self.update_tracking_costs(model.differential.costs, tasks, support_feet)
def create_model(self, support_feet=[], tasks=[], is_terminal=False):
@@ -175,10 +175,8 @@ class OCP:
:param support_feet: list of support feet ids
:return action model for a swing foot phase
"""
- if self.pd.useFixedBase == 0:
- actuation = crocoddyl.ActuationModelFloatingBase(self.state)
- else:
- actuation = crocoddyl.ActuationModelFull(self.state)
+
+ actuation = crocoddyl.ActuationModelFloatingBase(self.state)
nu = actuation.nu
control = crocoddyl.ControlParametrizationModelPolyZero(nu)
@@ -261,13 +259,13 @@ class OCP:
)
costs.addCost("control_bound", control_bound, self.pd.control_bound_w)
- self.update_tracking_costs(costs, tasks)
+ self.update_tracking_costs(costs, tasks, support_feet)
- def update_tracking_costs(self, costs, tasks):
+ def update_tracking_costs(self, costs, tasks, supportFootIds):
for i in self.pd.allContactIds:
name = self.pd.model.frames[i].name + "_foot_tracking"
index = 0
- if i in tasks[0]:
+ if i in tasks[0] and i not in supportFootIds:
costs.changeCostStatus(name, True)
costs.costs[name].cost.residual.reference = tasks[1][index]
index += 1
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index a26680a6d0cbd0ea7e48de32a0423c211cd271b5..2b4dea894d860b0c6a2cba68380995f760d48abb 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -8,8 +8,8 @@ class problemDataAbstract:
self.dt = param.dt_mpc # OCP dt
self.dt_wbc = param.dt_wbc
self.mpc_wbc_ratio = int(self.dt / self.dt_wbc)
- self.init_steps = 0
- self.target_steps = param.gait.shape[0]
+ # self.init_steps = 10
+ # self.target_steps = param.gait.shape[0]
self.T = param.gait.shape[0] - 1
self.robot = erd.load("solo12")
@@ -84,7 +84,7 @@ class ProblemData(problemDataAbstract):
def __init__(self, param):
super().__init__(param)
- self.useFixedBase = 1
+ self.useFixedBase = 0
# Cost function weights
# Cost function weights
@@ -103,8 +103,10 @@ class ProblemData(problemDataAbstract):
+ [1e-1] * 3
+ [1e1] * 6
)
- self.terminal_velocity_w = np.array([0] * 18 + [1e3] * 18)
- self.control_bound_w = 1e3
+ self.terminal_velocity_w = np.array(
+ [0] * self.nv + [1e3] * self.nv)
+ self.force_reg_w = 1e0
+
self.xref = self.x0
self.uref =self.u0