From c62c68d7446ce290b79b1c3d88fb01638a160a4c Mon Sep 17 00:00:00 2001
From: Jesper Smith <jesper@halodi.com>
Date: Fri, 13 May 2022 13:32:40 +0200
Subject: [PATCH 1/2] Fixed reading offsets for Sensors. Added Advanced
 Settings property to MjGeomEditor

---
 unity/Editor/Components/MjGeomEditor.cs                | 1 +
 unity/Runtime/Components/Sensors/MjSiteVectorSensor.cs | 3 ++-
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/unity/Editor/Components/MjGeomEditor.cs b/unity/Editor/Components/MjGeomEditor.cs
index c694fd46..0bd77ea0 100644
--- a/unity/Editor/Components/MjGeomEditor.cs
+++ b/unity/Editor/Components/MjGeomEditor.cs
@@ -24,6 +24,7 @@ public class MjGeomEditor : MjShapeComponentEditor {
     serializedObject.Update();
     EditorGUILayout.PropertyField(serializedObject.FindProperty("Mass"));
     EditorGUILayout.PropertyField(serializedObject.FindProperty("Density"));
+    EditorGUILayout.PropertyField(serializedObject.FindProperty("Settings"));
     serializedObject.ApplyModifiedProperties();
     base.OnInspectorGUI();
   }
diff --git a/unity/Runtime/Components/Sensors/MjSiteVectorSensor.cs b/unity/Runtime/Components/Sensors/MjSiteVectorSensor.cs
index f63bb528..758213fe 100644
--- a/unity/Runtime/Components/Sensors/MjSiteVectorSensor.cs
+++ b/unity/Runtime/Components/Sensors/MjSiteVectorSensor.cs
@@ -69,7 +69,8 @@ public class MjSiteVectorSensor : MjBaseSensor {
   }
 
   public override unsafe void OnSyncState(MujocoLib.mjData_* data) {
-    SensorReading = MjEngineTool.UnityVector3(data->sensordata, _sensorAddress);
+    Vector3 mjReading = new Vector3((float)data->sensordata[_sensorAddress], (float)data->sensordata[_sensorAddress + 1], (float)data->sensordata[_sensorAddress + 2]);
+    SensorReading = MjEngineTool.UnityVector3(mjReading);
   }
 }
 }

From a83a8f642f94c95e13bbf32f24bad949f0019938 Mon Sep 17 00:00:00 2001
From: Jesper Smith <jesper@halodi.com>
Date: Fri, 13 May 2022 17:40:11 +0200
Subject: [PATCH 2/2] Add priority settings to geometry

---
 unity/Runtime/Components/Joints/MjHingeJoint.cs   |  6 +++++-
 unity/Runtime/Components/Shapes/MjGeomSettings.cs | 10 ++++++++++
 2 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/unity/Runtime/Components/Joints/MjHingeJoint.cs b/unity/Runtime/Components/Joints/MjHingeJoint.cs
index eb5c2490..8549c738 100644
--- a/unity/Runtime/Components/Joints/MjHingeJoint.cs
+++ b/unity/Runtime/Components/Joints/MjHingeJoint.cs
@@ -30,6 +30,9 @@ namespace Mujoco {
     [Tooltip("Joint movement, in rad/sec. Read-only at play time.")]
     public float Velocity;
 
+    [Tooltip("The current joint angle without normalization, in radians. Read-only.")]
+    public double RawConfiguration;
+
     [Tooltip("Joint settings.")]
     public MjJointSettings Settings = MjJointSettings.Default;
 
@@ -42,7 +45,8 @@ namespace Mujoco {
     }
 
     public override unsafe void OnSyncState(MujocoLib.mjData_* data) {
-      var qpos = (float)data->qpos[QposAddress];
+      RawConfiguration = data->qpos[QposAddress];
+      var qpos = (float)RawConfiguration;
       Configuration = (qpos % (2 * Mathf.PI)) * Mathf.Rad2Deg;
       Velocity = (float)data->qvel[DofAddress];
     }
diff --git a/unity/Runtime/Components/Shapes/MjGeomSettings.cs b/unity/Runtime/Components/Shapes/MjGeomSettings.cs
index 19a931e6..c06d3981 100644
--- a/unity/Runtime/Components/Shapes/MjGeomSettings.cs
+++ b/unity/Runtime/Components/Shapes/MjGeomSettings.cs
@@ -21,6 +21,11 @@ namespace Mujoco {
 // Advanced settings of MjGeom component.
 [Serializable]
 public struct MjGeomSettings {
+
+
+  [Tooltip("Priority")]
+  public int Priority;
+
   [Tooltip("Solver accuracy settings.")]
   public GeomSolver Solver;
 
@@ -38,6 +43,9 @@ public struct MjGeomSettings {
   };
 
   public void FromMjcf(XmlElement mjcf) {
+
+    Priority = (int) mjcf.GetFloatAttribute("priority", 0);
+
     // Contact filtering settings.
     Filtering.Contype = (int)mjcf.GetFloatAttribute("contype", CollisionFiltering.Default.Contype);
     Filtering.Conaffinity = (int)mjcf.GetFloatAttribute(
@@ -71,6 +79,8 @@ public struct MjGeomSettings {
   }
 
   public void ToMjcf(XmlElement mjcf) {
+    mjcf.SetAttribute("priority", $"{Priority}");
+
     // Contact filtering settings.
     mjcf.SetAttribute("contype", $"{Filtering.Contype}");
     mjcf.SetAttribute("conaffinity", $"{Filtering.Conaffinity}");