Export filtered IMU data into signal attitudeSOUT.

src/sot-hrp2-device.cpp

@@ -35,7 +35,8 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName):
   timestep_(TIMESTEP_DEFAULT),
   previousState_ (),
   robotState_ ("StackOfTasks(" + RobotName + ")::output(vector)::robotState"),
-  mlforces (6)
+  mlforces (6),
+  pose ()
 {
   sotDEBUGIN(25) ;
   signalRegistration (robotState_);
@@ -83,7 +84,14 @@ void SoTHRP2Device::setupSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
 	mlforces(j) = forcesIn[i*6+j];
       forcesSOUT[i]->setConstant(mlforces);
     }
-      
+
+  vector<double> attitude = SensorsIn ["attitude"].getValues ();
+  for (unsigned int i = 0; i < 3; ++i)
+    for (unsigned int j = 0; j < 3; ++j)
+      pose (i, j) = attitude [i * 3 + j];
+  attitudeSOUT.setConstant (pose);
+  attitudeSOUT.setTime (t);
+
   updateRobotState(anglesIn);
   sotDEBUGOUT(25);
 }

src/sot-hrp2-device.hh

@@ -24,6 +24,7 @@
 #include <dynamic-graph/linear-algebra.h>
 #include <sot/core/device.hh>
 #include <sot/core/abstract-sot-external-interface.hh>
+#include <sot/core/matrix-rotation.hh>
 
 namespace dgsot=dynamicgraph::sot;
 
@@ -73,6 +74,7 @@ protected:
   /// Intermediate variables to avoid allocation during control
   ml::Vector mlforces;
   ml::Vector mlRobotState;
+  dgsot::MatrixRotation pose;
 
 };
 #endif /* _SOT_HRP2Device_H_*/