From 5bfe6a268278da8a3ac2e9144aa509adad6b25ad Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Thu, 11 Mar 2021 14:20:26 +0100
Subject: [PATCH] Tweaking solo behaviour during experiments
---
scripts/Controller.py | 2 +-
scripts/Estimator.py | 12 ++++++----
scripts/Joystick.py | 4 ++--
scripts/LoggerControl.py | 42 ++++++++++++++++++++++++++++++++++
scripts/main_solo12_control.py | 8 +++----
5 files changed, 56 insertions(+), 12 deletions(-)
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 436b35f0..b615d0ca 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -264,7 +264,7 @@ class Controller:
self.result.D = 0.2 * np.ones(12)
self.result.q_des[:] = self.myController.qdes[7:]
self.result.v_des[:] = self.myController.vdes[6:, 0]
- self.result.tau_ff[:] = 0.0 * self.myController.tau_ff
+ self.result.tau_ff[:] = 0.5 * self.myController.tau_ff
"""if self.k % 5 == 0:
self.solo.display(self.q)
diff --git a/scripts/Estimator.py b/scripts/Estimator.py
index 4470a35d..78087c33 100644
--- a/scripts/Estimator.py
+++ b/scripts/Estimator.py
@@ -250,7 +250,7 @@ class Estimator:
fc = 50.0 # Cut frequency
y = 1 - np.cos(2*np.pi*fc*dt)
self.alpha_v = -y+np.sqrt(y*y+2*y)
- self.alpha_v = 1.0 # TOREMOVE
+ # self.alpha_v = 1.0 # TOREMOVE
# Filtering velocities used for security checks
fc = 6.0
@@ -493,13 +493,15 @@ class Estimator:
a = np.ceil(np.max(self.k_since_contact)/10) - 1
b = remaining_steps
n = 1 # Nb of steps of margin around contact switch
- v = 0.97 # Minimum alpha value
+
+ v_max = 1.00
+ v_min = 0.97 # Minimum alpha value
c = ((a + b) - 2 * n) * 0.5
if (a <= (n-1)) or (b <= n): # If we are close from contact switch
- self.alpha = 1.0 # Only trust IMU data
+ self.alpha = v_max # Only trust IMU data
self.close_from_contact = True # Raise flag
else:
- self.alpha = v + (1 - v) * np.abs(c - (a - n)) / c
+ self.alpha = v_min + (v_max - v_min) * np.abs(c - (a - n)) / c
#self.alpha = 0.997
self.close_from_contact = False # Lower flag
@@ -534,7 +536,7 @@ class Estimator:
# Position of the center of the base from FGeometry and filtered velocity (world frame)
self.filt_lin_pos[:] = self.filter_xyz_pos.compute(
- self.FK_xyz[:] + self.xyz_mean_feet[:], ob_filt_lin_vel, alpha=0.995)
+ self.FK_xyz[:] + self.xyz_mean_feet[:], ob_filt_lin_vel, alpha=np.array([0.995, 0.995, 0.9]))
# Velocity of the center of the base (base frame)
self.filt_lin_vel[:] = b_filt_lin_vel
diff --git a/scripts/Joystick.py b/scripts/Joystick.py
index 25a2e851..07932ae7 100644
--- a/scripts/Joystick.py
+++ b/scripts/Joystick.py
@@ -39,7 +39,7 @@ class Joystick:
self.vX = 0.
self.vY = 0.
self.vYaw = 0.
- self.VxScale = 0.8
+ self.VxScale = 0.6
self.VyScale = 1.2
self.vYawScale = 1.6
@@ -205,7 +205,7 @@ class Joystick:
-0.3, 0.0])
elif velID == 2:
self.k_switch = np.array([0, 10000, 20000, 30000])
- self.v_switch = np.array([[0.0, 1.5, 0.0, 0.0],
+ self.v_switch = np.array([[0.0, 0.5, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0],
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index a0cda5a0..79fd689d 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -439,6 +439,48 @@ class LoggerControl():
plt.ylim([-1.0, 26.0])
plt.suptitle("Contact forces trajectories & Actual forces trajectories")
+ # Analysis of the complementary filter behaviour
+ clr = ["b", "darkred", "forestgreen"]
+ # Velocity complementary filter
+ lgd_Y = ["dx", "ddx", "alpha dx", "dx_out", "dy", "ddy", "alpha dy", "dy_out", "dz", "ddz", "alpha dz", "dz_out"]
+ plt.figure()
+ for i in range(12):
+ if i == 0:
+ ax0 = plt.subplot(3, 4, i+1)
+ else:
+ plt.subplot(3, 4, i+1, sharex=ax0)
+ if i % 4 == 0:
+ plt.plot(t_range, self.esti_HP_x[:, int(i/4)], color=clr[int(i/4)], linewidth=3, marker='') # x input of the velocity complementary filter
+ elif i % 4 == 1:
+ plt.plot(t_range, self.esti_HP_dx[:, int(i/4)], color=clr[int(i/4)], linewidth=3, marker='') # dx input of the velocity complementary filter
+ elif i % 4 == 2:
+ plt.plot(t_range, self.esti_HP_alpha[:, int(i/4)], color=clr[int(i/4)], linewidth=3, marker='') # alpha parameter of the velocity complementary filter
+ else:
+ plt.plot(t_range, self.esti_HP_filt_x[:, int(i/4)], color=clr[int(i/4)], linewidth=3, marker='') # filtered output of the velocity complementary filter
+
+ plt.legend([lgd_Y[i]], prop={'size': 8})
+ plt.suptitle("Evolution of the quantities of the velocity complementary filter")
+
+ # Position complementary filter
+ lgd_Y = ["x", "dx", "alpha x", "x_out", "y", "dy", "alpha y", "y_out", "z", "dz", "alpha z", "z_out"]
+ plt.figure()
+ for i in range(12):
+ if i == 0:
+ ax0 = plt.subplot(3, 4, i+1)
+ else:
+ plt.subplot(3, 4, i+1, sharex=ax0)
+ if i % 4 == 0:
+ plt.plot(t_range, self.esti_LP_x[:, int(i/4)], color=clr[int(i/4)], linewidth=3, marker='') # x input of the position complementary filter
+ elif i % 4 == 1:
+ plt.plot(t_range, self.esti_LP_dx[:, int(i/4)], color=clr[int(i/4)], linewidth=3, marker='') # dx input of the position complementary filter
+ elif i % 4 == 2:
+ plt.plot(t_range, self.esti_LP_alpha[:, int(i/4)], color=clr[int(i/4)], linewidth=3, marker='') # alpha parameter of the position complementary filter
+ else:
+ plt.plot(t_range, self.esti_LP_filt_x[:, int(i/4)], color=clr[int(i/4)], linewidth=3, marker='') # filtered output of the position complementary filter
+
+ plt.legend([lgd_Y[i]], prop={'size': 8})
+ plt.suptitle("Evolution of the quantities of the position complementary filter")
+
plt.show(block=True)
from IPython import embed
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index 94869294..6af4f530 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -15,7 +15,7 @@ from LoggerControl import LoggerControl
SIMULATION = False
-LOGGING = True
+LOGGING = False
PLOTTING = False
if SIMULATION:
@@ -106,7 +106,7 @@ def control_loop(name_interface, name_interface_clone=None):
################################
envID = 0 # Identifier of the environment to choose in which one the simulation will happen
- velID = 2 # Identifier of the reference velocity profile to choose which one will be sent to the robot
+ velID = 0 # 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
@@ -114,7 +114,7 @@ def control_loop(name_interface, name_interface_clone=None):
t = 0.0 # Time
T_gait = 0.32 # Duration of one gait period
T_mpc = 0.32 # Duration of the prediction horizon
- N_SIMULATION = 5000 # number of simulated wbc time steps
+ N_SIMULATION = 12000 # 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
@@ -224,7 +224,7 @@ def control_loop(name_interface, name_interface_clone=None):
# Send command to the robot
for i in range(1):
device.SendCommand(WaitEndOfCycle=True)
- """if ((device.cpt % 1000) == 0):
+ """if ((device.cpt % 100) == 0):
device.Print()"""
"""import os
--
GitLab