URDF transform to Unity working.

Author: micahpearlman

Runtime/Scripts/Util/Extensions/ZOROSConversionExtensions.cs

@@ -33,6 +33,26 @@ public static Vector3 Unity2RosRollPitchYaw(this Quaternion q) {
                             -q.eulerAngles.y * Mathf.Deg2Rad);
         }
 
+        public static Quaternion RosRollPitchYawToQuaternion(this Vector3 rpy) {
+            // Negate X and Z because we're going from Right to Left handed rotations. Y is handled because the axis itself is flipped
+            rpy.x *= -1;
+            rpy.z *= -1;
+            rpy *= Mathf.Rad2Deg;
+
+            // swap the angle values
+            rpy = new Vector3(rpy.y, rpy.z, rpy.x);
+
+            // Applying rotations in ZYX ordering, as indicated above
+            Quaternion q = Quaternion.identity;
+
+            q *= Quaternion.Euler(0, rpy.y, 0);
+            q *= Quaternion.Euler(rpy.x, 0, 0);
+            q *= Quaternion.Euler(0, 0, rpy.z);
+
+
+            return q;
+        }
+
         public static string ToXMLString(this Vector3 v) {
             return $"{v.x} {v.y} {v.z}";
         }
@@ -58,7 +78,7 @@ public static Vector3 FromURDFStringToVector3(this string s) {
             Debug.LogWarning($"Could not parse string: {s}");
 
             return Vector3.zero;
-        } 
+        }
 
 
     }

Runtime/Scripts/Util/ImportExport/ZOImportURDF.cs

@@ -61,30 +61,30 @@ public static ZOSimDocumentRoot Import(XmlDocument xmlDocument) {
                 XmlNode[] xmlVisuals = xmlLink.GetChildrenByName("visual");
 
                 foreach (XmlNode xmlVisual in xmlVisuals) {
-                    
-                    
+
+
                     XmlNode[] xmlGeometries = xmlVisual.GetChildrenByName("geometry");
 
                     foreach (XmlNode xmlGeom in xmlGeometries) {
-                        
+
                         GameObject visualGeo = null;
 
                         XmlNode xmlBox = xmlGeom.GetChildByName("box");
                         if (xmlBox != null) {
                             visualGeo = GameObject.CreatePrimitive(PrimitiveType.Cube);
                             Vector3 size = xmlBox.Attributes["size"].Value.FromURDFStringToVector3();
-                            
+
                             visualGeo.transform.localScale = size.Ros2UnityScale();
                         }
-                        
+
                         XmlNode xmlCylinder = xmlGeom.GetChildByName("cylinder");
                         if (xmlCylinder != null) {
                             visualGeo = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
                             float radius = float.Parse(xmlCylinder.Attributes["radius"].Value);
                             float length = float.Parse(xmlCylinder.Attributes["length"].Value);
                             visualGeo.transform.localScale = new Vector3(radius * 2.0f, length * 0.5f, radius * 2.0f);
                         }
-                        
+
                         XmlNode xmlSphere = xmlGeom.GetChildByName("sphere");
                         if (xmlSphere != null) {
                             visualGeo = GameObject.CreatePrimitive(PrimitiveType.Sphere);
@@ -97,14 +97,23 @@ public static ZOSimDocumentRoot Import(XmlDocument xmlDocument) {
                             // TODO
                         }
                         if (visualGeo != null) {
+                            // set parent
                             visualGeo.transform.SetParent(goVisualsEmpty.transform);
                             string visualName = xmlVisual.Attributes["name"].Value;
                             if (visualName != null) {
                                 visualGeo.name = visualName;
                             }
+
+                            // set transform
+                            XmlNode xmlOrigin = xmlVisual.GetChildByName("origin");
+                            Tuple<Vector3, Quaternion> transform = OriginXMLToUnity(xmlOrigin);
+                            visualGeo.transform.localPosition = transform.Item1;
+                            visualGeo.transform.localRotation = transform.Item2;
+
                         }
 
                     }
+
                 }
 
                 // get the origin
@@ -118,6 +127,8 @@ public static ZOSimDocumentRoot Import(XmlDocument xmlDocument) {
 
                 // find link parent
                 GameObject linkParent = rootObject;
+                GameObject linkChild = goLink.Value.Item2;
+                XmlNode workingJoint = null;
                 foreach (XmlNode joint in xmlJoints) {
                     XmlNode xmlChildLink = joint.GetChildByName("child");
                     string childName = xmlChildLink.Attributes["link"].Value;
@@ -126,18 +137,48 @@ public static ZOSimDocumentRoot Import(XmlDocument xmlDocument) {
                         XmlNode xmlParentLink = joint.GetChildByName("parent");
                         string parentName = xmlParentLink.Attributes["link"].Value;
                         linkParent = goLinks[parentName].Item2;
+
+                        workingJoint = joint;
+
                         break;
                     }
 
                 }
 
-                goLink.Value.Item2.transform.SetParent(linkParent.transform);
-                ZOSimOccurrence occurrence = goLink.Value.Item2.AddComponent<ZOSimOccurrence>();
+                linkChild.transform.SetParent(linkParent.transform);
+                ZOSimOccurrence occurrence = linkChild.AddComponent<ZOSimOccurrence>();
+
+                // set transform
+                if (workingJoint != null) {
+                    XmlNode xmlOrigin = workingJoint.GetChildByName("origin");
+                    Tuple<Vector3, Quaternion> transform = OriginXMLToUnity(xmlOrigin);
+                    linkChild.transform.localPosition = transform.Item1;
+                    linkChild.transform.localRotation = transform.Item2;
+
+                }
+
 
             }
 
             return simDocumentRoot;
         }
 
+        protected static Tuple<Vector3, Quaternion> OriginXMLToUnity(XmlNode xmlOrigin) {
+            Vector3 translation = Vector3.zero;
+            string xyz = xmlOrigin.Attributes["xyz"].Value;
+            if (xyz != null && xyz != "") {
+                translation = xyz.FromURDFStringToVector3().Ros2Unity();
+            }
+
+            Quaternion rotation = Quaternion.identity;
+            string rpy = xmlOrigin.Attributes["rpy"].Value;
+            if (rpy != null && rpy != "") {
+                rotation = rpy.FromURDFStringToVector3().RosRollPitchYawToQuaternion();
+            }
+            Tuple<Vector3, Quaternion> transform = new Tuple<Vector3, Quaternion>(translation, rotation);
+
+            return transform;
+        }
+
     }
 }