From 6f6ed608b750abf212c0523647da682d3fa5753a Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Wed, 5 Jun 2024 19:10:50 +0200 Subject: [PATCH] [RQT-JTC] limits from jtc controlled joints (#1146) (cherry picked from commit 0ae3e42d779daaea49a76803c839241aba447829) --- .../joint_limits_urdf.py | 94 ++++++++++--------- .../joint_trajectory_controller.py | 3 +- 2 files changed, 50 insertions(+), 47 deletions(-) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py index 5655e12f7a..bdde80dd54 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -45,7 +45,7 @@ def subscribe_to_robot_description(node, key="robot_description"): node.create_subscription(String, key, callback, qos_profile) -def get_joint_limits(node, use_smallest_joint_limits=True): +def get_joint_limits(node, joints_names, use_smallest_joint_limits=True): global description use_small = use_smallest_joint_limits use_mimic = True @@ -71,54 +71,56 @@ def get_joint_limits(node, use_smallest_joint_limits=True): if jtype == "fixed": continue name = child.getAttribute("name") - try: - limit = child.getElementsByTagName("limit")[0] + + if name in joints_names: try: - minval = float(limit.getAttribute("lower")) - maxval = float(limit.getAttribute("upper")) - except ValueError: - if jtype == "continuous": - minval = -pi - maxval = pi - else: + limit = child.getElementsByTagName("limit")[0] + try: + minval = float(limit.getAttribute("lower")) + maxval = float(limit.getAttribute("upper")) + except ValueError: + if jtype == "continuous": + minval = -pi + maxval = pi + else: + raise Exception( + f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!" + ) + try: + maxvel = float(limit.getAttribute("velocity")) + except ValueError: raise Exception( - f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!" + f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!" ) - try: - maxvel = float(limit.getAttribute("velocity")) - except ValueError: + except IndexError: raise Exception( - f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!" + f"Missing limits tag for the joint : {name} in the robot_description!" ) - except IndexError: - raise Exception( - f"Missing limits tag for the joint : {name} in the robot_description!" - ) - safety_tags = child.getElementsByTagName("safety_controller") - if use_small and len(safety_tags) == 1: - tag = safety_tags[0] - if tag.hasAttribute("soft_lower_limit"): - minval = max(minval, float(tag.getAttribute("soft_lower_limit"))) - if tag.hasAttribute("soft_upper_limit"): - maxval = min(maxval, float(tag.getAttribute("soft_upper_limit"))) - - mimic_tags = child.getElementsByTagName("mimic") - if use_mimic and len(mimic_tags) == 1: - tag = mimic_tags[0] - entry = {"parent": tag.getAttribute("joint")} - if tag.hasAttribute("multiplier"): - entry["factor"] = float(tag.getAttribute("multiplier")) - if tag.hasAttribute("offset"): - entry["offset"] = float(tag.getAttribute("offset")) - - dependent_joints[name] = entry - continue - - if name in dependent_joints: - continue - - joint = {"min_position": minval, "max_position": maxval} - joint["has_position_limits"] = jtype != "continuous" - joint["max_velocity"] = maxvel - free_joints[name] = joint + safety_tags = child.getElementsByTagName("safety_controller") + if use_small and len(safety_tags) == 1: + tag = safety_tags[0] + if tag.hasAttribute("soft_lower_limit"): + minval = max(minval, float(tag.getAttribute("soft_lower_limit"))) + if tag.hasAttribute("soft_upper_limit"): + maxval = min(maxval, float(tag.getAttribute("soft_upper_limit"))) + + mimic_tags = child.getElementsByTagName("mimic") + if use_mimic and len(mimic_tags) == 1: + tag = mimic_tags[0] + entry = {"parent": tag.getAttribute("joint")} + if tag.hasAttribute("multiplier"): + entry["factor"] = float(tag.getAttribute("multiplier")) + if tag.hasAttribute("offset"): + entry["offset"] = float(tag.getAttribute("offset")) + + dependent_joints[name] = entry + continue + + if name in dependent_joints: + continue + + joint = {"min_position": minval, "max_position": maxval} + joint["has_position_limits"] = jtype != "continuous" + joint["max_velocity"] = maxvel + free_joints[name] = joint return free_joints diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 72c11625e4..4cc6c901af 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -238,7 +238,8 @@ def _update_jtc_list(self): # for _all_ their joints running_jtc = self._running_jtc_info() if running_jtc and not self._robot_joint_limits: - self._robot_joint_limits = get_joint_limits(self._node) # Lazy evaluation + for jtc_info in running_jtc: + self._robot_joint_limits = get_joint_limits(self._node, _jtc_joint_names(jtc_info)) valid_jtc = [] if self._robot_joint_limits: for jtc_info in running_jtc: