From c7625616c10f60352582b134cabbf8ed4a054ea3 Mon Sep 17 00:00:00 2001 From: David Lu!! Date: Thu, 21 Feb 2013 16:43:12 -0800 Subject: [PATCH] Add Small Joint Flag --- .../joint_state_publisher/joint_state_publisher | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/visualization/joint_state_publisher/joint_state_publisher b/visualization/joint_state_publisher/joint_state_publisher index 35f8eff..67d1c86 100755 --- a/visualization/joint_state_publisher/joint_state_publisher +++ b/visualization/joint_state_publisher/joint_state_publisher @@ -27,6 +27,7 @@ class JointStatePublisher(): self.joint_list = [] # for maintaining the original order of the joints self.dependent_joints = get_param("dependent_joints", {}) use_mimic = get_param('use_mimic_tags', True) + use_small = get_param('use_smallest_joint_limits', True) pub_def_positions = get_param("publish_default_positions", True) pub_def_vels = get_param("publish_default_velocities", False) @@ -50,6 +51,14 @@ class JointStatePublisher(): minval = float(limit.getAttribute('lower')) maxval = float(limit.getAttribute('upper')) + 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]