Skip to content

Commit 659c1b3

Browse files
ioryiory
authored
Add 'skr transform-urdf command' to change urdf rotation with respect to world (#591)
1 parent 0aa0119 commit 659c1b3

File tree

5 files changed

+428
-1
lines changed

5 files changed

+428
-1
lines changed

‎skrobot/apps/cli.py‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@ def get_available_apps():
1616
'convert_urdf_mesh': ('Convert URDF mesh files', (3, 6)),
1717
'modularize_urdf': ('Modularize URDF files', None),
1818
'change_urdf_root': ('Change URDF root link', None),
19+
'transform_urdf': ('Add world link with transform to URDF', None),
1920
'visualize_mesh': ('Visualize mesh file', None),
2021
'urdf_hash': ('Calculate URDF hash', None),
2122
'convert_wheel_collision': ('Convert wheel collision model', None),
@@ -64,7 +65,8 @@ def main():
6465
for command_name, app_info in available_apps.items():
6566
app_parser = subparsers.add_parser(
6667
command_name,
67-
help=app_info['help']
68+
help=app_info['help'],
69+
add_help=False
6870
)
6971
app_parser.set_defaults(
7072
func=lambda args, module=app_info['module']: run_app(f'{module}:main', args)

‎skrobot/apps/transform_urdf.py‎

Lines changed: 167 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,167 @@
1+
#!/usr/bin/env python3
2+
3+
import argparse
4+
import os
5+
import sys
6+
7+
from skrobot.urdf import transform_urdf_with_world_link
8+
9+
10+
def main():
11+
"""Main entry point for the transform_urdf command.
12+
13+
This command line tool allows users to add a world link with transform
14+
to a URDF file by directly manipulating the XML structure and saving
15+
the result to a new file.
16+
"""
17+
parser = argparse.ArgumentParser(
18+
description='Add a transformed world link to a URDF file',
19+
formatter_class=argparse.RawDescriptionHelpFormatter,
20+
epilog="""
21+
Examples:
22+
# Add a world link with default transform
23+
transform_urdf robot.urdf output.urdf
24+
25+
# Add world link with translation
26+
transform_urdf robot.urdf output.urdf --x 1.0 --y 0.5 --z 0.2
27+
28+
# Add world link with rotation (in degrees)
29+
transform_urdf robot.urdf output.urdf --roll 10 --pitch 20 --yaw 30
30+
31+
# Add world link with custom name
32+
transform_urdf robot.urdf output.urdf --world-link-name my_world
33+
34+
# Combined translation and rotation
35+
transform_urdf robot.urdf output.urdf --x 1.0 --z 0.5 --yaw 45
36+
37+
# Modify file in place
38+
transform_urdf robot.urdf --inplace --x 1.0 --yaw 30
39+
""")
40+
41+
parser.add_argument(
42+
'input_urdf',
43+
help='Path to the input URDF file'
44+
)
45+
46+
parser.add_argument(
47+
'output_urdf',
48+
nargs='?',
49+
help='Path for the output URDF file (default: <input>_transformed.urdf)'
50+
)
51+
52+
# Translation arguments
53+
parser.add_argument(
54+
'--x',
55+
type=float, default=0.0,
56+
help='Translation in X (meters). Default: 0.0'
57+
)
58+
parser.add_argument(
59+
'--y',
60+
type=float, default=0.0,
61+
help='Translation in Y (meters). Default: 0.0'
62+
)
63+
parser.add_argument(
64+
'--z',
65+
type=float, default=0.0,
66+
help='Translation in Z (meters). Default: 0.0'
67+
)
68+
69+
# Rotation arguments (in degrees)
70+
parser.add_argument(
71+
'--roll',
72+
type=float, default=0.0,
73+
help='Rotation around X-axis (degrees). Default: 0.0'
74+
)
75+
parser.add_argument(
76+
'--pitch',
77+
type=float, default=0.0,
78+
help='Rotation around Y-axis (degrees). Default: 0.0'
79+
)
80+
parser.add_argument(
81+
'--yaw',
82+
type=float, default=0.0,
83+
help='Rotation around Z-axis (degrees). Default: 0.0'
84+
)
85+
86+
# World link name
87+
parser.add_argument(
88+
'--world-link-name',
89+
default='world',
90+
help='Name for the new world link. Default: "world"'
91+
)
92+
93+
# Verbose output
94+
parser.add_argument(
95+
'--verbose', '-v',
96+
action='store_true',
97+
help='Enable verbose output'
98+
)
99+
100+
# Inplace option
101+
parser.add_argument(
102+
'--inplace', '-i',
103+
action='store_true',
104+
help='Modify the input file in place (ignores output_urdf argument)'
105+
)
106+
107+
args = parser.parse_args()
108+
109+
# Check if input file exists
110+
if not os.path.exists(args.input_urdf):
111+
print(f"Error: Input file '{args.input_urdf}' not found.", file=sys.stderr)
112+
sys.exit(1)
113+
114+
# Determine output filename
115+
if args.inplace:
116+
output_urdf = args.input_urdf
117+
if args.verbose:
118+
print(f"Using inplace mode: will modify '{output_urdf}' directly")
119+
else:
120+
output_urdf = args.output_urdf
121+
if not output_urdf:
122+
base, ext = os.path.splitext(args.input_urdf)
123+
output_urdf = f"{base}_transformed{ext}"
124+
125+
# Check if output file already exists
126+
if os.path.exists(output_urdf):
127+
print(f"Error: Output file '{output_urdf}' already exists.", file=sys.stderr)
128+
print("Please specify a different output file or remove the existing file.", file=sys.stderr)
129+
sys.exit(1)
130+
131+
if args.verbose:
132+
print(f"Input URDF: {args.input_urdf}")
133+
print(f"Output URDF: {output_urdf}")
134+
print(f"World link name: {args.world_link_name}")
135+
print(f"Transform: x={args.x}, y={args.y}, z={args.z}")
136+
print(f"Rotation: roll={args.roll}°, pitch={args.pitch}°, yaw={args.yaw}°")
137+
138+
try:
139+
transform_urdf_with_world_link(
140+
input_file=args.input_urdf,
141+
output_file=output_urdf,
142+
x=args.x, y=args.y, z=args.z,
143+
roll=args.roll, pitch=args.pitch, yaw=args.yaw,
144+
world_link_name=args.world_link_name
145+
)
146+
147+
if args.verbose:
148+
print(f"Successfully created transformed URDF: {output_urdf}")
149+
else:
150+
print(f"Transformed URDF saved to: {output_urdf}")
151+
152+
except FileNotFoundError as e:
153+
print(f"Error: {e}", file=sys.stderr)
154+
sys.exit(1)
155+
except ValueError as e:
156+
print(f"Error: {e}", file=sys.stderr)
157+
sys.exit(1)
158+
except RuntimeError as e:
159+
print(f"Error: {e}", file=sys.stderr)
160+
sys.exit(1)
161+
except Exception as e:
162+
print(f"Unexpected error: {e}", file=sys.stderr)
163+
sys.exit(1)
164+
165+
166+
if __name__ == '__main__':
167+
main()

‎skrobot/urdf/__init__.py‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
from .aggregate import aggregate_urdf_mesh_files
22
from .modularize_urdf import find_root_link
33
from .modularize_urdf import transform_urdf_to_macro
4+
from .transform_urdf import transform_urdf_with_world_link
45
from .wheel_collision_converter import convert_wheel_collisions_to_cylinders
56
from .wheel_collision_converter import get_mesh_dimensions
67
from .xml_root_link_changer import change_urdf_root_link
@@ -12,6 +13,7 @@
1213
'URDFXMLRootLinkChanger',
1314
'find_root_link',
1415
'transform_urdf_to_macro',
16+
'transform_urdf_with_world_link',
1517
'aggregate_urdf_mesh_files',
1618
'convert_wheel_collisions_to_cylinders',
1719
'get_mesh_dimensions',

‎skrobot/urdf/transform_urdf.py‎

Lines changed: 92 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,92 @@
1+
import math
2+
import os
3+
import xml.etree.ElementTree as ET
4+
5+
from .modularize_urdf import find_root_link
6+
7+
8+
def transform_urdf_with_world_link(input_file, output_file,
9+
x=0.0, y=0.0, z=0.0,
10+
roll=0.0, pitch=0.0, yaw=0.0,
11+
world_link_name="world"):
12+
"""Add a transformed world link to a URDF file.
13+
14+
Parameters
15+
----------
16+
input_file : str
17+
Path to the input URDF file
18+
output_file : str
19+
Path for the output URDF file
20+
x : float, optional
21+
Translation in X (meters). Default: 0.0
22+
y : float, optional
23+
Translation in Y (meters). Default: 0.0
24+
z : float, optional
25+
Translation in Z (meters). Default: 0.0
26+
roll : float, optional
27+
Rotation around X-axis (degrees). Default: 0.0
28+
pitch : float, optional
29+
Rotation around Y-axis (degrees). Default: 0.0
30+
yaw : float, optional
31+
Rotation around Z-axis (degrees). Default: 0.0
32+
world_link_name : str, optional
33+
Name for the new world link. Default: 'world'
34+
35+
Raises
36+
------
37+
FileNotFoundError
38+
If the input file does not exist
39+
ValueError
40+
If the world link name already exists in the URDF or if root link cannot be determined
41+
"""
42+
if not os.path.exists(input_file):
43+
raise FileNotFoundError(f"URDF file not found: {input_file}")
44+
45+
# Register xacro namespace if present
46+
ET.register_namespace('xacro', "http://ros.org/wiki/xacro")
47+
48+
tree = ET.parse(input_file)
49+
root = tree.getroot()
50+
51+
# Find the original root link
52+
original_root_link = find_root_link(input_file)
53+
54+
# Check if the new world link name already exists
55+
if root.find(f"./link[@name='{world_link_name}']") is not None:
56+
raise ValueError(
57+
f"Link '{world_link_name}' already exists in the URDF. "
58+
f"Choose a different world link name.")
59+
60+
# Create the new world link element
61+
world_link = ET.Element('link', name=world_link_name)
62+
63+
# Create the new fixed joint to connect world to the original root
64+
joint_name = f"{world_link_name}_to_{original_root_link}"
65+
new_joint = ET.Element('joint', name=joint_name, type='fixed')
66+
67+
# Set parent (world) and child (original root)
68+
ET.SubElement(new_joint, 'parent', link=world_link_name)
69+
ET.SubElement(new_joint, 'child', link=original_root_link)
70+
71+
# Create the origin element with the specified transform
72+
# Convert degrees to radians for RPY
73+
roll_rad = math.radians(roll)
74+
pitch_rad = math.radians(pitch)
75+
yaw_rad = math.radians(yaw)
76+
77+
xyz_str = f"{x} {y} {z}"
78+
rpy_str = f"{roll_rad} {pitch_rad} {yaw_rad}"
79+
80+
ET.SubElement(new_joint, 'origin', xyz=xyz_str, rpy=rpy_str)
81+
82+
# Insert the new elements into the XML tree (at the beginning)
83+
root.insert(0, new_joint)
84+
root.insert(0, world_link)
85+
86+
# Try to use pretty printing if available (Python 3.9+)
87+
try:
88+
ET.indent(tree, space=" ")
89+
except AttributeError:
90+
pass
91+
92+
tree.write(output_file, encoding='utf-8', xml_declaration=True)

0 commit comments

Comments
 (0)