diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index a1e5b88f..afc7afab 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -750,6 +750,7 @@ endif() endif() if(${jsk_models_FOUND}) + attach_sensor_and_endeffector_to_staro_urdf(JAXON_BLUE JAXON_BLUE.urdf JAXON_BLUE_SENSORS.urdf jaxon_blue.yaml) attach_sensor_and_endeffector_to_staro_urdf(CHIDORI CHIDORI.urdf CHIDORI_SENSORS.urdf chidori.yaml) attach_sensor_and_endeffector_to_staro_urdf(TABLIS TABLIS.urdf TABLIS_SENSORS.urdf tablis.yaml) endif() diff --git a/hrpsys_ros_bridge_tutorials/README.md b/hrpsys_ros_bridge_tutorials/README.md new file mode 100644 index 00000000..66f7a61b --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/README.md @@ -0,0 +1,87 @@ +# How to easily check mass parameters, max torque, and other information for each link and joint in a URDF file (python2) +## Script to use : +rtmros_tutorials/hrpsys_ros_bridge_tutorials/scripts/print_urdf_info.py +## Usage example: +```console +$ roscd hrpsys_ros_bridge_tutorials +$ python scripts/print_urdf_info.py models/JAXON_BLUE.urdf +``` +## Output example: +```console +$ python scripts/print_urdf_info.py models/JAXON_BLUE.urdf +------------------------------------------------------------------------------------------------------------------------ +file_path : models/JAXON_BLUE.urdf +======================================================================================================================== +Link Name Mass Inertia +------------------------------------------------------------------------------------------------------------------------ +BODY 3.761 ixx=+0.01805, ixy=-0.00003, ixz=-0.00184, iyy=+0.01287, iyz=+0.00007, izz=+0.01943 +CHEST_LINK0 0.826 ixx=+0.00253, ixy=-0.00003, ixz=-0.00017, iyy=+0.00325, iyz=+0.00001, izz=+0.00231 +CHEST_LINK1 4.889 ixx=+0.03611, ixy=+0.00096, ixz=+0.00096, iyy=+0.03182, iyz=+0.00095, izz=+0.03449 +CHEST_LINK2 4.796 ixx=+0.04443, ixy=+0.00002, ixz=-0.00105, iyy=+0.01565, iyz=-0.00001, izz=+0.04734 +HEAD_LINK0 3.847 ixx=+0.10310, ixy=+0.00000, ixz=+0.00000, iyy=+0.00350, iyz=+0.00000, izz=+0.00119 +HEAD_LINK1 0.001 ixx=+0.03568, ixy=+0.00088, ixz=-0.00317, iyy=+0.04004, iyz=+0.00065, izz=+0.03535 +LARM_LINK0 0.652 ixx=+0.00165, ixy=-0.00003, ixz=-0.00003, iyy=+0.00118, iyz=+0.00002, izz=+0.00160 +LARM_LINK1 1.189 ixx=+0.00337, ixy=+0.00009, ixz=+0.00028, iyy=+0.00401, iyz=+0.00009, izz=+0.00197 +LARM_LINK2 1.538 ixx=+0.00437, ixy=-0.00008, ixz=-0.00008, iyy=+0.00395, iyz=-0.00017, izz=+0.00154 +LARM_LINK3 1.109 ixx=+0.00417, ixy=+0.00001, ixz=+0.00003, iyy=+0.00406, iyz=+0.00007, izz=+0.00098 +LARM_LINK4 0.687 ixx=+0.00098, ixy=+0.00003, ixz=-0.00001, iyy=+0.00101, iyz=-0.00016, izz=+0.00082 +LARM_LINK5 0.250 ixx=+0.00534, ixy=+0.00000, ixz=+0.00000, iyy=+0.00624, iyz=+0.00000, izz=+0.00206 +LARM_LINK6 0.250 ixx=+0.00090, ixy=+0.00000, ixz=+0.00000, iyy=+0.00063, iyz=+0.00000, izz=+0.00059 +RARM_LINK0 0.652 ixx=+0.00165, ixy=-0.00003, ixz=-0.00003, iyy=+0.00118, iyz=+0.00002, izz=+0.00160 +RARM_LINK1 1.189 ixx=+0.00337, ixy=+0.00009, ixz=+0.00028, iyy=+0.00401, iyz=+0.00009, izz=+0.00197 +RARM_LINK2 1.538 ixx=+0.00437, ixy=-0.00008, ixz=-0.00008, iyy=+0.00395, iyz=-0.00017, izz=+0.00154 +RARM_LINK3 1.109 ixx=+0.00417, ixy=+0.00001, ixz=+0.00003, iyy=+0.00406, iyz=+0.00007, izz=+0.00098 +RARM_LINK4 0.687 ixx=+0.00098, ixy=+0.00003, ixz=-0.00001, iyy=+0.00101, iyz=-0.00016, izz=+0.00082 +RARM_LINK5 0.250 ixx=+0.00534, ixy=+0.00000, ixz=+0.00000, iyy=+0.00624, iyz=+0.00000, izz=+0.00206 +RARM_LINK6 0.250 ixx=+0.00090, ixy=+0.00000, ixz=+0.00000, iyy=+0.00063, iyz=+0.00000, izz=+0.00059 +LLEG_LINK0 1.230 ixx=+0.00294, ixy=-0.00015, ixz=-0.00009, iyy=+0.00350, iyz=+0.00001, izz=+0.00265 +LLEG_LINK1 0.999 ixx=+0.00199, ixy=+0.00003, ixz=+0.00008, iyy=+0.00249, iyz=-0.00002, izz=+0.00274 +LLEG_LINK2 3.945 ixx=+0.06638, ixy=+0.00025, ixz=-0.00167, iyy=+0.06489, iyz=-0.00058, izz=+0.01059 +LLEG_LINK3 1.748 ixx=+0.02797, ixy=+0.00005, ixz=+0.00127, iyy=+0.02794, iyz=-0.00147, izz=+0.00286 +LLEG_LINK4 0.649 ixx=+0.00059, ixy=+0.00000, ixz=-0.00003, iyy=+0.00094, iyz=+0.00000, izz=+0.00080 +LLEG_LINK5 2.798 ixx=+0.00204, ixy=-0.00099, ixz=+0.00020, iyy=+0.00593, iyz=-0.00006, izz=+0.00626 +RLEG_LINK0 1.230 ixx=+0.00294, ixy=-0.00015, ixz=-0.00009, iyy=+0.00350, iyz=+0.00001, izz=+0.00265 +RLEG_LINK1 0.999 ixx=+0.00199, ixy=+0.00003, ixz=+0.00008, iyy=+0.00249, iyz=-0.00002, izz=+0.00274 +RLEG_LINK2 3.945 ixx=+0.06638, ixy=+0.00025, ixz=-0.00167, iyy=+0.06489, iyz=-0.00058, izz=+0.01059 +RLEG_LINK3 1.748 ixx=+0.02797, ixy=+0.00005, ixz=+0.00127, iyy=+0.02794, iyz=-0.00147, izz=+0.00286 +RLEG_LINK4 0.649 ixx=+0.00059, ixy=+0.00000, ixz=-0.00003, iyy=+0.00094, iyz=+0.00000, izz=+0.00080 +RLEG_LINK5 2.798 ixx=+0.00204, ixy=-0.00099, ixz=+0.00020, iyy=+0.00593, iyz=-0.00006, izz=+0.00626 +------------------------------------------------------------------------------------------------------------------------ +Total Mass 52.21 +======================================================================================================================== + +Joint Name Lower[rad] Upper[rad] Lower[deg] Upper[deg] Effort Velocity +------------------------------------------------------------------------------------------------------------------------ +CHEST_JOINT0 -0.79 0.79 -45.00 45.00 500.00 10.67 +CHEST_JOINT1 -0.59 1.40 -34.00 80.00 700.00 9.76 +CHEST_JOINT2 -1.57 1.57 -90.00 90.00 300.00 12.20 +HEAD_JOINT0 -0.39 0.39 -22.50 22.50 287.17 4.00 +HEAD_JOINT1 -0.55 0.67 -31.50 38.50 287.17 4.00 +LARM_JOINT0 -3.32 1.05 -190.00 60.00 190.00 9.76 +LARM_JOINT1 -0.52 3.32 -30.00 190.00 200.00 9.76 +LARM_JOINT2 -2.88 2.88 -165.00 165.00 145.00 12.80 +LARM_JOINT3 -2.44 0.00 -140.00 0.00 170.00 11.73 +LARM_JOINT4 -2.88 2.88 -165.00 165.00 145.00 12.80 +LARM_JOINT5 -1.57 1.57 -90.00 90.00 48.05 38.72 +LARM_JOINT6 -1.57 1.57 -90.00 90.00 48.05 38.72 +RARM_JOINT0 -3.32 1.05 -190.00 60.00 190.00 9.76 +RARM_JOINT1 -3.32 0.52 -190.00 30.00 200.00 9.76 +RARM_JOINT2 -2.88 2.88 -165.00 165.00 145.00 12.80 +RARM_JOINT3 -2.44 0.00 -140.00 0.00 170.00 11.73 +RARM_JOINT4 -2.88 2.88 -165.00 165.00 145.00 12.80 +RARM_JOINT5 -1.57 1.57 -90.00 90.00 48.05 38.72 +RARM_JOINT6 -1.57 1.57 -90.00 90.00 48.05 38.72 +LLEG_JOINT0 -1.57 1.57 -90.00 90.00 120.00 16.08 +LLEG_JOINT1 -0.52 1.05 -30.00 60.00 340.00 11.02 +LLEG_JOINT2 -2.09 0.96 -120.00 55.00 700.00 15.66 +LLEG_JOINT3 0.00 2.75 0.00 157.50 700.00 14.52 +LLEG_JOINT4 -1.40 1.31 -80.00 75.00 220.00 13.31 +LLEG_JOINT5 -1.05 1.05 -60.00 60.00 120.00 15.18 +RLEG_JOINT0 -1.57 1.57 -90.00 90.00 120.00 16.08 +RLEG_JOINT1 -1.05 0.52 -60.00 30.00 340.00 11.02 +RLEG_JOINT2 -2.09 0.96 -120.00 55.00 700.00 15.66 +RLEG_JOINT3 0.00 2.75 0.00 157.50 700.00 14.52 +RLEG_JOINT4 -1.40 1.31 -80.00 75.00 220.00 13.31 +RLEG_JOINT5 -1.05 1.05 -60.00 60.00 120.00 15.18 +------------------------------------------------------------------------------------------------------------------------ +``` diff --git a/hrpsys_ros_bridge_tutorials/models/jaxon_blue.yaml b/hrpsys_ros_bridge_tutorials/models/jaxon_blue.yaml index 437d75bd..3cea83d5 100644 --- a/hrpsys_ros_bridge_tutorials/models/jaxon_blue.yaml +++ b/hrpsys_ros_bridge_tutorials/models/jaxon_blue.yaml @@ -69,10 +69,10 @@ larm-end-coords: ## KAWADA FOOT : CAD -> -0.096; gomugutsu+midori -> -0.004 rleg-end-coords: parent: RLEG_LINK5 - translate : [0, 0, -0.100] + translate : [0.054, 0, -0.100] lleg-end-coords: parent: LLEG_LINK5 - translate : [0, 0, -0.100] + translate : [0.054, 0, -0.100] ## JSK FOOT # rleg-end-coords: # parent: RLEG_LINK5 diff --git a/hrpsys_ros_bridge_tutorials/scripts/print_urdf_info.py b/hrpsys_ros_bridge_tutorials/scripts/print_urdf_info.py new file mode 100644 index 00000000..807b7fc1 --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/scripts/print_urdf_info.py @@ -0,0 +1,91 @@ +import xml.etree.ElementTree as ET +import sys +import math + +def parse_urdf(file_path): + tree = ET.parse(file_path) + root = tree.getroot() + + links = [] + joints = [] + + for link in root.findall('link'): + name = link.get('name') + + mass_element = link.find('inertial/mass') + if mass_element is not None: + mass = float(mass_element.get('value')) + else: + mass = 0.0 + + inertia_element = link.find('inertial/inertia') + if inertia_element is not None: + ixx = float(inertia_element.get('ixx')) + ixy = float(inertia_element.get('ixy')) + ixz = float(inertia_element.get('ixz')) + iyy = float(inertia_element.get('iyy')) + iyz = float(inertia_element.get('iyz')) + izz = float(inertia_element.get('izz')) + inertia = (ixx, ixy, ixz, iyy, iyz, izz) + else: + inertia = None + + links.append((name, mass, inertia)) + + for joint in root.findall('joint'): + name = joint.get('name') + limit_element = joint.find('limit') + if limit_element is not None: + lower = float(limit_element.get('lower', 0.0)) + upper = float(limit_element.get('upper', 0.0)) + effort = float(limit_element.get('effort', 0.0)) + velocity = float(limit_element.get('velocity', 0.0)) + limits = (lower, upper, effort, velocity) + else: + limits = (0.0, 0.0, 0.0, 0.0) + + joints.append((name, limits)) + + return links, joints + +def main(): + if len(sys.argv) != 2: + print("Usage: python print_urdf_info.py path_to_your_urdf_file.urdf") + print("with python2") + sys.exit(1) + + file_path = sys.argv[1] + links, joints = parse_urdf(file_path) + + total_mass = sum(link[1] for link in links) + + print("-" * 120) + print("file_path : {}".format(file_path)) + print("=" * 120) + print("{:<20} {:<10} {:<70}".format("Link Name", "Mass", "Inertia")) + print("-" * 120) + + for link in links: + name = link[0] + mass = link[1] + if link[2] is not None: + inertia = "ixx={:+.5f}, ixy={:+.5f}, ixz={:+.5f}, iyy={:+.5f}, iyz={:+.5f}, izz={:+.5f}".format( + link[2][0], link[2][1], link[2][2], link[2][3], link[2][4], link[2][5]) + else: + inertia = "None" + print("{:<20} {:<10.3f} {:<70}".format(name, mass, inertia)) + + print("-" * 120) + print("{:<20} {:<10.2f}".format("Total Mass", total_mass)) + print("=" * 120) + print("\n{:<20} {:<10} {:<10} {:<10} {:<10} {:<10} {:<10}".format("Joint Name", "Lower[rad]", "Upper[rad]", "Lower[deg]", "Upper[deg]", "Effort", "Velocity")) + print("-" * 120) + + for joint in joints: + name = joint[0] + limits = joint[1] + print("{:<20} {:<10.2f} {:<10.2f} {:<10.2f} {:<10.2f} {:<10.2f} {:<10.2f}".format(name, limits[0], limits[1], limits[0]*180/math.pi, limits[1]*180/math.pi, limits[2], limits[3])) + + print("-" * 120) +if __name__ == "__main__": + main()