From 87b409b4ead583dbf57579c17e7518696e81808f Mon Sep 17 00:00:00 2001 From: Rick Staa Date: Wed, 20 Dec 2023 18:18:32 +0100 Subject: [PATCH] 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 | 3 + .../src/panda_gazebo/common/helpers.py | 30 ++ .../src/panda_gazebo/core/moveit_server.py | 58 +++- panda_gazebo/src/panda_gazebo/exceptions.py | 27 ++ .../tests/manual/panda_moveit_server_test.py | 288 +++++++++--------- 5 files changed, 253 insertions(+), 153 deletions(-) diff --git a/panda_gazebo/launch/put_robot_in_world.launch b/panda_gazebo/launch/put_robot_in_world.launch index 220e5215..d5651ffa 100644 --- a/panda_gazebo/launch/put_robot_in_world.launch +++ b/panda_gazebo/launch/put_robot_in_world.launch @@ -70,6 +70,9 @@ + + + diff --git a/panda_gazebo/src/panda_gazebo/common/helpers.py b/panda_gazebo/src/panda_gazebo/common/helpers.py index d8cb3f4e..f76a2c92 100644 --- a/panda_gazebo/src/panda_gazebo/common/helpers.py +++ b/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 diff --git a/panda_gazebo/src/panda_gazebo/core/moveit_server.py b/panda_gazebo/src/panda_gazebo/core/moveit_server.py index 00845af0..dd35888d 100644 --- a/panda_gazebo/src/panda_gazebo/core/moveit_server.py +++ b/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: diff --git a/panda_gazebo/src/panda_gazebo/exceptions.py b/panda_gazebo/src/panda_gazebo/exceptions.py index efb1d0aa..c6944f8d 100644 --- a/panda_gazebo/src/panda_gazebo/exceptions.py +++ b/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 diff --git a/panda_gazebo/tests/manual/panda_moveit_server_test.py b/panda_gazebo/tests/manual/panda_moveit_server_test.py index e819c6e9..b02b9ef4 100755 --- a/panda_gazebo/tests/manual/panda_moveit_server_test.py +++ b/panda_gazebo/tests/manual/panda_moveit_server_test.py @@ -64,147 +64,147 @@ resp = set_joint_positions_srv.call(req) print(resp.message) - # # -- Test panda_arm set robot joint positions service -- - # req = SetJointPositionsRequest() - # req.joint_positions = [-0.60, 0.33, -0.83, -1.46, 0.26, 1.69, -0.64, 0.016, 0.016] - # req.joint_names = ["panda_joint1", "panda_joint2"] - # setarm__joint_positions_srv = rospy.ServiceProxy( - # "panda_moveit_planner_server/panda_arm/set_joint_positions", SetJointPositions - # ) - # resp = setarm__joint_positions_srv.call(req) - # print(resp.message) - - # # -- Test set panda_hand robot joint positions service -- - # req = SetJointPositionsRequest() - # req.joint_names = ["panda_finger_joint1", "panda_finger_joint2"] - # req.joint_positions = [0.04, 0.0] - # # req.joint_names = ["gripper_width"] - # # req.joint_positions = [0.08] - # set_hand_joint_positions_srv = rospy.ServiceProxy( - # "panda_moveit_planner_server/panda_hand/set_joint_positions", - # SetJointPositions, - # ) - # resp = set_hand_joint_positions_srv.call(req) - # print(resp.message) - - # # -- Test set ee pose service -- - # req = SetEePoseRequest() - # req.pose.position.x = 0 - # req.pose.position.y = 0.4 - # req.pose.position.z = 0.5 - # set_ee_pose_srv = rospy.ServiceProxy( - # "panda_moveit_planner_server/panda_arm/set_ee_pose", SetEePose - # ) - # resp = set_ee_pose_srv.call(req) - # print(resp.message) - - # # -- Test get ee pose service -- - # req = GetEePoseRequest() - # get_ee_pose_srv = rospy.ServiceProxy( - # "panda_moveit_planner_server/panda_arm/get_ee_pose", GetEePose - # ) - # resp = get_ee_pose_srv.call(req) - # print(resp.message) - - # # -- Test get ee rpy service -- - # req = GetEeRpyRequest() - # get_ee_rpy_srv = rospy.ServiceProxy( - # "panda_moveit_planner_server/panda_arm/get_ee_rpy", GetEeRpy - # ) - # resp = get_ee_rpy_srv.call(req) - # print(resp.message) - - # # -- Test get ee service -- - # req = GetEeRequest() - # get_ee_srv = rospy.ServiceProxy( - # "panda_moveit_planner_server/panda_arm/get_ee", GetEe - # ) - # resp = get_ee_srv.call(req) - # print(resp.message) - - # # -- Test set ee service -- - # req = SetEeRequest() - # req.ee_name = "panda_link0" - # set_ee_srv = rospy.ServiceProxy( - # "panda_moveit_planner_server/panda_arm/set_ee", SetEe - # ) - # resp = set_ee_srv.call(req) - # print(resp.message) - - # # Set back. - # req = SetEeRequest() - # req.ee_name = "panda_link8" - # resp = set_ee_srv.call(req) - - # # -- Test get random joint positions service -- - # req = GetRandomJointPositionsRequest() - # req.joint_limits.names = [ - # "panda_joint1_min", - # "panda_joint1_max", - # "panda_finger_joint1_min", - # "panda_finger_joint1_max", - # ] - # req.joint_limits.values = [0.25, 0.5, 0.5, 0.10] - # get_random_joint_positions_srv = rospy.ServiceProxy( - # "panda_moveit_planner_server/get_random_joint_positions", - # GetRandomJointPositions, - # ) - # resp = get_random_joint_positions_srv.call(req) - # print(resp.message) - - # # -- Test get random pose service -- - # req = GetRandomEePoseRequest() - # req.bounding_region = BoundingRegion(x_min=0.0, x_max=0.05) - # get_random_ee_pose_srv = rospy.ServiceProxy( - # "panda_moveit_planner_server/get_random_ee_pose", - # GetRandomEePose, - # ) - # resp = get_random_ee_pose_srv.call(req) - # print(resp.message) - - # # -- Test get ee pose joint config service -- - # req = GetEePoseJointConfigRequest() - # req.pose.position.x = 0 - # req.pose.position.y = 0.5 - # req.pose.position.z = 0.5 - # get_ee_pose_joint_config_srv = rospy.ServiceProxy( - # "panda_moveit_planner_server/panda_arm/get_ee_pose_joint_config", - # GetEePoseJointConfig, - # ) - # resp = get_ee_pose_joint_config_srv.call(req) - # print(resp.joint_names) - # print(resp.joint_positions) - # print(resp.message) - - # # -- Test get controlled joints service -- - # req = GetMoveItControlledJointsRequest() - # get_controlled_joints_srv = rospy.ServiceProxy( - # "panda_moveit_planner_server/get_controlled_joints", - # GetMoveItControlledJoints, - # ) - # resp = get_controlled_joints_srv.call(req) - # print(resp.message) - - # # -- Test add Box service -- - # req = AddBoxRequest() - # req = AddBoxRequest( - # # name="box", pose=Pose(orientation=Quaternion(0, 0, 0, 1)), size=[1, 1, 1] - # # name="box", size=[1, 1, 1] - # name="box2", - # ) - # add_box_srv = rospy.ServiceProxy( - # "panda_moveit_planner_server/planning_scene/add_box", - # AddBox, - # ) - # resp = add_box_srv.call(req) - # print(resp.message) - - # # -- Test add plane service -- - # # req = AddPlaneRequest() - # req = AddPlaneRequest(name="plane", pose=Pose(orientation=Quaternion(0, 0, 0, 1))) - # add_plane_srv = rospy.ServiceProxy( - # "panda_moveit_planner_server/planning_scene/add_plane", - # AddPlane, - # ) - # resp = add_plane_srv.call(req) - # print(resp.message) + # -- Test panda_arm set robot joint positions service -- + req = SetJointPositionsRequest() + req.joint_positions = [-0.60, 0.33, -0.83, -1.46, 0.26, 1.69, -0.64, 0.016, 0.016] + req.joint_names = ["panda_joint1", "panda_joint2"] + setarm__joint_positions_srv = rospy.ServiceProxy( + "panda_moveit_planner_server/panda_arm/set_joint_positions", SetJointPositions + ) + resp = setarm__joint_positions_srv.call(req) + print(resp.message) + + # -- Test set panda_hand robot joint positions service -- + req = SetJointPositionsRequest() + req.joint_names = ["panda_finger_joint1", "panda_finger_joint2"] + req.joint_positions = [0.04, 0.0] + # req.joint_names = ["gripper_width"] + # req.joint_positions = [0.08] + set_hand_joint_positions_srv = rospy.ServiceProxy( + "panda_moveit_planner_server/panda_hand/set_joint_positions", + SetJointPositions, + ) + resp = set_hand_joint_positions_srv.call(req) + print(resp.message) + + # -- Test set ee pose service -- + req = SetEePoseRequest() + req.pose.position.x = 0 + req.pose.position.y = 0.4 + req.pose.position.z = 0.5 + set_ee_pose_srv = rospy.ServiceProxy( + "panda_moveit_planner_server/panda_arm/set_ee_pose", SetEePose + ) + resp = set_ee_pose_srv.call(req) + print(resp.message) + + # -- Test get ee pose service -- + req = GetEePoseRequest() + get_ee_pose_srv = rospy.ServiceProxy( + "panda_moveit_planner_server/panda_arm/get_ee_pose", GetEePose + ) + resp = get_ee_pose_srv.call(req) + print(resp.message) + + # -- Test get ee rpy service -- + req = GetEeRpyRequest() + get_ee_rpy_srv = rospy.ServiceProxy( + "panda_moveit_planner_server/panda_arm/get_ee_rpy", GetEeRpy + ) + resp = get_ee_rpy_srv.call(req) + print(resp.message) + + # -- Test get ee service -- + req = GetEeRequest() + get_ee_srv = rospy.ServiceProxy( + "panda_moveit_planner_server/panda_arm/get_ee", GetEe + ) + resp = get_ee_srv.call(req) + print(resp.message) + + # -- Test set ee service -- + req = SetEeRequest() + req.ee_name = "panda_link0" + set_ee_srv = rospy.ServiceProxy( + "panda_moveit_planner_server/panda_arm/set_ee", SetEe + ) + resp = set_ee_srv.call(req) + print(resp.message) + + # Set back. + req = SetEeRequest() + req.ee_name = "panda_link8" + resp = set_ee_srv.call(req) + + # -- Test get random joint positions service -- + req = GetRandomJointPositionsRequest() + req.joint_limits.names = [ + "panda_joint1_min", + "panda_joint1_max", + "panda_finger_joint1_min", + "panda_finger_joint1_max", + ] + req.joint_limits.values = [0.25, 0.5, 0.5, 0.10] + get_random_joint_positions_srv = rospy.ServiceProxy( + "panda_moveit_planner_server/get_random_joint_positions", + GetRandomJointPositions, + ) + resp = get_random_joint_positions_srv.call(req) + print(resp.message) + + # -- Test get random pose service -- + req = GetRandomEePoseRequest() + req.bounding_region = BoundingRegion(x_min=0.0, x_max=0.05) + get_random_ee_pose_srv = rospy.ServiceProxy( + "panda_moveit_planner_server/get_random_ee_pose", + GetRandomEePose, + ) + resp = get_random_ee_pose_srv.call(req) + print(resp.message) + + # -- Test get ee pose joint config service -- + req = GetEePoseJointConfigRequest() + req.pose.position.x = 0 + req.pose.position.y = 0.5 + req.pose.position.z = 0.5 + get_ee_pose_joint_config_srv = rospy.ServiceProxy( + "panda_moveit_planner_server/panda_arm/get_ee_pose_joint_config", + GetEePoseJointConfig, + ) + resp = get_ee_pose_joint_config_srv.call(req) + print(resp.joint_names) + print(resp.joint_positions) + print(resp.message) + + # -- Test get controlled joints service -- + req = GetMoveItControlledJointsRequest() + get_controlled_joints_srv = rospy.ServiceProxy( + "panda_moveit_planner_server/get_controlled_joints", + GetMoveItControlledJoints, + ) + resp = get_controlled_joints_srv.call(req) + print(resp.message) + + # -- Test add Box service -- + req = AddBoxRequest() + req = AddBoxRequest( + # name="box", pose=Pose(orientation=Quaternion(0, 0, 0, 1)), size=[1, 1, 1] + # name="box", size=[1, 1, 1] + name="box2", + ) + add_box_srv = rospy.ServiceProxy( + "panda_moveit_planner_server/planning_scene/add_box", + AddBox, + ) + resp = add_box_srv.call(req) + print(resp.message) + + # -- Test add plane service -- + # req = AddPlaneRequest() + req = AddPlaneRequest(name="plane", pose=Pose(orientation=Quaternion(0, 0, 0, 1))) + add_plane_srv = rospy.ServiceProxy( + "panda_moveit_planner_server/planning_scene/add_plane", + AddPlane, + ) + resp = add_plane_srv.call(req) + print(resp.message)