[Polymetis] Gripper stuck

Type of Issue

Select the type of issue:

[x] Bug report (to report a bug)
[x] Feature request (to request an additional feature)
[ ] Tracker (I am just using this as a tracker)
[ ] Refactor request
[ ] Documentation Ask

Description

A while ago, this issue found an issue when using polymetis to control franka gripper. This is a very critical problem as one cannot control gripper to grasp one object and release it.

This PR offers a partial fix to the issue, but on real robots this doesn't completely solve the problem.

Instead, one should follow the official C++ example from franka to achieve the desired behavior of getting gripper unstuck from previous commands. This example essentially calls gripper.stop() to terminate a previous command that cannot be achieved, whenever a new command is about to be sent.

This requires us to expose the GripperInterface.stop API, so one can call GripperInterface.stop() before GripperInterface.goto or GripperInterface.grasp to get gripper unstuck.

Current Behavior

gripper = GripperInterface()

gripper.grasp(speed=0.5, force=3.0)
# let's say robot grabs a 3cm by 3cm block

gripper.goto(width=0.08, speed=0.5, force=3.0)
# expect the robot to release the object
# the robot actually cannot release the object itself! 
# this is because previous grasp command asks it to go to width=0.0, which cannot be achieved due to the block

Expected Behavior

gripper = GripperInterface()

gripper.grasp(speed=0.5, force=3.0)
# let's say robot grabs a 3cm by 3cm block

gripper.stop()  # this is added by my PR
gripper.goto(width=0.08, speed=0.5, force=3.0)
# expect the robot to release the object

Steps to reproduce

See above

Links to any relevant pastes or documents

official C++ example to teleop gripper grasping from libfranka

facebookresearch / fairo