I am using the force_mode on the polyscope.
In the BeforeStart sequence i set wrench i.e. forces/torques to be a list of zeros [0,0,0,0,0,0] and I also tare the sensor using zero_ftsensor().
In my main program I continuasly execute force_mode() command with wrench= [0,0,0,0,0,0].
I would expect the robot to remain stationary and not move as we do not want the EEF to apply any forces or torques to the environment.
The robot moves very slowly and doesn’t remain stationary.
Can it be because of some noise of the internal control that it is unable to compensate for the gravity? Is there any solution to that problem?
If I move the zero_ftsensor() from BeforeStart to main program before the force_mode(). The robot remains stationary, however, for the application, I am trying to use, this zeros the force reading of the internal sensor, and thus I can no longer use the reading of the sensor for my purposes.
Putting the zero command in the before start vs main loop alone shouldn’t make any difference. Is the robot moving in between the zero and the force command? This could introduce some drift, it’s best to zero it when stationary right before entering force mode.
How does this make the reading not usable? If you can explain a bit more about how you’re using the data it might be possible to suggest an alternative approach.
The robot command of force_mode with zero wrenches is sent when the robot is initially at rest, so it is stationary before entering force mode.
We are working on impedance control. The robot is holding a cylindrical peg which it is trying to insert into a hole. The position of the hole in x and y is given with a small error so that the peg doesn’t insert smoothly to the hole instead it partially overlaps with it. This results in forces and torques being applied and also being measured by the internal sensor. We then use the impedance equation which based on the sensor measurements we predict the desired position to which the peg should move i.e. the corrected position.
Therefore, in order to send wrench commands that yield the correct position correction we need to be able to fully control the wrenches sent to the robot. For the test, we wanted to see what happens if we send zero wrenches, we would expect to receive no motion of EEF, but there is a small change of position of EEF which contributes to the error propagation. Something that’s very undesired for us.
We are trying to investigate this behavior to see what is the cause of that but so far without success.
I don’t believe there’s a straightforward way to cancel this out without re-zeroing the sensor after the contact. You could try to check the values that the sensor believes it’s experiencing with get_tcp_force() command, and adding small offsets to zeros in the wrench values in opposite direction to the force data to cancel their effects on the motion? Not a clean approach admittedly but it could help?
Thats exactly what I have been doing so far 
I measured forces and moments before contact and substructed it from force measurements. I was hoping for more elgant solution but I guess for now it will do.
Thanks