Supporting Ignore Collision tag in URDF (#4)

* Adding Ignore Collision Tag in URDF
* Supporting damping in controller
* Adding exception for two links having the same name

Author: vidurvij-Unity

Editor/NeedsRuntimeConversion/Extensions/UrdfRobotExtensions.cs

@@ -10,6 +10,7 @@ namespace RosSharp.Urdf.Editor
     public static class UrdfRobotExtensions
     {
         static string tagName = "robot";
+        static string collisionObjectName = "Collisions";
         public static ImportSettings importsettings;
 
         public static void Create()
@@ -63,6 +64,7 @@ public static void Create(string filename, ImportSettings settings)
             Selection.activeObject = robotGameObject;
 
             CorrectAxis(robotGameObject, settings.choosenAxis);
+            //CreateCollisionExceptions(robot, robotGameObject);
         }
 
         public static void CorrectAxis(GameObject robot, ImportSettings.axisType axis = ImportSettings.axisType.yAxis)
@@ -90,6 +92,23 @@ public static void CorrectAxis(GameObject robot, ImportSettings.axisType axis =
             }
 
         }
+
+        private static void CreateCollisionExceptions(Robot robot, GameObject robotGameObject)
+        {
+            List<CollisionIgnore> ColliisonList = new List<CollisionIgnore>();
+            if (robot.ignoreCollisionPair.Count > 0)
+            {
+                foreach(System.Tuple<string,string> ignoreCollision in robot.ignoreCollisionPair)
+                {
+                    Transform colliisonObject1 = GameObject.Find(ignoreCollision.Item1).transform.Find(collisionObjectName);
+                    Transform collisionObject2 = GameObject.Find(ignoreCollision.Item2).transform.Find(collisionObjectName);
+
+                    ColliisonList.Add(new CollisionIgnore(colliisonObject1, collisionObject2));
+                }
+            }
+            robotGameObject.GetComponent<UrdfRobot>().collisionExceptions = ColliisonList;
+        }
+        
         #endregion
 
         #region Export

Runtime/Controller/Controller.cs

@@ -166,7 +166,7 @@ public void UpdateControlType(JointControl joint)
             {
                 ArticulationDrive drive = joint.joint.xDrive;
                 drive.stiffness = stiffness;
-                drive.damping = 0;
+                drive.damping = damping;
                 joint.joint.xDrive = drive;
             }
         }

Runtime/RosSharpDefinitions/Robot.cs

@@ -21,6 +21,7 @@ public class Robot
         public List<Link> links;
         public List<Joint> joints;
         public List<Plugin> plugins;
+        public List<Tuple<string, string>> ignoreCollisionPair;
 
         public Robot(string filename)
         {
@@ -35,6 +36,7 @@ public Robot(string filename)
             links = ReadLinks(node);
             joints = ReadJoints(node);
             plugins = ReadPlugins(node);
+            ignoreCollisionPair = ReadDisableCollision(node);
 
 
             // build tree structure from link and joint lists:
@@ -69,10 +71,20 @@ from child in node.Elements("material")
 
         private static List<Link> ReadLinks(XElement node)
         {
-            var links =
-                from child in node.Elements("link")
-                select new Link(child);
-            return links.ToList();
+            List<String> importedLinks = new List<String>();
+            List<Link> links = new List<Link>();
+            foreach (XElement child in node.Elements("link"))
+            {
+                string name = (String)child.Attribute("name");
+                if (importedLinks.Find(p => name == p ? true : false) == null)
+                {
+                    links.Add(new Link(child));
+                    importedLinks.Add(name);
+                }
+                else
+                    throw new InvalidNameException("Two links cannot have the same name");
+            }
+            return links;
         }
 
         private static List<Joint> ReadJoints(XElement node)
@@ -92,6 +104,14 @@ from child in node.Elements()
             return plugins.ToList();
         }
 
+        private List<Tuple<string,string>> ReadDisableCollision(XElement node)
+        {
+            var disable_collisions =
+                from child in node.Elements("disable_collision")
+                select new Tuple<string,string>(child.Attribute("joint1").Value,child.Attribute("joint2").Value);
+            return disable_collisions.ToList();
+        }
+
         private static Link FindRootLink(List<Link> links, List<Joint> joints)
         {
             if (joints.Count == 0)
@@ -143,4 +163,15 @@ private void writeToUrdf()
             }
         }
     }
+    public class InvalidNameException : System.Exception
+    {
+        public InvalidNameException() : base() { }
+        public InvalidNameException(string message) : base(message) { }
+        public InvalidNameException(string message, System.Exception inner) : base(message, inner) { }
+
+        // A constructor is needed for serialization when an
+        // exception propagates from a remoting server to the client.
+        protected InvalidNameException(System.Runtime.Serialization.SerializationInfo info,
+            System.Runtime.Serialization.StreamingContext context) : base(info, context) { }
+    }
 }

Runtime/UrdfComponents/CollisionIgnore.cs

@@ -0,0 +1,18 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace RosSharp.Urdf
+{
+    [System.Serializable]
+    public class CollisionIgnore {
+        public Transform Link1;
+        public Transform Link2;
+
+        public CollisionIgnore(Transform l1, Transform l2)
+        {
+            Link1 = l1;
+            Link2 = l2;
+        }
+    }
+}

Runtime/UrdfComponents/CollisionIgnore.cs.meta

@@ -1,4 +1,6 @@
 using UnityEngine;
+using System;
+using System.Collections.Generic;
 
 namespace RosSharp.Urdf
 {
@@ -8,6 +10,7 @@ public class UrdfRobot : MonoBehaviour
     {
         public string FilePath;
         public ImportSettings.axisType choosenAxis;
+        public List<CollisionIgnore> collisionExceptions;
         #region Configure Robot
 
         public void SetCollidersConvex(bool convex)
@@ -79,6 +82,29 @@ public void SetAxis(ImportSettings.axisType setAxis)
             this.choosenAxis = setAxis;
         }
 
+        void Start()
+        {
+            CreateCollisionExceptions();
+        }
+
+        public void CreateCollisionExceptions()
+        {
+            if (collisionExceptions != null)
+            {
+                foreach (CollisionIgnore ignoreCollision in collisionExceptions)
+                {
+                    Collider[] collidersObject1 = ignoreCollision.Link1.GetComponentsInChildren<Collider>();
+                    Collider[] collidersObject2 = ignoreCollision.Link2.GetComponentsInChildren<Collider>();
+                    foreach (Collider colliderMesh1 in collidersObject1)
+                    {
+                        foreach (Collider colliderMesh2 in collidersObject2)
+                        {
+                            Physics.IgnoreCollision(colliderMesh1, colliderMesh2);
+                        }
+                    }
+                }
+            }
+        }
         #endregion
     }
 }