diff --git a/scripts/Controller.py b/scripts/Controller.py
index ed882e56622864b1c9ab0f68358132494f642bfb..ce733396e66600df3df7bdf56a972a1a95d759c4 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -115,6 +115,7 @@ class Controller:
self.q_wbc = np.zeros((18, 1))
self.dq_wbc = np.zeros((18, 1))
self.xgoals = np.zeros((12, 1))
+ self.xgoals[2, 0] = self.h_ref
self.statePlanner = lqrw.StatePlanner()
self.statePlanner.initialize(params)
@@ -324,7 +325,11 @@ class Controller:
self.feet_p_cmd = self.footTrajectoryGenerator.getFootPositionBaseFrame(oRh.transpose(), oTh)
#Â Desired position, orientation and velocities of the base
- self.xgoals[2, 0] = self.h_ref #Â Height (in horizontal frame!)
+ if not self.gait.getIsStatic():
+ self.xgoals[2:5, 0] = [self.h_ref, 0.0, 0.0] #Â Height (in horizontal frame!)
+ else:
+ self.xgoals[2:5, 0] += self.vref_filt_mpc[2:5, 0] * self.dt_wbc
+
self.xgoals[6:, 0] = self.vref_filt_mpc[:, 0] # Velocities (in horizontal frame!)
# Run InvKin + WBC QP
@@ -345,9 +350,9 @@ class Controller:
# Display robot in Gepetto corba viewer
if self.enable_corba_viewer and (self.k % 5 == 0):
- self.q_display[:3, 0] = np.array([0.0, 0.0, self.h_ref])
- self.q_display[3:7, 0] = np.array([0.0, 0.0, 0.0, 1.0])
- self.q_display[7:, 0] = self.wbcWrapper.qdes[:]
+ self.q_display[:3, 0] = self.q_wbc[0:3, 0]
+ self.q_display[3:7, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(self.q_wbc[3:6, 0])).coeffs()
+ self.q_display[7:, 0] = self.q_wbc[6:, 0]
self.solo.display(self.q_display)
t_wbc = time.time()
diff --git a/scripts/Joystick.py b/scripts/Joystick.py
index 6e2d6acf3a3ca145b22be93d075421a4f018c0dd..cc8d92508b546ed3a5f989a99a1b24e35588f0b1 100644
--- a/scripts/Joystick.py
+++ b/scripts/Joystick.py
@@ -42,7 +42,7 @@ class Joystick:
self.vYaw = 0.
self.VxScale = 0.5
self.VyScale = 0.8
- self.vYawScale = 3.0
+ self.vYawScale = 1.2
self.Vx_ref = 0.3
self.Vy_ref = 0.0
@@ -266,7 +266,7 @@ class Joystick:
[0.0, 0.0, 0.0, 0.0]])
elif velID == 10: # FORWAAAAAAAAAARD
self.t_switch = np.array([0, 2, 4, 6, 8, 11, 15])
- self.v_switch = np.array([[0.0, 0.4, 0.8, 1.2, 1.6, 1.8, 2.0],
+ self.v_switch = np.array([[0.0, 0.4, 0.8, 1.0, 1.0, 1.0, 1.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.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 408c771c6576476f7943ec0641bea4299a24a2f0..9cba02ea8d1b65c73a002c684537917f1234ae38 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -227,21 +227,23 @@ class LoggerControl():
#Â Reconstruct pos and vel of feet in base frame to compare them with the
# ones desired by the foot trajectory generator and whole-body control
from example_robot_data.robots_loader import Solo12Loader
- Solo12Loader.free_flyer = False
+ Solo12Loader.free_flyer = True
solo12 = Solo12Loader().robot
FL_FOOT_ID = solo12.model.getFrameId('FL_FOOT')
FR_FOOT_ID = solo12.model.getFrameId('FR_FOOT')
HL_FOOT_ID = solo12.model.getFrameId('HL_FOOT')
HR_FOOT_ID = solo12.model.getFrameId('HR_FOOT')
foot_ids = np.array([FL_FOOT_ID, FR_FOOT_ID, HL_FOOT_ID, HR_FOOT_ID])
- q = np.zeros((12, 1))
- dq = np.zeros((12, 1))
- pin.computeAllTerms(solo12.model, solo12.data, q, np.zeros((12, 1)))
+ q = np.zeros((19, 1))
+ dq = np.zeros((18, 1))
+ pin.computeAllTerms(solo12.model, solo12.data, q, np.zeros((18, 1)))
feet_pos = np.zeros([self.esti_q_filt.shape[0], 3, 4])
feet_vel = np.zeros([self.esti_q_filt.shape[0], 3, 4])
for i in range(self.esti_q_filt.shape[0]):
- q[:, 0] = self.loop_o_q[i, 6:]
- dq[:, 0] = self.loop_o_v[i, 6:]
+ q[:3, 0] = self.loop_q_filt_mpc[i, :3]
+ q[3:7, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(self.loop_q_filt_mpc[i, 3:6])).coeffs()
+ q[7:, 0] = self.loop_o_q[i, 6:]
+ dq[6:, 0] = self.loop_o_v[i, 6:]
pin.forwardKinematics(solo12.model, solo12.data, q, dq)
pin.updateFramePlacements(solo12.model, solo12.data)
for j, idx in enumerate(foot_ids):
diff --git a/scripts/crocoddyl_eval/test_6/compare_mpcs.py b/scripts/crocoddyl_eval/test_6/compare_mpcs.py
index 43d63ad418f5490e73318a536190bff0fd7cdb6b..a9a68a3156ded2ad8564f74535c1c582193ce983 100644
--- a/scripts/crocoddyl_eval/test_6/compare_mpcs.py
+++ b/scripts/crocoddyl_eval/test_6/compare_mpcs.py
@@ -59,8 +59,9 @@ def compute_RMSE(array, norm):
# [Linear, Non Linear, Planner, OSQP]
MPCs = [True, True, True, True] # Boolean to choose which MPC to plot
-MPCs_names = ["No FF", "-afeet", "JT fmpc", "JT fmpc + M ddq"]
+MPCs_names = ["No FF", "-afeet", "JT fmpc + M ddq", "Free flyer + Tasks"]
name_files = ["data_2021_08_27_16_08_0.npz", "data_2021_08_27_15_53_0.npz", "data_2021_08_27_15_42_0.npz", "data_2021_08_27_15_44_0.npz"] # Names of the files
+name_files = ["data_2021_09_02_16_51_0.npz", "data_2021_09_02_16_53_0.npz", "data_2021_09_02_16_55_0.npz", "data_2021_09_02_17_52_0.npz"] # Names of the files
folder_path = "" # Folder containing the 4 .npz files
# Common data shared by 4 MPCs
@@ -70,13 +71,17 @@ joy_v_ref = logs.get('joy_v_ref') # Ref velocity (Nx6) given by the joysti
planner_xref = logs.get("planner_xref") # Ref state
N = joy_v_ref.shape[0] # Size of the measures
data_ = np.zeros((N,12,4)) # Store states measured by MOCAP, 4 MPCs (pos,orientation,vel,ang vel)
-tau_ff_ = np.zeros((N,12,4)) # Store feedforward torques
+tau_ff_ = np.zeros((N,12,4)) # Store feedforward torques
+mpc_x_f = np.zeros((N,24,4)) # Store feedforward torques
+wbc_f_ctc = np.zeros((N,12,4)) # Store feedforward torques
# Get state measured
for i in range(4):
if MPCs[i]:
data_[:,:,i] = get_mocap_logs(folder_path + name_files[i])
tau_ff_[:, :, i] = np.load(folder_path + name_files[i]).get("wbc_tau_ff")
+ mpc_x_f[:, :, i] = np.load(folder_path + name_files[i]).get("mpc_x_f")[:, :, 0]
+ wbc_f_ctc[:, :, i] = np.load(folder_path + name_files[i]).get("wbc_f_ctc")
for j in range(4):
for i in range(12):
@@ -91,7 +96,7 @@ lgd = ["Position X", "Position Y", "Position Z", "Position Roll", "Position Pitc
index6 = [1, 3, 5, 2, 4, 6]
t_range = np.array([k*params.dt_wbc for k in range(N)])
-color = ["k", "b", "r", "g--"]
+color = ["k", "b", "r", "g"]
legend = []
for i in range(4):
if MPCs[i]:
@@ -123,7 +128,7 @@ for i in range(6):
plt.legend(legend, prop={'size': 8})
plt.ylabel(lgd[i])
-plt.suptitle("Measured postion and orientation - MOCAP - ")
+plt.suptitle("Measured velocities - MOCAP - ")
# Compute difference measured - reference
data_diff = np.zeros((N, 12,4))
@@ -276,6 +281,30 @@ for i in range(12):
plt.legend(legend, prop={'size': 8})
plt.ylim([-8.0, 8.0])
+####
+# Contact forces (MPC command) & WBC QP output
+####
+lgd1 = ["Ctct force X", "Ctct force Y", "Ctct force Z"]
+lgd2 = ["FL", "FR", "HL", "HR"]
+legend_tmp = []
+for name in legend:
+ legend_tmp.append(name + " MPC")
+ legend_tmp.append(name + " WBC")
+plt.figure()
+for i in range(3):
+ if i == 0:
+ ax0 = plt.subplot(3, 1, i+1)
+ else:
+ plt.subplot(3, 1, i+1, sharex=ax0)
+ for j in range(4):
+ if MPCs[j]:
+ h1, = plt.plot(t_range, mpc_x_f[:, 12+i, j], color[j], linewidth=3, linestyle="-")
+ h2, = plt.plot(t_range, wbc_f_ctc[:, i, j], color[j], linewidth=3, linestyle="--")
+ plt.xlabel("Time [s]")
+ plt.ylabel(lgd1[i % 3]+" "+lgd2[int(i/3)]+" [N]")
+ plt.legend(legend_tmp, prop={'size': 8})
+
+
# Display all graphs and wait
plt.show(block=True)
diff --git a/src/Gait.cpp b/src/Gait.cpp
index 531196a837bce6fa0d12c21ce84dff18223982c9..3d4fdc8b7c51408c464265227f521288e0625791 100644
--- a/src/Gait.cpp
+++ b/src/Gait.cpp
@@ -9,7 +9,7 @@ Gait::Gait()
T_mpc_(0.0),
remainingTime_(0.0),
newPhase_(false),
- is_static_(true),
+ is_static_(false),
switch_to_gait_(0),
q_static_(VectorN::Zero(19)) {
// Empty
@@ -227,8 +227,8 @@ bool Gait::changeGait(int const k, int const k_mpc, int const code) {
if (code != 0 && switch_to_gait_ == 0) {
switch_to_gait_ = code;
}
- is_static_ = false;
if (switch_to_gait_ != 0 && std::remainder(static_cast<double>(k - k_mpc), (k_mpc * T_gait_ * 0.5) / dt_) == 0.0) {
+ is_static_ = false;
switch (switch_to_gait_) {
case 1:
create_pacing();
@@ -240,6 +240,7 @@ bool Gait::changeGait(int const k, int const k_mpc, int const code) {
create_trot();
break;
case 4:
+ is_static_ = true;
create_static();
break;
}
diff --git a/src/config_solo12.yaml b/src/config_solo12.yaml
index 9f557cc7d3f50ced70a68dfaebb1e34dde33789e..8801f59ef2da7cea37e7011d8e215996d5bff473 100644
--- a/src/config_solo12.yaml
+++ b/src/config_solo12.yaml
@@ -2,16 +2,16 @@ robot:
# General parameters
interface: enp2s0 # Name of the communication interface (check with ifconfig)
SIMULATION: true # Enable/disable PyBullet simulation or running on real robot
- LOGGING: true # Enable/disable logging during the experiment
- PLOTTING: false # Enable/disable automatic plotting at the end of the experiment
+ LOGGING: false # Enable/disable logging during the experiment
+ PLOTTING: true # Enable/disable automatic plotting at the end of the experiment
envID: 0 # Identifier of the environment to choose in which one the simulation will happen
use_flat_plane: true # If True the ground is flat, otherwise it has bumps
- predefined_vel: true # If we are using a predefined reference velocity (True) or a joystick (False)
+ predefined_vel: false # If we are using a predefined reference velocity (True) or a joystick (False)
velID: 10 # Identifier of the reference velocity profile to choose which one will be sent to the robot
- N_SIMULATION: 5000 # Number of simulated wbc time steps
- enable_pyb_GUI: false # Enable/disable PyBullet GUI
- enable_corba_viewer: false # Enable/disable Corba Viewer
- enable_multiprocessing: false # Enable/disable running the MPC in another process in parallel of the main loop
+ N_SIMULATION: 60000 # Number of simulated wbc time steps
+ enable_pyb_GUI: true # Enable/disable PyBullet GUI
+ enable_corba_viewer: true # Enable/disable Corba Viewer
+ enable_multiprocessing: true # Enable/disable running the MPC in another process in parallel of the main loop
perfect_estimator: false # Enable/disable perfect estimator by using data directly from PyBullet
# General control parameters
@@ -43,14 +43,14 @@ robot:
# Parameters of InvKin
Kp_flyingfeet: 100.0 # Proportional gain for feet position tasks
- Kd_flyingfeet: 20.0 # Derivative gain for feet position tasks
+ Kd_flyingfeet: 10.0 # Derivative gain for feet position tasks
Kp_base_position: [100.0, 100.0, 100.0] # Proportional gains for the base position task
- Kd_base_position: [20.0, 20.0, 20.0] # Derivative gains for the base position task
- Kp_base_orientation: [100.0, 100.0, 0.0] # Proportional gains for the base orientation task
- Kd_base_orientation: [20.0, 20.0, 0.0] # Derivative gains for the base orientation task
+ Kd_base_position: [10.0, 10.0, 10.0] # Derivative gains for the base position task
+ Kp_base_orientation: [100.0, 100.0, 100.0] # Proportional gains for the base orientation task
+ Kd_base_orientation: [10.0, 10.0, 10.0] # Derivative gains for the base orientation task
# Parameters of WBC QP problem
Q1: 0.1 # Weights for the "delta articular accelerations" optimization variables
- Q2: 5.0 # Weights for the "delta contact forces" optimization variables
+ Q2: 1.0 # Weights for the "delta contact forces" optimization variables
Fz_max: 35.0 # Maximum vertical contact force [N]
Fz_min: 0.0 # Minimal vertical contact force [N]
\ No newline at end of file