[Polymetis] Fix modbus exception handling (#1384)

* Fix exception handling

* Add error code to indicate a gripper read fail

* Pin pymodbus version

* Add warning

polymetis/polymetis/environment.yml

@@ -45,7 +45,7 @@ dependencies:
     - pip
     - protobuf
     - pybullet ==3.17
-    - pymodbus
+    - pymodbus ==2.5.3
     - pyserial
     - pytest
     - pytest-benchmark

polymetis/polymetis/protos/polymetis.proto

@@ -85,6 +85,7 @@ message GripperState {
     float width = 2;
     bool is_grasped = 3;
     bool is_moving = 4;
+    int32 error_code = 5;
 }
 
 message LogInterval {

polymetis/polymetis/python/polymetis/robot_client/robotiq_gripper/robotiq_gripper_client.py

@@ -3,6 +3,7 @@
 # This source code is licensed under the MIT license found in the
 # LICENSE file in the root directory of this source tree.
 import time
+import logging
 from concurrent import futures
 
 import grpc
@@ -17,6 +18,8 @@
     Robotiq2FingerGripper,
 )
 
+log = logging.getLogger(__name__)
+
 
 class RobotiqGripperClient:
     """gRPC client that exposes controls of a Robotiq gripper to a PolymetisGripperServer
@@ -66,9 +69,15 @@ def __init__(self, server_ip, server_port, comport="/dev/ttyUSB0", hz=60):
         self.connection.InitRobotClient(metadata)
 
     def get_gripper_state(self):
-        self.gripper.getStatus()
-
         state = polymetis_pb2.GripperState()
+
+        if not self.gripper.getStatus():
+            # NOTE: getStatus returns False and does not update state if modbus read fails
+            state.error_code = 1
+            log.warning(
+                "Failed to read gripper state. Returning last observed state instead."
+            )
+
         state.timestamp.GetCurrentTime()
         state.width = self.gripper.get_pos()
         state.is_grasped = self.gripper.object_detected()

polymetis/polymetis/python/polymetis/robot_client/robotiq_gripper/third_party/robotiq_2finger_grippers/robotiq_modbus_rtu/comModbusRtu.py

@@ -50,6 +50,7 @@
 """
 
 from pymodbus.client.sync import ModbusSerialClient
+from pymodbus.exceptions import ModbusIOException
 from math import ceil
 
 
@@ -113,6 +114,9 @@ def getStatus(self, numBytes):
         if response is None:
             #   print("Failed to receive status")
             return None
+        # Newer versions of pymodbus returns a ModbusIOException instead
+        elif type(response) is ModbusIOException:
+            return None
 
         # Instantiate output as an empty list
         output = []