refactor: add joint limits error to random joint service (#202)

This commit ensures the joint limits given in the
`get_random_joint_positions` service are checked, and an error is
returned when the user specifies invalid joint limits.

panda_gazebo/launch/put_robot_in_world.launch

@@ -70,6 +70,9 @@
     <arg name="load_gripper" value="$(arg load_gripper)"/>
   </include>
 
+  <!--Load panda joint limits on parameter server-->
+  <rosparam ns="panda_gazebo/panda" file="$(find franka_description)/robots/panda/joint_limits.yaml" command="load"/>
+
   <!--Load the 'panda_gazebo' MoveIt server-->
   <!--  Only load required services-->
   <arg if="$(eval arg('control_type') == 'trajectory')" name="load_set_ee_pose_service" value="true"/>

panda_gazebo/src/panda_gazebo/common/helpers.py

@@ -591,3 +591,33 @@ def ros_exit_gracefully(shutdown_msg=None, exit_code=0):
             rospy.sleep(0.1)
     finally:
         sys.exit(exit_code)
+
+
+def load_panda_joint_limits():
+    """Loads the joint limits of the Panda robot from the parameter server.
+
+    Returns:
+        dict: Dictionary containing the joint position limits (i.e. min and max) for
+            each joint. Returns an empty dictionary if the joint limits could not be
+            loaded.
+    """
+    # Load arm joint limits from parameter server.
+    joint_limits = {}
+    for i in range(1, 8):
+        joint_name = f"joint{i}"
+        if rospy.has_param(f"panda_gazebo/panda/{joint_name}"):
+            limits = rospy.get_param(f"panda_gazebo/panda/{joint_name}")
+            joint_limits[f"panda_{joint_name}_min"] = limits["limit"]["lower"]
+            joint_limits[f"panda_{joint_name}_max"] = limits["limit"]["upper"]
+
+    # Add finger joint limits.
+    joint_limits.update(
+        {
+            "panda_finger_joint1_min": 0.0,
+            "panda_finger_joint1_max": 0.04,
+            "panda_finger_joint2_min": 0.0,
+            "panda_finger_joint2_max": 0.04,
+        }
+    )
+
+    return joint_limits

panda_gazebo/src/panda_gazebo/core/moveit_server.py

@@ -53,8 +53,9 @@
     ros_exit_gracefully,
     translate_moveit_error_code,
     normalize_vector,
+    load_panda_joint_limits,
 )
-from panda_gazebo.exceptions import InputMessageInvalidError
+from panda_gazebo.exceptions import InputMessageInvalidError, JointLimitsInvalidError
 from panda_gazebo.srv import (
     AddBox,
     AddBoxResponse,
@@ -376,6 +377,15 @@ def __init__(  # noqa: C901
                 for joint in self._joint_states.name
             ]
 
+        # Retrieve panda joint limits.
+        self._joint_limits = load_panda_joint_limits()
+        if not self._joint_limits:
+            rospy.logerr(
+                "Unable to load Panda joint limits. Ensure 'joint_limits.yaml' from "
+                "'franka_description' is loaded in 'put_robot_in_world.launch'."
+            )
+            ros_exit_gracefully(shutdown_msg="Shutting down.", exit_code=1)
+
     ################################################
     # Helper functions #############################
     ################################################
@@ -640,10 +650,8 @@ def _create_positions_moveit_setpoints(  # noqa: C901
                     "of different lengths."
                 ),
                 log_message=logwarn_msg,
-                details={
-                    "joint_positions_command_length": joint_positions_count,
-                    "joint_names_length": joint_names_count,
-                },
+                joint_positions_command_length=joint_positions_count,
+                joint_names_length=joint_names_count,
             )
 
         # Split joint positions into arm and hand joint positions and return.
@@ -833,6 +841,32 @@ def _get_bounded_random_joint_values(  # noqa: C901
         if not joint_limits:
             return random_arm_joint_values, random_hand_joint_values
 
+        # Notify the user if joint limits are not valid with the panda joint limits.
+        rospy.logdebug("Checking if joint limits are valid.")
+        invalid_joint_limits = [
+            joint_name
+            for joint_name in joint_limits
+            if (
+                joint_name.endswith("_min")
+                and joint_limits[joint_name] < self._joint_limits[joint_name]
+            )
+            or (
+                joint_name.endswith("_max")
+                and joint_limits[joint_name] > self._joint_limits[joint_name]
+            )
+        ]
+        if invalid_joint_limits:
+            verb = "is" if len(invalid_joint_limits) == 1 else "are"
+            log_msg = (
+                f"Joint limits '{invalid_joint_limits}' {verb} not valid since they are "
+                "outside the Panda joint limits specified in 'joint_limits.yaml'. "
+                f"Valid joint limits are '{self._joint_limits}'."
+            )
+            raise JointLimitsInvalidError(
+                message=("Invalid joint limits were given."),
+                log_message=log_msg,
+            )
+
         # If joint limits were given, retrieve bounded random joint values.
         joint_commands_valid = {
             "arm": False,
@@ -1418,10 +1452,16 @@ def _get_random_joint_positions_callback(  # noqa: C901
             joint_limits = {}
 
         # Retrieve random joint values.
-        (
-            random_arm_joint_values,
-            random_hand_joint_values,
-        ) = self._get_bounded_random_joint_values(joint_limits, max_attempts)
+        try:
+            (
+                random_arm_joint_values,
+                random_hand_joint_values,
+            ) = self._get_bounded_random_joint_values(joint_limits, max_attempts)
+        except JointLimitsInvalidError as e:
+            rospy.logwarn(e.log_message)
+            resp.success = False
+            resp.message = e.args[0]
+            return resp
 
         # Return failure if no random joint positions could be retrieved.
         if random_arm_joint_values is None or random_hand_joint_values is None:

panda_gazebo/src/panda_gazebo/exceptions.py

@@ -26,3 +26,30 @@ def __init__(self, message="", log_message="", **details):
         # Set attributes.
         self.log_message = log_message
         self.details = details
+
+
+class JointLimitsInvalidError(Exception):
+    """Custom exception that is thrown when the joint limits specified by the user are
+    not within the joint limits of the robot.
+
+    Attributes:
+        log_message (str): The full log message.
+        details (dict): Dictionary containing extra Exception information.
+    """
+
+    def __init__(self, message="", log_message="", **details):
+        """Initialise JointLimitsInvalidError exception object.
+
+        Args:
+            message (str, optional): Exception message specifying whether the exception
+                occurred. Defaults to ``""``.
+            log_message (str, optional): Full log message. Defaults to ``""``.
+            details: Additional keyword arguments that can be used to supply the user
+                with more details about why the exception occurred. These details are
+                stored in the ``details`` attribute.
+        """
+        super().__init__(message)
+
+        # Set attributes.
+        self.log_message = log_message
+        self.details = details