Commit 357b1467 authored by olivier stasse's avatar olivier stasse

Merge pull request #10 from andreadelprete/master

Increment time of accelerometer, gyrometer and robotState output signals
parents 6ec2f2a1 078151f2
......@@ -80,68 +80,74 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
it = SensorsIn.find("forces");
if (it!=SensorsIn.end())
{
{
// Implements force recollection.
const vector<double>& forcesIn = it->second.getValues();
for(int i=0;i<4;++i)
{
for(int j=0;j<6;++j)
mlforces(j) = forcesIn[i*6+j];
forcesSOUT[i]->setConstant(mlforces);
forcesSOUT[i]->setTime (t);
}
// Implements force recollection.
const vector<double>& forcesIn = it->second.getValues();
for(int i=0;i<4;++i)
{
for(int j=0;j<6;++j)
mlforces(j) = forcesIn[i*6+j];
forcesSOUT[i]->setConstant(mlforces);
forcesSOUT[i]->setTime (t);
}
}
it = SensorsIn.find("attitude");
if (it!=SensorsIn.end())
{
const vector<double>& attitude = it->second.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);
}
{
const vector<double>& attitude = it->second.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);
}
it = SensorsIn.find("joints");
if (it!=SensorsIn.end())
{
const vector<double>& anglesIn = it->second.getValues();
mlRobotState.resize (anglesIn.size () + 6);
for (unsigned i = 0; i < 6; ++i)
mlRobotState (i) = 0.;
updateRobotState(anglesIn);
}
{
const vector<double>& anglesIn = it->second.getValues();
mlRobotState.resize (anglesIn.size () + 6);
for (unsigned i = 0; i < 6; ++i)
mlRobotState (i) = 0.;
for (unsigned i = 0; i < anglesIn.size(); ++i)
mlRobotState (i + 6) = anglesIn[i];
robotState_.setConstant(mlRobotState);
robotState_.setTime(t);
}
it = SensorsIn.find("accelerometer_0");
if (it!=SensorsIn.end())
{
const vector<double>& accelerometer =
SensorsIn ["accelerometer_0"].getValues ();
for (std::size_t i=0; i<3; ++i)
accelerometer_ (i) = accelerometer [i];
accelerometerSOUT_.setConstant (accelerometer_);
}
{
const vector<double>& accelerometer =
SensorsIn ["accelerometer_0"].getValues ();
for (std::size_t i=0; i<3; ++i)
accelerometer_ (i) = accelerometer [i];
accelerometerSOUT_.setConstant (accelerometer_);
accelerometerSOUT_.setTime (t);
}
it = SensorsIn.find("gyrometer_0");
if (it!=SensorsIn.end())
{
const vector<double>& gyrometer = SensorsIn ["gyrometer_0"].getValues ();
for (std::size_t i=0; i<3; ++i)
gyrometer_ (i) = gyrometer [i];
gyrometerSOUT_.setConstant (gyrometer_);
}
{
const vector<double>& gyrometer = SensorsIn ["gyrometer_0"].getValues ();
for (std::size_t i=0; i<3; ++i)
gyrometer_ (i) = gyrometer [i];
gyrometerSOUT_.setConstant (gyrometer_);
gyrometerSOUT_.setTime (t);
}
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_);
}
{
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_);
pseudoTorqueSOUT.setTime(t);
}
sotDEBUGOUT(25);
}
......@@ -151,7 +157,6 @@ void SoTHRP2Device::setupSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
setSensors (SensorsIn);
}
void SoTHRP2Device::nominalSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
{
setSensors (SensorsIn);
......@@ -212,13 +217,3 @@ void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
controlOut["baseff"].setValues(baseff_);
sotDEBUGOUT(25) ;
}
void SoTHRP2Device::updateRobotState(const vector<double> &anglesIn)
{
sotDEBUGIN(25) ;
for (unsigned i = 0; i < anglesIn.size(); ++i)
mlRobotState (i + 6) = anglesIn[i];
robotState_.setConstant(mlRobotState);
sotDEBUGOUT(25) ;
}
......@@ -55,9 +55,6 @@ public:
void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
protected:
// Update output port with the control computed from the
// dynamic graph.
void updateRobotState(const std::vector<double> &anglesIn);
/// \brief Current integration step.
double timestep_;
......
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