diff --git a/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp b/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
index 29c65a13cb844e3ff1ef496153fe2d15ffde5c3f..c387aa1e4aa627d5d611d1476801306eb85d9393 100644
--- a/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
+++ b/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
@@ -541,10 +541,10 @@ namespace talos_sot_controller
 	std::string orientation_s("orientation_");
 	setSensorsImu(orientation_s, idIMU, DataOneIter_.orientation);
 
-	std::string gyrometer_s("gyrometer_");
+	std::string gyrometer_s("gyrometer_0");
 	setSensorsImu(gyrometer_s, idIMU, DataOneIter_.gyrometer);
 
-	std::string accelerometer_s("accelerometer_");
+	std::string accelerometer_s("accelerometer_0");
 	setSensorsImu(accelerometer_s, idIMU, DataOneIter_.accelerometer);
       }
   }
@@ -552,7 +552,6 @@ namespace talos_sot_controller
   void RCSotController::
   fillForceSensors()
   {
-    
     for(unsigned int idFS=0;idFS<ft_sensors_.size();
 	idFS++)
       {
diff --git a/talos_roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml b/talos_roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml
index 5a9cb80ee73e8ecca4ed309bfdcefe9e427c84fc..868173d6002b73d88bd65b76ea633693771775d1 100644
--- a/talos_roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml
+++ b/talos_roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml
@@ -8,6 +8,6 @@ sot_controller:
     arm_right_1_joint, arm_right_2_joint, arm_right_3_joint, arm_right_4_joint, arm_right_5_joint, arm_right_6_joint, arm_right_7_joint, gripper_right_joint,
     head_1_joint, head_2_joint ]
   map_rc_to_sot_device: { motor-angles: motor-angles ,
-  joint-angles: joint-angles, velocities: velocities,
+  joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
   torques: torques, cmd-joints: control, cmd-effort: control }
   control_mode: EFFORT