no oscillation

good behaviour, it takes more iteration to go that good

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})

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]

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")

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()

python/quadruped_reactive_walking/tools/LoggerControl.py

 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()
 

python/quadruped_reactive_walking/tools/kinematics_utils.py

+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)