diff --git a/sim/scripts/print_joints.py b/sim/scripts/print_joints.py index 0ab27bb1..b56d2379 100755 --- a/sim/scripts/print_joints.py +++ b/sim/scripts/print_joints.py @@ -1,4 +1,6 @@ -"""Parses the URDF file and prints the joint names and types.""" +"""Parses the URDF file and prints the joint names and types. +Assumes name-based heirarchy of URDF joints, with underscores as delimiters. +Additionally, will print out associated limits of revolute joints from the URDF.""" import argparse import xml.etree.ElementTree as ET @@ -18,14 +20,28 @@ def main() -> None: # Gets the relevant joint names. joint_names: List[str] = [] + joint_uppers: List[str] = [] + joint_lowers: List[str] = [] for joint in urdf.findall("joint"): joint_type = joint.attrib["type"] if joint_type in ignore_joint_type: - continue + continue # ignoring fixed joints, or whatever is defined in arguments joint_name = joint.attrib["name"] joint_names.append(joint_name) + joint_upper = joint.find('limit').get('upper') + joint_lower = joint.find('limit').get('lower') + joint_uppers.append(joint_upper) + joint_lowers.append(joint_lower) + + + for i in range(len(joint_names)): + # Concatenate the limits into the name string + joint_names[i] = joint_names[i] + " | Limits = [ Lower: " + joint_lowers[i] + ", Upper: " + joint_uppers[i] + " ]" + # This is pretty bad way of doing this, but trying make a proper data storage system work with the below implementation + # of the tree structure became overly hard. The dict itself can store more data theoretically, but there's no clear way to + # associated more data with the names as they're passed along and sorted into the tree. - # Makes a "tree" of the joints using common prefixes. +# Makes a "tree" of the joints using common prefixes. joint_names.sort() joint_tree: Dict = {} for joint_name in joint_names: @@ -62,7 +78,7 @@ def print_tree(tree: Dict, depth: int = 0) -> None: print(" " * depth + key) print_tree(value, depth + 1) else: - print(" " * depth + key + ": " + value) + print(" " * depth + key) #+ ": " + value) print_tree(joint_tree)