Commit 7c102c88 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

Export filtered IMU data into signal attitudeSOUT.

parent 860a2491
......@@ -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);
}
......
......@@ -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_*/
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment