Merge pull request #3 from Multiverse-Framework/AddUsdToUrdf

Add usd to urdf
Multiverse-Framework · Jul 4, 2023 · 061efe0 · 061efe0
2 parents 0797a2d + 18624ce
commit 061efe0
Show file tree

Hide file tree

Showing 1,147 changed files with 19,845 additions and 440 deletions.
diff --git a/install.sh b/install.sh
@@ -59,4 +59,7 @@ pip3 install cython
 pip3 install owlready2
 
 # Install jinja2
-pip3 install jinja2
+pip3 install jinja2
+
+# Install numpy-stl
+pip3 install numpy-stl
diff --git a/multiverse_ws/src/multiverse/multiverse_parser/examples/launch/usd_to_urdf.launch b/multiverse_ws/src/multiverse/multiverse_parser/examples/launch/usd_to_urdf.launch
@@ -0,0 +1,17 @@
+<launch>
+
+    <arg name="in_usd" default="$(find multiverse_parser)/examples/usd_to_urdf/usd/box_scenario_with_physics.usda"/>
+
+    <arg name="out_urdf" default="$(find multiverse_parser)/examples/usd_to_urdf/urdf/box_scenario.urdf"/>
+
+    <node pkg="multiverse_parser" type="usd_to_urdf.py" name="usd_to_urdf" output="screen" args="$(arg in_usd) $(arg out_urdf)"/>
+
+    <param name="robot_description" command="$(find xacro)/xacro '$(arg out_urdf)'" />
+
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+
+    <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
+
+    <node pkg="rviz" type="rviz" name="rviz" output="screen" args="-d $(find multiverse_parser)/examples/rviz_config/rviz.rviz"/>
+
+</launch>
diff --git a/multiverse_ws/src/multiverse/multiverse_parser/examples/mjcf_to_usd/mjcf/box_scenario.xml b/multiverse_ws/src/multiverse/multiverse_parser/examples/mjcf_to_usd/mjcf/box_scenario.xml
@@ -1,4 +1,4 @@
-<mujoco model="MuJoCo Model">
+<mujoco model="box_scenario">
     <compiler angle="radian" autolimits="true" boundmass="0.000001" boundinertia="0.000001"/>
     <option timestep="0.005" integrator="RK4" noslip_iterations="5">
         <flag energy="enable"/>

diff --git a/multiverse_ws/src/multiverse/multiverse_parser/examples/mjcf_to_usd/mjcf/kitchen.xml b/multiverse_ws/src/multiverse/multiverse_parser/examples/mjcf_to_usd/mjcf/kitchen.xml
@@ -1,4 +1,5 @@
 <mujoco model="kitchen">
+    <compiler angle="radian" autolimits="true" boundmass="0.000001" boundinertia="0.000001"/>
     <compiler meshdir="kitchen/stl/" />
     <asset>
         <mesh name="SinkArea0" file="Shape_IndexedFaceSet.001_Shape_IndexedFaceSet.273.stl" />

diff --git a/...erse/multiverse_parser/examples/mjcf_to_usd/usd/box_scenario_no_physics/usd/BoxFlap1.usda b/...erse/multiverse_parser/examples/mjcf_to_usd/usd/box_scenario_no_physics/usd/BoxFlap1.usda
@@ -9,7 +9,7 @@ def Mesh "Mesh_BoxFlap1"
 {
     int[] faceVertexCounts = [3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3]
     int[] faceVertexIndices = [0, 1, 2, 0, 2, 3, 4, 5, 6, 4, 6, 7, 8, 9, 10, 8, 10, 11, 12, 13, 14, 15, 14, 16, 17, 18, 19, 17, 19, 20, 21, 22, 23, 21, 23, 24, 25, 26, 23, 25, 23, 22, 27, 24, 23, 27, 23, 26, 28, 29, 30, 28, 30, 31, 17, 32, 30, 17, 30, 29, 33, 31, 30, 33, 30, 32, 34, 35, 36, 34, 36, 37, 5, 38, 36, 5, 36, 35, 39, 37, 36, 39, 36, 38, 4, 40, 41, 4, 41, 42, 43, 44, 41, 43, 41, 40, 20, 42, 41, 20, 41, 44, 45, 46, 47, 45, 47, 48, 49, 50, 47, 49, 47, 46, 51, 48, 47, 51, 47, 50, 52, 53, 54, 52, 54, 55, 56, 57, 54, 56, 54, 53, 58, 55, 54, 58, 54, 57, 6, 59, 60, 6, 60, 61, 62, 63, 60, 62, 60, 59, 64, 61, 60, 64, 60, 63, 7, 65, 66, 7, 66, 67, 68, 69, 66, 68, 66, 65, 70, 67, 66, 70, 66, 69, 71, 72, 24, 71, 24, 48, 48, 24, 27, 48, 27, 45, 73, 17, 29, 73, 29, 26, 26, 29, 28, 26, 28, 27, 6, 5, 35, 6, 35, 59, 59, 35, 74, 59, 74, 75, 5, 4, 42, 5, 42, 38, 38, 42, 20, 38, 20, 76, 77, 78, 37, 79, 37, 80, 80, 81, 76, 82, 76, 83, 43, 84, 85, 86, 85, 87, 87, 88, 17, 89, 17, 20, 90, 91, 53, 90, 53, 31, 31, 53, 52, 31, 52, 28, 4, 7, 67, 4, 67, 40, 40, 67, 92, 40, 92, 93, 94, 95, 63, 96, 63, 97, 97, 98, 75, 99, 75, 100, 101, 102, 69, 103, 69, 104, 104, 105, 68, 106, 68, 107, 108, 109, 65, 108, 65, 61, 61, 65, 7, 61, 7, 6, 110, 111, 46, 110, 46, 55, 55, 46, 45, 55, 45, 52, 28, 52, 45, 28, 45, 27]
-    normal3f[] normals = [(0, 0, -1), (0, 0, -1), (0, 0, -1), (0, 0, -1), (0.9999995, -0.00046040563, 0.00086519076), (0.99999803, -0.0018328703, -0.0008636712), (0.9999995, 0.0004604671, -0.0008651327), (0.99999803, 0.0018329321, 0.0008637292), (0, 0, 1), (0, 0, 1), (0, 0, 1), (0, 0, 1), (0, 1, 0), (0, 1, 0), (0, 1, 0), (0, 1, 0), (0, 1, 0), (-0.38254178, -0.92393756, 0.0011513755), (0, -1, 0), (0, -1, 0), (0.38260674, -0.9239112, 0.000576653), (-0.32504457, -0.32504514, -0.8880833), (-0.30288008, -0.6738931, -0.6738931), (-0.57734567, -0.5773496, -0.5773557), (-0.6038763, -0.00038059446, -0.7970781), (-0.32504687, -0.888079, -0.3250543), (-0.7970799, -0.60387385, -0.00035814132), (-0.99999815, -0.00091793685, -0.0017248064), (-0.9999995, -0.0009194577, 0.00043321648), (-0.60386425, -0.79708713, 0.00035814178), (-0.57734895, -0.57735294, 0.57734895), (-0.79707634, -0.00038068427, 0.60387856), (-0.3029085, -0.6738907, 0.67388266), (-0.32505813, -0.32505873, 0.8880732), (0.3250445, -0.32504514, -0.8880832), (0.7970716, -0.00038059754, -0.6038849), (0.5773456, -0.5773496, -0.5773557), (0.30287993, -0.673893, -0.6738933), (0.60386425, -0.79708713, -0.00035814184), (0.32504687, -0.88807905, -0.32505432), (0.60387856, -0.00038059804, 0.7970763), (0.577349, -0.5773529, 0.57734895), (0.79708, -0.6038738, 0.00035814152), (0.32505807, -0.3250587, 0.88807327), (0.30290848, -0.6738906, 0.6738827), (-0.9999995, 0.0009193962, -0.00043327428), (-0.60386425, 0.79708713, -0.00035817677), (-0.5773456, 0.5773496, -0.5773557), (-0.79707146, 0.00038059748, -0.6038849), (-0.32504687, 0.88807905, -0.32505432), (-0.30288005, 0.6738931, -0.6738931), (-0.32504448, 0.3250451, -0.8880832), (-0.9999981, 0.0009178752, 0.0017247484), (-0.6038786, 0.00038059815, 0.7970763), (-0.57734895, 0.5773529, 0.57734895), (-0.79708, 0.60387385, 0.00035809525), (-0.32505813, 0.32505873, 0.8880732), (-0.30290848, 0.67389065, 0.67388266), (-0.32505292, 0.8880774, 0.32505292), (0.60387635, 0.00038059452, -0.7970781), (0.57734567, 0.57734966, -0.5773556), (0.7970799, 0.60387385, -0.00035809525), (0.32504457, 0.32504514, -0.8880833), (0.30287996, 0.67389333, -0.6738929), (0.32504684, 0.888079, -0.32505426), (0.6038642, 0.79708713, 0.00035817665), (0.57734895, 0.5773529, 0.577349), (0.7970762, 0.000380684, 0.60387856), (0.32505277, 0.8880773, 0.32505292), (0.30290833, 0.67389053, 0.67388284), (0.32505813, 0.32505873, 0.8880732), (-0.38268822, 9.806773e-12, -0.9238776), (-0.38268822, 8.966193e-11, -0.92387754), (-0.38267168, -0.9238845, 0), (0.38268822, -3.502419e-11, -0.9238776), (0.38268825, 4.427058e-10, -0.92387754), (0.38267162, -0.92388445, -6.326462e-10), (0, -0.38268885, -0.92387736), (0, -0.38268885, -0.92387736), (0, -0.38268882, -0.9238773), (0, -0.7071068, -0.7071068), (0, -0.92388135, -0.38267908), (0, -0.92388135, -0.3826791), (0, -0.92388135, -0.3826791), (0, -0.38268393, 0.9238794), (0, -0.38268393, 0.9238793), (0, -0.38268393, 0.9238794), (0, -0.707111, 0.7071026), (0, -0.9238845, 0.38267162), (-0.14783852, -0.91373235, 0.37846664), (-0.38268337, -2.3521388e-8, 0.9238796), (-0.38268337, -2.3506818e-8, 0.9238796), (0.38268337, 2.3535957e-8, 0.9238796), (0.38268337, 2.3535957e-8, 0.9238796), (0, 0.92388135, -0.38267908), (0, 0.92388135, -0.38267908), (0, 0.92388135, -0.3826791), (0, 0.7071068, -0.7071068), (0, 0.38268882, -0.9238773), (0, 0.38268885, -0.92387736), (0, 0.38268885, -0.92387736), (0, 0.38268393, 0.9238794), (0, 0.38268393, 0.9238794), (0, 0.38268393, 0.9238793), (0, 0.70711094, 0.7071026), (0, 0.9238845, 0.38267165), (0, 0.9238845, 0.38267165), (0, 0.9238845, 0.38267165), (0.38267162, 0.92388445, 2.2130957e-8), (0.38267162, 0.92388445, 2.2119297e-8), (-0.38267162, 0.92388445, -2.2130957e-8), (-0.38267162, 0.92388445, -2.2119297e-8)]
+    normal3f[] normals = [(0, 0, 0), (0, 0, 0), (0.99999976, -0.0006499791, -0.00034591684), (0.99999976, 0.00064997905, 0.00034591678), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0.0007514678, -0.011356895, 0.9999352), (-0.4375984, -0.4905073, -0.75359815), (-0.6001879, -0.3086156, -0.7379234), (-0.62683135, -0.73003477, -0.2722714), (-0.43760356, -0.75359285, -0.49051085), (-0.8757902, -0.20643236, -0.4363225), (-0.8870263, -0.29931584, -0.35155985), (-0.8760428, -0.4358968, 0.20625976), (-0.8872246, -0.3516131, 0.29866478), (-0.41962987, -0.83348227, 0.3594691), (-0.4697895, -0.81758404, 0.33294776), (-0.62683, -0.27228206, 0.73003197), (-0.4376148, -0.49050716, 0.75358874), (0.6268197, -0.272278, -0.7300423), (0.43759847, -0.4905071, -0.7535982), (0.8757666, -0.43636996, -0.20643245), (0.8870158, -0.3515643, -0.29934192), (0.4376035, -0.7535927, -0.49051115), (0.6002034, -0.7379152, -0.30860487), (0.8760559, -0.20627126, 0.435865), (0.88723004, -0.2986586, 0.35160458), (0.43761474, -0.49050725, 0.75358874), (0.60020155, -0.3086155, 0.73791236), (0.59024614, -0.764237, 0.2599064), (0.41958633, -0.8336141, 0.35921413), (-0.8760419, 0.4358963, -0.20626472), (-0.88722366, 0.35161236, -0.2986686), (-0.43760368, 0.7535927, -0.490511), (-0.6002034, 0.7379153, -0.3086049), (-0.6268197, 0.2722779, -0.7300423), (-0.4375984, 0.49050716, -0.7535983), (-0.8757907, 0.20643298, 0.4363211), (-0.88702714, 0.2993159, 0.35155755), (-0.43761477, 0.49050725, 0.75358874), (-0.60020155, 0.3086156, 0.7379123), (-0.6268332, 0.7300345, 0.27226764), (-0.43761703, 0.7535931, 0.4904984), (0.8760553, 0.20627089, -0.43586615), (0.8872292, 0.2986584, -0.35160697), (0.43759826, 0.4905075, -0.75359815), (0.6001879, 0.3086157, -0.7379233), (0.6268313, 0.7300348, -0.27227136), (0.43760365, 0.7535929, -0.49051067), (0.87576735, 0.43637046, 0.20642744), (0.8870167, 0.35156476, 0.29933855), (0.4376168, 0.7535933, 0.4904984), (0.6002054, 0.73791534, 0.30860087), (0.62683004, 0.27228191, 0.73003197), (0.43761465, 0.49050725, 0.7535888), (0.0024887035, -0.99999595, 0.0013397419), (-0.009324098, 0.9998686, -0.013263729), (-0.009194662, -0.99994385, -0.0052549215), (-0.924968, 0.21808861, -0.31124195), (-0.49724156, -0.86746204, 0.016135981), (-0.00877295, -0.012479841, -0.9998836), (-0.0075655263, -0.0038585388, 0.99996394), (-0.94776696, -0.3189625, -0.0008303137), (0.94777346, -0.0008190855, -0.31894317), (0.008040192, 0.99995935, -0.00410038), (0.009324097, -0.9998686, -0.013263733), (0.0039060484, 0.9999484, -0.009374516), (0.91896284, -0.3092401, 0.24469957), (0.008652653, -0.004945729, -0.99995035), (0.014873801, -0.01858015, 0.9997167), (0.4972835, -0.867446, 0.015697747), (0, 0, -0), (0.1503727, -0.5492553, -0.82201374), (-0.48565406, 0.7268312, 0.48565048), (-0, 0, 0), (0.17579064, -0.98442763, 0), (-1, 0, -0), (0.4845785, 0.7273307, -0.48597708), (0.000054701923, -0.9996354, 0.027003016), (-0.49733907, 0, 0.8675563), (-0.00932459, -0.9998686, 0.013264504), (-0.0091954, 0.99994385, 0.005255191), (-0.924975, -0.21806143, 0.31124002), (0.94777465, 0.0008190839, 0.31893963), (0.008040918, -0.99995935, 0.004100647), (0.009324588, 0.9998686, 0.013264498), (0, 0, 0), (0, 0, -0), (0.1503725, 0.8220166, -0.5492511), (-0.48565447, -0.48565418, 0.7268286), (0, -0, 0), (-0, 0, 0), (0.15038702, 0.54925454, 0.82201165), (-0.18672936, -0.8168502, -0.5457911), (0, -0, 0), (-0.49732584, -0.86756384, -0), (0.008773003, 0.012479841, -0.9998837), (0.008652593, 0.0049457294, 0.99995035), (0.9189627, 0.3092401, -0.2446999), (0.4973257, -0.8675639, 0), (-0.008772998, 0.012479835, 0.9998837), (-0.007565464, 0.0038585362, -0.99996394), (-0.94776696, 0.31896234, 0.00085744815), (-0.99999976, 0.00031246128, 0.00066310074), (-0.99999976, -0.00029944204, -0.0006354714)]
     point3f[] points = [(3.1607434e-9, -0.15900001, -0.17), (3.2352492e-9, 0.15900001, -0.17), (3.4587666e-9, 0.15900001, -0.17), (3.3842609e-9, -0.15900001, -0.17), (0.0010000023, -0.15900001, 0.16899998), (0.0010000024, -0.15900001, -0.16900001), (0.0010000023, 0.15900001, -0.16900001), (0.0009999828, 0.15900001, 0.16899998), (-1.6447324e-8, 0.15900001, 0.16999999), (3.0862375e-9, -0.15900001, 0.16999999), (3.3972996e-9, -0.15900001, 0.16999999), (-1.613626e-8, 0.15900001, 0.16999999), (3.3842609e-9, 0.16, -0.16900001), (3.1028926e-9, 0.16, -0.16900001), (-1.6434283e-8, 0.16, 0.16899998), (3.3842609e-9, 0.16, -0.16900001), (-1.6152915e-8, 0.16, 0.16899998), (3.1607434e-9, -0.16, 0.16899998), (3.1607434e-9, -0.16, -0.16900001), (3.4587666e-9, -0.16, -0.16900001), (3.4587666e-9, -0.16, 0.16899998), (3.1607434e-9, -0.15900001, -0.17), (3.1607434e-9, -0.15970711, -0.1697071), (-0.00057734654, -0.15957735, -0.16957738), (-0.0007071029, -0.15900001, -0.1697071), (3.1607434e-9, -0.16, -0.16900001), (-0.0007071029, -0.15970711, -0.16900001), (-0.0009999959, -0.15900001, -0.16900001), (-0.0009999959, -0.15900001, 0.16899998), (-0.0007071029, -0.15970711, 0.16899998), (-0.0005773466, -0.15957735, 0.16957733), (-0.00070710294, -0.15900001, 0.16970709), (3.1389211e-9, -0.15970711, 0.16970709), (3.0862375e-9, -0.15900001, 0.16999999), (3.3842609e-9, -0.15900001, -0.17), (0.0007071096, -0.15900001, -0.1697071), (0.0005773532, -0.15957735, -0.16957738), (3.4215137e-9, -0.15970711, -0.1697071), (0.0007071096, -0.15970711, -0.16900001), (3.4587666e-9, -0.16, -0.16900001), (0.00070710946, -0.15900001, 0.16970709), (0.0005773532, -0.15957735, 0.16957733), (0.0007071095, -0.15970711, 0.16899998), (3.3972996e-9, -0.15900001, 0.16999999), (3.4369443e-9, -0.15970711, 0.16970709), (-0.0009999959, 0.15900001, -0.16900001), (-0.00070710294, 0.15970711, -0.16900001), (-0.0005773466, 0.15957735, -0.16957738), (-0.00070710294, 0.15900001, -0.1697071), (3.1028926e-9, 0.16, -0.16900001), (3.176174e-9, 0.15970711, -0.1697071), (3.2352492e-9, 0.15900001, -0.17), (-0.0010000154, 0.15900001, 0.16899998), (-0.0007071225, 0.15900001, 0.16970709), (-0.00057736615, 0.15957735, 0.16957733), (-0.0007071225, 0.15970711, 0.16899998), (-1.6447324e-8, 0.15900001, 0.16999999), (-1.6465146e-8, 0.15970711, 0.16970709), (-1.6434283e-8, 0.16, 0.16899998), (0.0007071095, 0.15900001, -0.1697071), (0.0005773532, 0.15957735, -0.16957738), (0.00070710946, 0.15970711, -0.16900001), (3.4587666e-9, 0.15900001, -0.17), (3.4369443e-9, 0.15970711, -0.1697071), (3.3842609e-9, 0.16, -0.16900001), (0.0007070899, 0.15970711, 0.16899998), (0.0005773336, 0.15957735, 0.16957733), (0.0007070899, 0.15900001, 0.16970709), (-1.6152915e-8, 0.16, 0.16899998), (-1.6167123e-8, 0.15970711, 0.16970709), (-1.613626e-8, 0.15900001, 0.16999999), (3.2352492e-9, 0.15900001, -0.17), (3.1607434e-9, -0.15900001, -0.17), (3.1607434e-9, -0.16, -0.16900001), (3.3842609e-9, -0.15900001, -0.17), (3.4587666e-9, 0.15900001, -0.17), (3.4587666e-9, -0.16, -0.16900001), (3.1607434e-9, -0.15900001, -0.17), (3.3842609e-9, -0.15900001, -0.17), (3.1607434e-9, -0.15900001, -0.17), (3.1607434e-9, -0.15970711, -0.1697071), (3.4215137e-9, -0.15970711, -0.1697071), (3.1607434e-9, -0.15970711, -0.1697071), (3.1607434e-9, -0.16, -0.16900001), (3.0862375e-9, -0.15900001, 0.16999999), (3.1389211e-9, -0.15970711, 0.16970709), (3.3972996e-9, -0.15900001, 0.16999999), (3.4369443e-9, -0.15970711, 0.16970709), (3.1389211e-9, -0.15970711, 0.16970709), (3.4369443e-9, -0.15970711, 0.16970709), (3.0862375e-9, -0.15900001, 0.16999999), (-1.6447324e-8, 0.15900001, 0.16999999), (-1.613626e-8, 0.15900001, 0.16999999), (3.3972996e-9, -0.15900001, 0.16999999), (3.1028926e-9, 0.16, -0.16900001), (3.3842609e-9, 0.16, -0.16900001), (3.1028926e-9, 0.16, -0.16900001), (3.176174e-9, 0.15970711, -0.1697071), (3.4369443e-9, 0.15970711, -0.1697071), (3.176174e-9, 0.15970711, -0.1697071), (3.2352492e-9, 0.15900001, -0.17), (-1.6447324e-8, 0.15900001, 0.16999999), (-1.613626e-8, 0.15900001, 0.16999999), (-1.6447324e-8, 0.15900001, 0.16999999), (-1.6465146e-8, 0.15970711, 0.16970709), (-1.6167123e-8, 0.15970711, 0.16970709), (-1.6465146e-8, 0.15970711, 0.16970709), (-1.6434283e-8, 0.16, 0.16899998), (3.3842609e-9, 0.16, -0.16900001), (-1.6152915e-8, 0.16, 0.16899998), (-1.6434283e-8, 0.16, 0.16899998), (3.1028926e-9, 0.16, -0.16900001)]
 }