diff --git a/scripts/MPC_Wrapper.py b/scripts/MPC_Wrapper.py
index f3004d1cdbb34cccdadc480e898f4cd57a41a00e..5787b068cb1a14c7289e7a0815fe27d782b5ca5d 100644
--- a/scripts/MPC_Wrapper.py
+++ b/scripts/MPC_Wrapper.py
@@ -3,10 +3,9 @@
 import numpy as np
 import libquadruped_reactive_walking as MPC
 from multiprocessing import Process, Value, Array
-from utils_mpc import quaternionToRPY
 import crocoddyl_class.MPC_crocoddyl as MPC_crocoddyl
 import crocoddyl_class.MPC_crocoddyl_planner as MPC_crocoddyl_planner
-
+import pinocchio as pin
 
 class Dummy:
     """Dummy class to store variables"""
@@ -77,7 +76,7 @@ class MPC_Wrapper:
         # Setup initial result for the first iteration of the main control loop
         x_init = np.zeros(12)
         x_init[0:3] = q_init[0:3, 0]
-        x_init[3:6] = quaternionToRPY(q_init[3:7, 0]).ravel()
+        x_init[3:6] = pin.rpy.matrixToRpy(pin.Quaternion(q_init[3:7, 0]).toRotationMatrix())
         if self.mpc_type == 3:  # Need more space to store optimized footsteps
             self.last_available_result = np.zeros((32, (np.int(self.n_steps))))
         else:
diff --git a/scripts/PyBulletSimulator.py b/scripts/PyBulletSimulator.py
index d09182ceefc161629deabfcc77413c4dc6489f73..235855c446e08b9204c4014e93851103a774c9e9 100644
--- a/scripts/PyBulletSimulator.py
+++ b/scripts/PyBulletSimulator.py
@@ -4,7 +4,6 @@ import pybullet_data
 import time as time
 import sys
 import pinocchio as pin
-from utils_mpc import quaternionToRPY
 
 
 class pybullet_simulator:
@@ -26,6 +25,8 @@ class pybullet_simulator:
         # Start the client for PyBullet
         if enable_pyb_GUI:
             pyb.connect(pyb.GUI)
+            pyb.configureDebugVisualizer(pyb.COV_ENABLE_GUI, 0)
+
         else:
             pyb.connect(pyb.DIRECT)
         # p.GUI for graphical version
@@ -609,7 +610,7 @@ class PyBulletSimulator():
 
         # Orientation of the base (quaternion)
         self.baseOrientation[:] = np.array(self.baseState[1])
-        RPY = quaternionToRPY(self.baseOrientation)
+        RPY = pin.rpy.matrixToRpy(pin.Quaternion(self.baseOrientation).toRotationMatrix())
         self.hardware.roll = RPY[0]
         self.hardware.pitch = RPY[1]
         self.hardware.yaw = RPY[2]