Merge branch 'feature/urdf_import' into develop

Author: micahpearlman

Editor/ZOZeroSimMenu.cs

@@ -53,5 +53,18 @@ static void ImportURDF(MenuCommand menuCommand) {
 
         }
 
+        [MenuItem("GameObject/ZeroSim/Import OBJ...", false, 0)]
+        static void ImportOBJ(MenuCommand menuCommand) {
+            string filePath = EditorUtility.OpenFilePanel("Import OBJ", ".", "obj");
+
+            if (filePath.Length == 0) {
+                return;
+            }
+
+            ZOImportOBJ.Import(filePath);
+
+        }
+
+
     }
 }

Runtime/Scripts/Physics/ZOHingeJoint.cs

@@ -79,6 +79,7 @@ public Vector3 Axis {
                 return UnityHingeJoint.axis;
             }
             set {
+                _axis = value;
                 UnityHingeJoint.axis = value;
             }
         }
@@ -92,6 +93,7 @@ public Vector3 Anchor {
                 return UnityHingeJoint.anchor;
             }
             set {
+                _anchor = value;
                 UnityHingeJoint.anchor = value;
             }
         }
@@ -112,6 +114,7 @@ public Rigidbody ConnectedBody {
             }
             set {
                 if (UnityHingeJoint.connectedBody != value) {
+                    _connectedBody = value;
                     UnityHingeJoint.connectedBody = value;
                 }
 

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