working inertial import

Author: micahpearlman

Runtime/Scripts/Util/ImportExport/ZOImportURDF.cs

@@ -128,15 +128,18 @@ public static ZOSimDocumentRoot Import(XmlDocument xmlDocument) {
                 // find link parent
                 GameObject linkParent = rootObject;
                 GameObject linkChild = goLink.Value.Item2;
+                XmlNode xmlChildLinkNode = goLink.Value.Item1;
+                XmlNode xmlParentLinkNode = null;
                 XmlNode workingJoint = null;
                 foreach (XmlNode joint in xmlJoints) {
-                    XmlNode xmlChildLink = joint.GetChildByName("child");
-                    string childName = xmlChildLink.Attributes["link"].Value;
+                    XmlNode xmlChildLinkName = joint.GetChildByName("child");
+                    string childName = xmlChildLinkName.Attributes["link"].Value;
 
                     if (goLink.Value.Item2.name == childName) {
-                        XmlNode xmlParentLink = joint.GetChildByName("parent");
-                        string parentName = xmlParentLink.Attributes["link"].Value;
+                        XmlNode xmlParentLinkName = joint.GetChildByName("parent");
+                        string parentName = xmlParentLinkName.Attributes["link"].Value;
                         linkParent = goLinks[parentName].Item2;
+                        xmlParentLinkNode = goLinks[parentName].Item1;
 
                         workingJoint = joint;
 
@@ -157,8 +160,29 @@ public static ZOSimDocumentRoot Import(XmlDocument xmlDocument) {
                     linkChild.transform.localRotation = transform.Item2;
 
                     // add rigidbody components
-                    Rigidbody childRigidBody = linkChild.AddComponent<Rigidbody>();
-                    Rigidbody parentRigidBody = linkParent.AddComponent<Rigidbody>();
+                    Rigidbody childRigidBody = null;
+                    Rigidbody parentRigidBody = null;
+
+                    XmlNode xmlInertial = xmlChildLinkNode.GetChildByName("inertial");
+                    if (xmlInertial != null) {
+                        childRigidBody = linkChild.AddComponent<Rigidbody>();
+                        
+                        float mass = 1.0f;
+                        float.TryParse(xmlInertial.GetChildByName("mass").Attributes["value"].Value, out mass);
+                        childRigidBody.mass = mass;
+
+                    }
+
+                    xmlInertial = xmlParentLinkNode.GetChildByName("inertial");
+                    parentRigidBody = linkParent.GetComponent<Rigidbody>();
+                    if (xmlInertial != null && parentRigidBody == null) {  
+                        parentRigidBody = linkParent.AddComponent<Rigidbody>();
+
+                        float mass = 1.0f;
+                        float.TryParse(xmlInertial.GetChildByName("mass").Attributes["value"].Value, out mass);
+                        parentRigidBody.mass = mass;
+
+                    }
 
                     // add joints
                     string jointType = workingJoint.Attributes["type"].Value;
@@ -172,6 +196,10 @@ public static ZOSimDocumentRoot Import(XmlDocument xmlDocument) {
                         hingeJoint.Axis = xmlAxis.Attributes["xyz"].Value.FromURDFStringToVector3().Ros2Unity();
                     }
 
+                    if (jointType == "prismatic") {
+                        Debug.LogWarning("WARNING: Prismatic joint not yet implemented.");
+                    }
+
                 }
 
 

Samples~/ZeroSimSamples/Scenes/URDF_test.unity

@@ -4951,6 +4951,36 @@ PrefabInstance:
       propertyPath: m_LocalEulerAnglesHint.z
       value: 0
       objectReference: {fileID: 0}
+    - target: {fileID: 968823325036578810, guid: e40f2c3c7691c519a99f10bf56f8b75f,
+        type: 3}
+      propertyPath: m_ConnectedAnchor.x
+      value: -0.1500001
+      objectReference: {fileID: 0}
+    - target: {fileID: 968823325036578810, guid: e40f2c3c7691c519a99f10bf56f8b75f,
+        type: 3}
+      propertyPath: m_ConnectedAnchor.y
+      value: -0.009999998
+      objectReference: {fileID: 0}
+    - target: {fileID: 968823325036578810, guid: e40f2c3c7691c519a99f10bf56f8b75f,
+        type: 3}
+      propertyPath: m_ConnectedAnchor.z
+      value: 0.099999905
+      objectReference: {fileID: 0}
+    - target: {fileID: 968823325036578812, guid: e40f2c3c7691c519a99f10bf56f8b75f,
+        type: 3}
+      propertyPath: m_ConnectedAnchor.x
+      value: 0.1500001
+      objectReference: {fileID: 0}
+    - target: {fileID: 968823325036578812, guid: e40f2c3c7691c519a99f10bf56f8b75f,
+        type: 3}
+      propertyPath: m_ConnectedAnchor.y
+      value: -0.009999998
+      objectReference: {fileID: 0}
+    - target: {fileID: 968823325036578812, guid: e40f2c3c7691c519a99f10bf56f8b75f,
+        type: 3}
+      propertyPath: m_ConnectedAnchor.z
+      value: 0.099999905
+      objectReference: {fileID: 0}
     m_RemovedComponents: []
   m_SourcePrefab: {fileID: 100100000, guid: e40f2c3c7691c519a99f10bf56f8b75f, type: 3}
 --- !u!1001 &3713500775682786879