fixed joint naming in URDF import

micahpearlman · micahpearlman · commit 7d09bf90ef69 · 2021-07-13T09:17:12.000-07:00
diff --git a/Runtime/Scripts/Physics/ZOHingeJoint.cs b/Runtime/Scripts/Physics/ZOHingeJoint.cs
@@ -119,19 +119,22 @@ public Rigidbody ConnectedBody {
                 }
 
                 // update the name
-                _name = Type;
+                if (string.IsNullOrEmpty(Name)) {
+                    Name = Type;
 
-                ZOSimOccurrence occurrence = GetComponent<ZOSimOccurrence>();
-                if (occurrence) {
-                    _name = _name + "_from_" + occurrence.Name;
-                }
+                    ZOSimOccurrence occurrence = GetComponent<ZOSimOccurrence>();
+                    if (occurrence) {
+                        Name = Name + "_from_" + occurrence.Name;
+                    }
 
-                if (UnityHingeJoint.connectedBody) {
-                    ZOSimOccurrence connected_occurrence = UnityHingeJoint.connectedBody.gameObject.GetComponent<ZOSimOccurrence>();
+                    if (UnityHingeJoint.connectedBody) {
+                        ZOSimOccurrence connected_occurrence = UnityHingeJoint.connectedBody.gameObject.GetComponent<ZOSimOccurrence>();
 
-                    if (connected_occurrence) {
-                        _name = _name + "_to_" + connected_occurrence.Name;
+                        if (connected_occurrence) {
+                            Name = Name + "_to_" + connected_occurrence.Name;
+                        }
                     }
+
                 }
 
             }
@@ -288,19 +291,19 @@ public void CreateRequirements() {
                 _hingeJoint = gameObject.AddComponent<UnityEngine.HingeJoint>();
             }
 
-            if (_name == null) {
-                _name = Type;
+            if (string.IsNullOrEmpty(Name)) {
+                Name = Type;
 
                 ZOSimOccurrence occurrence = GetComponent<ZOSimOccurrence>();
                 if (occurrence) {
-                    _name = _name + "_from_" + occurrence.Name;
+                    Name = Name + "_from_" + occurrence.Name;
                 }
 
                 if (UnityHingeJoint.connectedBody) {
                     ZOSimOccurrence connected_occurrence = UnityHingeJoint.connectedBody.gameObject.GetComponent<ZOSimOccurrence>();
 
                     if (connected_occurrence) {
-                        _name = _name + "_to_" + connected_occurrence.Name;
+                        Name = Name + "_to_" + connected_occurrence.Name;
                     }
                 }
             }
@@ -314,7 +317,6 @@ public void CreateRequirements() {
         // }
 
 
-        #region ZOSerializationInterface
         public string Type {
             get { return "joint.hinge"; }
         }
@@ -324,24 +326,13 @@ public string Name {
             get {
                 return _name;
             }
-            private set {
+            set {
                 _name = value;
             }
         }
 
-        private JObject _json;
-        public JObject JSON {
-            get {
-                // if (_json == null) {
-                //     _json = BuildJSON();
-                // }
-                return _json;
-
-            }
-        }
 
 
-        #endregion
 
     }
 
diff --git a/Runtime/Scripts/Util/ImportExport/ZOImportURDF.cs b/Runtime/Scripts/Util/ImportExport/ZOImportURDF.cs
@@ -234,7 +234,7 @@ public static ZOSimDocumentRoot Import(XmlDocument xmlDocument, string workingDi
                 GameObject linkChild = goLink.Value.Item2;
                 XmlNode xmlChildLinkNode = goLink.Value.Item1;
                 XmlNode xmlParentLinkNode = null;
-                XmlNode workingJoint = null;
+                XmlNode xmlWorkingJoint = null;
                 foreach (XmlNode joint in xmlJoints) {
                     XmlNode xmlChildLinkName = joint.GetChildByName("child");
                     string childName = xmlChildLinkName.Attributes["link"].Value;
@@ -245,7 +245,7 @@ public static ZOSimDocumentRoot Import(XmlDocument xmlDocument, string workingDi
                         linkParent = goLinks[parentName].Item2;
                         xmlParentLinkNode = goLinks[parentName].Item1;
 
-                        workingJoint = joint;
+                        xmlWorkingJoint = joint;
 
                         break;
                     }
@@ -256,9 +256,9 @@ public static ZOSimDocumentRoot Import(XmlDocument xmlDocument, string workingDi
                 ZOSimOccurrence occurrence = linkChild.AddComponent<ZOSimOccurrence>();
 
                 // set transform
-                if (workingJoint != null) {
+                if (xmlWorkingJoint != null) {
                     // set the transform
-                    XmlNode xmlOrigin = workingJoint.GetChildByName("origin");
+                    XmlNode xmlOrigin = xmlWorkingJoint.GetChildByName("origin");
                     Tuple<Vector3, Quaternion> transform = OriginXMLToUnity(xmlOrigin);
                     linkChild.transform.localPosition = transform.Item1;
                     linkChild.transform.localRotation = transform.Item2;
@@ -289,15 +289,20 @@ public static ZOSimDocumentRoot Import(XmlDocument xmlDocument, string workingDi
                     }
 
                     // add joints
-                    string jointType = workingJoint.Attributes["type"].Value;
+                    string jointType = xmlWorkingJoint.Attributes["type"].Value;
                     if (jointType == "revolute") {
 
                         ZOHingeJoint hingeJoint = linkParent.AddComponent<ZOHingeJoint>();
                         hingeJoint.ConnectedBody = childRigidBody;
                         hingeJoint.Anchor = transform.Item1;
 
-                        XmlNode xmlAxis = workingJoint.GetChildByName("axis");
+                        XmlNode xmlAxis = xmlWorkingJoint.GetChildByName("axis");
                         hingeJoint.Axis = xmlAxis.Attributes["xyz"].Value.FromURDFStringToVector3().Ros2Unity();
+
+                        if (xmlWorkingJoint.Attributes["name"] != null) {
+                            hingeJoint.Name = xmlWorkingJoint.Attributes["name"].Value;
+                        }
+
                     }
 
                     if (jointType == "prismatic") {
@@ -313,9 +318,6 @@ public static ZOSimDocumentRoot Import(XmlDocument xmlDocument, string workingDi
         }
 
         protected static Tuple<Vector3, Quaternion> OriginXMLToUnity(XmlNode xmlOrigin) {
-            if (xmlOrigin == null) {
-                Debug.Log("BLAH");
-            }
             Vector3 translation = Vector3.zero;
             string xyz = xmlOrigin.Attributes["xyz"].Value;
             if (xyz != null && xyz != "") {

Original file line number	Diff line number	Diff line change
`@@ -119,19 +119,22 @@ public Rigidbody ConnectedBody {`
`119`	`119`	`}`
`120`	`120`
`121`	`121`	`// update the name`
`122`		`- _name = Type;`
	`122`	`+ if (string.IsNullOrEmpty(Name)) {`
	`123`	`+ Name = Type;`
`123`	`124`
`124`		`- ZOSimOccurrence occurrence = GetComponent<ZOSimOccurrence>();`
`125`		`- if (occurrence) {`
`126`		`- _name = _name + "_from_" + occurrence.Name;`
`127`		`- }`
	`125`	`+ ZOSimOccurrence occurrence = GetComponent<ZOSimOccurrence>();`
	`126`	`+ if (occurrence) {`
	`127`	`+ Name = Name + "_from_" + occurrence.Name;`
	`128`	`+ }`
`128`	`129`
`129`		`- if (UnityHingeJoint.connectedBody) {`
`130`		`- ZOSimOccurrence connected_occurrence = UnityHingeJoint.connectedBody.gameObject.GetComponent<ZOSimOccurrence>();`
	`130`	`+ if (UnityHingeJoint.connectedBody) {`
	`131`	`+ ZOSimOccurrence connected_occurrence = UnityHingeJoint.connectedBody.gameObject.GetComponent<ZOSimOccurrence>();`
`131`	`132`
`132`		`- if (connected_occurrence) {`
`133`		`- _name = _name + "_to_" + connected_occurrence.Name;`
	`133`	`+ if (connected_occurrence) {`
	`134`	`+ Name = Name + "_to_" + connected_occurrence.Name;`
	`135`	`+ }`
`134`	`136`	`}`
	`137`	`+`
`135`	`138`	`}`
`136`	`139`
`137`	`140`	`}`
`@@ -288,19 +291,19 @@ public void CreateRequirements() {`
`288`	`291`	`_hingeJoint = gameObject.AddComponent<UnityEngine.HingeJoint>();`
`289`	`292`	`}`
`290`	`293`
`291`		`- if (_name == null) {`
`292`		`- _name = Type;`
	`294`	`+ if (string.IsNullOrEmpty(Name)) {`
	`295`	`+ Name = Type;`
`293`	`296`
`294`	`297`	`ZOSimOccurrence occurrence = GetComponent<ZOSimOccurrence>();`
`295`	`298`	`if (occurrence) {`
`296`		`- _name = _name + "_from_" + occurrence.Name;`
	`299`	`+ Name = Name + "_from_" + occurrence.Name;`
`297`	`300`	`}`
`298`	`301`
`299`	`302`	`if (UnityHingeJoint.connectedBody) {`
`300`	`303`	`ZOSimOccurrence connected_occurrence = UnityHingeJoint.connectedBody.gameObject.GetComponent<ZOSimOccurrence>();`
`301`	`304`
`302`	`305`	`if (connected_occurrence) {`
`303`		`- _name = _name + "_to_" + connected_occurrence.Name;`
	`306`	`+ Name = Name + "_to_" + connected_occurrence.Name;`
`304`	`307`	`}`
`305`	`308`	`}`
`306`	`309`	`}`
`@@ -314,7 +317,6 @@ public void CreateRequirements() {`
`314`	`317`	`// }`
`315`	`318`
`316`	`319`
`317`		`- #region ZOSerializationInterface`
`318`	`320`	`public string Type {`
`319`	`321`	`get { return "joint.hinge"; }`
`320`	`322`	`}`
`@@ -324,24 +326,13 @@ public string Name {`
`324`	`326`	`get {`
`325`	`327`	`return _name;`
`326`	`328`	`}`
`327`		`- private set {`
	`329`	`+ set {`
`328`	`330`	`_name = value;`
`329`	`331`	`}`
`330`	`332`	`}`
`331`	`333`
`332`		`- private JObject _json;`
`333`		`- public JObject JSON {`
`334`		`- get {`
`335`		`- // if (_json == null) {`
`336`		`- // _json = BuildJSON();`
`337`		`- // }`
`338`		`- return _json;`
`339`		`-`
`340`		`- }`
`341`		`- }`
`342`	`334`
`343`	`335`
`344`		`- #endregion`
`345`	`336`
`346`	`337`	`}`
`347`	`338`