[Polymetis] exposing new gripper api to completely fix gripper stuck issue

polymetis/polymetis/protos/polymetis.proto

@@ -80,6 +80,7 @@ message GripperCommand {
     bool grasp = 5;
     float epsilon_inner = 6;
     float epsilon_outer = 7;
+    bool stop = 8;
 }
 
 message GripperState {

polymetis/polymetis/python/polymetis/gripper_interface.py

@@ -75,7 +75,26 @@ def goto(self, width: float, speed: float, force: float, blocking: bool = True):
             force: Maximum force the gripper will exert
         """
         cmd = polymetis_pb2.GripperCommand(
-            width=width, speed=speed, force=force, grasp=False
+            width=width, speed=speed, force=force, grasp=False, stop=False
+        )
+        cmd.timestamp.GetCurrentTime()
+
+        self._send_gripper_command(
+            self.grpc_connection.Goto,
+            cmd,
+            blocking=blocking,
+        )
+
+    def stop(self, blocking: bool = True):
+        """Stop the previous command (grasp, goto) from executing
+
+        If you call grasp or goto but the robot controller cannot move the gripper 
+        to the desired width due to objects, the gripper controller will stop 
+        executing future commands. Call this method to get it unstuck in previous
+        command, and instead execute future commands.
+        """
+        cmd = polymetis_pb2.GripperCommand(
+            width=0., speed=0., force=0., grasp=False, stop=True
         )
         cmd.timestamp.GetCurrentTime()
 
@@ -118,6 +137,7 @@ def grasp(
             grasp=True,
             epsilon_inner=epsilon_inner,
             epsilon_outer=epsilon_outer,
+            stop=False
         )
         cmd.timestamp.GetCurrentTime()
 

polymetis/polymetis/src/clients/franka_panda_client/franka_hand_client.cpp

@@ -65,7 +65,8 @@ void FrankaHandClient::applyGripperCommand(void) {
     prev_cmd_successful_ =
         gripper_->grasp(gripper_cmd_.width(), gripper_cmd_.speed(),
                         gripper_cmd_.force(), eps_inner, eps_outer);
-
+  } else if (gripper_cmd_.stop()) {
+    prev_cmd_successful_ = gripper_->stop();
   } else {
     spdlog::info("Moving to width {} at speed={}", gripper_cmd_.width(),
                  gripper_cmd_.speed());