Skip to content

Commit 9bf2e24

Browse files
FlovaFlova
committed
Add a script to create a mojoco model based on our urdf
1 parent 7aa8058 commit 9bf2e24

File tree

3 files changed

+214
-0
lines changed

3 files changed

+214
-0
lines changed

‎bitbots_wolfgang/wolfgang_description/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -59,5 +59,6 @@ ament_export_include_directories(${INCLUDE_DIRS})
5959
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
6060
install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME})
6161
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
62+
install(DIRECTORY scripts DESTINATION share/${PROJECT_NAME})
6263

6364
ament_package()

‎bitbots_wolfgang/wolfgang_description/scripts/urdf_to_mujoco.py‎

Lines changed: 190 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,190 @@
1+
#!/usr/bin/env python
2+
3+
import os
4+
import sys
5+
import tempfile
6+
import mujoco
7+
from lxml import etree
8+
9+
# This script converts a URDF file to a MuJoCo XML model file.
10+
11+
INPUT_URDF = '/home/florian/Projekt/bitbots/bitbots_main/bitbots_wolfgang/wolfgang_description/urdf/robot.urdf'
12+
13+
ACTUATOR_TYPES = ['mx106', 'mx64', 'xh-540']
14+
15+
ACTUATOR_MAPPING = {
16+
'HeadPan': 'mx64',
17+
'HeadTilt': 'mx64',
18+
'LShoulderPitch': 'mx106',
19+
'LShoulderRoll': 'mx106',
20+
'LElbow': 'mx64',
21+
'LAnklePitch': 'mx106',
22+
'LAnkleRoll': 'mx106',
23+
'LHipYaw': 'mx106',
24+
'LHipRoll': 'mx106',
25+
'LHipPitch': 'mx106',
26+
'LKnee': 'xh-540',
27+
'RShoulderPitch': 'mx106',
28+
'RShoulderRoll': 'mx106',
29+
'RElbow': 'mx64',
30+
'RAnklePitch': 'mx106',
31+
'RAnkleRoll': 'mx106',
32+
'RHipYaw': 'mx106',
33+
'RHipRoll': 'mx106',
34+
'RHipPitch': 'mx106',
35+
'RKnee': 'xh-540',
36+
} # TODO verify the mapping
37+
38+
ACTUATOR_DEFAULTS = {
39+
'mx106': {
40+
"joint": {
41+
"damping": "1.23",
42+
"armature": "0.045",
43+
"frictionloss": "2.55",
44+
"limited": "true"
45+
},
46+
"position": { # This is for the position controller
47+
"kp": "21.1",
48+
"ctrlrange": "-3.141592 3.141592",
49+
"forcerange": "-5 5"
50+
}
51+
},
52+
'mx64': {
53+
"joint": {
54+
"damping": "0.65",
55+
"armature": "0.045",
56+
"frictionloss": "1.73",
57+
"limited": "true"
58+
},
59+
"position": { # This is for the position controller
60+
"kp": "21.1",
61+
"ctrlrange": "-3.141592 3.141592",
62+
"forcerange": "-5 5"
63+
}
64+
},
65+
'xh-540': {
66+
"joint": {
67+
"damping": "2.92",
68+
"armature": "0.045",
69+
"frictionloss": "1.49",
70+
"limited": "true"
71+
},
72+
"position": { # This is for the position controller
73+
"kp": "21.1",
74+
"ctrlrange": "-3.141592 3.141592",
75+
"forcerange": "-5 5"
76+
}
77+
}
78+
}
79+
80+
81+
def main():
82+
# Load the urdf file
83+
if not os.path.exists(INPUT_URDF):
84+
print('Error: URDF file not found:', INPUT_URDF)
85+
sys.exit(1)
86+
urdf_tree = etree.parse(INPUT_URDF)
87+
88+
# Add the mujoco tag to the URDF file to make it compatible with mujoco
89+
mujoco_tag = etree.Element('mujoco')
90+
mujoco_tag.append(etree.Element('compiler', discardvisual='false', meshdir=os.path.dirname(INPUT_URDF)))
91+
urdf_tree.getroot().append(mujoco_tag)
92+
93+
# Render the URDF file as a string
94+
urdf_string = etree.tostring(urdf_tree, pretty_print=True)
95+
96+
# Load the URDF file into mujoco
97+
model = mujoco.MjModel.from_xml_string(urdf_string)
98+
99+
# Save model as XML (temporary file)
100+
temp_xml_file_path = tempfile.mktemp()
101+
mujoco.mj_saveLastXML(temp_xml_file_path, model)
102+
103+
# Load the XML file into an etree
104+
tree = etree.parse(temp_xml_file_path)
105+
106+
# Apply some modifications / fixes
107+
108+
# Move everything in the worldbody into a new body called torso (also add freejoint and light) # TODO investigate why the torso is not the root body
109+
worldbody = tree.find('.//worldbody')
110+
torso = etree.Element('body', name='torso', pos="0 0 0.4274", quat="0.999 0.0 0.05 0.0") # TODO check if pos is correct
111+
for child in worldbody.getchildren():
112+
torso.append(child)
113+
torso.append(etree.Element('freejoint'))
114+
worldbody.clear()
115+
worldbody.append(torso)
116+
worldbody.append(etree.Element('light', name='spotlight', mode='targetbodycom', target='torso', pos='0 -1 2'))
117+
118+
119+
# Assign classes to all geometries
120+
# Find visual elements, meaning geometries with contype="0" conaffinity="0" group="1" density="0"
121+
for geom in tree.findall('.//geom[@contype="0"][@conaffinity="0"][@group="1"][@density="0"]'):
122+
# Remove the attributes
123+
geom.attrib.pop('contype', None)
124+
geom.attrib.pop('conaffinity', None)
125+
geom.attrib.pop('group', None)
126+
geom.attrib.pop('density', None)
127+
128+
# Also remove the rgba attribute
129+
geom.attrib.pop('rgba', None)
130+
131+
# Assign the class attribute
132+
geom.attrib['class'] = 'visual'
133+
134+
# Find geometries that don't have a class yet and assign them to the collision class
135+
for geom in tree.xpath('.//geom[not(@class)]'):
136+
geom.attrib['class'] = 'collision'
137+
138+
# Add defaults
139+
140+
defaults = etree.fromstring("""
141+
<default>
142+
<site group="5" type="sphere"/>
143+
<default class="collision">
144+
<geom group="3"/>
145+
</default>
146+
<default class="visual">
147+
<geom material="black" contype="0" conaffinity="0" group="2"/>
148+
</default>
149+
</default>
150+
""")
151+
152+
# Add actuator and joint defaults
153+
assert set(ACTUATOR_DEFAULTS.keys()) == set(ACTUATOR_TYPES)
154+
for actuator_type, actuator_defaults in ACTUATOR_DEFAULTS.items():
155+
default = etree.Element('default', **{'class': actuator_type})
156+
default.extend([
157+
etree.Element('joint', **actuator_defaults['joint']),
158+
etree.Element('position', **actuator_defaults['position'])
159+
])
160+
defaults.append(default)
161+
tree.getroot().insert(0, defaults)
162+
163+
# Remove meshdir attribute from compiler tag
164+
tree.find('.//compiler').attrib.pop('meshdir', None)
165+
166+
# Add 'black' material to assets
167+
tree.find('.//asset').append(etree.Element('material', name='black', rgba='0.2 0.2 0.2 1'))
168+
169+
# Remove damping and frictionloss from all joints in the worldbody
170+
for joint in tree.findall('.//worldbody/.//joint'):
171+
# Remove the attributes
172+
joint.attrib.pop('damping', None)
173+
joint.attrib.pop('frictionloss', None)
174+
joint.attrib.pop('limited', None)
175+
# Add class based on the actuator type
176+
joint.attrib['class'] = ACTUATOR_MAPPING[joint.attrib['name']]
177+
178+
# Add actuators to the top level
179+
actuator = etree.Element('actuator')
180+
for joint in tree.findall('.//worldbody/.//joint'):
181+
actuator.append(etree.Element('position', **{'joint': joint.attrib['name'], 'name': joint.attrib['name'], 'class': ACTUATOR_MAPPING[joint.attrib['name']] }))
182+
tree.getroot().append(actuator)
183+
184+
# Save the XML file with pretty formatting
185+
output_filename = os.path.splitext(INPUT_URDF)[0] + '.xml'
186+
tree.write(output_filename, pretty_print=True)
187+
print('Saved MuJoCo model to', output_filename)
188+
189+
if __name__ == '__main__':
190+
main()

‎bitbots_wolfgang/wolfgang_description/urdf/scene.xml‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
<mujoco model="wolfgang scene">
2+
<include file="robot.xml"/>
3+
4+
<statistic center="0 0 0.2" extent="0.6"/>
5+
6+
<visual>
7+
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
8+
<rgba haze="0.15 0.25 0.35 1"/>
9+
<global azimuth="160" elevation="-20"/>
10+
</visual>
11+
12+
<asset>
13+
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
14+
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
15+
markrgb="0.8 0.8 0.8" width="300" height="300"/>
16+
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
17+
</asset>
18+
19+
<worldbody>
20+
<light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
21+
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
22+
</worldbody>
23+
</mujoco>

0 commit comments

Comments
 (0)