export urdf documentation

Northstar-Robotics · May 17, 2021 · 60421fe · 60421fe
1 parent 3a4a5e3
commit 60421fe
Show file tree

Hide file tree

Showing 6 changed files with 211 additions and 12 deletions.
diff --git a/Documentation~/images/export_urdf.png b/Documentation~/images/export_urdf.png
diff --git a/Documentation~/images/select_simple_robot_arm.png b/Documentation~/images/select_simple_robot_arm.png
diff --git a/README.md b/README.md
@@ -10,6 +10,7 @@
     - [Using RViz for Turtlebot](#using-rviz-for-turtlebot)
     - [Running Universal Robot UR10 Arm Test Scene with MoveIt!](#running-universal-robot-ur10-arm-test-scene-with-moveit)
     - [Running Image Segmentation Test](#running-image-segmentation-test)
+    - [Export URDF](#export-urdf)
 
 ZeroSim is a robotics simulation engine built on the easy to use [Unity 3D](https://unity.com/)  development platform and the power of the [Robotics Operating System (ROS)](https://www.ros.org/).  ZeroSim is designed for ease of use and rapid development of all sorts of robotics and simulation -- from warehouses and industrial settings, to farming and outdoors -- from robotic arms to ground and drone based mobile robots.
 
@@ -55,9 +56,11 @@ ZeroSim provides a multitude of tools for building robots and environments in Un
 * Machine Learning tools:
   * Image Segmentation for training semantic segmentation algorithms. 
 
+* URDF Export
+
 * **COMING SOON:**
   * More complete documentation.
-  * URDF import & export.
+  * URDF import.
   * Finish Docker integration with Unity. (Currently incomplete)
   * Secure communications via WebSockets.
   * Support for other Physics engines such as Bullet or Havok.
@@ -186,3 +189,15 @@ roslaunch zero_sim_ros basic_unity_editor.launch
 7. In the VNC window press the *LEFT* mouse button and select "Terminal". ![noVNC Terminal](Documentation~/images/novnc_terminal.png)
 8. In the new terminal run `rqt_image_view /image/segmentation_image`.  
 9. Open up a second terminal and run `rqt_image_view /image/image_raw` ![RQT Image View Segmentation](Documentation~/images/rqt_image_view_segmentation.png)
+
+
+### Export URDF
+
+1. Make sure that the ZeroSim samples are installed as outlined above.
+2. Open scene the `URDF_test.scene` in the ZeroSim samples.
+3. Select `SimpleRobotArm` in the scene hierarchy.   
+![Select Simple Robot Arm](Documentation~/images/select_simple_robot_arm.png)
+4. Select `Export URDF` in the root properties view. ![Export URDF](Documentation~/images/export_urdf.png)
+5. Select the directory to export to.
+6. An excellent online URDF viewer is available: https://gkjohnson.github.io/urdf-loaders/javascript/example/index.html  Just drag and drop the files exported above.
+
diff --git a/Runtime/Scripts/Util/ImportExport/ZOExportOBJ.cs b/Runtime/Scripts/Util/ImportExport/ZOExportOBJ.cs
@@ -43,8 +43,49 @@ protected void End() {
             _startIndex = 0;
         }
 
+        protected string MeshToString(Mesh mesh, ZOExportOBJ.Orientation orientation) {
 
-        protected string MeshToString(MeshFilter meshFilter, Transform transform, bool applyLocalTransform, ZOExportOBJ.Orientation orientation) {
+            int numVertices = 0;
+
+            StringBuilder sb = new StringBuilder();
+
+            foreach (Vector3 vv in mesh.vertices) {                
+                Vector3 v = vv;
+                numVertices++;
+                if (orientation == Orientation.Unity) {
+                    sb.AppendLine($"v {v.x} {v.y} {v.z}");
+                } else if (orientation == Orientation.URDF) {
+                    sb.AppendLine($"v {v.z} {v.x} {v.y}");
+                }
+            }
+            sb.AppendLine();
+            foreach (Vector3 nn in mesh.normals) {
+                Vector3 v = nn;
+                if (orientation == Orientation.Unity) {
+                    sb.AppendLine($"vn {v.x} {v.y} {v.z}");
+                } else if (orientation == Orientation.URDF) {
+                    sb.AppendLine($"vn {v.z} {v.x} {v.y}");
+                }
+            }
+
+            for (int submesh = 0; submesh < mesh.subMeshCount; submesh++) {
+                int[] triangles = mesh.GetTriangles(submesh);
+                for (int i = 0; i < triangles.Length; i += 3) {
+                    sb.AppendLine(string.Format("f {0}//{0} {1}//{1} {2}//{2}", // only exporting position and normal, no UV
+                                        triangles[i] + 1 + _startIndex,
+                                        triangles[i + 1] + 1 + _startIndex,
+                                        triangles[i + 2] + 1 + _startIndex));
+
+                }
+
+            }
+
+            _startIndex += numVertices;
+            return sb.ToString();
+        }
+
+
+        protected string MeshFilterToString(MeshFilter meshFilter, Transform transform, bool applyLocalTransform, ZOExportOBJ.Orientation orientation) {
             Vector3 s = transform.localScale;
             Vector3 p = transform.localPosition;
             Quaternion r = transform.localRotation;
@@ -141,7 +182,7 @@ protected string ProcessTransform(Transform transform, bool makeSubmeshes, bool
 
             MeshFilter meshFilter = transform.GetComponent<MeshFilter>();
             if (meshFilter != null) {
-                meshString.Append(MeshToString(meshFilter, transform, applyLocalTransform, orientation));
+                meshString.Append(MeshFilterToString(meshFilter, transform, applyLocalTransform, orientation));
             }
 
             for (int i = 0; i < transform.childCount; i++) {
@@ -265,6 +306,26 @@ public void ExportToDirectory(GameObject gameObject, string directoryPath, bool
             }
 
         }
+
+        public void ExportMesh(Mesh mesh, string meshPath, ZOExportOBJ.Orientation orientation) {
+            Start();
+
+            StringBuilder meshString = new StringBuilder();
+
+            meshString.Append("#" + Path.GetFileName(meshPath)
+                              + "\n#" + System.DateTime.Now.ToLongDateString()
+                              + "\n#" + System.DateTime.Now.ToLongTimeString()
+                              + "\n#-------"
+                              + "\n\n");
+
+            meshString.Append(MeshToString(mesh, orientation));
+
+            OBJString = meshString.ToString();
+
+            using (StreamWriter sw = new StreamWriter(meshPath)) {
+                sw.Write(OBJString);
+            }
+        }
     }
 
 }
diff --git a/Runtime/Scripts/Util/ImportExport/ZOExportURDF.cs b/Runtime/Scripts/Util/ImportExport/ZOExportURDF.cs
@@ -57,8 +57,8 @@ public List<Transform> VisualMeshesToExport {
             get { return _visualMeshesToExport; }
         }
 
-        private List<Transform> _collisionMeshesToExport = new List<Transform>();
-        public List<Transform> CollisionMeshesToExport {
+        private List<Mesh> _collisionMeshesToExport = new List<Mesh>();
+        public List<Mesh> CollisionMeshesToExport {
             get { return _collisionMeshesToExport; }
         }
 
@@ -75,9 +75,10 @@ public static void ExportToDirectory(ZOSimDocumentRoot documentRoot, string dire
                 exportOBJ.ExportToDirectory(meshTransform.gameObject, directoryPath, true, false, ZOExportOBJ.Orientation.URDF);
             }
 
-            foreach (Transform meshTransform in exportURDF.CollisionMeshesToExport) {
+            foreach (Mesh mesh in exportURDF.CollisionMeshesToExport) {
                 ZOExportOBJ exportOBJ = new ZOExportOBJ();
-                exportOBJ.ExportToDirectory(meshTransform.gameObject, directoryPath, true, false, ZOExportOBJ.Orientation.URDF);
+                string collisionMeshFilePath = Path.Combine(directoryPath, $"{mesh.name}_collider.obj");
+                exportOBJ.ExportMesh(mesh, collisionMeshFilePath, ZOExportOBJ.Orientation.URDF);
             }
 
             Debug.Log($"INFO: ZOExportURDF Saved URDF: {urdfFilePath}");
@@ -322,8 +323,33 @@ protected void BuildURDFCollisions(Transform collisionTransform, XElement link,
                                         sphereCollider.center.z * sphereCollider.transform.localScale.z);
 
 
+                } else if (collider.GetType() == typeof(CapsuleCollider)) {
+                    CapsuleCollider capsuleCollider = collider as CapsuleCollider;
+                    XElement cylinder = new XElement("cylinder");
+                    float radius = capsuleCollider.radius * collider.transform.localScale.x;
+                    float height = capsuleCollider.height * collider.transform.localScale.y;
+                    cylinder.SetAttributeValue("radius", radius);
+                    cylinder.SetAttributeValue("length", height);
+                    geometry.Add(cylinder);
+
+                    center = new Vector3(capsuleCollider.center.x * capsuleCollider.transform.localScale.x,
+                                        capsuleCollider.center.y * capsuleCollider.transform.localScale.y,
+                                        capsuleCollider.center.z * capsuleCollider.transform.localScale.z);
+
+
+                } else if (collider.GetType() == typeof(MeshCollider)) {
+                    MeshCollider meshCollider = collider as MeshCollider;
+                    _collisionMeshesToExport.Add(meshCollider.sharedMesh);
+
+                    XElement mesh = new XElement("mesh");
+                    mesh.SetAttributeValue("filename", $"{meshCollider.sharedMesh.name}_collider.obj");
+                    Vector3 scale = collisionTransform.localScale;
+                    mesh.SetAttributeValue("scale", scale.ToXMLString());
+                    geometry.Add(mesh);
+
                 }
 
+
                 if (geometry.HasElements) {
                     // build origin
                     Vector3 position = collisionTransform.localPosition + anchorOffset + center;

diff --git a/Samples~/ZeroSimSamples/Scenes/URDF_test.unity b/Samples~/ZeroSimSamples/Scenes/URDF_test.unity
@@ -841,7 +841,7 @@ Transform:
   m_LocalScale: {x: 1, y: 1, z: 1}
   m_Children:
   - {fileID: 10280180}
-  - {fileID: 1357432954}
+  - {fileID: 2050636604}
   m_Father: {fileID: 63962903}
   m_RootOrder: 2
   m_LocalEulerAnglesHint: {x: 0, y: 45, z: 0}
@@ -2985,8 +2985,12 @@ GameObject:
   serializedVersion: 6
   m_Component:
   - component: {fileID: 1357432954}
+  - component: {fileID: 1357432958}
+  - component: {fileID: 1357432957}
+  - component: {fileID: 1357432956}
+  - component: {fileID: 1357432955}
   m_Layer: 0
-  m_Name: collisions
+  m_Name: TeapotCollision
   m_TagString: Untagged
   m_Icon: {fileID: 0}
   m_NavMeshLayer: 0
@@ -2999,13 +3003,69 @@ Transform:
   m_PrefabInstance: {fileID: 0}
   m_PrefabAsset: {fileID: 0}
   m_GameObject: {fileID: 1357432953}
-  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
+  m_LocalRotation: {x: -0, y: -0, z: -0, w: 1}
   m_LocalPosition: {x: 0, y: 0, z: 0}
   m_LocalScale: {x: 1, y: 1, z: 1}
   m_Children: []
-  m_Father: {fileID: 290498957}
-  m_RootOrder: 1
+  m_Father: {fileID: 2050636604}
+  m_RootOrder: 0
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!64 &1357432955
+MeshCollider:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1357432953}
+  m_Material: {fileID: 0}
+  m_IsTrigger: 0
+  m_Enabled: 1
+  serializedVersion: 4
+  m_Convex: 0
+  m_CookingOptions: 30
+  m_Mesh: {fileID: 8022488761877085130, guid: 17be92f99b8fb14eaa65adc226ac3fd4, type: 3}
+--- !u!64 &1357432956
+MeshCollider:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1357432953}
+  m_Material: {fileID: 0}
+  m_IsTrigger: 0
+  m_Enabled: 1
+  serializedVersion: 4
+  m_Convex: 0
+  m_CookingOptions: 30
+  m_Mesh: {fileID: 883126711037108357, guid: 17be92f99b8fb14eaa65adc226ac3fd4, type: 3}
+--- !u!64 &1357432957
+MeshCollider:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1357432953}
+  m_Material: {fileID: 0}
+  m_IsTrigger: 0
+  m_Enabled: 1
+  serializedVersion: 4
+  m_Convex: 0
+  m_CookingOptions: 30
+  m_Mesh: {fileID: -3111557506051347654, guid: 17be92f99b8fb14eaa65adc226ac3fd4, type: 3}
+--- !u!64 &1357432958
+MeshCollider:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1357432953}
+  m_Material: {fileID: 0}
+  m_IsTrigger: 0
+  m_Enabled: 1
+  serializedVersion: 4
+  m_Convex: 0
+  m_CookingOptions: 30
+  m_Mesh: {fileID: 6192547391499243942, guid: 17be92f99b8fb14eaa65adc226ac3fd4, type: 3}
 --- !u!21 &1360496404
 Material:
   serializedVersion: 6
@@ -3921,6 +3981,37 @@ PhysicMaterial:
   bounciness: 0
   frictionCombine: 0
   bounceCombine: 0
+--- !u!1 &2050636603
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 2050636604}
+  m_Layer: 0
+  m_Name: collisions
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!4 &2050636604
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 2050636603}
+  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
+  m_LocalPosition: {x: 0, y: 0, z: 0}
+  m_LocalScale: {x: 1, y: 1, z: 1}
+  m_Children:
+  - {fileID: 1357432954}
+  m_Father: {fileID: 290498957}
+  m_RootOrder: 1
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
 --- !u!134 &2126012772
 PhysicMaterial:
   m_ObjectHideFlags: 0