Fixes #122. This PR introduces a fix for a bug where the robot occasionally refuses to execute gripper commands. The underlying reason for this bug ended up being that the user's gripper commands were being preempted by the gripper's stall protection sentry within the Stretch Body library:
This bug was tricky to identify because it only appears at certain control rates. E.g. if you send the gripper commands at a rate faster than the sentry had time to kick in (>3hz), you wouldn't see this issue. On the other hand, if you were doing open loop control, you wouldn't see this issue, because the sentry only preempts the second command the user sends. Therefore, there's a small range of control rates (~2hz to 0.75hz) where this issue does appear, and fortunately, a Stretch user was able to discover it, write a reproducible test, and report it (see the tagged Github issue for details).
This bug was solved by waiting for the stall sentry to finish its work before sending the subsequent command. For fast control rate, the logic of this fix is never triggered. For slow control rates, it performs 0.05 second sleeps until the overload concern has passed.
Fixes #122. This PR introduces a fix for a bug where the robot occasionally refuses to execute gripper commands. The underlying reason for this bug ended up being that the user's gripper commands were being preempted by the gripper's stall protection sentry within the Stretch Body library:
https://github.com/hello-robot/stretch_body/blob/a97f03f46b7f58a0a3cf315004c13c25b75b2bb8/body/stretch_body/stretch_gripper.py#L95-L100
This bug was tricky to identify because it only appears at certain control rates. E.g. if you send the gripper commands at a rate faster than the sentry had time to kick in (>3hz), you wouldn't see this issue. On the other hand, if you were doing open loop control, you wouldn't see this issue, because the sentry only preempts the second command the user sends. Therefore, there's a small range of control rates (~2hz to 0.75hz) where this issue does appear, and fortunately, a Stretch user was able to discover it, write a reproducible test, and report it (see the tagged Github issue for details).
This bug was solved by waiting for the stall sentry to finish its work before sending the subsequent command. For fast control rate, the logic of this fix is never triggered. For slow control rates, it performs 0.05 second sleeps until the overload concern has passed.