Commit c3122074 authored by Pierre Gergondet's avatar Pierre Gergondet
Browse files

Read torques provided by OpenHRP

parent 808b11e7
......@@ -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);
}
......
......@@ -85,5 +85,6 @@ protected:
ml::Vector accelerometer_;
ml::Vector gyrometer_;
std::vector<double> baseff_;
ml::Vector torques_;
};
#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