Merge branch 'feature/urdf' into develop

Author: micahpearlman

Documentation~/images/export_urdf.png

@@ -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.
+

Documentation~/images/select_simple_robot_arm.png

@@ -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);
+            }
+        }
     }
 
 }

README.md

@@ -54,11 +54,11 @@ public Vector3 ConnectedAnchor {
 
         private List<Transform> _visualMeshesToExport = new List<Transform>();
         public List<Transform> VisualMeshesToExport {
-            get {return _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; }
         }
 
@@ -70,14 +70,15 @@ public static void ExportToDirectory(ZOSimDocumentRoot documentRoot, string dire
             urdfXML.Save(urdfFilePath);
 
             // save out visual and collision meshes
-            foreach(Transform meshTransform in exportURDF.VisualMeshesToExport) {
+            foreach (Transform meshTransform in exportURDF.VisualMeshesToExport) {
                 ZOExportOBJ exportOBJ = new ZOExportOBJ();
                 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}");
@@ -272,13 +273,104 @@ protected void BuildURDFLink(ZOSimOccurrence simOccurrence, XElement robot, Vect
                     foreach (Transform visualsChild in child) {
                         // check if it is a primitive type (cube, sphere, cylinder, etc)
                         BuildURDFVisuals(visualsChild, link, anchorOffset);
+
+                        // we will do any collider that are attached to the visual
+                        BuildURDFCollisions(visualsChild, link, anchorOffset);
+                    }
+                }
+
+                if (child.name.ToLower() == "collisions") {
+                    // go through the children of the collisions and get all the models
+                    foreach (Transform collisionChild in child) {
+                        BuildURDFCollisions(collisionChild, link, anchorOffset);
                     }
                 }
 
             }
         }
 
 
+        protected void BuildURDFCollisions(Transform collisionTransform, XElement link, Vector3 anchorOffset) {
+            Collider[] colliders = collisionTransform.GetComponentsInChildren<Collider>();
+            foreach (Collider collider in colliders) {
+                XElement collision = new XElement("collision");
+                collision.SetAttributeValue("name", collisionTransform.name);
+                XElement geometry = new XElement("geometry");
+                Vector3 center = Vector3.zero;
+
+                if (collider.GetType() == typeof(BoxCollider)) {
+                    BoxCollider boxCollider = collider as BoxCollider;
+                    XElement box = new XElement("box");
+
+                    Vector3 boxSize = new Vector3(boxCollider.size.x * collider.transform.localScale.x,
+                                                    boxCollider.size.y * collider.transform.localScale.y,
+                                                    boxCollider.size.z * collider.transform.localScale.z);
+                    box.SetAttributeValue("size", boxSize.Unity2RosScale().ToXMLString());
+                    geometry.Add(box);
+
+                    center = new Vector3(boxCollider.center.x * boxCollider.transform.localScale.x,
+                                        boxCollider.center.y * boxCollider.transform.localScale.y,
+                                        boxCollider.center.z * boxCollider.transform.localScale.z);
+                } else if (collider.GetType() == typeof(SphereCollider)) {
+                    SphereCollider sphereCollider = collider as SphereCollider;
+                    XElement sphere = new XElement("sphere");
+                    float radius = sphereCollider.radius * collider.transform.localScale.x;
+                    sphere.SetAttributeValue("radius", radius);
+                    geometry.Add(sphere);
+
+                    center = new Vector3(sphereCollider.center.x * sphereCollider.transform.localScale.x,
+                                        sphereCollider.center.y * sphereCollider.transform.localScale.y,
+                                        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;
+                    Vector3 xyz = position.Unity2Ros();
+                    Vector3 rpy = new Vector3(-collisionTransform.localEulerAngles.z * Mathf.Deg2Rad,
+                                                collisionTransform.localEulerAngles.x * Mathf.Deg2Rad,
+                                                -collisionTransform.localEulerAngles.y * Mathf.Deg2Rad);
+
+                    XElement origin = new XElement("origin");
+                    origin.SetAttributeValue("xyz", xyz.ToXMLString());
+                    origin.SetAttributeValue("rpy", rpy.ToXMLString());
+
+                    collision.Add(origin);
+
+                    collision.Add(geometry);
+                    link.Add(collision);
+                }
+
+            }
+        }
+
         protected void BuildURDFVisuals(Transform visualTransform, XElement link, Vector3 anchorOffset) {
             // build 3d primitive if exists
             MeshFilter meshFilter = visualTransform.GetComponent<MeshFilter>();