Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add print urdf info py #623

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Add print urdf info py #623

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
2dfb3e4
add print_urdf_info.py (python2) usage: scripts/print_urdf_info.py m…
Masanori-Konishi Jun 13, 2024
eebc39c
add README (about usage of print_urdf_info.py)
Masanori-Konishi Jun 13, 2024
b9c1f4b
fix print_urdf_info.py
Masanori-Konishi Jun 13, 2024
46debae
add jaxon_blue_sensors.urdf and fix endcoords
Masanori-Konishi Jun 14, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions hrpsys_ros_bridge_tutorials/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
87 changes: 87 additions & 0 deletions hrpsys_ros_bridge_tutorials/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
------------------------------------------------------------------------------------------------------------------------
```
4 changes: 2 additions & 2 deletions hrpsys_ros_bridge_tutorials/models/jaxon_blue.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
91 changes: 91 additions & 0 deletions hrpsys_ros_bridge_tutorials/scripts/print_urdf_info.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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()
Loading