Fix the translation from URDF to SoT device force sensors.

src/sot-talos-device.cpp

@@ -114,6 +114,7 @@ SoTTalosDevice::~SoTTalosDevice()
 
 void SoTTalosDevice::setSensorsForce(map<string,dgsot::SensorValues> &SensorsIn, int t)
 {
+  int map_sot_2_urdf[4] = { 2, 0, 3, 1};
   sotDEBUGIN(15);
   map<string,dgsot::SensorValues>::iterator it;
   it = SensorsIn.find("forces");
@@ -125,9 +126,10 @@ void SoTTalosDevice::setSensorsForce(map<string,dgsot::SensorValues> &SensorsIn,
     for(int i=0;i<4;++i)
     {
       sotDEBUG(15) << "Force sensor " << i << std::endl;
+      int idx_sensor = map_sot_2_urdf[i];
       for(int j=0;j<6;++j)
 	{
-	  dgforces_(j) = forcesIn[i*6+j];
+	  dgforces_(j) = forcesIn[idx_sensor*6+j];
 	  sotDEBUG(15) << "Force value " << j << ":" << dgforces_(j) << std::endl;
 	}
       forcesSOUT[i]->setConstant(dgforces_);