diff --git a/Controller.py b/Controller.py
index 5ac3c474ebbf71120718734f48007b0a61a22b8a..601f81a8e6aa4c89a7030246911b5e361f34b13e 100644
--- a/Controller.py
+++ b/Controller.py
@@ -180,7 +180,7 @@ class Controller:
 
         # Process Inverse Dynamics
         # If nothing wrong happened yet in TSID controller
-        if not self.myController.error:
+        if (not self.myController.error) and (not self.joystick.stop):
             proc.process_invdyn(self.solo, self.k, self.f_applied, self.estimator, self.interface, self.fstep_planner,
                                 self.myController, self.enable_hybrid_control, self.enable_gepetto_viewer)
 
@@ -188,7 +188,7 @@ class Controller:
             self.jointTorques[:, 0] = proc.process_pdp(self.myController, self.estimator)
 
         # If something wrong happened in TSID controller we stick to a security controller
-        if self.myController.error:
+        if self.myController.error or self.joystick.stop:
             # D controller to slow down the legs
             D = 0.05
             self.jointTorques[:, 0] = D * (- self.estimator.v_filt[6:, 0])
@@ -244,7 +244,7 @@ class Controller:
 
             tau = self.compute(device)
 
-            device.SetDesiredJointTorques(tau)
+            device.SetDesiredJointTorque(tau)
 
             device.SendCommand(WaitEndOfCycle=True)
 
diff --git a/Joystick.py b/Joystick.py
index 2096b9e5044a0f9c7fe986a7192e2a14fe1fb019..635a633d091395da1a2726d7f7f373599cbad02f 100644
--- a/Joystick.py
+++ b/Joystick.py
@@ -17,6 +17,7 @@ class Joystick:
         self.v_ref = np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
 
         self.reduced = False
+        self.stop = False
 
         # Bool to modify the update of v_ref
         # Used to launch multiple simulations
@@ -78,10 +79,14 @@ class Joystick:
         else:  # Otherwise the Vx, Vy, Vyaw is controlled
             self.v_gp = np.array([[- self.vY, - self.vX, 0.0, 0.0, 0.0, - self.vYaw]]).T
 
-        # Reduce the size of the support polygon by pressing Start
+        # Reduce the size of the support polygon by pressing Start
         if self.gp.startButton.value:
             self.reduced = not self.reduced
 
+        # Switch to safety controller if the Back key is pressed
+        if self.gp.backButton.value:
+            self.stop = True
+
         # Low pass filter to slow down the changes of velocity when moving the joysticks
         tc = 0.04  #  cutoff frequency at 50 Hz
         dT = 0.002  # velocity reference is updated every ms
diff --git a/utils_mpc.py b/utils_mpc.py
index ae192f9d15437a4b4b277e9efd3ec17295f23073..1e03bec1f0f32d14b4bb9452242c658662ea6fcd 100644
--- a/utils_mpc.py
+++ b/utils_mpc.py
@@ -732,7 +732,7 @@ class PyBulletSimulator():
         self.v_mes[:] = np.array([state[1] for state in jointStates])
 
         # Measured torques
-        self.torquesFromCurrentMeasurment[:] = self.jointTorques[:]
+        self.torquesFromCurrentMeasurment[:] = self.jointTorques[:].ravel()
 
         # Position and orientation of the trunk (PyBullet world frame)
         self.baseState = pyb.getBasePositionAndOrientation(self.pyb_sim.robotId)