diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index ebe0cb2d486478ec6689b7faf4bd8533b596792b..75ea685ee49e552c67f26caf06ad4cf2c5bc9f40 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -13,7 +13,7 @@ robot:
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
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
+ 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
# General control parameters
@@ -37,8 +37,9 @@ robot:
# Parameters of Gait
N_periods: 1
- gait: [10, 1, 1, 1, 1,
- 20, 1, 0, 1, 1] # Initial gait matrix
+ gait: [20, 1, 0, 0, 1,
+ 20, 0, 1, 1, 0,
+ 1, 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 9aeaff2027f6273857be1bb52f59581a0f68f321..2d06dd9f8839f1955ef9092a6a02770ec84f847e 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -171,13 +171,9 @@ class Controller:
self.result.FF = self.params.Kff_main * np.ones(12)
self.target = Target(params)
- self.footsteps = []
- for k in range(self.pd.T * self.pd.mpc_wbc_ratio):
- self.target_footstep = self.target.compute(k).copy()
- if k % self.pd.mpc_wbc_ratio == 0:
- self.footsteps.append(self.target_footstep.copy())
+ self.velocity_task = self.target.velocity_task
- self.mpc = WB_MPC_Wrapper.MPC_Wrapper(pd, params, self.footsteps, self.gait)
+ self.mpc = WB_MPC_Wrapper.MPC_Wrapper(pd, params, self.velocity_task, self.gait)
self.mpc_solved = False
self.k_result = 0
self.k_solve = 0
@@ -216,10 +212,6 @@ class Controller:
t_measures = time.time()
self.t_measures = t_measures - t_start
- 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:
self.k_solve = self.k
@@ -229,7 +221,7 @@ class Controller:
self.mpc.solve(
self.k,
m["x_m"],
- self.target_footstep.copy(),
+ self.velocity_task.copy(),
self.gait,
self.xs_init,
self.us_init,
@@ -252,6 +244,7 @@ class Controller:
# self.xs_init,
# self.us_init,
# )
+ self.gait = np.roll(self.gait, -1, axis=0)
except ValueError:
self.error = True
print("MPC Problem")
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 82e3a683a8854ea1dd52c220a3457f55ec13483f..ec8a94cd68d5fc9f670eeaf1f8d21972440953e2 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -10,9 +10,10 @@ from time import time
class OCP:
- def __init__(self, pd: ProblemData, footsteps, gait):
+ def __init__(self, pd: ProblemData, tasks, gait):
self.pd = pd
- self.max_iter = 3
+ self.max_iter = 100
+
self.state = crocoddyl.StateMultibody(self.pd.model)
self.initialized = False
@@ -20,7 +21,7 @@ class OCP:
self.t_update_last_model = 0.0
self.t_shift = 0.0
- rm, tm = self.initialize_models(gait, footsteps)
+ rm, tm = self.initialize_models(gait, tasks)
self.x0 = self.pd.x0
@@ -28,22 +29,25 @@ class OCP:
self.x0, rm, tm)
self.ddp = crocoddyl.SolverFDDP(self.problem)
- def initialize_models(self, gait, footsteps):
+ def initialize_models(self, gait, tasks):
models = []
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))
+ prev_support_feet = [self.pd.allContactIds[i] for i in np.nonzero(gait[t-1] == 1)[0]]
+ switching = True if t==0 or support_feet != prev_support_feet else False
+ models.append(self.create_model(support_feet, tasks, switching))
support_feet = [self.pd.allContactIds[i] for i in np.nonzero(gait[-1] == 1)[0]]
- terminal_model = self.create_model(support_feet, is_terminal=True)
+ prev_support_feet = [self.pd.allContactIds[i] for i in np.nonzero(gait[self.pd.T] == 1)[0]]
+ switching = True if support_feet != prev_support_feet else False
+ terminal_model = self.create_model(support_feet, switching=switching, is_terminal=True)
return models, terminal_model
- def solve(self, x0, footstep, gait, xs_init=None, us_init=None):
+ def solve(self, x0, tasks, gait, xs_init=None, us_init=None):
t_start = time()
self.x0 = x0
- self.make_ocp(footstep, gait)
+ self.make_ocp(tasks, gait)
t_update = time()
self.t_update = t_update - t_start
@@ -66,7 +70,7 @@ class OCP:
self.t_solve = time() - t_start
- def make_ocp(self, footstep, gait):
+ def make_ocp(self, tasks, gait):
"""
Create a shooting problem for a simple walking gait.
@@ -77,11 +81,7 @@ class OCP:
pin.updateFramePlacements(self.pd.model, self.pd.rdata)
if self.initialized:
- tasks = self.make_task(footstep)
- support_feet = [
- self.pd.allContactIds[i] for i in np.nonzero(gait[-1] == 1)[0]
- ]
- self.update_model(self.problem.runningModels[0], tasks, support_feet)
+ self.update_command_costs(self.problem.runningModels[0].differential.costs, tasks)
self.problem.circularAppend(
self.problem.runningModels[0],
@@ -92,14 +92,6 @@ class OCP:
self.initialized = True
- def make_task(self, footstep):
- task = [[], []]
- for foot in range(4):
- if footstep[:, foot].any():
- task[0].append(self.pd.allContactIds[foot])
- task[1].append(footstep[:, foot])
- return task
-
def get_results(self):
return (
self.ddp.xs.tolist().copy(),
@@ -141,15 +133,7 @@ class OCP:
for m in self.ddp.problem.runningDatas]
return acc
- def update_model(self, model, tasks, support_feet):
- 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, support_feet)
-
-
- def create_model(self, support_feet=[], tasks=[], is_terminal=False):
+ def create_model(self, support_feet=[], tasks=[], switching=False, is_terminal=False):
"""
Create the action model
@@ -159,7 +143,7 @@ class OCP:
:param isTterminal: true for the terminal node
:return action model for a swing foot phase
"""
- model = self.create_standard_model(support_feet)
+ model = self.create_standard_model(support_feet, switching)
if is_terminal:
self.make_terminal_model(model)
else:
@@ -167,7 +151,7 @@ class OCP:
return model
- def create_standard_model(self, support_feet):
+ def create_standard_model(self, support_feet, switching):
"""
Create a standard action model
@@ -178,20 +162,51 @@ class OCP:
actuation = crocoddyl.ActuationModelFloatingBase(self.state)
nu = actuation.nu
+ costs = crocoddyl.CostModelSum(self.state, nu)
control = crocoddyl.ControlParametrizationModelPolyZero(nu)
contacts = crocoddyl.ContactModelMultiple(self.state, nu)
- for i in self.pd.allContactIds:
+ for i in support_feet:
+ pin.forwardKinematics(self.pd.model, self.pd.rdata, self.pd.q0)
+ pin.updateFramePlacements(self.pd.model, self.pd.rdata)
+ start_pos = self.pd.rdata.oMf[i].translation
+
+ # Basic contact
name = self.pd.model.frames[i].name + "_contact"
contact = crocoddyl.ContactModel3D(
- self.state, i, np.zeros(3), nu, self.pd.baumgarte_gains
+ self.state, i, start_pos, nu, self.pd.baumgarte_gains
)
contacts.addContact(name, contact)
if i not in support_feet:
contacts.changeContactStatus(name, False)
- costs = crocoddyl.CostModelSum(self.state, nu)
+ # In case of landing feet land at right height
+ if switching:
+ impactResidual = crocoddyl.ResidualModelFrameTranslation(self.state, i, start_pos, actuation.nu)
+ impactAct = crocoddyl.ActivationModelWeightedQuad(np.array([0, 0, 1]))
+ impactCost = crocoddyl.CostModelResidual(self.state, impactAct, impactResidual)
+ costs.addCost(
+ "%s_altitudeimpact" % self.pd.model.frames[i].name,
+ impactCost,
+ self.pd.impact_altitude_weight / self.pd.dt,
+ )
+
+ impactVelResidual = crocoddyl.ResidualModelFrameVelocity(
+ self.state,
+ i,
+ pin.Motion.Zero(),
+ pin.ReferenceFrame.LOCAL,
+ actuation.nu,
+ )
+
+ impactVelCost = crocoddyl.CostModelResidual(self.state, impactVelResidual)
+ costs.addCost(
+ "%s_velimpact" % self.pd.model.frames[i].name,
+ impactVelCost,
+ self.pd.impact_velocity_weight / self.pd.dt,
+ )
+
residual = crocoddyl.ResidualModelState(self.state, self.pd.xref, nu)
activation = crocoddyl.ActivationModelWeightedQuad(self.pd.state_reg_w**2)
state_cost = crocoddyl.CostModelResidual(self.state, activation, residual)
@@ -222,7 +237,7 @@ class OCP:
"""
nu = model.differential.actuation.nu
costs = model.differential.costs
- for i in self.pd.allContactIds:
+ for i in support_feet:
cone = crocoddyl.FrictionCone(self.pd.Rsurf, self.pd.mu, 4, False, 3)
residual = crocoddyl.ResidualModelContactFrictionCone(
self.state, i, cone, nu
@@ -237,14 +252,12 @@ class OCP:
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)
+ name = "base_velocity_tracking"
+ ref = pin.Motion(tasks[0], tasks[1])
+ residual_base_velocity = crocoddyl.ResidualModelFrameVelocity(
+ self.state, self.pd.baseId, ref, pin.LOCAL, nu)
+ base_velocity = crocoddyl.CostModelResidual(self.state, residual_base_velocity)
+ costs.addCost(name, base_velocity, self.pd.base_velocity_tracking_w)
control_residual = crocoddyl.ResidualModelControl(self.state, self.pd.uref)
control_reg = crocoddyl.CostModelResidual(self.state, control_residual)
@@ -259,15 +272,11 @@ class OCP:
)
costs.addCost("control_bound", control_bound, self.pd.control_bound_w)
- self.update_tracking_costs(costs, tasks, support_feet)
+ self.update_command_costs(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] and i not in supportFootIds:
- costs.changeCostStatus(name, True)
- costs.costs[name].cost.residual.reference = tasks[1][index]
- index += 1
- else:
- costs.changeCostStatus(name, False)
+ def update_command_costs(self, costs, tasks):
+ name = "base_velocity_tracking"
+ costs.changeCostStatus(name, True)
+ costs.costs[name].cost.residual.reference = pin.Motion(tasks[0], tasks[1])
+
+
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 34a871e875bb0b1e6489b097817047a1b7c7af50..1777d0ed1ac940d05fcd1ce35284152a818c2647 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -66,7 +66,6 @@ class problemDataAbstract:
self.rfFootId = self.model.getFrameId(self.rfFoot)
self.lhFootId = self.model.getFrameId(self.lhFoot)
self.rhFootId = self.model.getFrameId(self.rhFoot)
-
self.Rsurf = np.eye(3)
def freeze(self):
@@ -88,21 +87,25 @@ class ProblemData(problemDataAbstract):
self.useFixedBase = 0
+ self.baseId = self.model.getFrameId("base_link")
+
# Cost function weights
# Cost function weights
self.mu = 0.7
- self.foot_tracking_w = 1e2
- self.friction_cone_w = 1e4
+ self.base_velocity_tracking_w = 1e2
+
+ self.friction_cone_w = 1e4
+ self.impact_altitude_weight = 1e3
+ self.impact_velocity_weight = 1e3
+
self.control_bound_w = 1e3
self.control_reg_w = 1e0
self.state_reg_w = np.array([0] * 3
+ [1e1] * 3
- + [1e1] * 3
- + [1e-3] * 3
- + [1e1] * 6
+ + [1e1] * 12
+ [0] * 6
+ [1e1] * 3
- + [1e-1] * 3
+ + [1e1] * 3
+ [1e1] * 6
)
self.terminal_velocity_w = np.array(
@@ -110,6 +113,7 @@ class ProblemData(problemDataAbstract):
self.force_reg_w = 1e0
+
self.xref = self.x0
self.uref =self.u0
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index b7af78d24b5aaf1cf6adb689fbcd3348bbc14ee6..713ded9218b40fb676556776df1e343289bb1298 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -10,229 +10,6 @@ class Target:
self.dt_wbc = params.dt_wbc
self.k_per_step = 160
- self.position = np.array(params.footsteps_under_shoulders).reshape(
- (3, 4), order="F"
- )
-
- if params.movement == "circle":
- self.A = np.array([0, 0.03, 0.03])
- self.offset = np.array([0, 0, 0.1])
- self.freq = np.array([0, 0.5 *0, 0.5*0])
- self.phase = np.array([0, np.pi / 2, 0])
- elif params.movement == "step":
- self.initial = self.position[:, 1].copy()
- self.target = self.position[:, 1].copy() + np.array([0.1, 0.0, 0.0])
-
- self.vert_time = params.vert_time
- self.max_height = params.max_height
- self.T = self.k_per_step * self.dt_wbc
- self.A = np.zeros((6, 3))
-
- self.update_time = -1
- else:
- self.target_footstep = self.position + np.array([0.0, 0.0, 0.10])
-
- def compute(self, k):
- footstep = np.zeros((3, 4))
- if self.params.movement == "circle":
- footstep[:, 1] = self.evaluate_circle(k)
- elif self.params.movement == "step":
- footstep[:, 1] = self.evaluate_step(1, k)
- else:
- footstep = self.target_footstep.copy()
-
- return footstep
-
- def evaluate_circle(self, k):
- return (
- self.position[:, 1]
- + self.offset
- + self.A * np.sin(2 * np.pi * self.freq * k * self.dt_wbc + self.phase)
- )
-
- def evaluate_step(self, j, k):
- n_step = k // self.k_per_step
- if n_step % 2 == 0:
- return self.initial.copy() if (n_step % 4 == 0) else self.target.copy()
-
- if n_step % 4 == 1:
- initial = self.initial
- target = self.target
- else:
- initial = self.target
- target = self.initial
-
- k_step = k % self.k_per_step
- if n_step != self.update_time:
- self.update_polynomial(initial, target)
- self.update_time = n_step
-
- t = k_step * self.dt_wbc
- return self.compute_position(j, t)
-
- def update_polynomial(self, initial, target):
-
- x0 = initial[0]
- y0 = initial[1]
-
- x1 = target[0]
- y1 = target[1]
-
- # elapsed time
- t = 0
- d = self.T - 2 * self.vert_time
-
- A = np.zeros((6, 3))
-
- A[0, 0] = 12 * (x0 - x1) / (2 * (t - d) ** 5)
- A[1, 0] = 30 * (x1 - x0) * (t + d) / (2 * (t - d) ** 5)
- A[2, 0] = 20 * (x0 - x1) * (t**2 + d**2 + 4 * t * d) / (2 * (t - d) ** 5)
- A[3, 0] = 60 * (x1 - x0) * (t * d**2 + t**2 * d) / (2 * (t - d) ** 5)
- A[4, 0] = 60 * (x0 - x1) * (t**2 * d**2) / (2 * (t - d) ** 5)
- A[5, 0] = (
- 2 * x1 * t**5
- - 10 * x1 * t**4 * d
- + 20 * x1 * t**3 * d**2
- - 20 * x0 * t**2 * d**3
- + 10 * x0 * t * d**4
- - 2 * x0 * d**5
- ) / (2 * (t - d) ** 5)
-
- A[0, 1] = 12 * (y0 - y1) / (2 * (t - d) ** 5)
- A[1, 1] = 30 * (y1 - y0) * (t + d) / (2 * (t - d) ** 5)
- A[2, 1] = 20 * (y0 - y1) * (t**2 + d**2 + 4 * t * d) / (2 * (t - d) ** 5)
- A[3, 1] = 60 * (y1 - y0) * (t * d**2 + t**2 * d) / (2 * (t - d) ** 5)
- A[4, 1] = 60 * (y0 - y1) * (t**2 * d**2) / (2 * (t - d) ** 5)
- A[5, 1] = (
- 2 * y1 * t**5
- - 10 * y1 * t**4 * d
- + 20 * y1 * t**3 * d**2
- - 20 * y0 * t**2 * d**3
- + 10 * y0 * t * d**4
- - 2 * y0 * d**5
- ) / (2 * (t - d) ** 5)
-
- A[0, 2] = -self.max_height / ((self.T / 2) ** 6)
- A[1, 2] = 3 * self.T * self.max_height / ((self.T / 2) ** 6)
- A[2, 2] = -3 * self.T**2 * self.max_height / ((self.T / 2) ** 6)
- A[3, 2] = self.T**3 * self.max_height / ((self.T / 2) ** 6)
-
- self.A = A
-
- def compute_position(self, j, t):
- A = self.A.copy()
-
- t_xy = t - self.vert_time
- t_xy = max(0.0, t_xy)
- t_xy = min(t_xy, self.T - 2 * self.vert_time)
- self.position[:2, j] = (
- A[5, :2]
- + A[4, :2] * t_xy
- + A[3, :2] * t_xy**2
- + A[2, :2] * t_xy**3
- + A[1, :2] * t_xy**4
- + A[0, :2] * t_xy**5
- )
-
- self.position[2, j] = (
- A[3, 2] * t**3 + A[2, 2] * t**4 + A[1, 2] * t**5 + A[0, 2] * t**6
- )
-
- return self.position[:, j]
-
- def evaluate_circle(self, k):
- return (
- self.position[:, 1]
- + self.offset
- + self.A * np.sin(2 * np.pi * self.freq * k * self.dt_wbc + self.phase)
- )
-
- def evaluate_step(self, j, k):
- n_step = k // self.k_per_step
- if n_step % 2 == 0:
- return self.initial.copy() if (n_step % 4 == 0) else self.target.copy()
-
- if n_step % 4 == 1:
- initial = self.initial
- target = self.target
- else:
- initial = self.target
- target = self.initial
-
- k_step = k % self.k_per_step
- if n_step != self.update_time:
- self.update_polynomial(initial, target)
- self.update_time = n_step
-
- t = k_step * self.dt_wbc
- return self.compute_position(j, t)
-
- def update_polynomial(self, initial, target):
-
- x0 = initial[0]
- y0 = initial[1]
-
- x1 = target[0]
- y1 = target[1]
-
- # elapsed time
- t = 0
- d = self.T - 2 * self.vert_time
-
- A = np.zeros((6, 3))
-
- A[0, 0] = 12 * (x0 - x1) / (2 * (t - d) ** 5)
- A[1, 0] = 30 * (x1 - x0) * (t + d) / (2 * (t - d) ** 5)
- A[2, 0] = 20 * (x0 - x1) * (t**2 + d**2 + 4 * t * d) / (2 * (t - d) ** 5)
- A[3, 0] = 60 * (x1 - x0) * (t * d**2 + t**2 * d) / (2 * (t - d) ** 5)
- A[4, 0] = 60 * (x0 - x1) * (t**2 * d**2) / (2 * (t - d) ** 5)
- A[5, 0] = (
- 2 * x1 * t**5
- - 10 * x1 * t**4 * d
- + 20 * x1 * t**3 * d**2
- - 20 * x0 * t**2 * d**3
- + 10 * x0 * t * d**4
- - 2 * x0 * d**5
- ) / (2 * (t - d) ** 5)
-
- A[0, 1] = 12 * (y0 - y1) / (2 * (t - d) ** 5)
- A[1, 1] = 30 * (y1 - y0) * (t + d) / (2 * (t - d) ** 5)
- A[2, 1] = 20 * (y0 - y1) * (t**2 + d**2 + 4 * t * d) / (2 * (t - d) ** 5)
- A[3, 1] = 60 * (y1 - y0) * (t * d**2 + t**2 * d) / (2 * (t - d) ** 5)
- A[4, 1] = 60 * (y0 - y1) * (t**2 * d**2) / (2 * (t - d) ** 5)
- A[5, 1] = (
- 2 * y1 * t**5
- - 10 * y1 * t**4 * d
- + 20 * y1 * t**3 * d**2
- - 20 * y0 * t**2 * d**3
- + 10 * y0 * t * d**4
- - 2 * y0 * d**5
- ) / (2 * (t - d) ** 5)
-
- A[0, 2] = -self.max_height / ((self.T / 2) ** 6)
- A[1, 2] = 3 * self.T * self.max_height / ((self.T / 2) ** 6)
- A[2, 2] = -3 * self.T**2 * self.max_height / ((self.T / 2) ** 6)
- A[3, 2] = self.T**3 * self.max_height / ((self.T / 2) ** 6)
-
- self.A = A
-
- def compute_position(self, j, t):
- A = self.A.copy()
-
- t_xy = t - self.vert_time
- t_xy = max(0.0, t_xy)
- t_xy = min(t_xy, self.T - 2 * self.vert_time)
- self.position[:2, j] = (
- A[5, :2]
- + A[4, :2] * t_xy
- + A[3, :2] * t_xy**2
- + A[2, :2] * t_xy**3
- + A[1, :2] * t_xy**4
- + A[0, :2] * t_xy**5
- )
-
- self.position[2, j] = (
- A[3, 2] * t**3 + A[2, 2] * t**4 + A[1, 2] * t**5 + A[0, 2] * t**6
- )
-
- return self.position[:, j]
+ self.linear_velocity = np.array([10, 0, 0])
+ self.angular_velocity = np.array([0, 0, 0])
+ self.velocity_task = [self.linear_velocity, self.angular_velocity]
\ No newline at end of file
diff --git a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
index 2316e985e79aa6120ce1a195913e489db0a2959f..cbcb8aa74b7bdc08145265b462d4dfd0589912ad 100644
--- a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
+++ b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
@@ -55,7 +55,7 @@ class MPC_Wrapper:
self.last_available_result = Result(pd)
self.new_result = Value("b", False)
- def solve(self, k, x0, footstep, gait, xs=None, us=None):
+ def solve(self, k, x0, tasks, gait, xs=None, us=None):
"""
Call either the asynchronous MPC or the synchronous MPC depending on the value
of multiprocessing during the creation of the wrapper
@@ -64,9 +64,9 @@ class MPC_Wrapper:
k (int): Number of inv dynamics iterations since the start of the simulation
"""
if self.multiprocessing:
- self.run_MPC_asynchronous(k, x0, footstep, gait, xs, us)
+ self.run_MPC_asynchronous(k, x0, tasks, gait, xs, us)
else:
- self.run_MPC_synchronous(x0, footstep, gait, xs, us)
+ self.run_MPC_synchronous(x0, tasks, gait, xs, us)
def get_latest_result(self):
"""
@@ -91,11 +91,11 @@ class MPC_Wrapper:
return self.last_available_result
- def run_MPC_synchronous(self, x0, footstep, gait, xs, us):
+ def run_MPC_synchronous(self, x0, tasks, gait, xs, us):
"""
Run the MPC (synchronous version)
"""
- self.ocp.solve(x0, footstep, gait, xs, us)
+ self.ocp.solve(x0, tasks, gait, xs, us)
(
self.last_available_result.xs,
self.last_available_result.us,
@@ -104,7 +104,7 @@ class MPC_Wrapper:
) = self.ocp.get_results()
self.new_result.value = True
- def run_MPC_asynchronous(self, k, x0, footstep, gait, xs, us):
+ def run_MPC_asynchronous(self, k, x0, tasks, gait, xs, us):
"""
Run the MPC (asynchronous version)
"""
@@ -113,7 +113,7 @@ class MPC_Wrapper:
p = Process(target=self.MPC_asynchronous)
p.start()
- self.add_new_data(k, x0, footstep, gait, xs, us)
+ self.add_new_data(k, x0, tasks, gait, xs, us)
def MPC_asynchronous(self):
"""
@@ -125,27 +125,27 @@ class MPC_Wrapper:
self.new_data.value = False
- k, x0, footstep, gait, xs, us = self.decompress_dataIn()
+ k, x0, tasks, gait, xs, us = self.decompress_dataIn()
if k == 0:
loop_ocp = OCP(self.pd, self.footsteps_plan, self.initial_gait)
- loop_ocp.solve(x0, footstep, gait, xs, us)
+ loop_ocp.solve(x0, tasks, gait, xs, us)
xs, us, K, solving_time = loop_ocp.get_results()
self.compress_dataOut(xs, us, K, solving_time)
self.new_result.value = True
- def add_new_data(self, k, x0, footstep, gait, xs, us):
+ def add_new_data(self, k, x0, tasks, gait, xs, us):
"""
Compress data in a C-type structure that belongs to the shared memory to send
data from the main control loop to the asynchronous MPC and notify the process
that there is a new data
"""
- self.compress_dataIn(k, x0, footstep, gait, xs, us)
+ self.compress_dataIn(k, x0, tasks, gait, xs, us)
self.new_data.value = True
- def compress_dataIn(self, k, x0, footstep, gait, xs, us):
+ def compress_dataIn(self, k, x0, tasks, gait, xs, us):
"""
Decompress data from a C-type structure that belongs to the shared memory to
retrieve data from the main control loop in the asynchronous MPC
@@ -156,7 +156,7 @@ class MPC_Wrapper:
with self.in_x0.get_lock():
np.frombuffer(self.in_x0.get_obj()).reshape(self.pd.nx)[:] = x0
with self.in_footstep.get_lock():
- np.frombuffer(self.in_footstep.get_obj()).reshape((3, 4))[:, :] = footstep
+ np.frombuffer(self.in_footstep.get_obj()).reshape((3, 4))[:, :] = tasks
with self.in_gait.get_lock():
np.frombuffer(self.in_gait.get_obj()).reshape((self.pd.T + 1, 4))[:, :] = gait
@@ -183,12 +183,12 @@ class MPC_Wrapper:
with self.in_x0.get_lock():
x0 = np.frombuffer(self.in_x0.get_obj()).reshape(self.pd.nx)
with self.in_footstep.get_lock():
- footstep = np.frombuffer(self.in_footstep.get_obj()).reshape((3, 4))
+ tasks = np.frombuffer(self.in_footstep.get_obj()).reshape((3, 4))
with self.in_gait.get_lock():
gait = np.frombuffer(self.in_gait.get_obj()).reshape((self.pd.T + 1, 4))
if not self.in_warm_start.value:
- return k, x0, footstep, gait, None, None
+ return k, x0, tasks, gait, None, None
with self.in_xs.get_lock():
xs = list(
@@ -199,7 +199,7 @@ class MPC_Wrapper:
np.frombuffer(self.in_us.get_obj()).reshape((self.pd.T, self.pd.nu))
)
- return k, x0, footstep, gait, xs, us
+ return k, x0, tasks, gait, xs, us
def compress_dataOut(self, xs, us, K, solving_time):
"""
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index 97f58caa2d21a6e16cfdbb411a1c2d603bf3febf..c357a2e426cd11a5babae80a9997ed495b5e2f18 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -104,10 +104,10 @@ def damp_control(device, nb_motors):
device.parse_sensor_data()
# Set desired quantities for the actuators
- device.joints.set_position_gains(np.zeros(nb_motors))
- device.joints.set_velocity_gains(0.1 * np.ones(nb_motors))
- device.joints.set_desired_positions(np.zeros(nb_motors))
- device.joints.set_desired_velocities(np.zeros(nb_motors))
+ # device.joints.set_position_gains(np.zeros(nb_motors))
+ # device.joints.set_velocity_gains(0.1 * np.ones(nb_motors))
+ # device.joints.set_desired_positions(np.zeros(nb_motors))
+ # device.joints.set_desired_velocities(np.zeros(nb_motors))
device.joints.set_torques(np.zeros(nb_motors))
# Send command to the robot
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index 5a8d1ac9b4f2956466ef948e728e8764a1f2f95a..126d69d19cf91968f8ca535fe01fa1182bba91ee 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -121,9 +121,9 @@ class LoggerControl:
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]
+ self.target[i] = controller.velocity_task[0]
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]
+ self.target[self.i + self.pd.T * self.pd.mpc_wbc_ratio] = controller.velocity_task[0]
if not self.params.enable_multiprocessing:
self.t_ocp_update[self.i] = controller.mpc.ocp.t_update