Commit f5b28288 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Fix the translation from URDF to SoT device force sensors.

parent 1fe46902
Pipeline #2265 failed with stage
in 4 seconds
......@@ -114,6 +114,7 @@ SoTTalosDevice::~SoTTalosDevice()
void SoTTalosDevice::setSensorsForce(map<string,dgsot::SensorValues> &SensorsIn, int t)
{
int map_sot_2_urdf[4] = { 2, 0, 3, 1};
sotDEBUGIN(15);
map<string,dgsot::SensorValues>::iterator it;
it = SensorsIn.find("forces");
......@@ -125,9 +126,10 @@ void SoTTalosDevice::setSensorsForce(map<string,dgsot::SensorValues> &SensorsIn,
for(int i=0;i<4;++i)
{
sotDEBUG(15) << "Force sensor " << i << std::endl;
int idx_sensor = map_sot_2_urdf[i];
for(int j=0;j<6;++j)
{
dgforces_(j) = forcesIn[i*6+j];
dgforces_(j) = forcesIn[idx_sensor*6+j];
sotDEBUG(15) << "Force value " << j << ":" << dgforces_(j) << std::endl;
}
forcesSOUT[i]->setConstant(dgforces_);
......
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