Read torques provided by OpenHRP

src/sot-hrp2-device.cpp

@@ -42,6 +42,7 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName):
   pose (),
   accelerometer_ (3),
   gyrometer_ (3),
+  torques_(),
   baseff_ ()
 {
   sotDEBUGIN(25) ;
@@ -131,6 +132,16 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
         gyrometer_ (i) = gyrometer [i];
       gyrometerSOUT_.setConstant (gyrometer_);
     }
+
+  it = SensorsIn.find("torques");
+  if (it!=SensorsIn.end())
+    {
+      const std::vector<double>& torques = SensorsIn["torques"].getValues();
+      torques_.resize(torques.size());
+      for(std::size_t i = 0; i < torques.size(); ++i)
+        torques_ (i) = torques [i];
+      pseudoTorqueSOUT.setConstant(torques_);
+    }
   
   sotDEBUGOUT(25);
 }

src/sot-hrp2-device.hh

@@ -85,5 +85,6 @@ protected:
   ml::Vector accelerometer_;
   ml::Vector gyrometer_;
   std::vector<double> baseff_;
+  ml::Vector torques_;
 };
 #endif /* _SOT_HRP2Device_H_*/