bool updateModule()
{
double verg = 0;
Bottle bin;
Bottle bout;
inport.read(bin);
if (reset_offset)
{
initial_offset[0] = bin.get(0).asDouble();
initial_offset[1] = bin.get(1).asDouble();
initial_offset[2] = bin.get(2).asDouble();
initial_offset[3] = bin.get(3).asDouble();
initial_offset[4] = bin.get(4).asDouble();
initial_offset[5] = bin.get(5).asDouble();
reset_offset = false;
}
double x_torque = fabs(bin.get(3).asDouble()) - initial_offset [3];
double y_force = fabs(bin.get(1).asDouble()) - initial_offset[1];
bout.addInt(2);
double j_torque = x_torque + y_force * 0.70; //Nm + N * m
bout.addDouble(j_torque); //j0
bout.addDouble(j_torque); //j1
bout.addDouble(j_torque); //j2
bout.addDouble(j_torque); //j3
bout.addDouble(j_torque); //j4
bout.addDouble(j_torque); //j5
outport.write(bout);
return true;
}
virtual void run() override
{
p.write(image);
yarp::os::Time::delay(1);
}