diff --git a/polymetis/polymetis/protos/polymetis.proto b/polymetis/polymetis/protos/polymetis.proto index 5f0ce68753..8654e5edf4 100644 --- a/polymetis/polymetis/protos/polymetis.proto +++ b/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 { diff --git a/polymetis/polymetis/python/polymetis/gripper_interface.py b/polymetis/polymetis/python/polymetis/gripper_interface.py index 100229cb88..3c92a823a5 100644 --- a/polymetis/polymetis/python/polymetis/gripper_interface.py +++ b/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() diff --git a/polymetis/polymetis/src/clients/franka_panda_client/franka_hand_client.cpp b/polymetis/polymetis/src/clients/franka_panda_client/franka_hand_client.cpp index 41e3486a58..c14458632a 100644 --- a/polymetis/polymetis/src/clients/franka_panda_client/franka_hand_client.cpp +++ b/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());