diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 092f1d00d36676fa2eb301c54164544f7c54442a..271dfba2f3f0d754cdfd873e6bfa38ed22234127 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -40,6 +40,7 @@ set(${PY_NAME}_TOOLS_PYTHON
LoggerControl.py
PyBulletSimulator.py
qualisysClient.py
+ kinematics_utils.py
)
foreach(python ${${PY_NAME}_PYTHON})
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 6f05de62f01fe3452ba583df33548a0d3751d6a1..55fb4731b90abe72b648e9ec443017f7cda76fcb 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -102,11 +102,11 @@ class Controller:
self.mpc_result, self.mpc_cost = self.mpc.get_latest_result()
#self.result.P = np.array(self.params.Kp_main.tolist() * 4)
- self.result.P = np.array([5] * 3 + [1] * 3 + [5]*6)
+ self.result.P = np.array([5] * 3 + [3] * 3 + [5]*6)
#self.result.D = np.array(self.params.Kd_main.tolist() * 4)
self.result.D = np.array([0.3] * 3 + [0.5] * 3 + [0.3]*6)
- #tauFF = self.mpc_result.u[0] + np.dot(self.mpc_result.K[0], self.mpc.ocp.state.diff(m["x_m"], self.mpc_result.x[0]))
- #self.result.FF = self.params.Kff_main * np.array([0] * 3 + list(tauFF) + [0]*6)
+ tauFF = self.mpc_result.u[0] + np.dot(self.mpc_result.K[0], self.mpc.ocp.state.diff(m["x_m"], self.mpc_result.x[0]))
+ self.result.FF = self.params.Kff_main * np.array([0] * 3 + list(tauFF) + [0]*6)
# Keep only the actuated joints and set the other to default values
self.mpc_result.q = np.array([self.pd.q0] * (self.pd.T + 1))[:, 7: 19]
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 30d62f4eacaf7ac913be6ec83d93fd9e991e71cf..dddcf933989fbc78d5fddd81ef68135064b49281 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -9,8 +9,8 @@ class problemDataAbstract:
self.dt_bldc = 0.0005
self.r1 = int(self.dt / self.dt_sim)
self.r2 = int(self.dt_sim / self.dt_bldc)
- self.init_steps = 20 # full stand phase
- self.target_steps = 60 # manipulation steps
+ self.init_steps = 10 # full stand phase
+ self.target_steps = 50 # manipulation steps
self.T = self.init_steps + self.target_steps -1
self.robot = erd.load("solo12")
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index 5decbb164a2799ebe3f5195856de02c18febbece..9f664493a3d6d3ed8039725e5c386178604a78cc 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -142,7 +142,7 @@ def control_loop():
qc = QualisysClient(ip="140.93.16.160", body_id=0)
if params.LOGGING or params.PLOTTING:
- loggerControl = LoggerControl(log_size=params.N_SIMULATION-1)
+ loggerControl = LoggerControl(pd, log_size=params.N_SIMULATION-1)
if params.SIMULATION:
device.Init(
@@ -234,9 +234,10 @@ def control_loop():
device.Stop()
print("End of script")
+ return loggerControl
if __name__ == "__main__":
#os.nice(-20)
- control_loop()
+ log = control_loop()
#quit()
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index 779f642c3ebf43c4faa779c36198ace529bb43bd..184a65672aa364b02ade356f9071c13b598586c3 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -1,10 +1,11 @@
from datetime import datetime
from time import time
import numpy as np
+from .kinematics_utils import get_translation, get_translation_array
class LoggerControl:
- def __init__(self, log_size=60e3, loop_buffer=False, file=None):
+ def __init__(self, pd, log_size=60e3, loop_buffer=False, file=None):
if file is not None:
self.data = np.load(file, allow_pickle=True)
@@ -13,6 +14,7 @@ class LoggerControl:
self.loop_buffer = loop_buffer
size = self.log_size
+ self.pd = pd
# IMU and actuators:
self.q_mes = np.zeros([size, 12])
@@ -42,6 +44,7 @@ class LoggerControl:
# Controller timings: MPC time, ...
self.ocp_timings = np.zeros([size])
+ self.ocp_storage = {"xs": np.zeros([size, pd.T + 1, pd.nx])}
# MPC
@@ -91,6 +94,7 @@ class LoggerControl:
# Logging from model predictive control
self.ocp_timings[self.i] = controller.mpc.ocp.results.solver_time
+ self.ocp_storage["xs"][self.i] = np.array(controller.mpc.ocp.results.x)
# Logging from whole body control
self.wbc_P[self.i] = controller.result.P
@@ -110,6 +114,14 @@ class LoggerControl:
import matplotlib.pyplot as plt
matplotlib.use("QtAgg")
plt.style.use("seaborn")
+
+ horizon = self.ocp_storage["xs"].shape[0]
+ t15 = np.linspace(0, horizon*self.pd.dt, horizon+1)
+ t1 = np.linspace(0, (horizon)*self.pd.dt, (horizon)*self.pd.r1+1)
+ t_mpc = np.linspace(0, (horizon)*self.pd.dt, horizon+1)
+
+ all_ocp_feet_p_log = {idx: [get_translation_array(self.pd, x, idx)[0] for x in self.ocp_storage["xs"]] for idx in self.pd.allContactIds}
+ for foot in all_ocp_feet_p_log: all_ocp_feet_p_log[foot] = np.array(all_ocp_feet_p_log[foot])
legend = ['Hip', 'Shoulder', 'Knee']
plt.figure(figsize=(12, 6), dpi = 90)
@@ -155,6 +167,19 @@ class LoggerControl:
plt.legend(legend)
plt.draw()
+ legend = ['x', 'y', 'z']
+ plt.figure(figsize=(12, 18), dpi = 90)
+ for p in range(3):
+ plt.subplot(3,1, p+1)
+ plt.title('Free foot on ' + legend[p])
+ for i in range(horizon-1):
+ t = np.linspace(i*self.pd.dt, (self.pd.T+ i)*self.pd.dt, self.pd.T+1)
+ y = all_ocp_feet_p_log[self.pd.rfFootId][i+1][:,p]
+ for j in range(len(y) - 1):
+ plt.plot(t[j:j+2], y[j:j+2], color='royalblue', linewidth = 3, marker='o' ,alpha=max([1 - j/len(y), 0]))
+ #plt.plot(t_mpc, feet_p_log_mpc[18][:, p], linewidth=0.8, color = 'tomato', marker='o')
+ #plt.plot(t1, feet_p_log_m[18][:, p], linewidth=2, color = 'lightgreen')
+ plt.draw()
plt.show()
diff --git a/python/quadruped_reactive_walking/tools/kinematics_utils.py b/python/quadruped_reactive_walking/tools/kinematics_utils.py
new file mode 100644
index 0000000000000000000000000000000000000000..5dfcf9073bd9820ff5ba41c94d6ea54d66d460c4
--- /dev/null
+++ b/python/quadruped_reactive_walking/tools/kinematics_utils.py
@@ -0,0 +1,29 @@
+import pinocchio as pin
+import numpy as np
+from ..WB_MPC.ProblemData import ProblemData
+
+def get_translation(pd:ProblemData, x, idx, ref_frame=pin.WORLD):
+ q = x[: pd.nq]
+ v = x[pd.nq :]
+ pin.forwardKinematics(pd.model, pd.rdata, q, v)
+ pin.updateFramePlacements(pd.model, pd.rdata)
+ frame_p = pd.rdata.oMf[idx].translation
+ frame_v = pin.getFrameVelocity(pd.model, pd.rdata, idx, ref_frame).linear
+ return frame_p, frame_v
+
+def get_translation_array(pd:ProblemData, x, idx, ref_frame=pin.WORLD, x0=None):
+ frame_p = []
+ frame_v = []
+ if isinstance(x0, np.ndarray):
+ xiter = np.concatenate([x0.reshape(1,-1), x])
+ else:
+ xiter = x
+
+ for xs in xiter:
+ q = xs[: pd.nq]
+ v = xs[pd.nq :]
+ pin.forwardKinematics(pd.model, pd.rdata, q, v)
+ pin.updateFramePlacements(pd.model, pd.rdata)
+ frame_p += [pd.rdata.oMf[idx].translation.copy()]
+ frame_v += [pin.getFrameVelocity(pd.model, pd.rdata, idx, ref_frame).linear.copy()]
+ return np.array(frame_p), np.array(frame_v)