diff --git a/scripts/Controller.py b/scripts/Controller.py
index 908dee6d96358d2846f80c8a214dc3f53765ba02..c072d51a7d8e63068410777e795f74514633f861 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -175,7 +175,7 @@ class Controller:
self.error_flag = 0
self.q_security = np.array([np.pi*0.4, np.pi*80/180, np.pi] * 4)
- self.q_filt_mpc = np.zeros((6, 1))
+ self.q_filt_mpc = np.zeros((18, 1))
self.h_v_filt_mpc = np.zeros((6, 1))
self.h_v_bis_filt_mpc = np.zeros((6, 1))
self.vref_filt_mpc = np.zeros((6, 1))
@@ -225,8 +225,7 @@ class Controller:
oTh = self.estimator.getoTh().reshape((3, 1))
self.v_ref[0:6, 0] = self.estimator.getVRef()
self.h_v[0:6, 0] = self.estimator.getHV()
- self.h_v_bis[0:3, 0] = self.estimator.getHVBis()
- self.h_v_bis[3:6, 0] = self.h_v[3:6, 0].copy()
+ self.h_v_bis[0:6, 0] = self.estimator.getHVBis()
self.q[:, 0] = self.estimator.getQUpdated()
self.yaw_estim = self.estimator.getYawEstim()
# TODO: Understand why using Python or C++ h_v leads to a slightly different result since the
@@ -237,8 +236,9 @@ class Controller:
# Update gait
self.gait.updateGait(self.k, self.k_mpc, self.joystick.joystick_code)
- # Quantities go through a 1st order low pass filter with fc = 15 Hz
- self.q_filt_mpc[:, 0] = self.filter_mpc_q.filter(self.q[:6, 0:1], True)
+ # Quantities go through a 1st order low pass filter with fc = 15 Hz (avoid >25Hz foldback)
+ self.q_filt_mpc[:6, 0] = self.filter_mpc_q.filter(self.q[:6, 0:1], True)
+ self.q_filt_mpc[6:, 0] = self.q[6:, 0].copy()
self.h_v_filt_mpc[:, 0] = self.filter_mpc_v.filter(self.h_v[:6, 0:1], False)
self.h_v_bis_filt_mpc[:, 0] = self.filter_mpc_v_bis.filter(self.h_v_bis[:6, 0:1], False)
self.vref_filt_mpc[:, 0] = self.filter_mpc_vref.filter(self.v_ref[:6, 0:1], False)
@@ -246,9 +246,9 @@ class Controller:
# Compute target footstep based on current and reference velocities
o_targetFootstep = self.footstepPlanner.updateFootsteps(self.k % self.k_mpc == 0 and self.k != 0,
int(self.k_mpc - self.k % self.k_mpc),
- self.q[:, 0:1],
- self.h_v_bis[0:6, 0:1].copy(),
- self.v_ref[0:6, 0])
+ self.q_filt_mpc[:, 0],
+ self.h_v_bis_filt_mpc[0:6, 0:1].copy(),
+ self.vref_filt_mpc[0:6, 0])
# Run state planner (outputs the reference trajectory of the base)
self.statePlanner.computeReferenceStates(self.q[0:6, 0:1], self.h_v[0:6, 0:1].copy(),
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index c7447cef13221626cf741f092fc64ac7cd1434be..3ba7310d28f7c5fbef8149fe7e487eebce5b9843 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -54,6 +54,7 @@ class LoggerControl():
self.loop_t_mpc = np.zeros([logSize]) # time taken by the mcp
self.loop_t_wbc = np.zeros([logSize]) # time taken by the whole body control
self.loop_t_loop = np.zeros([logSize]) # time taken by the whole loop (without interface)
+ self.loop_t_loop_if = np.zeros([logSize]) # time taken by the whole loop (with interface)
self.loop_q_filt_mpc = np.zeros([logSize, 6]) #Â position in world frame filtered by 1st order low pass
self.loop_h_v_filt_mpc = np.zeros([logSize, 6]) #Â vel in base frame filtered by 1st order low pass
self.loop_h_v_bis_filt_mpc = np.zeros([logSize, 6]) #Â alt vel in base frame filtered by 1st order low pass
@@ -86,6 +87,7 @@ class LoggerControl():
self.mpc_x_f = np.zeros([logSize, 32, statePlanner.getNSteps()])
else:
self.mpc_x_f = np.zeros([logSize, 24, statePlanner.getNSteps()])
+ self.mpc_solving_duration = np.zeros([logSize])
# Whole body control
self.wbc_P = np.zeros([logSize, 12]) # proportionnal gains of the PD+
@@ -104,7 +106,7 @@ class LoggerControl():
# Timestamps
self.tstamps = np.zeros(logSize)
- def sample(self, joystick, estimator, loop, gait, statePlanner, footstepPlanner, footTrajectoryGenerator, wbc):
+ def sample(self, joystick, estimator, loop, gait, statePlanner, footstepPlanner, footTrajectoryGenerator, wbc, dT_whole):
if (self.i >= self.logSize):
if self.ringBuffer:
self.i = 0
@@ -147,7 +149,8 @@ class LoggerControl():
self.loop_t_mpc[self.i] = loop.t_mpc
self.loop_t_wbc[self.i] = loop.t_wbc
self.loop_t_loop[self.i] = loop.t_loop
- self.loop_q_filt_mpc[self.i] = loop.q_filt_mpc[:, 0]
+ self.loop_t_loop_if[self.i] = dT_whole
+ self.loop_q_filt_mpc[self.i] = loop.q_filt_mpc[:6, 0]
self.loop_h_v_filt_mpc[self.i] = loop.h_v_filt_mpc[:, 0]
self.loop_h_v_bis_filt_mpc[self.i] = loop.h_v_bis_filt_mpc[:, 0]
self.loop_vref_filt_mpc[self.i] = loop.vref_filt_mpc[:, 0]
@@ -166,6 +169,7 @@ class LoggerControl():
# Logging from model predictive control
self.mpc_x_f[self.i] = loop.x_f_mpc
+ self.mpc_solving_duration[self.i] = loop.mpc_wrapper.t_mpc_solving_duration
# Logging from whole body control
self.wbc_P[self.i] = loop.result.P
@@ -188,19 +192,26 @@ class LoggerControl():
def processMocap(self, N, loggerSensors):
- self.mocap_b_v = np.zeros([N, 3])
+ self.mocap_pos = np.zeros([N, 3])
+ self.mocap_h_v = np.zeros([N, 3])
self.mocap_b_w = np.zeros([N, 3])
self.mocap_RPY = np.zeros([N, 3])
+
+ for i in range(N):
+ self.mocap_RPY[i] = pin.rpy.matrixToRpy(pin.Quaternion(loggerSensors.mocapOrientationQuat[i]).toRotationMatrix())
+
+ # Robot world to Mocap initial translationa and rotation
+ mTo = np.array([loggerSensors.mocapPosition[0, 0], loggerSensors.mocapPosition[0, 1], 0.02])
+ mRo = pin.rpy.rpyToMatrix(0.0, 0.0, self.mocap_RPY[0, 2])
for i in range(N):
oRb = loggerSensors.mocapOrientationMat9[i]
- """from IPython import embed
- embed()"""
+ oRh = pin.rpy.rpyToMatrix(0.0, 0.0, self.mocap_RPY[i, 2] - self.mocap_RPY[0, 2])
- self.mocap_b_v[i] = (oRb.transpose() @ loggerSensors.mocapVelocity[i].reshape((3, 1))).ravel()
+ self.mocap_h_v[i] = (oRh.transpose() @ mRo.transpose() @ loggerSensors.mocapVelocity[i].reshape((3, 1))).ravel()
self.mocap_b_w[i] = (oRb.transpose() @ loggerSensors.mocapAngularVelocity[i].reshape((3, 1))).ravel()
- self.mocap_RPY[i] = pin.rpy.matrixToRpy(pin.Quaternion(loggerSensors.mocapOrientationQuat[i]).toRotationMatrix())
+ self.mocap_pos[i] = (mRo.transpose() @ (loggerSensors.mocapPosition[i, :] - mTo).reshape((3, 1))).ravel()
def plotAll(self, loggerSensors):
@@ -312,7 +323,7 @@ class LoggerControl():
plt.plot(t_range, self.planner_xref[:, i, 0], "b", linewidth=2)
plt.plot(t_range, self.planner_xref[:, i, 1], "r", linewidth=3)
if i < 3:
- plt.plot(t_range, loggerSensors.mocapPosition[:, i], "k", linewidth=3)
+ plt.plot(t_range, self.mocap_pos[:, i], "k", linewidth=3)
else:
plt.plot(t_range, self.mocap_RPY[:, i-3], "k", linewidth=3)
# plt.plot(t_range, self.log_q[i, :], "grey", linewidth=4)
@@ -334,10 +345,8 @@ class LoggerControl():
plt.plot(t_range, self.loop_h_v[:, i], "b", linewidth=2)
plt.plot(t_range, self.joy_v_ref[:, i], "r", linewidth=3)
if i < 3:
- plt.plot(t_range, self.mocap_b_v[:, i], "k", linewidth=3)
- plt.plot(t_range, self.loop_h_v_bis[:, i], "forestgreen", linewidth=2)
- # plt.plot(t_range, self.esti_FK_lin_vel[:, i], "violet", linewidth=3, linestyle="--")
- plt.plot(t_range, self.esti_filt_lin_vel[:, i], "violet", linewidth=3, linestyle="--")
+ plt.plot(t_range, self.mocap_h_v[:, i], "k", linewidth=3)
+ plt.plot(t_range, self.loop_h_v_bis_filt_mpc[:, i], "forestgreen", linewidth=2)
else:
plt.plot(t_range, self.mocap_b_w[:, i-3], "k", linewidth=3)
@@ -599,7 +608,15 @@ class LoggerControl():
plt.plot(t_range, self.loop_t_mpc, 'b+')
plt.plot(t_range, self.loop_t_wbc, '+', color="violet")
plt.plot(t_range, self.loop_t_loop, 'k+')
- plt.legend(["Estimator", "Planner", "MPC", "WBC", "Whole loop"])
+ plt.plot(t_range, self.loop_t_loop_if, '+', color="rebeccapurple")
+ plt.legend(["Estimator", "Planner", "MPC", "WBC", "Control loop", "Whole loop"])
+ plt.xlabel("Time [s]")
+ plt.ylabel("Time [s]")
+
+ # Plot estimated solving time of the model prediction control
+ plt.figure()
+ plt.plot(t_range[35:], self.mpc_solving_duration[35:], 'k+')
+ plt.legend(["Solving duration"])
plt.xlabel("Time [s]")
plt.ylabel("Time [s]")
@@ -686,10 +703,11 @@ class LoggerControl():
loop_t_mpc=self.loop_t_mpc,
loop_t_wbc=self.loop_t_wbc,
loop_t_loop=self.loop_t_loop,
+ loop_t_loop_if=self.loop_t_loop_if,
loop_q_filt_mpc=self.loop_q_filt_mpc,
loop_h_v_filt_mpc=self.loop_h_v_filt_mpc,
loop_h_v_bis_filt_mpc=self.loop_h_v_bis_filt_mpc,
- loop_vref_mpc=self.loop_vref_filt_mpc,
+ loop_vref_filt_mpc=self.loop_vref_filt_mpc,
planner_q_static=self.planner_q_static,
planner_RPY_static=self.planner_RPY_static,
@@ -703,6 +721,7 @@ class LoggerControl():
planner_h_ref=self.planner_h_ref,
mpc_x_f=self.mpc_x_f,
+ mpc_solving_duration=self.mpc_solving_duration,
wbc_P=self.wbc_P,
wbc_D=self.wbc_D,
@@ -779,6 +798,7 @@ class LoggerControl():
self.loop_t_mpc = data["loop_t_mpc"]
self.loop_t_wbc = data["loop_t_wbc"]
self.loop_t_loop = data["loop_t_loop"]
+ self.loop_t_loop_if = data["loop_t_loop_if"]
self.loop_q_filt_mpc = data["loop_q_filt_mpc"]
self.loop_h_v_filt_mpc = data["loop_h_v_filt_mpc"]
self.loop_h_v_bis_filt_mpc = data["loop_h_v_bis_filt_mpc"]
@@ -796,6 +816,7 @@ class LoggerControl():
self.planner_h_ref = data["planner_h_ref"]
self.mpc_x_f = data["mpc_x_f"]
+ self.mpc_solving_duration = data["mpc_solving_duration"]
self.wbc_P = data["wbc_P"]
self.wbc_D = data["wbc_D"]
@@ -1018,13 +1039,13 @@ if __name__ == "__main__":
import LoggerSensors
# Create loggers
- loggerSensors = LoggerSensors.LoggerSensors(logSize=5997)
- logger = LoggerControl(0.002, 100, logSize=5997)
+ loggerSensors = LoggerSensors.LoggerSensors(logSize=15000-3)
+ logger = LoggerControl(0.001, 30, logSize=15000-3)
# Load data from .npz file
logger.loadAll(loggerSensors)
# Call all ploting functions
- #logger.plotAll(loggerSensors)
+ logger.plotAll(loggerSensors)
- logger.slider_predicted_trajectory()
+ # logger.slider_predicted_trajectory()
diff --git a/scripts/MPC_Wrapper.py b/scripts/MPC_Wrapper.py
index 0fcc40d6767ce8510b7ba525479fb51d8611145b..beb7db3920a116c4cf697a55c9ada2467d5e6c05 100644
--- a/scripts/MPC_Wrapper.py
+++ b/scripts/MPC_Wrapper.py
@@ -6,6 +6,7 @@ from multiprocessing import Process, Value, Array
import crocoddyl_class.MPC_crocoddyl as MPC_crocoddyl
import crocoddyl_class.MPC_crocoddyl_planner as MPC_crocoddyl_planner
import pinocchio as pin
+from time import time
class Dummy:
"""Dummy class to store variables"""
@@ -39,6 +40,9 @@ class MPC_Wrapper:
self.params = params
+ self.t_mpc_solving_start = 0.0
+ self.t_mpc_solving_duration = 0.0
+
# Number of WBC steps for 1 step of the MPC
self.k_mpc = int(params.dt_mpc/params.dt_wbc)
@@ -95,6 +99,8 @@ class MPC_Wrapper:
l_targetFootstep (3x4 array) : 4*[x, y, z]^T target position in local frame, to stop the optimisation of the feet location around it
"""
+ self.t_mpc_solving_start = time()
+
if self.multiprocessing: # Run in parallel process
self.run_MPC_asynchronous(k, xref, fsteps, l_targetFootstep)
else: # Run in the same process than main loop
@@ -125,6 +131,7 @@ class MPC_Wrapper:
if self.multiprocessing:
if self.newResult.value:
self.newResult.value = False
+ self.t_mpc_solving_duration = time() - self.t_mpc_solving_start
# Retrieve desired contact forces with through the memory shared with the asynchronous
self.last_available_result = self.convert_dataOut()
return self.last_available_result