diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index efa158c674202783d52057c96f72b94cfe8cefab..31541d457efb9b9021fa9bf357c70f8af873bc9c 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -11,7 +11,7 @@ robot:
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
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: 5000 # Number of simulated wbc time steps
enable_corba_viewer: false # Enable/disable Corba Viewer
enable_multiprocessing: true # 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
@@ -22,11 +22,11 @@ robot:
# q_init: [0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407] #Â h_com = 0.218
q_init: [0.0, 0.7, -1.4, 0.0, 0.7, -1.4, 0.0, -0.7, 1.4, 0.0, -0.7, 1.4] # Initial articular positions
dt_wbc: 0.001 # Time step of the whole body control
- dt_mpc: 0.01 # 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
save_guess: false # true to interpolate the impedance quantities between nodes of the MPC
movement: "step" # name of the movement to perform
- interpolate_mpc: false # true to interpolate the impedance quantities between nodes of the MPC
+ 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
# Kp_main: [0.0, 0.0, 0.0] # Proportional gains for the PD+
# Kd_main: [0., 0., 0.] # Derivative gains for the PD+
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 83108329178d3bc0fbded04385842520ca1b2f0d..416524f17bf279df264ceeae4e370890582d6ecc 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -208,7 +208,7 @@ class Controller:
t_measures = time.time()
self.t_measures = t_measures - t_start
- self.target_footstep = self.target.compute(self.k + self.pd.T).copy()
+ self.target_footstep = self.target.compute(self.k + self.pd.T * self.pd.mpc_wbc_ratio)
if self.k % self.pd.mpc_wbc_ratio == 0:
if self.mpc_solved:
@@ -221,7 +221,7 @@ class Controller:
self.mpc.solve(
self.k,
self.mpc_result.xs[1],
- self.target_footstep,
+ self.target_footstep.copy(),
self.gait,
self.xs_init,
self.us_init,
@@ -230,7 +230,7 @@ class Controller:
self.mpc.solve(
self.k,
m["x_m"],
- self.target_footstep,
+ self.target_footstep.copy(),
self.gait,
self.xs_init,
self.us_init,
@@ -287,8 +287,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()
diff --git a/python/quadruped_reactive_walking/WB_MPC/BenchmarkCrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/BenchmarkCrocoddylOCP.py
index 16ea9c2086260ef56b2c224e54432a303a384cb7..c8423ab79ab970bc5e441439ff41622b7bb8cda8 100644
--- a/python/quadruped_reactive_walking/WB_MPC/BenchmarkCrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/BenchmarkCrocoddylOCP.py
@@ -17,7 +17,7 @@ MAXITER = 1
def createProblem():
params = qrw.Params()
pd = ProblemDataFull(params)
- target = Target(pd)
+ target = Target(params)
x0 = pd.x0_reduced
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 247c74a1e2a6e6e0ca3dcfee4175fd1b8835d5b4..63de4ecc821a991997b0cffd842ceeab1bbb3fe3 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -78,13 +78,20 @@ class OCP:
pin.updateFramePlacements(self.pd.model, self.pd.rdata)
if self.initialized:
- task = self.make_task(np.reshape(footstep, (3, 4), order="F"))
- support_feet = [self.pd.allContactIds[i] for i in np.nonzero(gait[-1] == 1)[0]]
-
- self.nodes[0].update_model(support_feet, task)
+ support_feet = [
+ self.pd.allContactIds[i] for i in np.nonzero(gait[-1] == 1)[0]
+ ]
+ update_model(
+ self.problem.runningModels[0],
+ "FR_FOOT_footTrack",
+ footstep[:, 1],
+ support_feet,
+ self.pd,
+ )
self.problem.circularAppend(
- self.nodes[0].model, self.nodes[0].model.createData()
+ self.problem.runningModels[0],
+ self.problem.runningModels[0].createData(),
)
self.problem.x0 = self.x0
@@ -139,6 +146,20 @@ class OCP:
return acc
+def update_model(model, name, footstep, support_feet, pd):
+ model.differential.costs.costs[name].cost.residual.reference = footstep
+
+ for i in pd.allContactIds:
+ if i in support_feet:
+ model.differential.contacts.changeContactStatus(
+ pd.model.frames[i].name + "_contact", True
+ )
+ else:
+ model.differential.contacts.changeContactStatus(
+ pd.model.frames[i].name + "_contact", False
+ )
+
+
class Node:
def __init__(
self, pd, state, supportFootIds=[], swingFootTask=[], isTerminal=False
@@ -169,16 +190,21 @@ class Node:
:param swingFootTask: swinging foot task
:return action model for a swing foot phase
"""
-
self.contactModel = crocoddyl.ContactModelMultiple(self.state, self.nu)
- for i in supportFootIds:
+ for i in self.pd.allContactIds:
supportContactModel = crocoddyl.ContactModel3D(
self.state, i, np.array([0.0, 0.0, 0.0]), self.nu, np.array([0.0, 0.0])
)
+
self.contactModel.addContact(
self.pd.model.frames[i].name + "_contact", supportContactModel
)
+ if i not in supportFootIds:
+ self.contactModel.changeContactStatus(
+ self.pd.model.frames[i].name + "_contact", False
+ )
+
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, self.nu)
@@ -200,17 +226,6 @@ class Node:
self.dmodel, self.control, self.pd.dt
)
- def update_contact_model(self, supportFootIds):
- self.remove_contacts()
- self.contactModel = crocoddyl.ContactModelMultiple(self.state, self.nu)
- for i in supportFootIds:
- supportContactModel = crocoddyl.ContactModel3D(
- self.state, i, np.array([0.0, 0.0, 0.0]), self.nu, np.array([0.0, 0.0])
- )
- self.dmodel.contacts.addContact(
- self.pd.model.frames[i].name + "_contact", supportContactModel
- )
-
def make_terminal_model(self):
self.isTerminal = True
stateResidual = crocoddyl.ResidualModelState(self.state, self.pd.xref, self.nu)
@@ -267,11 +282,6 @@ class Node:
if "terminalVelocity" in self.dmodel.costs.active.tolist():
self.dmodel.costs.removeCost("terminalVelocity")
- def remove_contacts(self):
- allContacts = self.dmodel.contacts.contacts.todict()
- for c in allContacts:
- self.dmodel.contacts.removeContact(c)
-
def tracking_cost(self, task):
if task is not None:
for (id, pose) in task:
@@ -293,8 +303,3 @@ class Node:
footTrack,
self.pd.foot_tracking_w,
)
-
- def update_model(self, support_feet=[], task=[]):
- self.update_contact_model(support_feet)
- if not self.isTerminal:
- self.tracking_cost(task)
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 1ec400f9cb1008f6247294e2ac8afe8c9b9ca075..46e520d871b6b7d4a0a3afaa35a92d45a1e79959 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -9,7 +9,7 @@ class problemDataAbstract:
self.dt_wbc = param.dt_wbc
self.mpc_wbc_ratio = int(self.dt / self.dt_wbc)
self.init_steps = 0
- self.target_steps = 150
+ self.target_steps = 100
self.T = self.init_steps + self.target_steps - 1
self.robot = erd.load("solo12")
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index 06a95e7e9b18a3ab47a18e728af595334eb13081..cbb447f4c934de0c13f38c8caa299042e4874ba0 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -100,10 +100,6 @@ class LoggerControl:
self.mocapOrientationQuat[self.i] = device.baseState[1]
# Controller timings: MPC time, ...
- if self.i < self.pd.T * self.pd.mpc_wbc_ratio:
- self.target[self.i] = controller.footsteps[self.i // self.pd.mpc_wbc_ratio][:, 1]
- else:
- self.target[self.i] = controller.target.compute(controller.k)[:, 1]
self.t_mpc[self.i] = controller.t_mpc
self.t_send[self.i] = controller.t_send
self.t_loop[self.i] = controller.t_loop
@@ -122,6 +118,13 @@ class LoggerControl:
self.t_loop[self.i] = controller.t_loop
self.t_ocp_ddp[self.i] = controller.mpc_result.solving_duration
+
+ if self.i == 0:
+ for i in range(self.pd.T * self.pd.mpc_wbc_ratio):
+ self.target[i] = controller.footsteps[i // self.pd.mpc_wbc_ratio][:, 1]
+ if self.i + self.pd.T * self.pd.mpc_wbc_ratio < self.log_size:
+ self.target[self.i + self.pd.T * self.pd.mpc_wbc_ratio] = controller.target_footstep[:, 1]
+
if not self.params.enable_multiprocessing:
self.t_ocp_update[self.i] = controller.mpc.ocp.t_update
self.t_ocp_warm_start[self.i] = controller.mpc.ocp.t_warm_start