Source Code for Module rqt_joint_trajectory_controller.joint_limits_urdf

 1  #!/usr/bin/env python 
 2   
 3  # TODO: Use urdf_parser_py.urdf instead. I gave it a try, but got 
 4  #  Exception: Required attribute not set in XML: upper 
 5  # upper is an optional attribute, so I don't understand what's going on 
 6  # See comments in https://github.com/ros/urdfdom/issues/36 
 7   
 8  import xml.dom.minidom 
 9  from math import pi 
10   
11  import rospy 
12   
13   
14 -def get_joint_limits(key='robot_description', use_smallest_joint_limits=True): 
15      use_small = use_smallest_joint_limits 
16      use_mimic = True 
17   
18      # Code inspired on the joint_state_publisher package by David Lu!!! 
19      # https://github.com/ros/robot_model/blob/indigo-devel/ 
20      # joint_state_publisher/joint_state_publisher/joint_state_publisher 
21      description = rospy.get_param(key) 
22      robot = xml.dom.minidom.parseString(description)\ 
23          .getElementsByTagName('robot')[0] 
24      free_joints = {} 
25      dependent_joints = {} 
26   
27      # Find all non-fixed joints 
28      for child in robot.childNodes: 
29          if child.nodeType is child.TEXT_NODE: 
30              continue 
31          if child.localName == 'joint': 
32              jtype = child.getAttribute('type') 
33              if jtype == 'fixed': 
34                  continue 
35              name = child.getAttribute('name') 
36              try: 
37                  limit = child.getElementsByTagName('limit')[0] 
38              except: 
39                  continue 
40              if jtype == 'continuous': 
41                  minval = -pi 
42                  maxval = pi 
43              else: 
44                  try: 
45                      minval = float(limit.getAttribute('lower')) 
46                      maxval = float(limit.getAttribute('upper')) 
47                  except: 
48                      continue 
49              try: 
50                  maxvel = float(limit.getAttribute('velocity')) 
51              except: 
52                  continue 
53              safety_tags = child.getElementsByTagName('safety_controller') 
54              if use_small and len(safety_tags) == 1: 
55                  tag = safety_tags[0] 
56                  if tag.hasAttribute('soft_lower_limit'): 
57                      minval = max(minval, 
58                                   float(tag.getAttribute('soft_lower_limit'))) 
59                  if tag.hasAttribute('soft_upper_limit'): 
60                      maxval = min(maxval, 
61                                   float(tag.getAttribute('soft_upper_limit'))) 
62   
63              mimic_tags = child.getElementsByTagName('mimic') 
64              if use_mimic and len(mimic_tags) == 1: 
65                  tag = mimic_tags[0] 
66                  entry = {'parent': tag.getAttribute('joint')} 
67                  if tag.hasAttribute('multiplier'): 
68                      entry['factor'] = float(tag.getAttribute('multiplier')) 
69                  if tag.hasAttribute('offset'): 
70                      entry['offset'] = float(tag.getAttribute('offset')) 
71   
72                  dependent_joints[name] = entry 
73                  continue 
74   
75              if name in dependent_joints: 
76                  continue 
77   
78              joint = {'min_position': minval, 'max_position': maxval} 
79              joint["has_position_limits"] = jtype != 'continuous' 
80              joint['max_velocity'] = maxvel 
81              free_joints[name] = joint 
82      return free_joints 
83