diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 5c93fd9f833ab610cdb1a1f2729eb5845fbb5abd..cebbe101029ea22359c3288b89d84bdee51e8c5b 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -98,14 +98,16 @@ class Controller:
try:
self.mpc.solve(self.k, m["x_m"], self.guess) # Closed loop mpc
- # Trajectory tracking
+ ## Trajectory tracking
#if self.initialized:
# self.mpc.solve(self.k, self.mpc_result.x[1], self.guess)
#else:
# self.mpc.solve(self.k, m["x_m"], self.guess)
- ### ONLY IF YOU WANT TO STORE THE FIRST SOLUTION TO WARMSTART THE INITIAL Problem ###
- #np.save(open('/tmp/init_guess.npy', "wb"), {"xs": self.mpc.ocp.get_results().x, "us": self.mpc.ocp.get_results().u} )
+ # ### ONLY IF YOU WANT TO STORE THE FIRST SOLUTION TO WARMSTART THE INITIAL Problem ###
+ #if not self.initialized:
+ # np.save(open('/tmp/init_guess.npy', "wb"), {"xs": self.mpc.ocp.get_results().x, "us": self.mpc.ocp.get_results().u} )
+ # print("Initial guess saved")
except ValueError:
self.error = True
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index e37f5fb9cbbf2db74188b80fcf4cac1e1af648ab..b6f3b96c65184bebba03c0114d9bf50259e6cfdd 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -119,10 +119,8 @@ class OCP:
def get_results(self):
self.results.x = self.ddp.xs.tolist()
- self.results.a = self.get_croco_acc()
self.results.u = self.ddp.us.tolist()
self.results.K = self.ddp.K
- self.results.solver_time = self.t_solve
return self.results
def get_croco_forces(self):
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index d2ae98018445ff400722dd8d903e3cd89c422da9..c598408e987329d317167f31acb54d35ab9c9e56 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_bldc = 0.0005
self.r1 = int(self.dt / self.dt_sim)
self.init_steps = 0
- self.target_steps = 120
+ self.target_steps = 90
self.T = self.init_steps + self.target_steps -1
self.robot = erd.load("solo12")
@@ -116,7 +116,7 @@ class ProblemDataFull(problemDataAbstract):
self.foot_tracking_w = 1e3
#self.friction_cone_w = 1e3 * 0
self.control_bound_w = 1e3
- self.control_reg_w = 1e1
+ self.control_reg_w = 1e0
self.state_reg_w = np.array([1e-3] * 3 + [5*1e-1]*3)
self.terminal_velocity_w = np.array([0] * 3 + [1e3] * 3 )
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index f7013c66d3db7bb360bcd87b70830057f9bb4517..f1443e502ec1737be443d9a56b9f14483d09afab 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -29,7 +29,7 @@ class Target:
pin.updateFramePlacements(pd.model, pd.rdata)
self.FR_foot0 = pd.rdata.oMf[pd.rfFootId].translation.copy()
self.A = np.array([0, 0.03, 0.03])
- self.offset = np.array([0.08, 0.0, 0.06])
+ self.offset = np.array([0.08, 0, 0.06])
self.freq = np.array([0, 2.5*0, 2.5*0])
self.phase = np.array([0, 0, np.pi / 2])
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index 9edb2a25fd3b1e90530cd0e38552d190de5f741f..57a054bc7c146213fff621532f1125414956a791 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -180,6 +180,7 @@ def control_loop():
target.shift_gait()
if controller.compute(device, qc):
break
+
if t <= 10 * params.dt_wbc and check_position_error(device, controller):
break
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index 66a412721cf458d1d3580790447264ed5f96f5df..4258d900106845fa840000e3631bed93a2dd212f 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -145,6 +145,13 @@ class LoggerControl:
def plot(self, save=False, fileName="tmp/"):
import matplotlib.pyplot as plt
+ x_mes = np.concatenate([self.q_mes[:, 3:6], self.v_mes[:, 3:6]], axis = 1)
+
+ x_mpc = [self.ocp_storage["xs"][0][0, :]]
+ [x_mpc.append(x[1, :]) for x in self.ocp_storage["xs"][:-1]]
+ x_mpc = np.array(x_mpc)
+
+ # Feet positions calcuilated by every ocp
all_ocp_feet_p_log = {
idx: [
get_translation_array(self.pd, x, idx)[0]
@@ -155,12 +162,19 @@ class LoggerControl:
for foot in all_ocp_feet_p_log:
all_ocp_feet_p_log[foot] = np.array(all_ocp_feet_p_log[foot])
- x_mes = np.concatenate([self.q_mes[:, 3:6], self.v_mes[:, 3:6]], axis = 1)
- feet_p_log = {
+ # Measured feet positions
+ m_feet_p_log = {
idx:
get_translation_array(self.pd, x_mes, idx)[0]
for idx in self.pd.allContactIds
}
+
+ # Predicted eet positions
+ feet_p_log = {
+ idx:
+ get_translation_array(self.pd, x_mpc, idx)[0]
+ for idx in self.pd.allContactIds
+ }
@@ -228,8 +242,9 @@ class LoggerControl:
plt.subplot(3,1, p+1)
plt.title('Free foot on ' + legend[p])
plt.plot(self.target[:, p])
+ plt.plot(m_feet_p_log[self.pd.rfFootId][:, p])
plt.plot(feet_p_log[self.pd.rfFootId][:, p])
- plt.legend(["Desired", "Measured"])
+ plt.legend(["Target", "Measured", "Predicted"])
self.plot_controller_times()
self.plot_OCP_times()