Commit f050ffa9 authored by andreadelprete's avatar andreadelprete
Browse files

Increment time of accelerometer, gyrometer and robotState output signals

parent 6ec2f2a1
......@@ -111,7 +111,10 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
mlRobotState.resize (anglesIn.size () + 6);
for (unsigned i = 0; i < 6; ++i)
mlRobotState (i) = 0.;
updateRobotState(anglesIn);
for (unsigned i = 0; i < anglesIn.size(); ++i)
mlRobotState (i + 6) = anglesIn[i];
robotState_.setConstant(mlRobotState);
robotState_.setTime(t);
}
it = SensorsIn.find("accelerometer_0");
......@@ -122,6 +125,7 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
for (std::size_t i=0; i<3; ++i)
accelerometer_ (i) = accelerometer [i];
accelerometerSOUT_.setConstant (accelerometer_);
accelerometerSOUT_.setTime (t);
}
it = SensorsIn.find("gyrometer_0");
......@@ -131,6 +135,7 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
for (std::size_t i=0; i<3; ++i)
gyrometer_ (i) = gyrometer [i];
gyrometerSOUT_.setConstant (gyrometer_);
gyrometerSOUT_.setTime (t);
}
it = SensorsIn.find("torques");
......@@ -141,6 +146,7 @@ void SoTHRP2Device::setSensors(map<string,dgsot::SensorValues> &SensorsIn)
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