diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index fb2a2508b71f23c7f3c13a53e182e131dc8c3aad..fe705107a98c3255a756866c1d6198d6e795520a 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -46,16 +46,17 @@ class Interpolation:
if self.params.interpolation_type == 1:
beta = self.v1
gamma = self.q0
+ alpha = 1/2 * self.v1 * (self.v1 - self.v0) / (self.q1 - self.q0)
v_t = beta
q_t = gamma + beta * t
# Perfect match, but wrong
if self.params.interpolation_type == 2:
- if (self.q1-self.q0 == 0).any():
+ if (self.q1 - self.q0 == 0).any():
alpha = np.zeros(len(self.q0))
else:
- alpha = (self.v1**2 - self.v0**2)/(self.q1 - self.q0)
+ alpha = (self.v1**2 - self.v0**2) / (self.q1 - self.q0)
beta = self.v0
gamma = self.q0
@@ -65,33 +66,34 @@ class Interpolation:
# Quadratic
if self.params.interpolation_type == 3:
- if (self.q1-self.q0 == 0).any():
+ if (self.q1 - self.q0 == 0).any():
alpha = np.zeros(len(self.q0))
else:
- alpha = self.v1 * (self.v1 - self.v0)/(self.q1 - self.q0)
+ alpha = self.v1 * (self.v1 - self.v0) / (self.q1 - self.q0)
beta = self.v0
gamma = self.q0
v_t = beta + alpha * t
- q_t = gamma + beta * t + 1/2 * alpha * t**2
+ q_t = gamma + beta * t + 1 / 2 * alpha * t**2
return q_t, v_t
def plot_interpolation(self, n, dt):
import matplotlib.pyplot as plt
+
plt.style.use("seaborn")
- t = np.linspace(0, n*dt, n + 1)
- q_t = np.array([self.interpolate((i) * dt)[0] for i in range(n+1)])
- v_t = np.array([self.interpolate((i) * dt)[1] for i in range(n+1)])
+ t = np.linspace(0, n * dt, n + 1)
+ q_t = np.array([self.interpolate((i) * dt)[0] for i in range(n + 1)])
+ v_t = np.array([self.interpolate((i) * dt)[1] for i in range(n + 1)])
for i in range(3):
- plt.subplot(3, 2, (i*2) + 1)
+ plt.subplot(3, 2, (i * 2) + 1)
plt.title("Position interpolation")
plt.plot(t, q_t[:, i])
plt.scatter(y=self.q0[i], x=t[0], color="violet", marker="+")
plt.scatter(y=self.q1[i], x=t[-1], color="violet", marker="+")
- plt.subplot(3, 2, (i*2) + 2)
+ plt.subplot(3, 2, (i * 2) + 2)
plt.title("Velocity interpolation")
plt.plot(t, v_t[:, i])
plt.scatter(y=self.v0[i], x=t[0], color="violet", marker="+")
@@ -152,9 +154,6 @@ class Controller:
self.result = Result(params)
self.result.q_des = self.pd.q0[7:].copy()
self.result.v_des = self.pd.v0[6:].copy()
-
- device = DummyDevice()
- device.joints.positions = q_init
try:
file = np.load("/tmp/init_guess.npy", allow_pickle=True).item()
self.xs_init = list(file["xs"])
@@ -165,6 +164,10 @@ class Controller:
self.us_init = None
print("No initial guess\n")
+ device = DummyDevice()
+ device.joints.positions = q_init
+ self.compute(device)
+
def compute(self, device, qc=None):
"""
Run one iteration of the main control loop
@@ -184,10 +187,10 @@ class Controller:
try:
self.target.update(self.cnt_mpc)
self.target.shift_gait()
- #self.cnt_wbc = 0
+ if not self.params.enable_multiprocessing:
+ self.cnt_wbc = 0
self.mpc.solve(self.k, m["x_m"], self.xs_init, self.us_init)
- if self.k == 0: time.sleep(1)
self.cnt_mpc += 1
except ValueError:
@@ -202,12 +205,16 @@ class Controller:
if self.params.enable_multiprocessing:
if self.mpc_result.new_result:
print("new result! at iter: ", str(self.cnt_wbc))
- #self.cnt_wbc = 0
-
-
- print("MPC iter: ", self.cnt_mpc,
- " / Counter value: ", self.cnt_wbc,
- " / k value: ", self.k )
+ self.cnt_wbc = 0
+
+ print(
+ "MPC iter: ",
+ self.cnt_mpc,
+ " / Counter value: ",
+ self.cnt_wbc,
+ " / k value: ",
+ self.k,
+ )
# ## ONLY IF YOU WANT TO STORE THE FIRST SOLUTION TO WARM-START THE INITIAL Problem ###
# if not self.initialized:
# np.save(open('/tmp/init_guess.npy', "wb"), {"xs": self.mpc_result.xs, "us": self.mpc_result.us} )
@@ -217,20 +224,19 @@ class Controller:
self.result.FF = self.params.Kff_main * np.ones(12)
actuated_tau_ff = self.compute_torque(m)
- self.result.tau_ff = np.array(
- [0] * 3 + list(actuated_tau_ff) + [0] * 6)
+ self.result.tau_ff = np.array([0] * 3 + list(actuated_tau_ff) + [0] * 6)
if self.params.interpolate_mpc:
# load the data to be interpolated only once per mpc solution
if self.cnt_wbc == 0:
x = np.array(self.mpc_result.xs)
- self.interpolator.load_data(
- x[:, : self.pd.nq], x[:, self.pd.nq:])
+ self.interpolator.load_data(x[:, : self.pd.nq], x[:, self.pd.nq :])
q, v = self.interpolator.interpolate(
- (self.cnt_wbc + 1) * self.pd.dt_wbc)
+ (self.k % int(self.params.dt_mpc / self.params.dt_wbc) + self.cnt_wbc + 1) * self.pd.dt_wbc
+ )
- #self.interpolator.plot_interpolation(self.pd.r1, self.pd.dt_wbc)
+ # self.interpolator.plot_interpolation(self.pd.r1, self.pd.dt_wbc)
else:
q, v = self.integrate_x(m)
@@ -357,7 +363,6 @@ class Controller:
self.result.tau_ff[:] = np.zeros(12)
def read_state(self, device):
- device.parse_sensor_data()
qj_m = device.joints.positions
vj_m = device.joints.velocities
x_m = np.concatenate([qj_m[3:6], vj_m[3:6]])
@@ -368,16 +373,17 @@ class Controller:
"""
Compute the feedforward torque using ricatti gains
"""
- x_diff = np.concatenate(
- [
- pin.difference(
- self.pd.model,
- m["x_m"][: self.pd.nq],
- self.mpc_result.xs[0][: self.pd.nq],
- ),
- m["x_m"][self.pd.nq:] - self.mpc_result.xs[0][self.pd.nq:],
- ]
- )
+ # x_diff = np.concatenate(
+ # [
+ # pin.difference(
+ # self.pd.model,
+ # m["x_m"][: self.pd.nq],
+ # self.mpc_result.xs[0][: self.pd.nq],
+ # ),
+ # m["x_m"][self.pd.nq :] - self.mpc_result.xs[0][self.pd.nq :],
+ # ]
+ # )
+ x_diff = m["x_m"] - self.mpc_result.xs[0]
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 7b2dfba24e676e0d5fb170c17a826c3c20680294..07eccab1e298a4422f716ad0f9e4d1a903054bd2 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -12,13 +12,13 @@ class OCP:
def __init__(self, pd: ProblemData, target: Target):
self.pd = pd
self.target = target
- self.max_iter=5
+ self.max_iter = 10
self.state = crocoddyl.StateMultibody(self.pd.model)
self.initialized = False
self.t_problem_update = 0
- self.t_update_last_model = 0.
- self.t_shift = 0.
+ self.t_update_last_model = 0.0
+ self.t_shift = 0.0
self.initialize_models()
@@ -65,9 +65,9 @@ class OCP:
task = self.make_task(
self.target.evaluate_in_t(self.pd.T - 1),
self.target.contactSequence[self.pd.T - 1],
- ) # model without contact for this task
- self.nodes[0].update_model(
- self.target.contactSequence[self.pd.T - 1], task)
+ )
+
+ self.nodes[0].update_model(self.target.contactSequence[self.pd.T - 1], task)
t_update_last_model = time()
self.t_update_last_model = t_update_last_model - t_FK
@@ -82,7 +82,7 @@ class OCP:
# If you need update terminal model
t_update_terminal_model = time()
- self.t_update_terminal_model = 0.
+ self.t_update_terminal_model = 0.0
# self.t_update_terminal_model = t_update_terminal_model - self.t_shift
self.initialized = True
@@ -166,8 +166,7 @@ class OCP:
def get_croco_acc(self):
acc = []
- [acc.append(m.differential.xout)
- for m in self.ddp.problem.runningDatas]
+ [acc.append(m.differential.xout) for m in self.ddp.problem.runningDatas]
return acc
@@ -183,8 +182,7 @@ class Node:
self.actuation = crocoddyl.ActuationModelFloatingBase(self.state)
else:
self.actuation = crocoddyl.ActuationModelFull(self.state)
- self.control = crocoddyl.ControlParametrizationModelPolyZero(
- self.actuation.nu)
+ self.control = crocoddyl.ControlParametrizationModelPolyZero(self.actuation.nu)
self.nu = self.actuation.nu
self.createStandardModel(supportFootIds)
@@ -206,8 +204,7 @@ class Node:
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.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
@@ -216,8 +213,7 @@ class Node:
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, self.nu)
- stateResidual = crocoddyl.ResidualModelState(
- self.state, self.pd.xref, self.nu)
+ stateResidual = crocoddyl.ResidualModelState(self.state, self.pd.xref, self.nu)
stateActivation = crocoddyl.ActivationModelWeightedQuad(
self.pd.state_reg_w**2
)
@@ -240,8 +236,7 @@ class Node:
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.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
@@ -249,8 +244,7 @@ class Node:
def make_terminal_model(self):
self.isTerminal = True
- stateResidual = crocoddyl.ResidualModelState(
- self.state, self.pd.xref, self.nu)
+ stateResidual = crocoddyl.ResidualModelState(self.state, self.pd.xref, self.nu)
stateActivation = crocoddyl.ActivationModelWeightedQuad(
self.pd.terminal_velocity_w**2
)
@@ -281,17 +275,14 @@ class Node:
ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual)
self.costModel.addCost("ctrlReg", ctrlReg, self.pd.control_reg_w)
- ctrl_bound_residual = crocoddyl.ResidualModelControl(
- self.state, self.nu)
+ ctrl_bound_residual = crocoddyl.ResidualModelControl(self.state, self.nu)
ctrl_bound_activation = crocoddyl.ActivationModelQuadraticBarrier(
- crocoddyl.ActivationBounds(-self.pd.effort_limit,
- self.pd.effort_limit)
+ crocoddyl.ActivationBounds(-self.pd.effort_limit, self.pd.effort_limit)
)
ctrl_bound = crocoddyl.CostModelResidual(
self.state, ctrl_bound_activation, ctrl_bound_residual
)
- self.costModel.addCost("ctrlBound", ctrl_bound,
- self.pd.control_bound_w)
+ self.costModel.addCost("ctrlBound", ctrl_bound, self.pd.control_bound_w)
self.tracking_cost(swingFootTask)
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index 1a9a3ba87165c4305ac80f4bdc6359124695419b..b9ee5d3550709420110c32e0c73692a0b962181d 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -23,7 +23,7 @@ class Target:
self.FR_foot0 = pd.rdata.oMf[pd.rfFootId].translation.copy()
self.A = np.array([0, 0.03, 0.03])
self.offset = np.array([0.05, -0.02, 0.06])
- self.freq = np.array([0, 0.5 * 0 , 0.5*0])
+ self.freq = np.array([0, 0.5, 0.5])
self.phase = np.array([0, np.pi / 2, 0])
def patternToId(self, gait):
diff --git a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
index 117401973e18adedd54417c39c72657d2dc64976..874e55c0201d3402885939e388d78f60154e0822 100644
--- a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
+++ b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
@@ -83,6 +83,7 @@ class MPC_Wrapper:
self.last_available_result.solving_duration,
) = self.decompress_dataOut()
self.last_available_result.new_result = True
+
elif self.multiprocessing and not self.new_result.value:
self.last_available_result.new_result = False
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index 609b997e05d56499441a37f65260d5deebd0dac7..a01ef48572280c4aaa58c37f34aa09e0d259518f 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -175,6 +175,7 @@ def control_loop():
while (not device.is_timeout) and (t < t_max) and (not controller.error):
t_start_whole = time.time()
+ device.parse_sensor_data()
if controller.compute(device, qc):
break