From 3de310d3ce235c51767ddde031291fa55b7ab8dc Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Wed, 21 Apr 2021 18:31:55 +0200
Subject: [PATCH] Add possibility to use estimator with perfect height and
linear velocity estimation (when in simulation)
---
scripts/Controller.py | 14 ++++++++++++--
scripts/Estimator.py | 20 ++++++++++++++------
scripts/main_solo12_control.py | 3 ++-
scripts/utils_mpc.py | 5 +++--
4 files changed, 31 insertions(+), 11 deletions(-)
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 7a6b92cf..7ad95641 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -49,7 +49,8 @@ class dummyDevice:
class Controller:
def __init__(self, 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, kf_enabled, N0_gait):
+ pyb_feedback, on_solo8, use_flat_plane, predefined_vel, enable_pyb_GUI, kf_enabled, N0_gait,
+ isSimulation):
"""Function that runs a simulation scenario based on a reference velocity profile, an environment and
various parameters to define the gait
@@ -71,6 +72,7 @@ class Controller:
enable_pyb_GUI (bool): to display PyBullet GUI or not
kf_enabled (bool): complementary filter (False) or kalman filter (True)
N0_gait (int): number of spare lines in the gait matrix
+ isSimulation (bool): if we are in simulation mode
"""
########################################################################
@@ -98,12 +100,18 @@ class Controller:
'''if self.enable_gepetto_viewer:
self.view = viewerClient()'''
+ # Enable/Disable perfect estimator
+ perfectEstimator = False
+ if not isSimulation:
+ perfectEstimator = False # Cannot use perfect estimator if we are running on real robot
+
# Initialisation of the solo model/data and of the Gepetto viewer
self.solo, self.fsteps_init, self.h_init = utils_mpc.init_robot(q_init, self.enable_gepetto_viewer)
# Create Joystick, FootstepPlanner, Logger and Interface objects
self.joystick, self.logger, self.estimator = utils_mpc.init_objects(
- dt_wbc, dt_mpc, N_SIMULATION, k_mpc, T_gait, type_MPC, predefined_vel, self.h_init, kf_enabled)
+ dt_wbc, dt_mpc, N_SIMULATION, k_mpc, T_gait, type_MPC, predefined_vel, self.h_init, kf_enabled,
+ perfectEstimator)
# Enable/Disable hybrid control
self.enable_hybrid_control = True
@@ -179,6 +187,8 @@ class Controller:
dDevice.baseLinearAcceleration = np.zeros(3)
dDevice.baseAngularVelocity = np.zeros(3)
dDevice.baseOrientation = np.array([0.0, 0.0, 0.0, 1.0])
+ dDevice.dummyPos = np.array([0.0, 0.0, q_init[2]])
+ dDevice.b_baseVel = np.zeros(3)
self.compute(dDevice)
def compute(self, device):
diff --git a/scripts/Estimator.py b/scripts/Estimator.py
index c3af6b66..6d442208 100644
--- a/scripts/Estimator.py
+++ b/scripts/Estimator.py
@@ -239,13 +239,17 @@ class Estimator:
N_simulation (int): maximum number of iterations of the main control loop
h_init (float): initial height of the robot base
kf_enabled (bool): False for complementary filter, True for simple Kalman filter
+ perfectEstimator (bool): If we are using a perfect estimator (direct simulator data)
"""
- def __init__(self, dt, N_simulation, h_init=0.22294615, kf_enabled=False):
+ def __init__(self, dt, N_simulation, h_init=0.22294615, kf_enabled=False, perfectEstimator=False):
# Sample frequency
self.dt = dt
+ # If the IMU is perfect
+ self.perfectEstimator = perfectEstimator
+
# Filtering estimated linear velocity
fc = 50.0 # Cut frequency
y = 1 - np.cos(2*np.pi*fc*dt)
@@ -585,13 +589,18 @@ class Estimator:
# Output filtered position vector (19 x 1)
self.q_filt[0:3, 0] = self.filt_lin_pos
+ if self.perfectEstimator: # Base height directly from PyBullet
+ self.q_filt[2, 0] = device.dummyPos[2] - 0.0155 # Minus feet radius
self.q_filt[3:7, 0] = self.filt_ang_pos
- self.q_filt[7:, 0] = self.actuators_pos
+ self.q_filt[7:, 0] = self.actuators_pos # Actuators pos are already directly from PyBullet
# Output filtered velocity vector (18 x 1)
- self.v_filt[0:3, 0] = (1 - self.alpha_v) * self.v_filt[0:3, 0] + self.alpha_v * self.filt_lin_vel
- self.v_filt[3:6, 0] = self.filt_ang_vel
- self.v_filt[6:, 0] = self.actuators_vel
+ if self.perfectEstimator: # Linear velocities directly from PyBullet
+ self.v_filt[0:3, 0] = (1 - self.alpha_v) * self.v_filt[0:3, 0] + self.alpha_v * device.b_baseVel
+ else:
+ self.v_filt[0:3, 0] = (1 - self.alpha_v) * self.v_filt[0:3, 0] + self.alpha_v * self.filt_lin_vel
+ self.v_filt[3:6, 0] = self.filt_ang_vel # Angular velocities are already directly from PyBullet
+ self.v_filt[6:, 0] = self.actuators_vel # Actuators velocities are already directly from PyBullet
###
@@ -608,7 +617,6 @@ class Estimator:
###
-
# Output filtered actuators velocity for security checks
self.v_secu[:] = (1 - self.alpha_secu) * self.actuators_vel + self.alpha_secu * self.v_secu[:]
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index 5e3d9470..ac28183d 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -146,7 +146,8 @@ def control_loop(name_interface, name_interface_clone=None):
# 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, kf_enabled, N0_gait)
+ pyb_feedback, on_solo8, use_flat_plane, predefined_vel, enable_pyb_GUI, kf_enabled,
+ N0_gait, SIMULATION)
####
diff --git a/scripts/utils_mpc.py b/scripts/utils_mpc.py
index e0c70c1d..0fea358d 100644
--- a/scripts/utils_mpc.py
+++ b/scripts/utils_mpc.py
@@ -129,7 +129,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, kf_enabled):
+def init_objects(dt_tsid, dt_mpc, k_max_loop, k_mpc, T_mpc, type_MPC, predefined, h_init, kf_enabled, perfectEstimator):
"""Create several objects that are used in the control loop
Args:
@@ -142,6 +142,7 @@ def init_objects(dt_tsid, dt_mpc, k_max_loop, k_mpc, T_mpc, type_MPC, predefined
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)
+ perfectEstimator (bool): if we use a perfect estimator
"""
# Create Joystick object
@@ -151,7 +152,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, kf_enabled)
+ estimator = Estimator.Estimator(dt_tsid, k_max_loop, h_init, kf_enabled, perfectEstimator)
return joystick, logger, estimator
--
GitLab