diff --git a/scripts/Controller.py b/scripts/Controller.py
index 0a98dc67eb2c1d26a8c7b8077036226dd0d10849..0ec295130f27333fc67b8d27cb164325b86204e0 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -48,7 +48,7 @@ class dummyDevice:
class Controller:
def __init__(self, q_init, envID, velID, dt_tsid, dt_mpc, k_mpc, t, T_gait, T_mpc, N_SIMULATION, type_MPC,
- pyb_feedback, on_solo8, use_flat_plane, predefined_vel, enable_pyb_GUI):
+ pyb_feedback, on_solo8, use_flat_plane, predefined_vel, enable_pyb_GUI, kf_enabled):
"""Function that runs a simulation scenario based on a reference velocity profile, an environment and
various parameters to define the gait
@@ -68,6 +68,7 @@ class Controller:
use_flat_plane (bool): to use either a flat ground or a rough ground
predefined_vel (bool): to use either a predefined velocity profile or a gamepad
enable_pyb_GUI (bool): to display PyBullet GUI or not
+ kf_enabled (bool): complementary filter (False) or kalman filter (True)
"""
########################################################################
@@ -99,7 +100,7 @@ class Controller:
# Create Joystick, FootstepPlanner, Logger and Interface objects
self.joystick, self.logger, self.estimator = utils_mpc.init_objects(
- dt_tsid, dt_mpc, N_SIMULATION, k_mpc, T_gait, type_MPC, predefined_vel, self.h_init)
+ dt_tsid, dt_mpc, N_SIMULATION, k_mpc, T_gait, type_MPC, predefined_vel, self.h_init, kf_enabled)
# Enable/Disable hybrid control
self.enable_hybrid_control = True
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index 4bce112ba89d2703e2838db0ca050d875de748bb..a0cda5a0233f168b9b631c34ee2f5497ecf03976 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -39,6 +39,9 @@ class LoggerControl():
self.esti_LP_alpha = np.zeros([logSize, 3]) # alpha parameter of the position complementary filter
self.esti_LP_filt_x = np.zeros([logSize, 3]) # filtered output of the position complementary filter
+ self.esti_kf_X = np.zeros([logSize, 18]) # state of the Kalman filter
+ self.esti_kf_Z = np.zeros([logSize, 16]) # measurement for the Kalman filter
+
# Loop
self.loop_o_q_int = np.zeros([logSize, 19]) # position in world frame (esti_q_filt + dt * loop_o_v)
self.loop_o_v = np.zeros([logSize, 18]) # estimated velocity in world frame
@@ -96,16 +99,19 @@ class LoggerControl():
self.esti_FK_lin_vel[self.i] = estimator.FK_lin_vel[:]
self.esti_FK_xyz[self.i] = estimator.FK_xyz[:]
self.esti_xyz_mean_feet[self.i] = estimator.xyz_mean_feet[:]
-
- self.esti_HP_x[self.i] = estimator.filter_xyz_vel.x
- self.esti_HP_dx[self.i] = estimator.filter_xyz_vel.dx
- self.esti_HP_alpha[self.i] = estimator.filter_xyz_vel.alpha
- self.esti_HP_filt_x[self.i] = estimator.filter_xyz_vel.filt_x
-
- self.esti_LP_x[self.i] = estimator.filter_xyz_pos.x
- self.esti_LP_dx[self.i] = estimator.filter_xyz_pos.dx
- self.esti_LP_alpha[self.i] = estimator.filter_xyz_pos.alpha
- self.esti_LP_filt_x[self.i] = estimator.filter_xyz_pos.filt_x
+ if not estimator.kf_enabled:
+ self.esti_HP_x[self.i] = estimator.filter_xyz_vel.x
+ self.esti_HP_dx[self.i] = estimator.filter_xyz_vel.dx
+ self.esti_HP_alpha[self.i] = estimator.filter_xyz_vel.alpha
+ self.esti_HP_filt_x[self.i] = estimator.filter_xyz_vel.filt_x
+
+ self.esti_LP_x[self.i] = estimator.filter_xyz_pos.x
+ self.esti_LP_dx[self.i] = estimator.filter_xyz_pos.dx
+ self.esti_LP_alpha[self.i] = estimator.filter_xyz_pos.alpha
+ self.esti_LP_filt_x[self.i] = estimator.filter_xyz_pos.filt_x
+ else:
+ self.esti_kf_X[self.i] = estimator.kf.X[:, 0]
+ self.esti_kf_Z[self.i] = estimator.Z[:, 0]
# Logging from the main loop
self.loop_o_q_int[self.i] = loop.q_estim[:, 0]
@@ -465,11 +471,14 @@ class LoggerControl():
esti_LP_alpha=self.esti_LP_alpha,
esti_LP_filt_x=self.esti_LP_filt_x,
+ esti_kf_X=self.esti_kf_X,
+ esti_kf_Z=self.esti_kf_Z,
+
loop_o_q_int=self.loop_o_q_int,
loop_o_v=self.loop_o_v,
- loop_q_static=self.loop_q_static,
- loop_RPY_static=self.loop_RPY_static,
+ planner_q_static=self.planner_q_static,
+ planner_RPY_static=self.planner_RPY_static,
planner_xref=self.planner_xref,
planner_fsteps=self.planner_fsteps,
planner_gait=self.planner_gait,
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index bcbc1b74c6ac3933cacd1bf13fc89a7f4d9557d2..f7468d121840617cb37b52e4412b4d65e287b999 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -14,6 +14,7 @@ from LoggerControl import LoggerControl
SIMULATION = True
LOGGING = False
+PLOTTING = False
if SIMULATION:
from PyBulletSimulator import PyBulletSimulator
@@ -77,7 +78,7 @@ def control_loop(name_interface):
################################
envID = 0 # Identifier of the environment to choose in which one the simulation will happen
- velID = 1 # Identifier of the reference velocity profile to choose which one will be sent to the robot
+ velID = 2 # Identifier of the reference velocity profile to choose which one will be sent to the robot
dt_wbc = DT # Time step of the whole body control
dt_mpc = 0.02 # Time step of the model predictive control
@@ -85,7 +86,7 @@ def control_loop(name_interface):
t = 0.0 # Time
T_gait = 0.32 # Duration of one gait period
T_mpc = 0.32 # Duration of the prediction horizon
- N_SIMULATION = 50000 # number of simulated wbc time steps
+ N_SIMULATION = 20000 # number of simulated wbc time steps
# Which MPC solver you want to use
# True to have PA's MPC, to False to have Thomas's MPC
@@ -103,6 +104,9 @@ def control_loop(name_interface):
# If we are using a predefined reference velocity (True) or a joystick (False)
predefined_vel = True
+ # Use complementary filter (False) or kalman filter (True) for the estimator
+ kf_enabled = False
+
# Enable or disable PyBullet GUI
enable_pyb_GUI = True
if not SIMULATION:
@@ -113,7 +117,7 @@ def control_loop(name_interface):
# Run a scenario and retrieve data thanks to the logger
controller = Controller(q_init, envID, velID, dt_wbc, dt_mpc, k_mpc, t, T_gait, T_mpc, N_SIMULATION, type_MPC,
- pyb_feedback, on_solo8, use_flat_plane, predefined_vel, enable_pyb_GUI)
+ pyb_feedback, on_solo8, use_flat_plane, predefined_vel, enable_pyb_GUI, kf_enabled)
####
@@ -124,12 +128,10 @@ def control_loop(name_interface):
device = Solo12(name_interface, dt=DT)
qc = QualisysClient(ip="140.93.16.160", body_id=0)
- if LOGGING:
- logger = Logger(device, qualisys=qc, logSize=N_SIMULATION)
-
- loggerSensors = LoggerSensors(device, qualisys=qc, logSize=N_SIMULATION-3)
- loggerControl = LoggerControl(dt_wbc, joystick=controller.joystick, estimator=controller.estimator,
- loop=controller, planner=controller.planner, logSize=N_SIMULATION-3)
+ if LOGGING or PLOTTING:
+ loggerSensors = LoggerSensors(device, qualisys=qc, logSize=N_SIMULATION-3)
+ loggerControl = LoggerControl(dt_wbc, joystick=controller.joystick, estimator=controller.estimator,
+ loop=controller, planner=controller.planner, logSize=N_SIMULATION-3)
# Number of motors
nb_motors = device.nb_motors
@@ -148,11 +150,28 @@ def control_loop(name_interface):
t = 0.0
t_max = (N_SIMULATION-2) * dt_wbc
+ """log_cpt = 0
+ log_o_v = np.zeros((3, N_SIMULATION))
+ log_b_v = np.zeros((3, N_SIMULATION))
+ log_o_v_truth = np.zeros((3, N_SIMULATION))
+ log_b_v_truth = np.zeros((3, N_SIMULATION))
+ o_v = np.zeros((3, 1))
+ b_v = np.zeros((3, 1))"""
while ((not device.hardware.IsTimeout()) and (t < t_max) and (not controller.myController.error)):
# Update sensor data (IMU, encoders, Motion capture)
device.UpdateMeasurment()
+ """rot_oMb = pin.Quaternion(np.array([device.baseOrientation]).transpose()).toRotationMatrix()
+ a = np.array([device.baseLinearAcceleration]).T
+ o_v[:, 0:1] += 0.002 * (rot_oMb @ a)
+ b_v[:, 0:1] = rot_oMb.T @ o_v
+ log_o_v[:, log_cpt] = o_v[:, 0]
+ log_b_v[:, log_cpt] = b_v[:, 0]
+ log_o_v_truth[:, log_cpt] = device.o_baseVel[:, 0]
+ log_b_v_truth[:, log_cpt] = device.b_baseVel[:]
+ log_cpt += 1"""
+
# Desired torques
controller.compute(device)
@@ -172,11 +191,10 @@ def control_loop(name_interface):
device.SetDesiredJointTorque(controller.result.tau_ff.ravel())
# Call logger
- if LOGGING:
- logger.sample(device, qualisys=qc, estimator=controller.estimator)
- loggerSensors.sample(device, qc)
- loggerControl.sample(controller.joystick, controller.estimator,
- controller, controller.planner, controller.myController)
+ if LOGGING or PLOTTING:
+ loggerSensors.sample(device, qc)
+ loggerControl.sample(controller.joystick, controller.estimator,
+ controller, controller.planner, controller.myController)
# Send command to the robot
for i in range(1):
@@ -251,7 +269,25 @@ def control_loop(name_interface):
# controller.estimator.plot_graphs()
- """import matplotlib.pylab as plt
+ """from matplotlib import pyplot as plt
+
+ index6 = [1, 3, 5, 2, 4, 6]
+ plt.figure()
+ for i in range(6):
+ if i == 0:
+ ax0 = plt.subplot(3, 2, index6[i])
+ else:
+ plt.subplot(3, 2, index6[i], sharex=ax0)
+
+ if i < 3:
+ plt.plot(log_o_v_truth[i, :], c='r', linewidth=3)
+ plt.plot(log_o_v[i, :], c='b', linewidth=3)
+ else:
+ plt.plot(log_b_v_truth[i-3, :], c='r', linewidth=3)
+ plt.plot(log_b_v[i-3, :], c='b', linewidth=3)
+ plt.show()"""
+
+ """from matplotlib import pyplot as plt
plt.figure()
plt.plot(controller.t_list_filter[1:], 'r+')
plt.plot(controller.t_list_planner[1:], 'g+')
@@ -265,7 +301,7 @@ def control_loop(name_interface):
plt.title("Loop time [s]")
plt.show(block=True)"""
- """import matplotlib.pylab as plt
+ """from matplotlib import pyplot as plt
N = len(controller.log_tmp2)
t_range = np.array([k*0.002 for k in range(N)])
plt.figure()
@@ -275,16 +311,13 @@ def control_loop(name_interface):
plt.plot(t_range, controller.log_tmp4, 'g')
# plt.show(block=True)"""
- # controller.myController.saveAll(fileName="push_pyb_with_ff", log_date=False)
- if LOGGING:
- controller.myController.saveAll(fileName="data_control", log_date=True)
- print("-- Controller log saved --")
- # controller.myController.show_logs()
- loggerControl.plotAll(loggerSensors)
+ # Plot recorded data
+ if PLOTTING:
+ loggerControl.plotAll(loggerSensors)
# Save the logs of the Logger object
if LOGGING:
- logger.saveAll()
+ loggerControl.saveAll(loggerSensors)
print("Log saved")
if SIMULATION and enable_pyb_GUI:
diff --git a/scripts/utils_mpc.py b/scripts/utils_mpc.py
index 94bf8749bc27cebca6c396385c63b41319fc949f..96739fde8c5712314a9903bde85ce2a4a9709797 100644
--- a/scripts/utils_mpc.py
+++ b/scripts/utils_mpc.py
@@ -125,7 +125,7 @@ def init_robot(q_init, enable_viewer):
return solo, fsteps_init, h_init
-def init_objects(dt_tsid, dt_mpc, k_max_loop, k_mpc, T_mpc, type_MPC, predefined, h_init):
+def init_objects(dt_tsid, dt_mpc, k_max_loop, k_mpc, T_mpc, type_MPC, predefined, h_init, kf_enabled):
"""Create several objects that are used in the control loop
Args:
@@ -137,6 +137,7 @@ def init_objects(dt_tsid, dt_mpc, k_max_loop, k_mpc, T_mpc, type_MPC, predefined
type_MPC (bool): which MPC you want to use (PA's or Thomas')
predefined (bool): if we are using a predefined reference velocity (True) or a joystick (False)
h_init (float): initial height of the robot base
+ kf_enabled (bool): complementary filter (False) or kalman filter (True)
"""
# Create Joystick object
@@ -146,7 +147,7 @@ def init_objects(dt_tsid, dt_mpc, k_max_loop, k_mpc, T_mpc, type_MPC, predefined
logger = Logger.Logger(k_max_loop, dt_tsid, dt_mpc, k_mpc, T_mpc, type_MPC)
# Create Estimator object
- estimator = Estimator.Estimator(dt_tsid, k_max_loop, h_init)
+ estimator = Estimator.Estimator(dt_tsid, k_max_loop, h_init, kf_enabled)
return joystick, logger, estimator