[xs_modules] Reject NaN joint commands (#83)

* Reject arm commands with NaNs

* Reject NaNs in single joint command

Author: lukeschmitt-tr

interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/arm.py

@@ -326,6 +326,12 @@ def _check_joint_limits(self, positions: List[float]) -> bool:
         :return: `True` if all positions are within limits; `False` otherwise
         """
         self.core.get_node().logdebug(f'Checking joint limits for {positions=}')
+
+        # Reject any commands containing NaN values
+        if any(math.isnan(elem) for elem in positions):
+            self.core.get_node().logwarn('Rejecting NaN values in position command')
+            return False
+
         theta_list = [int(elem * 1000) / 1000.0 for elem in positions]
         speed_list = [
             abs(goal - current) / float(self.moving_time)
@@ -354,6 +360,12 @@ def _check_single_joint_limit(self, joint_name: str, position: float) -> bool:
         self.core.get_node().logdebug(
             f"Checking joint '{joint_name}' limits for {position=}"
         )
+
+        # Reject any commands containing NaN values
+        if math.isnan(position):
+            self.core.get_node().logwarn('Rejecting NaN value in position command')
+            return False
+
         theta = int(position * 1000) / 1000.0
         speed = abs(
             theta - self.joint_commands[self.info_index_map[joint_name]]