diff --git a/scripts/Controller.py b/scripts/Controller.py
index 32a08fec0fc190e85a4abf5a7d844fe06a6f1593..3f03fc4789f1b4c4b8131503439de33cbe822789 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -94,7 +94,7 @@ class Controller:
         self.ID_deb_lines = []
 
         # Enable/Disable Gepetto viewer
-        self.enable_gepetto_viewer = True
+        self.enable_gepetto_viewer = False
         '''if self.enable_gepetto_viewer:
             self.view = viewerClient()'''
 
diff --git a/scripts/Estimator.py b/scripts/Estimator.py
index 313b9d80aadc12568249a60d0060829dd9247807..0f3831021c076e8d0482e9c3d5c2a97c97360441 100644
--- a/scripts/Estimator.py
+++ b/scripts/Estimator.py
@@ -2,7 +2,7 @@
 
 import numpy as np
 import pinocchio as pin
-from example_robot_data import load
+from example_robot_data.robots_loader import Solo12Loader
 
 
 class KFilter:
@@ -291,7 +291,8 @@ class Estimator:
         self.k_since_contact = np.zeros(4)
 
         # Load the URDF model to get Pinocchio data and model structures
-        robot = load('solo12')
+        Solo12Loader.free_flyer = True
+        robot = Solo12Loader().robot
         self.data = robot.data.copy()  # for velocity estimation (forward kinematics)
         self.model = robot.model.copy()  # for velocity estimation (forward kinematics)
         self.data_for_xyz = robot.data.copy()  # for position estimation (forward geometry)
diff --git a/scripts/Joystick.py b/scripts/Joystick.py
index 0e2485f256c013b53f664fe84f4d52ccad13689f..fe27d8c7c3b6a819bddc9b3293cfc47393990665 100644
--- a/scripts/Joystick.py
+++ b/scripts/Joystick.py
@@ -35,6 +35,9 @@ class Joystick:
         # If we are using a predefined reference velocity (True) or a joystick (False)
         self.predefined = predefined
 
+        # If we are performing an analysis from outside
+        self.analysis = False
+
         # Joystick variables (linear and angular velocity and their scaling for the joystick)
         self.vX = 0.
         self.vY = 0.
@@ -66,6 +69,8 @@ class Joystick:
         if self.predefined:
             if self.multi_simu:
                 self.update_v_ref_multi_simu(k_loop)
+            elif self.analysis:
+                self.handle_v_switch(k_loop)
             else:
                 self.update_v_ref_predefined(k_loop, velID)
         else:
@@ -308,3 +313,14 @@ class Joystick:
             [[self.Vx_ref*alpha_x, self.Vy_ref*alpha_y, 0.0, 0.0, 0.0, self.Vw_ref*alpha_w]]).T
 
         return 0
+
+    def update_for_analysis(self, des_vel_analysis, N_analysis, N_steady):
+
+        self.analysis = True
+
+        self.k_switch = np.array([0, 500, N_analysis, N_analysis + N_steady])
+        self.v_switch = np.zeros((6, 4))
+        self.v_switch[:, 2] = des_vel_analysis
+        self.v_switch[:, 3] = des_vel_analysis
+
+        return 0
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index e0c11018bc036b28e463025ea0bc5c2310679521..d8be4b19352a10fcfce9d1b2d6ba285f396e8c90 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -311,9 +311,9 @@ class LoggerControl():
             else:
                 plt.plot(t_range, self.mocap_b_w[:, i-3], "k", linewidth=3)
 
-                N = 2000
+                """N = 2000
                 y = np.convolve(self.mocap_b_w[:, i-3], np.ones(N)/N, mode='valid')
-                plt.plot(t_range[int(N/2)-1:-int(N/2)], y, linewidth=3, linestyle="--")
+                plt.plot(t_range[int(N/2)-1:-int(N/2)], y, linewidth=3, linestyle="--")"""
 
             # plt.plot(t_range, self.log_dq[i, :], "g", linewidth=2)
             # plt.plot(t_range[:-2], self.log_dx_invkin[i, :-2], "g", linewidth=2)
diff --git a/scripts/QP_WBC.py b/scripts/QP_WBC.py
index b69082fb8bb6897dcd4959d29d002c387febdb43..4dc1f738a3e62066d2a3af04a03e955b409ea421 100644
--- a/scripts/QP_WBC.py
+++ b/scripts/QP_WBC.py
@@ -6,7 +6,7 @@ from solo12InvKin import Solo12InvKin
 from time import perf_counter as clock
 from time import time
 import libquadruped_reactive_walking as lrw
-from example_robot_data import load
+from example_robot_data.robots_loader import Solo12Loader
 
 class wbc_controller():
     """Whole body controller which contains an Inverse Kinematics step and a BoxQP step
@@ -17,7 +17,8 @@ class wbc_controller():
 
     def __init__(self, dt, N_SIMULATION):
 
-        self.robot = load('solo12')
+        Solo12Loader.free_flyer = True
+        self.robot = Solo12Loader().robot
 
         self.dt = dt  # Time step
 
diff --git a/scripts/crocoddyl_eval/test_4/analyse_simu.py b/scripts/crocoddyl_eval/test_4/analyse_simu.py
index 583f6cbd6deedc06c82c67d925b13443e25432f1..0643b510a59f5727e21575340d1c9e51331fdf93 100644
--- a/scripts/crocoddyl_eval/test_4/analyse_simu.py
+++ b/scripts/crocoddyl_eval/test_4/analyse_simu.py
@@ -3,10 +3,10 @@
 
 import matplotlib.pylab as plt
 import numpy as np
-import sys
+"""import sys
 import os
 from sys import argv
-sys.path.insert(0, os.getcwd())  # adds current directory to python path
+sys.path.insert(0, os.getcwd())  # adds current directory to python path"""
 
 
 ####################
@@ -15,60 +15,68 @@ sys.path.insert(0, os.getcwd())  # adds current directory to python path
 
 folder_name = ""
 pathIn = "crocoddyl_eval/test_4/log_eval/"
-res = np.load(pathIn + folder_name + "results_wyaw_all_false.npy", allow_pickle=True)
-# res1 = np.load(pathIn + folder_name + "results_osqp_wyaw.npy" , allow_pickle=True )
+#res = np.load(pathIn + folder_name + "results_wyaw_all_false.npy", allow_pickle=True)
 
+N_lin = 6
+X = np.linspace(1.4, -1.4, N_lin)
+Y = np.linspace(-1.4, 1.4, N_lin)
+W = np.linspace(-2.2, 2.2, N_lin)
 
-X = np.linspace(1, -1, 25)
-# Y = np.linspace(-1,1,65)
-W = np.linspace(-2.2, 2.2, 25)
-
-
-def find_nearest(Vx, Vy):
-    idx = (np.abs(X - Vx)).argmin()
-    idy = (np.abs(W - Vy)).argmin()
 
+def find_nearest(A, B, C, D):
+    idx = (np.abs(A - B)).argmin()
+    idy = (np.abs(C - D)).argmin()
     return idx, idy
 
+res = np.load(pathIn + folder_name + "results_osqp_vy.npy", allow_pickle=True)
+plt.figure()
+for elt in res :
+    if elt[0] == True :
+        plt.plot(elt[1][0] , elt[1][1] , "bs" , markerSize= "13")
+    else :
+        plt.plot(elt[1][0] , elt[1][1] , "rs" , markerSize= "13")
+print(res)
+plt.xlim([-1.5,1.5])
+plt.ylim([-1.5,1.5])
+plt.show(block=True)
 
-XX, YY = np.meshgrid(X, W)
-Z = np.zeros((XX.shape[0], YY.shape[1]))
-Z_osqp = np.zeros((XX.shape[0], YY.shape[1]))
-# plt.figure()
+####################
+# Plotting
+####################
 
-# for elt in res :
-#     if elt[0] == True :
-#         plt.plot(elt[1][0] , elt[1][1] , "bs" , markerSize= "13")
-#     else :
-#         pass
+# Plotting Forward vel VS Lateral vel for OSQP
+res_osqp_vy = np.load(pathIn + folder_name + "results_osqp_vy.npy", allow_pickle=True)
+XX, YY = np.meshgrid(X, Y)
+Z = np.zeros((XX.shape[0], YY.shape[1]))
+Z_osqp_vy = np.zeros((XX.shape[0], YY.shape[1]))
+for elt in res_osqp_vy:
+    idx, idy = find_nearest(X, elt[1][0], Y, elt[1][1])
+    Z_osqp_vy[idx, idy] = elt[0]
 
-# plt.xlim([-1,1])
-# plt.ylim([-1,1])
 
 plt.figure()
-
-for elt in res:
-    idx, idy = find_nearest(elt[1][0], elt[1][5])
-    Z[idx, idy] = elt[0]
-
 plt.rc('text', usetex=True)
-im = plt.imshow(Z, cmap=plt.cm.binary, extent=(-2.2, 2.2, -1, 1))
+im = plt.imshow(Z_osqp_vy, cmap=plt.cm.binary, extent=(-1, 1, -1, 1))
 plt.xlabel("Lateral Velocity $\dot{p_y} \hspace{2mm} [m.s^{-1}]$", fontsize=12)
 plt.ylabel("Forward Velocity $\dot{p_x} \hspace{2mm} [m.s^{-1}]$", fontsize=12)
-plt.title("Viable Operating Regions (DDP and foot optimization)", fontsize=14)
-
-
-# plt.figure()
+plt.title("Viable Operating Regions OSQP", fontsize=14)
 
-# for elt in res1 :
-#     idx , idy = find_nearest(elt[1][0] , elt[1][5])
-#     Z_osqp[idx,idy] = elt[0]
-
-# plt.rc('text', usetex=True)
-# im = plt.imshow(Z_osqp ,cmap = plt.cm.binary , extent=(-2.2,2.2,-1,1))
-# plt.xlabel("Lateral Velocity $\dot{p_y} \hspace{2mm} [m.s^{-1}]$" , fontsize=12)
-# plt.ylabel("Forward Velocity $\dot{p_x} \hspace{2mm} [m.s^{-1}]$" , fontsize=12)
-# plt.title("Viable Operating Regions OSQP" , fontsize=14)
+# Plotting Forward vel VS Angular vel for OSQP
+"""
+res_osqp_wyaw = np.load(pathIn + folder_name + "results_osqp_wyaw.npy", allow_pickle=True)
+XX, YY = np.meshgrid(X, W)
+Z = np.zeros((XX.shape[0], YY.shape[1]))
+Z_osqp_wyaw = np.zeros((XX.shape[0], YY.shape[1]))
+for elt in res_osqp_wyaw:
+    idx, idy = find_nearest(X, elt[1][0], W, elt[1][5])
+    Z_osqp_wyaw[idx, idy] = elt[0]
 
+plt.figure()
+plt.rc('text', usetex=True)
+im = plt.imshow(Z_osqp_vy, cmap=plt.cm.binary, extent=(-2.2, 2.2, -1, 1))
+plt.xlabel("Angular Velocity $\dot{p_y} \hspace{2mm} [rad.s^{-1}]$", fontsize=12)
+plt.ylabel("Forward Velocity $\dot{p_x} \hspace{2mm} [m.s^{-1}]$", fontsize=12)
+plt.title("Viable Operating Regions OSQP", fontsize=14)
+"""
 
 plt.show()
diff --git a/scripts/crocoddyl_eval/test_4/main.py b/scripts/crocoddyl_eval/test_4/main.py
index 7a8995c73b83802f1e952e9f2db4331571508202..f15697286f8ea042366d2487a3fcfdbcc732dd39 100644
--- a/scripts/crocoddyl_eval/test_4/main.py
+++ b/scripts/crocoddyl_eval/test_4/main.py
@@ -11,10 +11,10 @@ import time
 import utils
 import matplotlib.pylab as plt
 import numpy as np
-import sys
+"""import sys
 import os
 from sys import argv
-sys.path.insert(0, os.getcwd())  # adds current directory to python path
+sys.path.insert(0, os.getcwd())  # adds current directory to python path"""
 
 
 def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION_, type_MPC, pyb_feedback, desired_speed):
diff --git a/scripts/crocoddyl_eval/test_4/run_scenarios.py b/scripts/crocoddyl_eval/test_4/run_scenarios.py
index 29f8e6b4778f0df461bc54ecfdffd760b7342e83..5f70e4594855ae89029b8248f1bc8e90bdd4ed47 100644
--- a/scripts/crocoddyl_eval/test_4/run_scenarios.py
+++ b/scripts/crocoddyl_eval/test_4/run_scenarios.py
@@ -1,34 +1,9 @@
 # coding: utf8
-from crocoddyl_eval.test_4.main import run_scenario
+
 import time
 import multiprocessing as mp
-import Joystick
-from IPython import embed
-from TSID_Debug_controller_four_legs_fb_vel import controller, dt
-import matplotlib.pylab as plt
 import numpy as np
-import sys
-import os
-from sys import argv
-sys.path.insert(0, os.getcwd())  # adds current directory to python path
-
-
-envID = 0
-velID = 0
-
-dt_mpc = 0.02  # Time step of the MPC
-k_mpc = int(dt_mpc / dt)  # dt is dt_tsid, defined in the TSID controller script
-t = 0.0  # Time
-n_periods = 1  # Number of periods in the prediction horizon
-T_gait = 0.64  # Duration of one gait period
-N_SIMULATION = 5000  # number of simulated TSID time steps
-
-# Which MPC solver you want to use
-# True to have PA's MPC, to False to have Thomas's MPC
-type_MPC = False
-
-# Whether PyBullet feedback is enabled or not
-pyb_feedback = True
+from main_solo12_control import control_loop
 
 #################
 # RUN SCENARIOS #
@@ -40,7 +15,7 @@ def run_simu_wyaw(speed):
     desired_speed[0] = speed[0]
     desired_speed[5] = speed[1]
 
-    return run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION, type_MPC, pyb_feedback, desired_speed)
+    return control_loop("test", None, desired_speed)
 
 
 def run_simu_vy(speed):
@@ -48,9 +23,10 @@ def run_simu_vy(speed):
     desired_speed[0] = speed[0]
     desired_speed[1] = speed[1]
 
-    return run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION, type_MPC, pyb_feedback, desired_speed)
+    return control_loop("test", None, desired_speed)
 
 
+"""
 # List of arguments
 X = np.linspace(-1, 1, 25)
 W = np.linspace(-2.2, 2.2, 25)
@@ -73,6 +49,7 @@ mem_time = time.time() - start_time
 pathIn = "crocoddyl_eval/test_4/log_eval/"
 print("Saving logs...")
 np.save(pathIn + "results_wyaw_all_true.npy", np.array(res))
+"""
 
 ###########################
 # New simulation with osqp,
@@ -102,12 +79,16 @@ np.save(pathIn + "results_wyaw_all_true.npy", np.array(res))
 ###########################
 
 # List of arguments
-X = np.linspace(-1, 1, 25)
-Y = np.linspace(-1, 1, 25)
+X = np.hstack((np.linspace(-1.4, 1.4, 15)))
+Y = np.hstack((np.linspace(-1.4, 1.4, 15)))
 list_param = []
 for i in range(X.size):
     for j in range(Y.size):
-        list_param.append([X[i], Y[j]])
+        if (np.abs(X[i]) >= 0.79) and (np.abs(Y[j]) >= 0.79):
+            list_param.append([X[i], Y[j]])
+
+"""from IPython import embed
+embed()"""
 
 type_MPC = False
 
@@ -119,11 +100,12 @@ with mp.Pool(mp.cpu_count()-1) as pool:
     res2 = pool.map(run_simu_vy, list_param)
 
 # print("Temps d execution ddp wyaw: %s secondes ---" % (mem_time))
-print("Temps d execution ddp vy: %s secondes ---" % (time.time() - start_time))
+print("Temps d execution osqp vy: %s secondes ---" % (time.time() - start_time))
 
 # Logger of the results
 pathIn = "crocoddyl_eval/test_4/log_eval/"
 print("Saving logs...")
-np.save(pathIn + "results_vy_all_true.npy", np.array(res2))
+np.save(pathIn + "results_osqp_vy.npy", np.array(res2))
+# np.save(pathIn + "results_vy_all_true.npy", np.array(res2))
 
 quit()
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index 911ee957713a29658d27917144503d8af18e7160..0599d582ad344e46aa63e9fdb6dd3c1ca100a091 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -5,7 +5,6 @@ import sys
 
 sys.path.insert(0, './solopython')
 
-
 import threading
 from Controller import Controller
 import numpy as np
@@ -89,7 +88,7 @@ def clone_movements(name_interface_clone, q_init, cloneP, cloneD, cloneQdes, clo
     return 0
 
 
-def control_loop(name_interface, name_interface_clone=None):
+def control_loop(name_interface, name_interface_clone=None, des_vel_analysis=None):
     """Main function that calibrates the robot, get it into a default waiting position then launch
     the main control loop once the user has pressed the Enter key
 
@@ -111,6 +110,14 @@ def control_loop(name_interface, name_interface_clone=None):
     # Default position after calibration
     q_init = np.array([0.0, 0.7, -1.4, -0.0, 0.7, -1.4, 0.0, -0.7, +1.4, -0.0, -0.7, +1.4])
 
+    if params.SIMULATION and (des_vel_analysis is not None):
+        print("Analysis: %1.1f %1.1f %1.1f" % (des_vel_analysis[0], des_vel_analysis[1], des_vel_analysis[5]))
+        acceleration_rate = 0.1  # m/s^2
+        steady_state_duration = 3  # s
+        N_analysis = int(np.max(np.abs(des_vel_analysis)) / acceleration_rate / params.dt_wbc) + 1
+        N_steady = int(steady_state_duration / params.dt_wbc)
+        params.N_SIMULATION = N_analysis + N_steady
+
     # Run a scenario and retrieve data thanks to the logger
     controller = Controller(q_init, params.envID, params.velID, params.dt_wbc, params.dt_mpc,
                             int(params.dt_mpc / params.dt_wbc), t, params.T_gait,
@@ -118,6 +125,9 @@ def control_loop(name_interface, name_interface_clone=None):
                             params.predefined_vel, enable_pyb_GUI, params.kf_enabled, params.N_gait,
                             params.SIMULATION)
 
+    if params.SIMULATION and (des_vel_analysis is not None):
+        controller.joystick.update_for_analysis(des_vel_analysis, N_analysis, N_steady)
+
     ####
 
     if params.SIMULATION:
@@ -233,6 +243,11 @@ def control_loop(name_interface, name_interface_clone=None):
 
     # ****************************************************************
 
+    if (t >= t_max):
+        finished = True
+    else:
+        finished = False
+
     # Stop clone interface running in parallel process
     if not params.SIMULATION and name_interface_clone is not None:
         cloneResult.value = False
@@ -276,7 +291,7 @@ def control_loop(name_interface, name_interface_clone=None):
 
     # Plot estimated computation time for each step for the control architecture
     from matplotlib import pyplot as plt
-    plt.figure()
+    """plt.figure()
     plt.plot(controller.t_list_filter[1:], 'r+')
     plt.plot(controller.t_list_planner[1:], 'g+')
     plt.plot(controller.t_list_mpc[1:], 'b+')
@@ -286,7 +301,12 @@ def control_loop(name_interface, name_interface_clone=None):
     plt.plot(controller.t_list_QPWBC[1:], 'o', color="royalblue")
     plt.legend(["Estimator", "Planner", "MPC", "WBC", "Whole loop", "InvKin", "QP WBC"])
     plt.title("Loop time [s]")
-    plt.show(block=True)
+    plt.show(block=True)"""
+    """plt.figure()
+    for i in range(3):
+        plt.subplot(3, 1, i+1)
+        plt.plot(controller.o_log_foot[:, i, 0])
+    plt.show(block=True)"""
 
     # Plot recorded data
     if params.PLOTTING:
@@ -311,8 +331,8 @@ def control_loop(name_interface, name_interface_clone=None):
         print(controller.error_value)
 
     print("End of script")
-    quit()
 
+    return finished, des_vel_analysis
 
 def main():
     """Main function
@@ -329,7 +349,9 @@ def main():
                         help='Name of the clone interface that will reproduce the movement of the first one \
                               (use ifconfig in a terminal), for instance "enp1s0"')
 
-    control_loop(parser.parse_args().interface, parser.parse_args().clone)
+    f, v = control_loop(parser.parse_args().interface, parser.parse_args().clone)
+    print(f, v)
+    quit()
 
 
 if __name__ == "__main__":
diff --git a/scripts/utils_mpc.py b/scripts/utils_mpc.py
index 108592f8fc707fba9c41f51603bfc2a30796443e..eb164f399cf703a4fee146c25702a1b6b600b163 100644
--- a/scripts/utils_mpc.py
+++ b/scripts/utils_mpc.py
@@ -1,7 +1,7 @@
 import math
 import numpy as np
 
-from example_robot_data import load
+from example_robot_data.robots_loader import Solo12Loader
 
 import Joystick
 import Logger
@@ -83,6 +83,29 @@ def EulerToQuaternion(roll_pitch_yaw):
     qw = cr * cp * cy + sr * sp * sy
     return [qx, qy, qz, qw]
 
+
+def EulerToRotation(roll, pitch, yaw):
+    c_roll = math.cos(roll)
+    s_roll = math.sin(roll)
+    c_pitch = math.cos(pitch)
+    s_pitch = math.sin(pitch)
+    c_yaw = math.cos(yaw)
+    s_yaw = math.sin(yaw)
+    Rz_yaw = np.array([
+        [c_yaw, -s_yaw, 0],
+        [s_yaw,  c_yaw, 0],
+        [0, 0, 1]])
+    Ry_pitch = np.array([
+        [c_pitch, 0, s_pitch],
+        [0, 1, 0],
+        [-s_pitch, 0, c_pitch]])
+    Rx_roll = np.array([
+        [1, 0, 0],
+        [0, c_roll, -s_roll],
+        [0, s_roll,  c_roll]])
+    # R = RzRyRx
+    return np.dot(Rz_yaw, np.dot(Ry_pitch, Rx_roll))
+
 ##################
 # Initialisation #
 ##################
@@ -98,12 +121,11 @@ def init_robot(q_init, enable_viewer):
 
     # Load robot model and data
     # Initialisation of the Gepetto viewer
-    print(enable_viewer)
-    solo = load('solo12', display=enable_viewer)
+    Solo12Loader.free_flyer = True
+    solo = Solo12Loader().robot  # TODO:enable_viewer
     q = solo.q0.reshape((-1, 1))
     q[7:, 0] = q_init
 
-    
     """if enable_viewer:
         solo.initViewer(loadModel=True)
         if ('viewer' in solo.viz.__dict__):
@@ -111,7 +133,6 @@ def init_robot(q_init, enable_viewer):
             solo.viewer.gui.setRefreshIsSynchronous(False)"""
     if enable_viewer:
         solo.display(q)
-    print("PASS")
 
     # Initialisation of model quantities
     pin.centerOfMass(solo.model, solo.data, q, np.zeros((18, 1)))