From 9f7e260911dbbd82391781ac30d570168582dc92 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Thu, 8 Jan 2026 16:12:45 +0100 Subject: [PATCH 01/63] Quick tests of imu swigging in Unity --- AGXUnity/Sensor/ImuSensor.cs | 228 ++++++++++++++++++ AGXUnity/Sensor/ImuSensor.cs.meta | 11 + .../AGXUnity+Sensor+ImuSensorEditor.cs | 9 + .../AGXUnity+Sensor+ImuSensorEditor.cs.meta | 2 + 4 files changed, 250 insertions(+) create mode 100644 AGXUnity/Sensor/ImuSensor.cs create mode 100644 AGXUnity/Sensor/ImuSensor.cs.meta create mode 100644 Editor/CustomEditors/AGXUnity+Sensor+ImuSensorEditor.cs create mode 100644 Editor/CustomEditors/AGXUnity+Sensor+ImuSensorEditor.cs.meta diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs new file mode 100644 index 00000000..e085fea4 --- /dev/null +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -0,0 +1,228 @@ +using agx; +using agxSensor; +using AGXUnity.Utils; +using UnityEngine; + +namespace AGXUnity.Sensor +{ + + /// + /// IMU Sensor Component + /// + [DisallowMultipleComponent] + [AddComponentMenu( "AGXUnity/Sensors/IMU Sensor" )] + //[HelpURL( "https://us.download.algoryx.se/AGXUnity/documentation/current/editor_interface.html#simulating-lidar-sensors" )] + public class ImuSensor : ScriptComponent + { + /// + /// Native instance, created in Start/Initialize. + /// + public IMU Native { get; private set; } = null; + public IMUModel m_nativeModel = null; + + /// + /// Local sensor rotation relative to the parent GameObject transform. + /// + [Tooltip("Local sensor rotation relative to the parent GameObject transform.")] + public Vector3 LocalRotation = Vector3.zero; + + /// + /// Local sensor offset relative to the parent GameObject transform. + /// + [Tooltip("Local sensor offset relative to the parent GameObject transform.")] + public Vector3 LocalPosition = Vector3.zero; + + /// + /// The local transformation matrix from the sensor frame to the parent GameObject frame + /// + public UnityEngine.Matrix4x4 LocalTransform => UnityEngine.Matrix4x4.TRS( LocalPosition, Quaternion.Euler( LocalRotation ), Vector3.one ); + + /// + /// The global transformation matrix from the sensor frame to the world frame. + /// + public UnityEngine.Matrix4x4 GlobalTransform => transform.localToWorldMatrix * LocalTransform; + + // TODO tidy up etc + public RigidBody MeasuredBody = null; + + //TODO probably move to own class + private uint m_outputID = 0; // Must be greater than 0 to be valid + private IMUOutputNineDoF m_output = null; + + //private void Sync() + //{ + // var xform = GlobalTransform; +// + // Native.getFrame().setTranslate( xform.GetPosition().ToHandedVec3() ); + // Native.getFrame().setRotate( xform.rotation.ToHandedQuat() ); +// +// + // //Debug.Log( Native.getFrame().getTranslate() ); + // //Native.setFrame( new Frame( + // // new AffineMatrix4x4( + // // xform.rotation.ToHandedQuat(), + // // xform.GetPosition().ToHandedVec3() ) ) ); + //} + + protected override bool Initialize() + { + SensorEnvironment.Instance.GetInitialized(); + + // TODO save the models in private vars to update values + + // Accelerometer + var accelerometer_modifiers = new ITriaxialSignalSystemNodeRefVector + { + //new GyroscopeLinearAccelerationEffects( new Vec3( 2.62 * 1e-4 ) ), + //new TriaxialSpectralGaussianNoise( new Vec3( 1.75 * 1e-4 ) ) + }; + + var accelerometer_model = new AccelerometerModel( + new TriaxialRange(), + new TriaxialCrossSensitivity(0.0), + new Vec3(0.0), + accelerometer_modifiers + ); + + // Gyroscope + var gyroscope_modifiers = new ITriaxialSignalSystemNodeRefVector { + //new GyroscopeLinearAccelerationEffects( new Vec3( 2.62 * 1e-4 ) ), + //new TriaxialSpectralGaussianNoise( new Vec3( 1.75 * 1e-4 ) ) + }; + + var gyroscope_model = new GyroscopeModel( + new TriaxialRange(new agx.RangeReal(-1, 1)), + new TriaxialCrossSensitivity(0.01), + new Vec3(0.0), + gyroscope_modifiers + ); + + // Magnetometer + var magnetometer_modifiers = new ITriaxialSignalSystemNodeRefVector { + //new TriaxialGaussianNoise( new Vec3( 4.5e-7 ) ) + }; + + var magnetometer_model = new MagnetometerModel( + new TriaxialRange(), + new TriaxialCrossSensitivity(0.01), + new Vec3(0.0), + magnetometer_modifiers + ); + + var imu_attachments = new IMUModelSensorAttachmentRefVector + { + new IMUModelAccelerometerAttachment( + new AffineMatrix4x4(), accelerometer_model + ), + new IMUModelGyroscopeAttachment( + new AffineMatrix4x4(), gyroscope_model + ), + new IMUModelMagnetometerAttachment( + new AffineMatrix4x4(), magnetometer_model + ) + }; + + m_nativeModel = IMUModel.makeIdealNineDoFModel(); + //m_nativeModel = new IMUModel( imu_attachments ); + + if ( m_nativeModel == null ) + return false; + + PropertySynchronizer.Synchronize( this ); + + // TODO moveme to function that can get called both when setting RB and from here + var measuredRB = MeasuredBody.GetInitialized().Native; + SensorEnvironment.Instance.Native.add( measuredRB ); + + var rbFrame = measuredRB.getFrame(); + if ( rbFrame == null ) + Debug.LogError( "Need RB to follow" ); + + Native = new IMU( rbFrame, m_nativeModel ); + + // TODO temp depth firsth output implementation + m_outputID = SensorEnvironment.Instance.GenerateOutputID(); + m_output = new IMUOutputNineDoF(); + Native.getOutputHandler().add( m_outputID, m_output ); + + //Simulation.Instance.StepCallbacks.PreSynchronizeTransforms += Sync; + Simulation.Instance.StepCallbacks.PostStepForward += OnPostStepForward; + + SensorEnvironment.Instance.Native.add( Native ); + + return true; + } + + // TODO removeme used for testing + private void OnPostStepForward() + { + NineDoFView view = Native.getOutputHandler().get( m_outputID ).viewNineDoF(); + + // Debug.Log( "Test: " + view.size()); // Returns 1 + + Debug.Log( "Pos: " + Native.getFrame().getTranslate() ); + + Debug.Log( "Accelerometer 0: " + view[ 0 ].getTriplet( 0 ).length() ); + Debug.Log( "Gyroscope 1: " + view[ 0 ].getTriplet( 1 ).length() ); + Debug.Log( "Magnetometer 2: " + view[ 0 ].getTriplet( 2 ).length() ); + } + + protected override void OnEnable() + { + Native?.setEnable( true ); + } + + protected override void OnDisable() + { + Native?.setEnable( false ); + } + + + protected override void OnDestroy() + { + if ( SensorEnvironment.HasInstance ) + SensorEnvironment.Instance.Native?.remove( Native ); + + if ( Simulation.HasInstance ) + { + //Simulation.Instance.StepCallbacks.PreSynchronizeTransforms -= Sync; + Simulation.Instance.StepCallbacks.PostStepForward -= OnPostStepForward; + } + + //TODO remove modules and modifiers, possibly + m_output.Dispose(); + + Native?.Dispose(); + Native = null; + m_nativeModel?.Dispose(); + m_nativeModel = null; + + base.OnDestroy(); + } + + //TODO draw something useful + private void OnDrawGizmosSelected() + { +#if UNITY_EDITOR + var xform = GlobalTransform; + + var pos = xform.GetPosition(); + var scale = UnityEditor.HandleUtility.GetHandleSize(pos) * 1.5f; + Gizmos.DrawLine( pos, xform.MultiplyPoint( Vector3.forward * scale ) ); // Up + Gizmos.DrawLine( pos, xform.MultiplyPoint( Vector3.left * scale ) ); + + int numPoints = 25; + Vector3[] disc = new Vector3[numPoints]; + + Vector3 x = xform.MultiplyVector(Vector3.right * scale); + Vector3 y = xform.MultiplyVector(Vector3.up * scale); + + for ( int i = 0; i < numPoints; i++ ) { + float ang = Mathf.PI * 2 * i / numPoints; + disc[ i ] = pos + x * Mathf.Cos( ang ) + y * Mathf.Sin( ang ); + } + Gizmos.DrawLineStrip( disc, true ); +#endif + } + } +} diff --git a/AGXUnity/Sensor/ImuSensor.cs.meta b/AGXUnity/Sensor/ImuSensor.cs.meta new file mode 100644 index 00000000..01d10369 --- /dev/null +++ b/AGXUnity/Sensor/ImuSensor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: db4a13a72fd873242a5a2eafba17c99f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {fileID: 2800000, guid: 291d5afecb12a3747931bd119c01ad37, type: 3} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Editor/CustomEditors/AGXUnity+Sensor+ImuSensorEditor.cs b/Editor/CustomEditors/AGXUnity+Sensor+ImuSensorEditor.cs new file mode 100644 index 00000000..2b6dd319 --- /dev/null +++ b/Editor/CustomEditors/AGXUnity+Sensor+ImuSensorEditor.cs @@ -0,0 +1,9 @@ +using UnityEditor; + +namespace AGXUnityEditor.Editors +{ + [CustomEditor( typeof( AGXUnity.Sensor.ImuSensor ) )] + [CanEditMultipleObjects] + public class AGXUnitySensorImuSensorEditor : InspectorEditor + { } +} diff --git a/Editor/CustomEditors/AGXUnity+Sensor+ImuSensorEditor.cs.meta b/Editor/CustomEditors/AGXUnity+Sensor+ImuSensorEditor.cs.meta new file mode 100644 index 00000000..a81ccfd6 --- /dev/null +++ b/Editor/CustomEditors/AGXUnity+Sensor+ImuSensorEditor.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 79cf0f4efe162aa449bda63df746c92a \ No newline at end of file From d969f00d1481e54e73e37e85c96b983175722477 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 12 Jan 2026 18:28:32 +0100 Subject: [PATCH 02/63] Sketch of RuntimeValue attribute --- AGXUnity/Attributes.cs | 8 ++++++ Editor/AGXUnityEditor/InspectorEditor.cs | 33 ++++++++++++++++++++++++ 2 files changed, 41 insertions(+) diff --git a/AGXUnity/Attributes.cs b/AGXUnity/Attributes.cs index b8f5cce8..b2ce1a8f 100644 --- a/AGXUnity/Attributes.cs +++ b/AGXUnity/Attributes.cs @@ -241,4 +241,12 @@ public DynamicallyShowInInspector( string name, bool isMethod = false, bool inve public string Name { get; private set; } public bool Invert { get; private set; } } + + [AttributeUsage( AttributeTargets.Field | AttributeTargets.Property, AllowMultiple = false )] + public class RuntimeValue : Attribute + { + public string Unit { get; private set; } + public RuntimeValue( string unit = "" ) { this.Unit = unit; } + } + } diff --git a/Editor/AGXUnityEditor/InspectorEditor.cs b/Editor/AGXUnityEditor/InspectorEditor.cs index 142dc630..b452c567 100644 --- a/Editor/AGXUnityEditor/InspectorEditor.cs +++ b/Editor/AGXUnityEditor/InspectorEditor.cs @@ -2,6 +2,7 @@ using AGXUnity.Utils; using System; using System.Linq; +using System.Collections.Generic; using System.Reflection; using UnityEditor; using UnityEngine; @@ -70,10 +71,20 @@ public static void DrawMembersGUI( Object[] targets, InvokeWrapper[] fieldsAndProperties = InvokeWrapper.FindFieldsAndProperties( objects[ 0 ].GetType() ); var group = InspectorGroupHandler.Create(); + + var runtimeValues = new List(); + foreach ( var wrapper in fieldsAndProperties ) { if ( !ShouldBeShownInInspector( wrapper.Member, objects ) ) continue; + // Runtimevalues are drawn separately + bool isRuntimeValue = wrapper.Member.IsDefined( typeof( RuntimeValue ), true ); + if ( isRuntimeValue ) { + runtimeValues.Add( wrapper ); + continue; + } + group.Update( wrapper, objects[ 0 ] ); if ( group.IsHidden ) @@ -85,6 +96,28 @@ public static void DrawMembersGUI( Object[] targets, HandleType( wrapper, objects, fallback ); } group.Dispose(); + + // Draw runtime values in one disabled block + // TODO style etc + if ( runtimeValues.Count > 0 ) { + + InspectorGUI.Separator( 1, EditorGUIUtility.singleLineHeight ); + var style = GUI.Align( GUI.Skin.label, TextAnchor.MiddleLeft ); + GUILayout.Label( GUI.MakeLabel( "Runtime Values", true, "" ), style ); + + using ( new GUI.EnabledBlock( false ) ) { + group = InspectorGroupHandler.Create(); + foreach ( var wrapper in runtimeValues ) { + group.Update( wrapper, objects[ 0 ] ); + + if ( group.IsHidden ) + continue; + + HandleType( wrapper, objects, fallback ); + } + group.Dispose(); + } + } } } From a2eafd3465c54879bf7d4c9fd70bfb35f2e4f7b2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 12 Jan 2026 18:28:59 +0100 Subject: [PATCH 03/63] Some basic sensor attachment configs --- AGXUnity/Sensor/ImuSensor.cs | 261 +++++++++++++----- .../InvokeWrapperInspectorDrawer.cs | 19 ++ 2 files changed, 204 insertions(+), 76 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index e085fea4..f3e21e1d 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -2,15 +2,52 @@ using agxSensor; using AGXUnity.Utils; using UnityEngine; +using System; namespace AGXUnity.Sensor { + // public interface IImuAttachment { } + + [Serializable] + // TODO name could be ImuAttachmentConfig + public class ImuAttachment// : IImuAttachment + { + /// + /// Detectable measurement range, in m/s^2 / radians/s / T + /// + [Tooltip("Measurement range - values outside of range will be truncated")] + // TODO should really be three ranges, not 1... Special type needed, or maybe rangeX, rangeY, rangeZ + // Could also be a bool for using maxrange + //public Vec2 TriaxialRange = new Vec2(double.MinValue, double.MaxValue); // TODO double?? + public Vector2 TriaxialRange; + + /// + /// Cross axis sensitivity + /// + [Tooltip("Cross axis sensitivity")] + // TODO float or Matrix3x3... + public float CrossAxisSensitivity; + + /// + /// Bias reported in each axis under conditions without externally applied transformation, in m/s^2 + /// + [Tooltip("todo")] + public float ZeroRateBias; + + public ImuAttachment( Vector2 triaxialRange, float crossAxisSensitivity, float zeroRateBias ) + { + TriaxialRange = triaxialRange; + CrossAxisSensitivity = crossAxisSensitivity; + ZeroRateBias = zeroRateBias; + } + } + /// /// IMU Sensor Component /// [DisallowMultipleComponent] - [AddComponentMenu( "AGXUnity/Sensors/IMU Sensor" )] + [AddComponentMenu("AGXUnity/Sensors/IMU Sensor")] //[HelpURL( "https://us.download.algoryx.se/AGXUnity/documentation/current/editor_interface.html#simulating-lidar-sensors" )] public class ImuSensor : ScriptComponent { @@ -20,6 +57,70 @@ public class ImuSensor : ScriptComponent public IMU Native { get; private set; } = null; public IMUModel m_nativeModel = null; + + + + /// + /// When enabled, show configuration for the IMU attachment and create attachment when initializing object + /// + [field: SerializeField] + [Tooltip( "When enabled, show configuration for the IMU attachment and create attachment when initializing object." )] + [DisableInRuntimeInspector] + public bool EnableAccelerometer { get; set; } = true; + + /// + /// Accelerometer TODO + /// + [field: SerializeField] + [DynamicallyShowInInspector("EnableAccelerometer")] + [DisableInRuntimeInspector] + public ImuAttachment AccelerometerAttachment { get; private set; } = new ImuAttachment( new Vector2(float.MinValue, float.MaxValue), 0.01f, 260f); + + + + + /// + /// When enabled, show configuration for the IMU attachment and create attachment when initializing object + /// + [field: SerializeField] + [Tooltip( "When enabled, show configuration for the IMU attachment and create attachment when initializing object." )] + [DisableInRuntimeInspector] + public bool EnableGyroscope { get; set; } = true; + + /// + /// Gyroscope TODO + /// + [field: SerializeField] + [DynamicallyShowInInspector("EnableGyroscope")] + [DisableInRuntimeInspector] + public ImuAttachment GyroscopeAttachment { get; private set; } = new ImuAttachment( new Vector2(float.MinValue, float.MaxValue), 0.01f, 3f); + + + + + /// + /// When enabled, show configuration for the IMU attachment and create attachment when initializing object + /// + [field: SerializeField] + [Tooltip( "When enabled, show configuration for the IMU attachment and create attachment when initializing object." )] + [DisableInRuntimeInspector] + public bool EnableMagnetometer { get; set; } = true; + + /// + /// Magnetometer TODO + /// + [field: SerializeField] + [DynamicallyShowInInspector("EnableMagnetometer")] + [DisableInRuntimeInspector] + public ImuAttachment MagnetometerAttachment { get; private set; } = new ImuAttachment( new Vector2(float.MinValue, float.MaxValue), 0.01f, 0f); + + + + + + + [RuntimeValue("m/s")] public int test = 3; + /// /// Local sensor rotation relative to the parent GameObject transform. /// @@ -35,7 +136,7 @@ public class ImuSensor : ScriptComponent /// /// The local transformation matrix from the sensor frame to the parent GameObject frame /// - public UnityEngine.Matrix4x4 LocalTransform => UnityEngine.Matrix4x4.TRS( LocalPosition, Quaternion.Euler( LocalRotation ), Vector3.one ); + public UnityEngine.Matrix4x4 LocalTransform => UnityEngine.Matrix4x4.TRS(LocalPosition, Quaternion.Euler(LocalRotation), Vector3.one); /// /// The global transformation matrix from the sensor frame to the world frame. @@ -52,11 +153,11 @@ public class ImuSensor : ScriptComponent //private void Sync() //{ // var xform = GlobalTransform; -// + // // Native.getFrame().setTranslate( xform.GetPosition().ToHandedVec3() ); // Native.getFrame().setRotate( xform.rotation.ToHandedQuat() ); -// -// + // + // // //Debug.Log( Native.getFrame().getTranslate() ); // //Native.setFrame( new Frame( // // new AffineMatrix4x4( @@ -68,122 +169,130 @@ protected override bool Initialize() { SensorEnvironment.Instance.GetInitialized(); - // TODO save the models in private vars to update values + // TODO do we need to save these in private vars? We'll see - // Accelerometer - var accelerometer_modifiers = new ITriaxialSignalSystemNodeRefVector - { - //new GyroscopeLinearAccelerationEffects( new Vec3( 2.62 * 1e-4 ) ), - //new TriaxialSpectralGaussianNoise( new Vec3( 1.75 * 1e-4 ) ) - }; + var imu_attachments = new IMUModelSensorAttachmentRefVector(); - var accelerometer_model = new AccelerometerModel( - new TriaxialRange(), - new TriaxialCrossSensitivity(0.0), - new Vec3(0.0), - accelerometer_modifiers - ); + // Accelerometer + if ( EnableAccelerometer ) { + var modifiers = new ITriaxialSignalSystemNodeRefVector + { + //new TriaxialSpectralGaussianNoise( new Vec3( 1.75 * 1e-4 ) ) + }; + + var accelerometer = new AccelerometerModel( + new TriaxialRange( new agx.RangeReal( AccelerometerAttachment.TriaxialRange.x, AccelerometerAttachment.TriaxialRange.y ) ), + new TriaxialCrossSensitivity( AccelerometerAttachment.CrossAxisSensitivity ), + new Vec3( AccelerometerAttachment.ZeroRateBias ), + modifiers + ); + + imu_attachments.Add( new IMUModelAccelerometerAttachment( AffineMatrix4x4.identity(), accelerometer ) ); + } // Gyroscope - var gyroscope_modifiers = new ITriaxialSignalSystemNodeRefVector { - //new GyroscopeLinearAccelerationEffects( new Vec3( 2.62 * 1e-4 ) ), - //new TriaxialSpectralGaussianNoise( new Vec3( 1.75 * 1e-4 ) ) - }; - - var gyroscope_model = new GyroscopeModel( - new TriaxialRange(new agx.RangeReal(-1, 1)), - new TriaxialCrossSensitivity(0.01), - new Vec3(0.0), - gyroscope_modifiers - ); - - // Magnetometer - var magnetometer_modifiers = new ITriaxialSignalSystemNodeRefVector { - //new TriaxialGaussianNoise( new Vec3( 4.5e-7 ) ) - }; - - var magnetometer_model = new MagnetometerModel( - new TriaxialRange(), - new TriaxialCrossSensitivity(0.01), - new Vec3(0.0), - magnetometer_modifiers - ); - - var imu_attachments = new IMUModelSensorAttachmentRefVector - { - new IMUModelAccelerometerAttachment( - new AffineMatrix4x4(), accelerometer_model - ), - new IMUModelGyroscopeAttachment( - new AffineMatrix4x4(), gyroscope_model - ), - new IMUModelMagnetometerAttachment( - new AffineMatrix4x4(), magnetometer_model - ) - }; + if ( EnableGyroscope ) { + var gyroscope_modifiers = new ITriaxialSignalSystemNodeRefVector + { + //new GyroscopeLinearAccelerationEffects( new Vec3( 2.62 * 1e-4 ) ), + //new TriaxialSpectralGaussianNoise( new Vec3( 1.75 * 1e-4 ) ) + }; + + var gyroscope = new GyroscopeModel( + new TriaxialRange( new agx.RangeReal( GyroscopeAttachment.TriaxialRange.x, GyroscopeAttachment.TriaxialRange.y ) ), + new TriaxialCrossSensitivity( GyroscopeAttachment.CrossAxisSensitivity ), + new Vec3( GyroscopeAttachment.ZeroRateBias ), + gyroscope_modifiers + ); + + imu_attachments.Add( new IMUModelGyroscopeAttachment( AffineMatrix4x4.identity(), gyroscope ) ); + } - m_nativeModel = IMUModel.makeIdealNineDoFModel(); - //m_nativeModel = new IMUModel( imu_attachments ); + // magnetometer + MagnetometerModel magnetometer = null; + if ( EnableMagnetometer ) { + var magnetometer_modifiers = new ITriaxialSignalSystemNodeRefVector + { + //new magnetometerLinearAccelerationEffects( new Vec3( 2.62 * 1e-4 ) ), + //new TriaxialSpectralGaussianNoise( new Vec3( 1.75 * 1e-4 ) ) + }; + + magnetometer = new MagnetometerModel( + new TriaxialRange( new agx.RangeReal( MagnetometerAttachment.TriaxialRange.x, MagnetometerAttachment.TriaxialRange.y ) ), + new TriaxialCrossSensitivity( MagnetometerAttachment.CrossAxisSensitivity ), + new Vec3( MagnetometerAttachment.ZeroRateBias ), + magnetometer_modifiers + ); + + imu_attachments.Add( new IMUModelMagnetometerAttachment( AffineMatrix4x4.identity(), magnetometer ) ); + } - if ( m_nativeModel == null ) - return false; + if (imu_attachments.Count == 0 ) { + Debug.LogWarning( "No sensor attachments on IMU means the component will do nothing" ); + return true; + } - PropertySynchronizer.Synchronize( this ); + m_nativeModel = new IMUModel( imu_attachments ); + + if (m_nativeModel == null) + return false; // TODO error + + PropertySynchronizer.Synchronize(this); // TODO moveme to function that can get called both when setting RB and from here var measuredRB = MeasuredBody.GetInitialized().Native; - SensorEnvironment.Instance.Native.add( measuredRB ); + SensorEnvironment.Instance.Native.add(measuredRB); var rbFrame = measuredRB.getFrame(); - if ( rbFrame == null ) - Debug.LogError( "Need RB to follow" ); + if (rbFrame == null) + Debug.LogError("Need RB to follow"); - Native = new IMU( rbFrame, m_nativeModel ); + Native = new IMU(rbFrame, m_nativeModel); // TODO temp depth firsth output implementation m_outputID = SensorEnvironment.Instance.GenerateOutputID(); m_output = new IMUOutputNineDoF(); - Native.getOutputHandler().add( m_outputID, m_output ); + Native.getOutputHandler().add(m_outputID, m_output); //Simulation.Instance.StepCallbacks.PreSynchronizeTransforms += Sync; Simulation.Instance.StepCallbacks.PostStepForward += OnPostStepForward; - SensorEnvironment.Instance.Native.add( Native ); + SensorEnvironment.Instance.Native.add(Native); return true; } - + // TODO removeme used for testing private void OnPostStepForward() { - NineDoFView view = Native.getOutputHandler().get( m_outputID ).viewNineDoF(); + NineDoFView view = Native.getOutputHandler().get(m_outputID).viewNineDoF(); // Debug.Log( "Test: " + view.size()); // Returns 1 - Debug.Log( "Pos: " + Native.getFrame().getTranslate() ); + Debug.Log("Pos: " + Native.getFrame().getTranslate()); - Debug.Log( "Accelerometer 0: " + view[ 0 ].getTriplet( 0 ).length() ); - Debug.Log( "Gyroscope 1: " + view[ 0 ].getTriplet( 1 ).length() ); - Debug.Log( "Magnetometer 2: " + view[ 0 ].getTriplet( 2 ).length() ); + Debug.Log("Accelerometer 0: " + view[0].getTriplet(0).length()); + Debug.Log("Gyroscope 1: " + view[0].getTriplet(1).length()); + Debug.Log("Magnetometer 2: " + view[0].getTriplet(2).length()); } protected override void OnEnable() { - Native?.setEnable( true ); + Native?.setEnable(true); } protected override void OnDisable() { - Native?.setEnable( false ); + Native?.setEnable(false); } protected override void OnDestroy() { - if ( SensorEnvironment.HasInstance ) - SensorEnvironment.Instance.Native?.remove( Native ); + if (SensorEnvironment.HasInstance) + SensorEnvironment.Instance.Native?.remove(Native); - if ( Simulation.HasInstance ) + if (Simulation.HasInstance) { //Simulation.Instance.StepCallbacks.PreSynchronizeTransforms -= Sync; Simulation.Instance.StepCallbacks.PostStepForward -= OnPostStepForward; diff --git a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs index 7e2f3f72..6a90104b 100644 --- a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs +++ b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs @@ -1486,5 +1486,24 @@ public static object LidarDistanceGaussianNoiseDrawer( object[] objects, InvokeW return null; } + + [InspectorDrawer( typeof( AGXUnity.Sensor.ImuAttachment ) )] + public static object ImuAttachmentDrawer( object[] objects, InvokeWrapper wrapper ) + { + if ( objects.Length != 1 ) { + InspectorGUI.WarningLabel( "Multi-select of ImuAttachment Elements isn't supported." ); + return null; + } + + var data = wrapper.Get( objects[0] ); + using ( new InspectorGUI.IndentScope() ) { + data.TriaxialRange = EditorGUILayout.Vector2Field( FindGUIContentFor( data.GetType(), "TriaxialRange" ), data.TriaxialRange ); + data.CrossAxisSensitivity = EditorGUILayout.FloatField( FindGUIContentFor( data.GetType(), "CrossAxisSensitivity" ), data.CrossAxisSensitivity ); + data.ZeroRateBias = EditorGUILayout.FloatField( FindGUIContentFor( data.GetType(), "ZeroRateBias" ), data.ZeroRateBias ); + } + + return null; + } + } } From fdcc8ba473b81d7cd20de465f893d3c240f1f566 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 13 Jan 2026 09:54:12 +0100 Subject: [PATCH 04/63] Default value by type, output settings --- AGXUnity/Sensor/ImuSensor.cs | 72 +++++++++++++------ .../InvokeWrapperInspectorDrawer.cs | 54 +++++++++++++- 2 files changed, 103 insertions(+), 23 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index f3e21e1d..02222023 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -6,13 +6,28 @@ namespace AGXUnity.Sensor { - - // public interface IImuAttachment { } - + [Serializable] + [Flags] + public enum OutputXYZ + { + None = 0, + X = 1 << 0, + Y = 1 << 1, + Z = 1 << 2, + } [Serializable] // TODO name could be ImuAttachmentConfig - public class ImuAttachment// : IImuAttachment + public class ImuAttachment { + public enum ImuAttachmentType + { + Accelerometer, + Gyroscope, + Magnetometer + } + + public ImuAttachmentType type { get; private set; } + /// /// Detectable measurement range, in m/s^2 / radians/s / T /// @@ -30,13 +45,27 @@ public class ImuAttachment// : IImuAttachment public float CrossAxisSensitivity; /// - /// Bias reported in each axis under conditions without externally applied transformation, in m/s^2 + /// Bias reported in each axis under conditions without externally applied transformation /// - [Tooltip("todo")] + [Tooltip("Bias reported in each axis under conditions without externally applied transformation")] public float ZeroRateBias; - public ImuAttachment( Vector2 triaxialRange, float crossAxisSensitivity, float zeroRateBias ) + /// + /// Applies an offset to the zero rate bias depending on the linear acceleration that the gyroscope is exposed to + /// + [Tooltip("Offset to the zero rate bias depending on the linear acceleration")] + // TODO could be a matrix3x3 + public Vector3 LinearAccelerationEffects; + + /// + /// Output flags - which, if any, of x y z should be used in output view + /// + public OutputXYZ OutputFlags = OutputXYZ.X | OutputXYZ.Y;// | OutputXYZ.Z; + + // Constructor to set different default values for different sensor types + public ImuAttachment( ImuAttachmentType type, Vector2 triaxialRange, float crossAxisSensitivity, float zeroRateBias ) { + this.type = type; TriaxialRange = triaxialRange; CrossAxisSensitivity = crossAxisSensitivity; ZeroRateBias = zeroRateBias; @@ -58,8 +87,6 @@ public class ImuSensor : ScriptComponent public IMUModel m_nativeModel = null; - - /// /// When enabled, show configuration for the IMU attachment and create attachment when initializing object /// @@ -72,11 +99,13 @@ public class ImuSensor : ScriptComponent /// Accelerometer TODO /// [field: SerializeField] - [DynamicallyShowInInspector("EnableAccelerometer")] + [DynamicallyShowInInspector( "EnableAccelerometer" )] [DisableInRuntimeInspector] - public ImuAttachment AccelerometerAttachment { get; private set; } = new ImuAttachment( new Vector2(float.MinValue, float.MaxValue), 0.01f, 260f); - - + public ImuAttachment AccelerometerAttachment { get; private set; } = new ImuAttachment( + ImuAttachment.ImuAttachmentType.Accelerometer, + new Vector2( float.MinValue, float.MaxValue ), + 0.01f, + 260f ); /// @@ -93,9 +122,11 @@ public class ImuSensor : ScriptComponent [field: SerializeField] [DynamicallyShowInInspector("EnableGyroscope")] [DisableInRuntimeInspector] - public ImuAttachment GyroscopeAttachment { get; private set; } = new ImuAttachment( new Vector2(float.MinValue, float.MaxValue), 0.01f, 3f); - - + public ImuAttachment GyroscopeAttachment { get; private set; } = new ImuAttachment( + ImuAttachment.ImuAttachmentType.Gyroscope, + new Vector2( float.MinValue, float.MaxValue ), + 0.01f, + 3f ); /// @@ -112,10 +143,11 @@ public class ImuSensor : ScriptComponent [field: SerializeField] [DynamicallyShowInInspector("EnableMagnetometer")] [DisableInRuntimeInspector] - public ImuAttachment MagnetometerAttachment { get; private set; } = new ImuAttachment( new Vector2(float.MinValue, float.MaxValue), 0.01f, 0f); - - - + public ImuAttachment MagnetometerAttachment { get; private set; } = new ImuAttachment( + ImuAttachment.ImuAttachmentType.Magnetometer, + new Vector2( float.MinValue, float.MaxValue ), + 0.01f, + 0f ); diff --git a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs index 6a90104b..ca7629ca 100644 --- a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs +++ b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs @@ -1490,6 +1490,8 @@ public static object LidarDistanceGaussianNoiseDrawer( object[] objects, InvokeW [InspectorDrawer( typeof( AGXUnity.Sensor.ImuAttachment ) )] public static object ImuAttachmentDrawer( object[] objects, InvokeWrapper wrapper ) { + var target = objects[ 0 ] as Object; + if ( objects.Length != 1 ) { InspectorGUI.WarningLabel( "Multi-select of ImuAttachment Elements isn't supported." ); return null; @@ -1497,13 +1499,59 @@ public static object ImuAttachmentDrawer( object[] objects, InvokeWrapper wrappe var data = wrapper.Get( objects[0] ); using ( new InspectorGUI.IndentScope() ) { - data.TriaxialRange = EditorGUILayout.Vector2Field( FindGUIContentFor( data.GetType(), "TriaxialRange" ), data.TriaxialRange ); - data.CrossAxisSensitivity = EditorGUILayout.FloatField( FindGUIContentFor( data.GetType(), "CrossAxisSensitivity" ), data.CrossAxisSensitivity ); - data.ZeroRateBias = EditorGUILayout.FloatField( FindGUIContentFor( data.GetType(), "ZeroRateBias" ), data.ZeroRateBias ); + if ( data.type == ImuAttachment.ImuAttachmentType.Gyroscope ) + data.LinearAccelerationEffects = EditorGUILayout.Vector3Field( "Linear Acceleration Effects", data.LinearAccelerationEffects ); + data.TriaxialRange = EditorGUILayout.Vector2Field( "Triaxial Range", data.TriaxialRange ); + data.CrossAxisSensitivity = EditorGUILayout.FloatField( "Cross Axis Sensitivity", data.CrossAxisSensitivity ); + data.ZeroRateBias = EditorGUILayout.FloatField( "Zero Rate Bias", data.ZeroRateBias ); + EditorGUI.BeginChangeCheck(); + data.OutputFlags = OutputXYZGUI( data.OutputFlags ); + if ( EditorGUI.EndChangeCheck() ) + EditorUtility.SetDirty( target ); } return null; } + public static OutputXYZ OutputXYZGUI( OutputXYZ state ) + { + var skin = InspectorEditor.Skin; + + using ( new EditorGUILayout.HorizontalScope() ) { + EditorGUILayout.PrefixLabel( GUI.MakeLabel( "Output values", true ), + InspectorEditor.Skin.LabelMiddleLeft ); + + var xEnabled = state.HasFlag(OutputXYZ.X); + var yEnabled = state.HasFlag(OutputXYZ.Y); + var zEnabled = state.HasFlag(OutputXYZ.Z); + + if ( GUILayout.Toggle( xEnabled, + GUI.MakeLabel( "X", + xEnabled, + "Use sensor X value in output" ), + skin.GetButton( InspectorGUISkin.ButtonType.Left ), + GUILayout.Width( 76 ) ) != xEnabled ) + state ^= OutputXYZ.X; + if ( GUILayout.Toggle( yEnabled, + GUI.MakeLabel( "Y", + yEnabled, + "Use sensor X value in output" ), + skin.GetButton( InspectorGUISkin.ButtonType.Middle ), + GUILayout.Width( 76 ) ) != yEnabled ) + state ^= OutputXYZ.Y; + if ( GUILayout.Toggle( zEnabled, + GUI.MakeLabel( "Z", + yEnabled, + "Use sensor Z value in output" ), + skin.GetButton( InspectorGUISkin.ButtonType.Right ), + GUILayout.Width( 76 ) ) != zEnabled ) + state ^= OutputXYZ.Z; + } + + return state; + } + + } } + From f99ef2b0a2d3e2dccc339dcff02bbee8f187d1e7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 13 Jan 2026 14:50:09 +0100 Subject: [PATCH 05/63] TriaxialRangeData class --- AGXUnity/Sensor/ImuSensor.cs | 27 +++++------ AGXUnity/Sensor/TriaxialTypes.cs | 47 +++++++++++++++++++ AGXUnity/Sensor/TriaxialTypes.cs.meta | 2 + .../InvokeWrapperInspectorDrawer.cs | 33 ++++++++++--- 4 files changed, 87 insertions(+), 22 deletions(-) create mode 100644 AGXUnity/Sensor/TriaxialTypes.cs create mode 100644 AGXUnity/Sensor/TriaxialTypes.cs.meta diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 02222023..c5963a02 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -35,7 +35,7 @@ public enum ImuAttachmentType // TODO should really be three ranges, not 1... Special type needed, or maybe rangeX, rangeY, rangeZ // Could also be a bool for using maxrange //public Vec2 TriaxialRange = new Vec2(double.MinValue, double.MaxValue); // TODO double?? - public Vector2 TriaxialRange; + public TriaxialRangeData TriaxialRange; /// /// Cross axis sensitivity @@ -63,7 +63,7 @@ public enum ImuAttachmentType public OutputXYZ OutputFlags = OutputXYZ.X | OutputXYZ.Y;// | OutputXYZ.Z; // Constructor to set different default values for different sensor types - public ImuAttachment( ImuAttachmentType type, Vector2 triaxialRange, float crossAxisSensitivity, float zeroRateBias ) + public ImuAttachment( ImuAttachmentType type, TriaxialRangeData triaxialRange, float crossAxisSensitivity, float zeroRateBias ) { this.type = type; TriaxialRange = triaxialRange; @@ -76,7 +76,7 @@ public ImuAttachment( ImuAttachmentType type, Vector2 triaxialRange, float cross /// IMU Sensor Component /// [DisallowMultipleComponent] - [AddComponentMenu("AGXUnity/Sensors/IMU Sensor")] + [AddComponentMenu( "AGXUnity/Sensors/IMU Sensor" )] //[HelpURL( "https://us.download.algoryx.se/AGXUnity/documentation/current/editor_interface.html#simulating-lidar-sensors" )] public class ImuSensor : ScriptComponent { @@ -86,7 +86,6 @@ public class ImuSensor : ScriptComponent public IMU Native { get; private set; } = null; public IMUModel m_nativeModel = null; - /// /// When enabled, show configuration for the IMU attachment and create attachment when initializing object /// @@ -103,11 +102,10 @@ public class ImuSensor : ScriptComponent [DisableInRuntimeInspector] public ImuAttachment AccelerometerAttachment { get; private set; } = new ImuAttachment( ImuAttachment.ImuAttachmentType.Accelerometer, - new Vector2( float.MinValue, float.MaxValue ), + new TriaxialRangeData(), 0.01f, 260f ); - /// /// When enabled, show configuration for the IMU attachment and create attachment when initializing object /// @@ -120,15 +118,14 @@ public class ImuSensor : ScriptComponent /// Gyroscope TODO /// [field: SerializeField] - [DynamicallyShowInInspector("EnableGyroscope")] + [DynamicallyShowInInspector( "EnableGyroscope" )] [DisableInRuntimeInspector] public ImuAttachment GyroscopeAttachment { get; private set; } = new ImuAttachment( ImuAttachment.ImuAttachmentType.Gyroscope, - new Vector2( float.MinValue, float.MaxValue ), + new TriaxialRangeData(), 0.01f, 3f ); - /// /// When enabled, show configuration for the IMU attachment and create attachment when initializing object /// @@ -141,16 +138,14 @@ public class ImuSensor : ScriptComponent /// Magnetometer TODO /// [field: SerializeField] - [DynamicallyShowInInspector("EnableMagnetometer")] + [DynamicallyShowInInspector( "EnableMagnetometer" )] [DisableInRuntimeInspector] public ImuAttachment MagnetometerAttachment { get; private set; } = new ImuAttachment( ImuAttachment.ImuAttachmentType.Magnetometer, - new Vector2( float.MinValue, float.MaxValue ), + new TriaxialRangeData(), 0.01f, 0f ); - - [RuntimeValue("m/s")] public int test = 3; /// @@ -168,7 +163,7 @@ public class ImuSensor : ScriptComponent /// /// The local transformation matrix from the sensor frame to the parent GameObject frame /// - public UnityEngine.Matrix4x4 LocalTransform => UnityEngine.Matrix4x4.TRS(LocalPosition, Quaternion.Euler(LocalRotation), Vector3.one); + public UnityEngine.Matrix4x4 LocalTransform => UnityEngine.Matrix4x4.TRS( LocalPosition, Quaternion.Euler( LocalRotation ), Vector3.one ); /// /// The global transformation matrix from the sensor frame to the world frame. @@ -213,7 +208,7 @@ protected override bool Initialize() }; var accelerometer = new AccelerometerModel( - new TriaxialRange( new agx.RangeReal( AccelerometerAttachment.TriaxialRange.x, AccelerometerAttachment.TriaxialRange.y ) ), + AccelerometerAttachment.TriaxialRange.GenerateTriaxialRange(), new TriaxialCrossSensitivity( AccelerometerAttachment.CrossAxisSensitivity ), new Vec3( AccelerometerAttachment.ZeroRateBias ), modifiers @@ -250,7 +245,7 @@ protected override bool Initialize() }; magnetometer = new MagnetometerModel( - new TriaxialRange( new agx.RangeReal( MagnetometerAttachment.TriaxialRange.x, MagnetometerAttachment.TriaxialRange.y ) ), + MagnetometerAttachment.TriaxialRange.GenerateTriaxialRange(), new TriaxialCrossSensitivity( MagnetometerAttachment.CrossAxisSensitivity ), new Vec3( MagnetometerAttachment.ZeroRateBias ), magnetometer_modifiers diff --git a/AGXUnity/Sensor/TriaxialTypes.cs b/AGXUnity/Sensor/TriaxialTypes.cs new file mode 100644 index 00000000..438d3b59 --- /dev/null +++ b/AGXUnity/Sensor/TriaxialTypes.cs @@ -0,0 +1,47 @@ +using System; +using UnityEngine; + +namespace AGXUnity.Sensor +{ + /// + /// Helper class for triaxial range type configurations + /// + [Serializable] + public class TriaxialRangeData + { + public enum ConfigurationMode + { + MaxRange, + EqualAxisRanges, + IndividualAxisRanges + } + + [SerializeField] + public ConfigurationMode Mode = ConfigurationMode.MaxRange; + + [SerializeField] + public Vector2 EqualAxesRange = new(float.MinValue, float.MaxValue); + + [SerializeField] + public Vector2 RangeX = new(float.MinValue, float.MaxValue); + public Vector2 RangeY = new(float.MinValue, float.MaxValue); + public Vector2 RangeZ = new(float.MinValue, float.MaxValue); + + public agxSensor.TriaxialRange GenerateTriaxialRange() + { + switch ( Mode ) { + case ConfigurationMode.MaxRange: + return new agxSensor.TriaxialRange( new agx.RangeReal( float.MinValue, float.MaxValue ) ); + case ConfigurationMode.EqualAxisRanges: + return new agxSensor.TriaxialRange( new agx.RangeReal( EqualAxesRange.x, EqualAxesRange.y ) ); + case ConfigurationMode.IndividualAxisRanges: + return new agxSensor.TriaxialRange( + new agx.RangeReal( RangeX.x, RangeX.y ), + new agx.RangeReal( RangeY.x, RangeY.y ), + new agx.RangeReal( RangeZ.x, RangeZ.y ) ); + default: + return null; + } + } + } +} diff --git a/AGXUnity/Sensor/TriaxialTypes.cs.meta b/AGXUnity/Sensor/TriaxialTypes.cs.meta new file mode 100644 index 00000000..5d00b93d --- /dev/null +++ b/AGXUnity/Sensor/TriaxialTypes.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: d8282f9366480f04eb6ad8388116cef4 \ No newline at end of file diff --git a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs index ca7629ca..0f1e7990 100644 --- a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs +++ b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs @@ -1501,7 +1501,7 @@ public static object ImuAttachmentDrawer( object[] objects, InvokeWrapper wrappe using ( new InspectorGUI.IndentScope() ) { if ( data.type == ImuAttachment.ImuAttachmentType.Gyroscope ) data.LinearAccelerationEffects = EditorGUILayout.Vector3Field( "Linear Acceleration Effects", data.LinearAccelerationEffects ); - data.TriaxialRange = EditorGUILayout.Vector2Field( "Triaxial Range", data.TriaxialRange ); + data.TriaxialRange = TriaxialRangeDataGUI( data.TriaxialRange ); data.CrossAxisSensitivity = EditorGUILayout.FloatField( "Cross Axis Sensitivity", data.CrossAxisSensitivity ); data.ZeroRateBias = EditorGUILayout.FloatField( "Zero Rate Bias", data.ZeroRateBias ); EditorGUI.BeginChangeCheck(); @@ -1513,9 +1513,30 @@ public static object ImuAttachmentDrawer( object[] objects, InvokeWrapper wrappe return null; } + public static TriaxialRangeData TriaxialRangeDataGUI( TriaxialRangeData data ) + { + data.Mode = ( TriaxialRangeData.ConfigurationMode )EditorGUILayout.EnumPopup( "Sensor Measurement Range" , data.Mode ); + + using ( new InspectorGUI.IndentScope() ) { + switch ( data.Mode ) { + case TriaxialRangeData.ConfigurationMode.MaxRange: + break; + case TriaxialRangeData.ConfigurationMode.EqualAxisRanges: + data.EqualAxesRange = EditorGUILayout.Vector2Field( "XYZ range", data.EqualAxesRange ); + break; + case TriaxialRangeData.ConfigurationMode.IndividualAxisRanges: + data.RangeX = EditorGUILayout.Vector2Field( "X axis range", data.RangeX ); + data.RangeY = EditorGUILayout.Vector2Field( "Y axis range", data.RangeY ); + data.RangeZ = EditorGUILayout.Vector2Field( "Z axis range", data.RangeZ ); + break; + } + } + return data; + } + public static OutputXYZ OutputXYZGUI( OutputXYZ state ) { - var skin = InspectorEditor.Skin; + var skin = InspectorEditor.Skin; using ( new EditorGUILayout.HorizontalScope() ) { EditorGUILayout.PrefixLabel( GUI.MakeLabel( "Output values", true ), @@ -1524,28 +1545,28 @@ public static OutputXYZ OutputXYZGUI( OutputXYZ state ) var xEnabled = state.HasFlag(OutputXYZ.X); var yEnabled = state.HasFlag(OutputXYZ.Y); var zEnabled = state.HasFlag(OutputXYZ.Z); - + if ( GUILayout.Toggle( xEnabled, GUI.MakeLabel( "X", xEnabled, "Use sensor X value in output" ), skin.GetButton( InspectorGUISkin.ButtonType.Left ), GUILayout.Width( 76 ) ) != xEnabled ) - state ^= OutputXYZ.X; + state ^= OutputXYZ.X; if ( GUILayout.Toggle( yEnabled, GUI.MakeLabel( "Y", yEnabled, "Use sensor X value in output" ), skin.GetButton( InspectorGUISkin.ButtonType.Middle ), GUILayout.Width( 76 ) ) != yEnabled ) - state ^= OutputXYZ.Y; + state ^= OutputXYZ.Y; if ( GUILayout.Toggle( zEnabled, GUI.MakeLabel( "Z", yEnabled, "Use sensor Z value in output" ), skin.GetButton( InspectorGUISkin.ButtonType.Right ), GUILayout.Width( 76 ) ) != zEnabled ) - state ^= OutputXYZ.Z; + state ^= OutputXYZ.Z; } return state; From 9dee2b0965cc9ea24f4b996ba269ab73d81e9af7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 13 Jan 2026 16:14:59 +0100 Subject: [PATCH 06/63] Working output generation --- AGXUnity/Sensor/ImuSensor.cs | 99 +++++++++++++++++++++--------------- AGXUnity/Utils/Math.cs | 9 ++++ 2 files changed, 68 insertions(+), 40 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index c5963a02..b16c0111 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -176,21 +176,7 @@ public class ImuSensor : ScriptComponent //TODO probably move to own class private uint m_outputID = 0; // Must be greater than 0 to be valid private IMUOutputNineDoF m_output = null; - - //private void Sync() - //{ - // var xform = GlobalTransform; - // - // Native.getFrame().setTranslate( xform.GetPosition().ToHandedVec3() ); - // Native.getFrame().setRotate( xform.rotation.ToHandedQuat() ); - // - // - // //Debug.Log( Native.getFrame().getTranslate() ); - // //Native.setFrame( new Frame( - // // new AffineMatrix4x4( - // // xform.rotation.ToHandedQuat(), - // // xform.GetPosition().ToHandedVec3() ) ) ); - //} + private double[] m_outputBuffer; protected override bool Initialize() { @@ -226,7 +212,7 @@ protected override bool Initialize() }; var gyroscope = new GyroscopeModel( - new TriaxialRange( new agx.RangeReal( GyroscopeAttachment.TriaxialRange.x, GyroscopeAttachment.TriaxialRange.y ) ), + GyroscopeAttachment.TriaxialRange.GenerateTriaxialRange(), new TriaxialCrossSensitivity( GyroscopeAttachment.CrossAxisSensitivity ), new Vec3( GyroscopeAttachment.ZeroRateBias ), gyroscope_modifiers @@ -254,74 +240,107 @@ protected override bool Initialize() imu_attachments.Add( new IMUModelMagnetometerAttachment( AffineMatrix4x4.identity(), magnetometer ) ); } - if (imu_attachments.Count == 0 ) { + if ( imu_attachments.Count == 0 ) { Debug.LogWarning( "No sensor attachments on IMU means the component will do nothing" ); return true; } m_nativeModel = new IMUModel( imu_attachments ); - if (m_nativeModel == null) + if ( m_nativeModel == null ) return false; // TODO error - PropertySynchronizer.Synchronize(this); + PropertySynchronizer.Synchronize( this ); // TODO moveme to function that can get called both when setting RB and from here var measuredRB = MeasuredBody.GetInitialized().Native; - SensorEnvironment.Instance.Native.add(measuredRB); + SensorEnvironment.Instance.Native.add( measuredRB ); var rbFrame = measuredRB.getFrame(); - if (rbFrame == null) - Debug.LogError("Need RB to follow"); + if ( rbFrame == null ) + Debug.LogError( "Need RB to follow" ); - Native = new IMU(rbFrame, m_nativeModel); + Native = new IMU( rbFrame, m_nativeModel ); - // TODO temp depth firsth output implementation + // For SWIG reasons, we will create a ninedof output and use the fields selectively m_outputID = SensorEnvironment.Instance.GenerateOutputID(); m_output = new IMUOutputNineDoF(); - Native.getOutputHandler().add(m_outputID, m_output); + Native.getOutputHandler().add( m_outputID, m_output ); + uint outputCount = 0; + outputCount += EnableAccelerometer ? Utils.Math.PopCount( (uint)AccelerometerAttachment.OutputFlags ) : 0; + outputCount += EnableGyroscope ? Utils.Math.PopCount( (uint)GyroscopeAttachment.OutputFlags ) : 0; + outputCount += EnableMagnetometer ? Utils.Math.PopCount( (uint)MagnetometerAttachment.OutputFlags ) : 0; + Debug.Log( "Enabled field count: " + outputCount ); // TODO removeme + m_outputBuffer = new double[ outputCount ]; //Simulation.Instance.StepCallbacks.PreSynchronizeTransforms += Sync; Simulation.Instance.StepCallbacks.PostStepForward += OnPostStepForward; - SensorEnvironment.Instance.Native.add(Native); + SensorEnvironment.Instance.Native.add( Native ); return true; } + public void GetOutput() + { + if ( Native == null ) + return; + + NineDoFValue view = Native.getOutputHandler().get(m_outputID).viewNineDoF()[0]; + + int i = 0; + + if ( EnableAccelerometer ) { + var a = AccelerometerAttachment.OutputFlags; + if ( ( a & OutputXYZ.X ) != 0 ) m_outputBuffer[ i++ ] = view.x0; + if ( ( a & OutputXYZ.Y ) != 0 ) m_outputBuffer[ i++ ] = view.y0; + if ( ( a & OutputXYZ.Z ) != 0 ) m_outputBuffer[ i++ ] = view.z0; + } + + if ( EnableGyroscope ) { + var g = GyroscopeAttachment.OutputFlags; + if ( ( g & OutputXYZ.X ) != 0 ) m_outputBuffer[ i++ ] = view.x1; + if ( ( g & OutputXYZ.Y ) != 0 ) m_outputBuffer[ i++ ] = view.y1; + if ( ( g & OutputXYZ.Z ) != 0 ) m_outputBuffer[ i++ ] = view.z1; + } + + if ( EnableMagnetometer ) { + var m = MagnetometerAttachment.OutputFlags; + if ( ( m & OutputXYZ.X ) != 0 ) m_outputBuffer[ i++ ] = view.x2; + if ( ( m & OutputXYZ.Y ) != 0 ) m_outputBuffer[ i++ ] = view.y2; + if ( ( m & OutputXYZ.Z ) != 0 ) m_outputBuffer[ i++ ] = view.z2; + } + } + // TODO removeme used for testing private void OnPostStepForward() { - NineDoFView view = Native.getOutputHandler().get(m_outputID).viewNineDoF(); - - // Debug.Log( "Test: " + view.size()); // Returns 1 + GetOutput(); - Debug.Log("Pos: " + Native.getFrame().getTranslate()); + string output = ""; + for ( int i = 0; i < m_outputBuffer.Length; i++ ) + output += m_outputBuffer[ i ].ToString() + " "; - Debug.Log("Accelerometer 0: " + view[0].getTriplet(0).length()); - Debug.Log("Gyroscope 1: " + view[0].getTriplet(1).length()); - Debug.Log("Magnetometer 2: " + view[0].getTriplet(2).length()); + Debug.Log( "Output: " + output ); } protected override void OnEnable() { - Native?.setEnable(true); + Native?.setEnable( true ); } protected override void OnDisable() { - Native?.setEnable(false); + Native?.setEnable( false ); } protected override void OnDestroy() { - if (SensorEnvironment.HasInstance) - SensorEnvironment.Instance.Native?.remove(Native); + if ( SensorEnvironment.HasInstance ) + SensorEnvironment.Instance.Native?.remove( Native ); - if (Simulation.HasInstance) - { - //Simulation.Instance.StepCallbacks.PreSynchronizeTransforms -= Sync; + if ( Simulation.HasInstance ) { Simulation.Instance.StepCallbacks.PostStepForward -= OnPostStepForward; } diff --git a/AGXUnity/Utils/Math.cs b/AGXUnity/Utils/Math.cs index 7d4fed57..407f4d5d 100644 --- a/AGXUnity/Utils/Math.cs +++ b/AGXUnity/Utils/Math.cs @@ -61,5 +61,14 @@ public static void Swap( ref T lhs, ref T rhs ) lhs = rhs; rhs = tmp; } + + // Counts enabled bits + public static uint PopCount( uint value ) + { + uint c = 0; + for ( ; value != 0; c++ ) + value &= value - 1; + return c; + } } } From 6447d5a72c27cef804907d356a0a24d98e339cdc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Wed, 14 Jan 2026 16:40:24 +0100 Subject: [PATCH 07/63] Modifiers and bugfixes / workaround for output --- AGXUnity/Sensor/ImuSensor.cs | 130 ++++++++++++------ .../InvokeWrapperInspectorDrawer.cs | 57 +++++++- 2 files changed, 137 insertions(+), 50 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index b16c0111..890d9f29 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -16,7 +16,7 @@ public enum OutputXYZ Z = 1 << 2, } [Serializable] - // TODO name could be ImuAttachmentConfig + // TODO probably move this to inside IMU comp public class ImuAttachment { public enum ImuAttachmentType @@ -26,22 +26,21 @@ public enum ImuAttachmentType Magnetometer } - public ImuAttachmentType type { get; private set; } + /// + /// Accelerometer / Gyroscope / Magnetometer + /// + public ImuAttachmentType Type { get; private set; } /// /// Detectable measurement range, in m/s^2 / radians/s / T /// [Tooltip("Measurement range - values outside of range will be truncated")] - // TODO should really be three ranges, not 1... Special type needed, or maybe rangeX, rangeY, rangeZ - // Could also be a bool for using maxrange - //public Vec2 TriaxialRange = new Vec2(double.MinValue, double.MaxValue); // TODO double?? public TriaxialRangeData TriaxialRange; /// /// Cross axis sensitivity /// [Tooltip("Cross axis sensitivity")] - // TODO float or Matrix3x3... public float CrossAxisSensitivity; /// @@ -50,22 +49,40 @@ public enum ImuAttachmentType [Tooltip("Bias reported in each axis under conditions without externally applied transformation")] public float ZeroRateBias; + public bool EnableLinearAccelerationEffects = false; /// /// Applies an offset to the zero rate bias depending on the linear acceleration that the gyroscope is exposed to /// [Tooltip("Offset to the zero rate bias depending on the linear acceleration")] - // TODO could be a matrix3x3 public Vector3 LinearAccelerationEffects; + public bool EnableTotalGaussianNoise = false; + /// + /// Base level noise in the measurement signal + /// + public Vector3 TotalGaussianNoise; + + public bool EnableSignalScaling = false; + /// + /// Constant scaling to the triaxial signal + /// + public Vector3 SignalScaling = Vector3.one; + + public bool EnableGaussianSpectralNoise = false; + /// + /// Sample frequency dependent Gaussian noise + /// + public Vector3 GaussianSpectralNoise; + /// /// Output flags - which, if any, of x y z should be used in output view /// - public OutputXYZ OutputFlags = OutputXYZ.X | OutputXYZ.Y;// | OutputXYZ.Z; + public OutputXYZ OutputFlags = OutputXYZ.X | OutputXYZ.Y | OutputXYZ.Z; - // Constructor to set different default values for different sensor types + // Constructor enables us to set different default values per sensor type public ImuAttachment( ImuAttachmentType type, TriaxialRangeData triaxialRange, float crossAxisSensitivity, float zeroRateBias ) { - this.type = type; + this.Type = type; TriaxialRange = triaxialRange; CrossAxisSensitivity = crossAxisSensitivity; ZeroRateBias = zeroRateBias; @@ -175,23 +192,32 @@ public class ImuSensor : ScriptComponent //TODO probably move to own class private uint m_outputID = 0; // Must be greater than 0 to be valid - private IMUOutputNineDoF m_output = null; private double[] m_outputBuffer; protected override bool Initialize() { SensorEnvironment.Instance.GetInitialized(); - // TODO do we need to save these in private vars? We'll see - + Func buildModifiers = a => + { + var modifiers = new ITriaxialSignalSystemNodeRefVector(); + + if ( a.EnableTotalGaussianNoise ) + modifiers.Add(new TriaxialGaussianNoise(a.TotalGaussianNoise.ToVec3())); // TODO handedness + if ( a.EnableSignalScaling ) + modifiers.Add(new TriaxialSignalScaling(a.SignalScaling.ToVec3())); + if ( a.EnableGaussianSpectralNoise ) + modifiers.Add(new TriaxialSpectralGaussianNoise(a.GaussianSpectralNoise.ToVec3()) ); + if ( a.EnableLinearAccelerationEffects && a.Type == ImuAttachment.ImuAttachmentType.Gyroscope ) + modifiers.Add( new GyroscopeLinearAccelerationEffects(a.GaussianSpectralNoise.ToVec3()) ); + + return modifiers; + }; var imu_attachments = new IMUModelSensorAttachmentRefVector(); // Accelerometer if ( EnableAccelerometer ) { - var modifiers = new ITriaxialSignalSystemNodeRefVector - { - //new TriaxialSpectralGaussianNoise( new Vec3( 1.75 * 1e-4 ) ) - }; + var modifiers = buildModifiers(AccelerometerAttachment); var accelerometer = new AccelerometerModel( AccelerometerAttachment.TriaxialRange.GenerateTriaxialRange(), @@ -205,36 +231,28 @@ protected override bool Initialize() // Gyroscope if ( EnableGyroscope ) { - var gyroscope_modifiers = new ITriaxialSignalSystemNodeRefVector - { - //new GyroscopeLinearAccelerationEffects( new Vec3( 2.62 * 1e-4 ) ), - //new TriaxialSpectralGaussianNoise( new Vec3( 1.75 * 1e-4 ) ) - }; + var modifiers = buildModifiers(GyroscopeAttachment); var gyroscope = new GyroscopeModel( GyroscopeAttachment.TriaxialRange.GenerateTriaxialRange(), new TriaxialCrossSensitivity( GyroscopeAttachment.CrossAxisSensitivity ), new Vec3( GyroscopeAttachment.ZeroRateBias ), - gyroscope_modifiers + modifiers ); imu_attachments.Add( new IMUModelGyroscopeAttachment( AffineMatrix4x4.identity(), gyroscope ) ); } - // magnetometer + // Magnetometer MagnetometerModel magnetometer = null; if ( EnableMagnetometer ) { - var magnetometer_modifiers = new ITriaxialSignalSystemNodeRefVector - { - //new magnetometerLinearAccelerationEffects( new Vec3( 2.62 * 1e-4 ) ), - //new TriaxialSpectralGaussianNoise( new Vec3( 1.75 * 1e-4 ) ) - }; + var modifiers = buildModifiers(MagnetometerAttachment); magnetometer = new MagnetometerModel( MagnetometerAttachment.TriaxialRange.GenerateTriaxialRange(), new TriaxialCrossSensitivity( MagnetometerAttachment.CrossAxisSensitivity ), new Vec3( MagnetometerAttachment.ZeroRateBias ), - magnetometer_modifiers + modifiers ); imu_attachments.Add( new IMUModelMagnetometerAttachment( AffineMatrix4x4.identity(), magnetometer ) ); @@ -245,6 +263,11 @@ protected override bool Initialize() return true; } + if ( imu_attachments.Count == 1 ) { + Debug.LogWarning( "KNOWN BUG: currently two sensors are needed for output to work properly! Suggested workaround: Enable another one and disable output" ); + return true; + } + m_nativeModel = new IMUModel( imu_attachments ); if ( m_nativeModel == null ) @@ -264,16 +287,18 @@ protected override bool Initialize() // For SWIG reasons, we will create a ninedof output and use the fields selectively m_outputID = SensorEnvironment.Instance.GenerateOutputID(); - m_output = new IMUOutputNineDoF(); - Native.getOutputHandler().add( m_outputID, m_output ); uint outputCount = 0; outputCount += EnableAccelerometer ? Utils.Math.PopCount( (uint)AccelerometerAttachment.OutputFlags ) : 0; outputCount += EnableGyroscope ? Utils.Math.PopCount( (uint)GyroscopeAttachment.OutputFlags ) : 0; outputCount += EnableMagnetometer ? Utils.Math.PopCount( (uint)MagnetometerAttachment.OutputFlags ) : 0; + + var output = new IMUOutputNineDoF(); + // output.setPaddingValue( 0 ); // TODO remove all thoughts of padding + Native.getOutputHandler().add( m_outputID, output ); + Debug.Log( "Enabled field count: " + outputCount ); // TODO removeme m_outputBuffer = new double[ outputCount ]; - //Simulation.Instance.StepCallbacks.PreSynchronizeTransforms += Sync; Simulation.Instance.StepCallbacks.PostStepForward += OnPostStepForward; SensorEnvironment.Instance.Native.add( Native ); @@ -286,29 +311,47 @@ public void GetOutput() if ( Native == null ) return; + //var imuOutput = Native.getOutputHandler().get(m_outputID); + //Debug.Log( "getElementSize: " + imuOutput.getElementSize() + ", hasUnreadData: " + imuOutput.hasUnreadData()); + //Debug.Log( "Tri: " + imuOutput.viewTriaxialXYZ().size() ); + //Debug.Log( "Six: " + imuOutput.viewSixDoF().size() ); + //Debug.Log( "Nine: " + imuOutput.viewNineDoF().size() ); + //return; + NineDoFValue view = Native.getOutputHandler().get(m_outputID).viewNineDoF()[0]; - int i = 0; + // This is all kind of a workaround to use a ninedof buffer with an arbitrary number + // of doubles read based on settings. If Native isn't null we have at least one sensor. + int i = 0, j = 0; + + var triplets = new Vec3[] + { + view.getTriplet( 0 ), + view.getTriplet( 1 ), + view.getTriplet( 2 ) + }; if ( EnableAccelerometer ) { var a = AccelerometerAttachment.OutputFlags; - if ( ( a & OutputXYZ.X ) != 0 ) m_outputBuffer[ i++ ] = view.x0; - if ( ( a & OutputXYZ.Y ) != 0 ) m_outputBuffer[ i++ ] = view.y0; - if ( ( a & OutputXYZ.Z ) != 0 ) m_outputBuffer[ i++ ] = view.z0; + if ( ( a & OutputXYZ.X ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].x; + if ( ( a & OutputXYZ.Y ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].y; + if ( ( a & OutputXYZ.Z ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].z; + j++; } if ( EnableGyroscope ) { var g = GyroscopeAttachment.OutputFlags; - if ( ( g & OutputXYZ.X ) != 0 ) m_outputBuffer[ i++ ] = view.x1; - if ( ( g & OutputXYZ.Y ) != 0 ) m_outputBuffer[ i++ ] = view.y1; - if ( ( g & OutputXYZ.Z ) != 0 ) m_outputBuffer[ i++ ] = view.z1; + if ( ( g & OutputXYZ.X ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].x; + if ( ( g & OutputXYZ.Y ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].y; + if ( ( g & OutputXYZ.Z ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].z; + j++; } if ( EnableMagnetometer ) { var m = MagnetometerAttachment.OutputFlags; - if ( ( m & OutputXYZ.X ) != 0 ) m_outputBuffer[ i++ ] = view.x2; - if ( ( m & OutputXYZ.Y ) != 0 ) m_outputBuffer[ i++ ] = view.y2; - if ( ( m & OutputXYZ.Z ) != 0 ) m_outputBuffer[ i++ ] = view.z2; + if ( ( m & OutputXYZ.X ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].x; + if ( ( m & OutputXYZ.Y ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].y; + if ( ( m & OutputXYZ.Z ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].z; } } @@ -345,7 +388,6 @@ protected override void OnDestroy() } //TODO remove modules and modifiers, possibly - m_output.Dispose(); Native?.Dispose(); Native = null; diff --git a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs index 0f1e7990..9f56fee0 100644 --- a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs +++ b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs @@ -1499,8 +1499,6 @@ public static object ImuAttachmentDrawer( object[] objects, InvokeWrapper wrappe var data = wrapper.Get( objects[0] ); using ( new InspectorGUI.IndentScope() ) { - if ( data.type == ImuAttachment.ImuAttachmentType.Gyroscope ) - data.LinearAccelerationEffects = EditorGUILayout.Vector3Field( "Linear Acceleration Effects", data.LinearAccelerationEffects ); data.TriaxialRange = TriaxialRangeDataGUI( data.TriaxialRange ); data.CrossAxisSensitivity = EditorGUILayout.FloatField( "Cross Axis Sensitivity", data.CrossAxisSensitivity ); data.ZeroRateBias = EditorGUILayout.FloatField( "Zero Rate Bias", data.ZeroRateBias ); @@ -1508,14 +1506,61 @@ public static object ImuAttachmentDrawer( object[] objects, InvokeWrapper wrappe data.OutputFlags = OutputXYZGUI( data.OutputFlags ); if ( EditorGUI.EndChangeCheck() ) EditorUtility.SetDirty( target ); + + if ( InspectorGUI.Foldout( EditorData.Instance.GetData( target, wrapper.Member.Name ), + GUI.MakeLabel( "Modifiers", true, "Optional signal output modifiers" ) ) ) { + using ( new InspectorGUI.IndentScope() ) { + ( data.EnableTotalGaussianNoise, data.TotalGaussianNoise ) = OptionalVector3GUI( + data.EnableTotalGaussianNoise, + data.TotalGaussianNoise, + "Total Gaussian Noise", + "Tooltip todo" ); + ( data.EnableSignalScaling, data.SignalScaling ) = OptionalVector3GUI( + data.EnableSignalScaling, + data.SignalScaling, + "Signal Scaling", + "Tooltip todo" ); + ( data.EnableGaussianSpectralNoise, data.GaussianSpectralNoise ) = OptionalVector3GUI( + data.EnableGaussianSpectralNoise, + data.GaussianSpectralNoise, + "Gaussian Spectral Noise", + "Tooltip todo" ); + if ( data.Type == ImuAttachment.ImuAttachmentType.Gyroscope ) { + ( data.EnableLinearAccelerationEffects, data.LinearAccelerationEffects ) = OptionalVector3GUI( + data.EnableLinearAccelerationEffects, + data.LinearAccelerationEffects, + "Linear Acceleration Effects", + "Tooltip todo" ); + } + } + } } + InspectorGUI.Separator(); + return null; } - public static TriaxialRangeData TriaxialRangeDataGUI( TriaxialRangeData data ) + private static (bool, Vector3) OptionalVector3GUI(bool toggle, Vector3 value, string label, string tooltip) + { + using (new GUILayout.HorizontalScope() ) { + var rect = EditorGUILayout.GetControlRect(); + var xMaxOriginal = rect.xMax; + rect.xMax = EditorGUIUtility.labelWidth + 20; + //InspectorGUI.MakeLabel( wrapper.Member ); + toggle = EditorGUI.ToggleLeft( rect, GUI.MakeLabel( label, false, tooltip ), toggle ); + using ( new GUI.EnabledBlock( UnityEngine.GUI.enabled && toggle)) { + rect.x = rect.xMax - 30; + rect.xMax = xMaxOriginal; + value = EditorGUI.Vector3Field(rect, "", value ); + } + } + return (toggle, value); + } + + private static TriaxialRangeData TriaxialRangeDataGUI( TriaxialRangeData data ) { - data.Mode = ( TriaxialRangeData.ConfigurationMode )EditorGUILayout.EnumPopup( "Sensor Measurement Range" , data.Mode ); + data.Mode = (TriaxialRangeData.ConfigurationMode)EditorGUILayout.EnumPopup( "Sensor Measurement Range", data.Mode ); using ( new InspectorGUI.IndentScope() ) { switch ( data.Mode ) { @@ -1534,12 +1579,12 @@ public static TriaxialRangeData TriaxialRangeDataGUI( TriaxialRangeData data ) return data; } - public static OutputXYZ OutputXYZGUI( OutputXYZ state ) + private static OutputXYZ OutputXYZGUI( OutputXYZ state ) { var skin = InspectorEditor.Skin; using ( new EditorGUILayout.HorizontalScope() ) { - EditorGUILayout.PrefixLabel( GUI.MakeLabel( "Output values", true ), + EditorGUILayout.PrefixLabel( GUI.MakeLabel( "Output values", false ), InspectorEditor.Skin.LabelMiddleLeft ); var xEnabled = state.HasFlag(OutputXYZ.X); From d41a4cb435732a612f0e30ac7373b685328f8531 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Thu, 15 Jan 2026 11:34:25 +0100 Subject: [PATCH 08/63] Magnetic field settings --- AGXUnity/Sensor/SensorEnvironment.cs | 56 ++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) diff --git a/AGXUnity/Sensor/SensorEnvironment.cs b/AGXUnity/Sensor/SensorEnvironment.cs index 09e4c7a0..e2aab7d7 100644 --- a/AGXUnity/Sensor/SensorEnvironment.cs +++ b/AGXUnity/Sensor/SensorEnvironment.cs @@ -17,6 +17,13 @@ namespace AGXUnity.Sensor [HelpURL( "https://us.download.algoryx.se/AGXUnity/documentation/current/editor_interface.html#sensor-environment" )] public class SensorEnvironment : UniqueGameObject { + public enum MagneticFieldType + { + NONE, + UNIFORM, + DIPOLE + } + /// /// Native instance, created in Start/Initialize. /// @@ -33,7 +40,45 @@ public class SensorEnvironment : UniqueGameObject /// [Tooltip("Show log messages on each thing added to the sensor environment")] public bool DebugLogOnAdd = false; +/* + [InspectorGroupBegin(Name = "Magnetic Field", DefaultExpanded = true)] + /// + /// Set type of magnetic field used in the simulation + /// + [Tooltip("Set type of magnetic field used in the simulation")] + [HideInRuntimeInspector] + public MagneticFieldType FieldType = MagneticFieldType.UNIFORM; + + private bool UsingUniformMagneticField => FieldType == MagneticFieldType.UNIFORM; + private bool UsingDipoleMagneticField => FieldType == MagneticFieldType.DIPOLE; + + /// + /// Set the field vector of the uniform magnetic field used in the simulation [in Tesla] + /// + [Tooltip("Set the field vector of the uniform magnetic field used in the simulation [in Tesla]")] + [HideInRuntimeInspector] + [DynamicallyShowInInspector( "UsingUniformMagneticField" )] + public Vector3 MagneticFieldVector = new Vector3( 19.462e-6f, 44.754e-6f, 7.8426e-6f ); + + /// + /// Magnetic moment vector [in m^2 * A] + /// + [Tooltip("Magnetic dipole moment vector [in m^2 * A]")] + [HideInRuntimeInspector] + [DynamicallyShowInInspector( "UsingDipoleMagneticField" )] + public Vector3 MagneticMoment = new Vector3( -2.69e19f, -7.65e22f, 1.5e22f ); + + /// + /// Magnetic dipole center + /// + [Tooltip("Magnetic dipole center")] + [HideInRuntimeInspector] + [DynamicallyShowInInspector( "UsingDipoleMagneticField" )] + public Vector3 DipoleCenter = new Vector3( 1.9e-10f, 20.79e3f, -6.369e6f ); + + [InspectorGroupEnd] +*/ /// /// Select which layers to include game objects from /// @@ -458,6 +503,17 @@ protected override bool Initialize() FindValidComponents( true ).ForEach( c => TrackIfSupported( c ) ); +// switch ( FieldType ) { +// case MagneticFieldType.UNIFORM: +// Native.setMagneticField( new UniformMagneticField( MagneticFieldVector.ToHandedVec3() ) ); +// break; +// case MagneticFieldType.DIPOLE: +// Native.setMagneticField( new DipoleMagneticField( MagneticMoment.ToHandedVec3(), DipoleCenter.ToHandedVec3() ) ); +// break; +// default: +// break; +// } + UpdateEnvironment(); Simulation.Instance.StepCallbacks.PreStepForward += AddNew; From 353a16aed2bbe0f292dc82f697930692a809ec62 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Thu, 15 Jan 2026 11:35:08 +0100 Subject: [PATCH 09/63] Rename to ZeroBias and change it to Vector3 --- AGXUnity/Sensor/ImuSensor.cs | 18 +++++++++--------- .../InvokeWrapperInspectorDrawer.cs | 2 +- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 890d9f29..917d0602 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -47,7 +47,7 @@ public enum ImuAttachmentType /// Bias reported in each axis under conditions without externally applied transformation /// [Tooltip("Bias reported in each axis under conditions without externally applied transformation")] - public float ZeroRateBias; + public Vector3 ZeroBias; public bool EnableLinearAccelerationEffects = false; /// @@ -80,12 +80,12 @@ public enum ImuAttachmentType public OutputXYZ OutputFlags = OutputXYZ.X | OutputXYZ.Y | OutputXYZ.Z; // Constructor enables us to set different default values per sensor type - public ImuAttachment( ImuAttachmentType type, TriaxialRangeData triaxialRange, float crossAxisSensitivity, float zeroRateBias ) + public ImuAttachment( ImuAttachmentType type, TriaxialRangeData triaxialRange, float crossAxisSensitivity, Vector3 zeroRateBias ) { this.Type = type; TriaxialRange = triaxialRange; CrossAxisSensitivity = crossAxisSensitivity; - ZeroRateBias = zeroRateBias; + ZeroBias = zeroRateBias; } } @@ -121,7 +121,7 @@ public class ImuSensor : ScriptComponent ImuAttachment.ImuAttachmentType.Accelerometer, new TriaxialRangeData(), 0.01f, - 260f ); + Vector3.one * 260f ); /// /// When enabled, show configuration for the IMU attachment and create attachment when initializing object @@ -141,7 +141,7 @@ public class ImuSensor : ScriptComponent ImuAttachment.ImuAttachmentType.Gyroscope, new TriaxialRangeData(), 0.01f, - 3f ); + Vector3.one * 3f ); /// /// When enabled, show configuration for the IMU attachment and create attachment when initializing object @@ -161,7 +161,7 @@ public class ImuSensor : ScriptComponent ImuAttachment.ImuAttachmentType.Magnetometer, new TriaxialRangeData(), 0.01f, - 0f ); + Vector3.one * 0f ); [RuntimeValue("m/s")] public int test = 3; @@ -222,7 +222,7 @@ protected override bool Initialize() var accelerometer = new AccelerometerModel( AccelerometerAttachment.TriaxialRange.GenerateTriaxialRange(), new TriaxialCrossSensitivity( AccelerometerAttachment.CrossAxisSensitivity ), - new Vec3( AccelerometerAttachment.ZeroRateBias ), + AccelerometerAttachment.ZeroBias.ToVec3(), modifiers ); @@ -236,7 +236,7 @@ protected override bool Initialize() var gyroscope = new GyroscopeModel( GyroscopeAttachment.TriaxialRange.GenerateTriaxialRange(), new TriaxialCrossSensitivity( GyroscopeAttachment.CrossAxisSensitivity ), - new Vec3( GyroscopeAttachment.ZeroRateBias ), + GyroscopeAttachment.ZeroBias.ToVec3(), modifiers ); @@ -251,7 +251,7 @@ protected override bool Initialize() magnetometer = new MagnetometerModel( MagnetometerAttachment.TriaxialRange.GenerateTriaxialRange(), new TriaxialCrossSensitivity( MagnetometerAttachment.CrossAxisSensitivity ), - new Vec3( MagnetometerAttachment.ZeroRateBias ), + MagnetometerAttachment.ZeroBias.ToVec3(), modifiers ); diff --git a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs index 9f56fee0..6bcd432c 100644 --- a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs +++ b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs @@ -1501,7 +1501,7 @@ public static object ImuAttachmentDrawer( object[] objects, InvokeWrapper wrappe using ( new InspectorGUI.IndentScope() ) { data.TriaxialRange = TriaxialRangeDataGUI( data.TriaxialRange ); data.CrossAxisSensitivity = EditorGUILayout.FloatField( "Cross Axis Sensitivity", data.CrossAxisSensitivity ); - data.ZeroRateBias = EditorGUILayout.FloatField( "Zero Rate Bias", data.ZeroRateBias ); + data.ZeroBias = EditorGUILayout.Vector3Field( "Zero Rate Bias", data.ZeroBias ); EditorGUI.BeginChangeCheck(); data.OutputFlags = OutputXYZGUI( data.OutputFlags ); if ( EditorGUI.EndChangeCheck() ) From 121d1b545136526ce74d093ba70d651898b0f892 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Thu, 15 Jan 2026 11:58:53 +0100 Subject: [PATCH 10/63] Handed vectors --- AGXUnity/Sensor/ImuSensor.cs | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 917d0602..bf20c5e1 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -203,13 +203,13 @@ protected override bool Initialize() var modifiers = new ITriaxialSignalSystemNodeRefVector(); if ( a.EnableTotalGaussianNoise ) - modifiers.Add(new TriaxialGaussianNoise(a.TotalGaussianNoise.ToVec3())); // TODO handedness + modifiers.Add(new TriaxialGaussianNoise(a.TotalGaussianNoise.ToHandedVec3())); if ( a.EnableSignalScaling ) - modifiers.Add(new TriaxialSignalScaling(a.SignalScaling.ToVec3())); + modifiers.Add(new TriaxialSignalScaling(a.SignalScaling.ToHandedVec3())); if ( a.EnableGaussianSpectralNoise ) - modifiers.Add(new TriaxialSpectralGaussianNoise(a.GaussianSpectralNoise.ToVec3()) ); + modifiers.Add(new TriaxialSpectralGaussianNoise(a.GaussianSpectralNoise.ToHandedVec3()) ); if ( a.EnableLinearAccelerationEffects && a.Type == ImuAttachment.ImuAttachmentType.Gyroscope ) - modifiers.Add( new GyroscopeLinearAccelerationEffects(a.GaussianSpectralNoise.ToVec3()) ); + modifiers.Add( new GyroscopeLinearAccelerationEffects(a.GaussianSpectralNoise.ToHandedVec3()) ); return modifiers; }; @@ -222,7 +222,7 @@ protected override bool Initialize() var accelerometer = new AccelerometerModel( AccelerometerAttachment.TriaxialRange.GenerateTriaxialRange(), new TriaxialCrossSensitivity( AccelerometerAttachment.CrossAxisSensitivity ), - AccelerometerAttachment.ZeroBias.ToVec3(), + AccelerometerAttachment.ZeroBias.ToHandedVec3(), modifiers ); @@ -236,7 +236,7 @@ protected override bool Initialize() var gyroscope = new GyroscopeModel( GyroscopeAttachment.TriaxialRange.GenerateTriaxialRange(), new TriaxialCrossSensitivity( GyroscopeAttachment.CrossAxisSensitivity ), - GyroscopeAttachment.ZeroBias.ToVec3(), + GyroscopeAttachment.ZeroBias.ToHandedVec3(), modifiers ); @@ -251,7 +251,7 @@ protected override bool Initialize() magnetometer = new MagnetometerModel( MagnetometerAttachment.TriaxialRange.GenerateTriaxialRange(), new TriaxialCrossSensitivity( MagnetometerAttachment.CrossAxisSensitivity ), - MagnetometerAttachment.ZeroBias.ToVec3(), + MagnetometerAttachment.ZeroBias.ToHandedVec3(), modifiers ); From aea589aa09a0bd42f715d333423258f8235f2e70 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Thu, 15 Jan 2026 16:20:05 +0100 Subject: [PATCH 11/63] Ununcomment --- AGXUnity/Sensor/SensorEnvironment.cs | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/AGXUnity/Sensor/SensorEnvironment.cs b/AGXUnity/Sensor/SensorEnvironment.cs index e2aab7d7..34f6d070 100644 --- a/AGXUnity/Sensor/SensorEnvironment.cs +++ b/AGXUnity/Sensor/SensorEnvironment.cs @@ -40,7 +40,7 @@ public enum MagneticFieldType /// [Tooltip("Show log messages on each thing added to the sensor environment")] public bool DebugLogOnAdd = false; -/* + [InspectorGroupBegin(Name = "Magnetic Field", DefaultExpanded = true)] /// @@ -78,7 +78,7 @@ public enum MagneticFieldType public Vector3 DipoleCenter = new Vector3( 1.9e-10f, 20.79e3f, -6.369e6f ); [InspectorGroupEnd] -*/ + /// /// Select which layers to include game objects from /// @@ -503,16 +503,16 @@ protected override bool Initialize() FindValidComponents( true ).ForEach( c => TrackIfSupported( c ) ); -// switch ( FieldType ) { -// case MagneticFieldType.UNIFORM: -// Native.setMagneticField( new UniformMagneticField( MagneticFieldVector.ToHandedVec3() ) ); -// break; -// case MagneticFieldType.DIPOLE: -// Native.setMagneticField( new DipoleMagneticField( MagneticMoment.ToHandedVec3(), DipoleCenter.ToHandedVec3() ) ); -// break; -// default: -// break; -// } + switch ( FieldType ) { + case MagneticFieldType.UNIFORM: + Native.setMagneticField( new UniformMagneticField( MagneticFieldVector.ToHandedVec3() ) ); + break; + case MagneticFieldType.DIPOLE: + Native.setMagneticField( new DipoleMagneticField( MagneticMoment.ToHandedVec3(), DipoleCenter.ToHandedVec3() ) ); + break; + default: + break; + } UpdateEnvironment(); From 2ea12d481dc8fcbb1ed0127cecbd2387e01f08a4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Thu, 15 Jan 2026 17:49:00 +0100 Subject: [PATCH 12/63] Neater runtime output and cleanup --- AGXUnity/Sensor/ImuSensor.cs | 59 +++++++++++-------- .../InvokeWrapperInspectorDrawer.cs | 8 +-- 2 files changed, 38 insertions(+), 29 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index bf20c5e1..31bf3014 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -163,8 +163,6 @@ public class ImuSensor : ScriptComponent 0.01f, Vector3.one * 0f ); - [RuntimeValue("m/s")] public int test = 3; - /// /// Local sensor rotation relative to the parent GameObject transform. /// @@ -187,17 +185,27 @@ public class ImuSensor : ScriptComponent /// public UnityEngine.Matrix4x4 GlobalTransform => transform.localToWorldMatrix * LocalTransform; - // TODO tidy up etc - public RigidBody MeasuredBody = null; - //TODO probably move to own class - private uint m_outputID = 0; // Must be greater than 0 to be valid + [RuntimeValue] public RigidBody TrackedRigidBody; + [RuntimeValue] public Vector3 OutputRow1; + [RuntimeValue] public Vector3 OutputRow2; + [RuntimeValue] public Vector3 OutputRow3; + + private uint m_outputID = 0; private double[] m_outputBuffer; protected override bool Initialize() { SensorEnvironment.Instance.GetInitialized(); + var rigidBody = GetComponentInParent(); + if (rigidBody == null) + { + Debug.LogWarning( "No Rigidbody found in this object or parents, IMU will be inactive" ); + return true; + } + TrackedRigidBody = rigidBody; + Func buildModifiers = a => { var modifiers = new ITriaxialSignalSystemNodeRefVector(); @@ -259,7 +267,7 @@ protected override bool Initialize() } if ( imu_attachments.Count == 0 ) { - Debug.LogWarning( "No sensor attachments on IMU means the component will do nothing" ); + Debug.LogWarning( "No sensor attachments on IMU means the component will be inactive" ); return true; } @@ -271,18 +279,22 @@ protected override bool Initialize() m_nativeModel = new IMUModel( imu_attachments ); if ( m_nativeModel == null ) - return false; // TODO error + { + Debug.LogWarning( "Could not create native imu model, component will be inactive" ); + return true; + } PropertySynchronizer.Synchronize( this ); // TODO moveme to function that can get called both when setting RB and from here - var measuredRB = MeasuredBody.GetInitialized().Native; + var measuredRB = rigidBody.GetInitialized().Native; SensorEnvironment.Instance.Native.add( measuredRB ); var rbFrame = measuredRB.getFrame(); - if ( rbFrame == null ) - Debug.LogError( "Need RB to follow" ); - + if ( rbFrame == null ) + { + Debug.LogWarning( "Could not get rigid body frame" ); + } Native = new IMU( rbFrame, m_nativeModel ); // For SWIG reasons, we will create a ninedof output and use the fields selectively @@ -311,13 +323,6 @@ public void GetOutput() if ( Native == null ) return; - //var imuOutput = Native.getOutputHandler().get(m_outputID); - //Debug.Log( "getElementSize: " + imuOutput.getElementSize() + ", hasUnreadData: " + imuOutput.hasUnreadData()); - //Debug.Log( "Tri: " + imuOutput.viewTriaxialXYZ().size() ); - //Debug.Log( "Six: " + imuOutput.viewSixDoF().size() ); - //Debug.Log( "Nine: " + imuOutput.viewNineDoF().size() ); - //return; - NineDoFValue view = Native.getOutputHandler().get(m_outputID).viewNineDoF()[0]; // This is all kind of a workaround to use a ninedof buffer with an arbitrary number @@ -355,16 +360,20 @@ public void GetOutput() } } - // TODO removeme used for testing + // TODO where do we update runtime values? private void OnPostStepForward() { GetOutput(); - string output = ""; - for ( int i = 0; i < m_outputBuffer.Length; i++ ) - output += m_outputBuffer[ i ].ToString() + " "; - - Debug.Log( "Output: " + output ); + // If using runtime values + for ( int i = 0; i < m_outputBuffer.Length; i++ ) { + if ( i < 3 ) + OutputRow1[ i ] = (float)m_outputBuffer[ i ]; + else if ( i < 6 ) + OutputRow2[ i % 3 ] = (float)m_outputBuffer[ i ]; + else + OutputRow3[ i % 6 ] = (float)m_outputBuffer[ i ]; + } } protected override void OnEnable() diff --git a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs index 6bcd432c..db413a6a 100644 --- a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs +++ b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs @@ -1514,23 +1514,23 @@ public static object ImuAttachmentDrawer( object[] objects, InvokeWrapper wrappe data.EnableTotalGaussianNoise, data.TotalGaussianNoise, "Total Gaussian Noise", - "Tooltip todo" ); + "" ); ( data.EnableSignalScaling, data.SignalScaling ) = OptionalVector3GUI( data.EnableSignalScaling, data.SignalScaling, "Signal Scaling", - "Tooltip todo" ); + "" ); ( data.EnableGaussianSpectralNoise, data.GaussianSpectralNoise ) = OptionalVector3GUI( data.EnableGaussianSpectralNoise, data.GaussianSpectralNoise, "Gaussian Spectral Noise", - "Tooltip todo" ); + "" ); if ( data.Type == ImuAttachment.ImuAttachmentType.Gyroscope ) { ( data.EnableLinearAccelerationEffects, data.LinearAccelerationEffects ) = OptionalVector3GUI( data.EnableLinearAccelerationEffects, data.LinearAccelerationEffects, "Linear Acceleration Effects", - "Tooltip todo" ); + "" ); } } } From 80cd1331b8dc0477b50ad9e9175d87ca721c023d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Fri, 16 Jan 2026 10:32:20 +0100 Subject: [PATCH 13/63] Output adjustments --- AGXUnity/Sensor/ImuSensor.cs | 52 +++++++++++++++++------------------- 1 file changed, 24 insertions(+), 28 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 31bf3014..f285d847 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -192,7 +192,7 @@ public class ImuSensor : ScriptComponent [RuntimeValue] public Vector3 OutputRow3; private uint m_outputID = 0; - private double[] m_outputBuffer; + public double[] OutputBuffer { get; private set; } protected override bool Initialize() { @@ -271,11 +271,6 @@ protected override bool Initialize() return true; } - if ( imu_attachments.Count == 1 ) { - Debug.LogWarning( "KNOWN BUG: currently two sensors are needed for output to work properly! Suggested workaround: Enable another one and disable output" ); - return true; - } - m_nativeModel = new IMUModel( imu_attachments ); if ( m_nativeModel == null ) @@ -309,7 +304,7 @@ protected override bool Initialize() Native.getOutputHandler().add( m_outputID, output ); Debug.Log( "Enabled field count: " + outputCount ); // TODO removeme - m_outputBuffer = new double[ outputCount ]; + OutputBuffer = new double[ outputCount ]; Simulation.Instance.StepCallbacks.PostStepForward += OnPostStepForward; @@ -318,12 +313,12 @@ protected override bool Initialize() return true; } - public void GetOutput() + public void GetOutput(IMUOutput output) { if ( Native == null ) return; - NineDoFValue view = Native.getOutputHandler().get(m_outputID).viewNineDoF()[0]; + NineDoFValue view = output.viewNineDoF()[0]; // This is all kind of a workaround to use a ninedof buffer with an arbitrary number // of doubles read based on settings. If Native isn't null we have at least one sensor. @@ -338,41 +333,42 @@ public void GetOutput() if ( EnableAccelerometer ) { var a = AccelerometerAttachment.OutputFlags; - if ( ( a & OutputXYZ.X ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].x; - if ( ( a & OutputXYZ.Y ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].y; - if ( ( a & OutputXYZ.Z ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].z; + if ( ( a & OutputXYZ.X ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].x; + if ( ( a & OutputXYZ.Y ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].y; + if ( ( a & OutputXYZ.Z ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].z; j++; } if ( EnableGyroscope ) { var g = GyroscopeAttachment.OutputFlags; - if ( ( g & OutputXYZ.X ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].x; - if ( ( g & OutputXYZ.Y ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].y; - if ( ( g & OutputXYZ.Z ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].z; + if ( ( g & OutputXYZ.X ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].x; + if ( ( g & OutputXYZ.Y ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].y; + if ( ( g & OutputXYZ.Z ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].z; j++; } if ( EnableMagnetometer ) { var m = MagnetometerAttachment.OutputFlags; - if ( ( m & OutputXYZ.X ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].x; - if ( ( m & OutputXYZ.Y ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].y; - if ( ( m & OutputXYZ.Z ) != 0 ) m_outputBuffer[ i++ ] = triplets[ j ].z; + if ( ( m & OutputXYZ.X ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].x; + if ( ( m & OutputXYZ.Y ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].y; + if ( ( m & OutputXYZ.Z ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].z; } } // TODO where do we update runtime values? private void OnPostStepForward() { - GetOutput(); - - // If using runtime values - for ( int i = 0; i < m_outputBuffer.Length; i++ ) { - if ( i < 3 ) - OutputRow1[ i ] = (float)m_outputBuffer[ i ]; - else if ( i < 6 ) - OutputRow2[ i % 3 ] = (float)m_outputBuffer[ i ]; - else - OutputRow3[ i % 6 ] = (float)m_outputBuffer[ i ]; + GetOutput(Native.getOutputHandler().get(m_outputID)); + + if ( Application.isEditor ) { + for ( int i = 0; i < OutputBuffer.Length; i++ ) { + if ( i < 3 ) + OutputRow1[ i ] = (float)OutputBuffer[ i ]; + else if ( i < 6 ) + OutputRow2[ i % 3 ] = (float)OutputBuffer[ i ]; + else + OutputRow3[ i % 6 ] = (float)OutputBuffer[ i ]; + } } } From b492876ff7a69e9c3d64f1b4754a189e992038ab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Fri, 16 Jan 2026 15:34:18 +0100 Subject: [PATCH 14/63] Draw halfsphere gizmo --- AGXUnity/Sensor/ImuSensor.cs | 32 ++++++++++++++------------------ 1 file changed, 14 insertions(+), 18 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index f285d847..b70b2eb5 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -392,8 +392,6 @@ protected override void OnDestroy() Simulation.Instance.StepCallbacks.PostStepForward -= OnPostStepForward; } - //TODO remove modules and modifiers, possibly - Native?.Dispose(); Native = null; m_nativeModel?.Dispose(); @@ -402,28 +400,26 @@ protected override void OnDestroy() base.OnDestroy(); } - //TODO draw something useful + + [NonSerialized] + private Mesh m_nodeGizmoMesh = null; + private void OnDrawGizmosSelected() { #if UNITY_EDITOR var xform = GlobalTransform; - var pos = xform.GetPosition(); - var scale = UnityEditor.HandleUtility.GetHandleSize(pos) * 1.5f; - Gizmos.DrawLine( pos, xform.MultiplyPoint( Vector3.forward * scale ) ); // Up - Gizmos.DrawLine( pos, xform.MultiplyPoint( Vector3.left * scale ) ); - - int numPoints = 25; - Vector3[] disc = new Vector3[numPoints]; - - Vector3 x = xform.MultiplyVector(Vector3.right * scale); - Vector3 y = xform.MultiplyVector(Vector3.up * scale); + if ( m_nodeGizmoMesh == null ) + m_nodeGizmoMesh = Resources.Load( @"Debug/Models/HalfSphere" ); + + if ( m_nodeGizmoMesh == null ) + return; - for ( int i = 0; i < numPoints; i++ ) { - float ang = Mathf.PI * 2 * i / numPoints; - disc[ i ] = pos + x * Mathf.Cos( ang ) + y * Mathf.Sin( ang ); - } - Gizmos.DrawLineStrip( disc, true ); + Gizmos.color = Color.yellow; + Gizmos.DrawWireMesh( m_nodeGizmoMesh, + xform.GetPosition(), + xform.GetRotation(), + Vector3.one * 0.2f ); #endif } } From ae42a09c2077be85c7ae7776eccc9ed9721cd0cd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Fri, 16 Jan 2026 15:36:15 +0100 Subject: [PATCH 15/63] Remove local position / rotation offset --- AGXUnity/Sensor/ImuSensor.cs | 29 ++--------------------------- 1 file changed, 2 insertions(+), 27 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index b70b2eb5..7ac2f066 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -163,29 +163,6 @@ public class ImuSensor : ScriptComponent 0.01f, Vector3.one * 0f ); - /// - /// Local sensor rotation relative to the parent GameObject transform. - /// - [Tooltip("Local sensor rotation relative to the parent GameObject transform.")] - public Vector3 LocalRotation = Vector3.zero; - - /// - /// Local sensor offset relative to the parent GameObject transform. - /// - [Tooltip("Local sensor offset relative to the parent GameObject transform.")] - public Vector3 LocalPosition = Vector3.zero; - - /// - /// The local transformation matrix from the sensor frame to the parent GameObject frame - /// - public UnityEngine.Matrix4x4 LocalTransform => UnityEngine.Matrix4x4.TRS( LocalPosition, Quaternion.Euler( LocalRotation ), Vector3.one ); - - /// - /// The global transformation matrix from the sensor frame to the world frame. - /// - public UnityEngine.Matrix4x4 GlobalTransform => transform.localToWorldMatrix * LocalTransform; - - [RuntimeValue] public RigidBody TrackedRigidBody; [RuntimeValue] public Vector3 OutputRow1; [RuntimeValue] public Vector3 OutputRow2; @@ -407,8 +384,6 @@ protected override void OnDestroy() private void OnDrawGizmosSelected() { #if UNITY_EDITOR - var xform = GlobalTransform; - if ( m_nodeGizmoMesh == null ) m_nodeGizmoMesh = Resources.Load( @"Debug/Models/HalfSphere" ); @@ -417,8 +392,8 @@ private void OnDrawGizmosSelected() Gizmos.color = Color.yellow; Gizmos.DrawWireMesh( m_nodeGizmoMesh, - xform.GetPosition(), - xform.GetRotation(), + transform.position, + transform.rotation, Vector3.one * 0.2f ); #endif } From 1f9aa2cf0f22d2e9124bb0280bbb8aae37c564cc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Fri, 16 Jan 2026 15:43:28 +0100 Subject: [PATCH 16/63] Fix some todos --- AGXUnity/Sensor/ImuSensor.cs | 36 +++++++++++++++------------------- AGXUnity/Sensor/LidarSensor.cs | 2 +- 2 files changed, 17 insertions(+), 21 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 7ac2f066..e5392207 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -16,7 +16,7 @@ public enum OutputXYZ Z = 1 << 2, } [Serializable] - // TODO probably move this to inside IMU comp + // Data class to store IMU sensor attachment configuration public class ImuAttachment { public enum ImuAttachmentType @@ -94,7 +94,7 @@ public ImuAttachment( ImuAttachmentType type, TriaxialRangeData triaxialRange, f /// [DisallowMultipleComponent] [AddComponentMenu( "AGXUnity/Sensors/IMU Sensor" )] - //[HelpURL( "https://us.download.algoryx.se/AGXUnity/documentation/current/editor_interface.html#simulating-lidar-sensors" )] + [HelpURL( "https://us.download.algoryx.se/AGXUnity/documentation/current/editor_interface.html#simulating-imu-sensors" )] public class ImuSensor : ScriptComponent { /// @@ -112,7 +112,7 @@ public class ImuSensor : ScriptComponent public bool EnableAccelerometer { get; set; } = true; /// - /// Accelerometer TODO + /// Accelerometer sensor attachment /// [field: SerializeField] [DynamicallyShowInInspector( "EnableAccelerometer" )] @@ -132,7 +132,7 @@ public class ImuSensor : ScriptComponent public bool EnableGyroscope { get; set; } = true; /// - /// Gyroscope TODO + /// Gyroscope sensor attachment /// [field: SerializeField] [DynamicallyShowInInspector( "EnableGyroscope" )] @@ -152,7 +152,7 @@ public class ImuSensor : ScriptComponent public bool EnableMagnetometer { get; set; } = true; /// - /// Magnetometer TODO + /// Magnetometer sensor attachment /// [field: SerializeField] [DynamicallyShowInInspector( "EnableMagnetometer" )] @@ -258,7 +258,6 @@ protected override bool Initialize() PropertySynchronizer.Synchronize( this ); - // TODO moveme to function that can get called both when setting RB and from here var measuredRB = rigidBody.GetInitialized().Native; SensorEnvironment.Instance.Native.add( measuredRB ); @@ -277,10 +276,8 @@ protected override bool Initialize() outputCount += EnableMagnetometer ? Utils.Math.PopCount( (uint)MagnetometerAttachment.OutputFlags ) : 0; var output = new IMUOutputNineDoF(); - // output.setPaddingValue( 0 ); // TODO remove all thoughts of padding Native.getOutputHandler().add( m_outputID, output ); - Debug.Log( "Enabled field count: " + outputCount ); // TODO removeme OutputBuffer = new double[ outputCount ]; Simulation.Instance.StepCallbacks.PostStepForward += OnPostStepForward; @@ -290,7 +287,7 @@ protected override bool Initialize() return true; } - public void GetOutput(IMUOutput output) + public void GetOutput(IMUOutput output, double[] buffer) { if ( Native == null ) return; @@ -310,32 +307,31 @@ public void GetOutput(IMUOutput output) if ( EnableAccelerometer ) { var a = AccelerometerAttachment.OutputFlags; - if ( ( a & OutputXYZ.X ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].x; - if ( ( a & OutputXYZ.Y ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].y; - if ( ( a & OutputXYZ.Z ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].z; + if ( ( a & OutputXYZ.X ) != 0 ) buffer[ i++ ] = triplets[ j ].x; + if ( ( a & OutputXYZ.Y ) != 0 ) buffer[ i++ ] = triplets[ j ].y; + if ( ( a & OutputXYZ.Z ) != 0 ) buffer[ i++ ] = triplets[ j ].z; j++; } if ( EnableGyroscope ) { var g = GyroscopeAttachment.OutputFlags; - if ( ( g & OutputXYZ.X ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].x; - if ( ( g & OutputXYZ.Y ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].y; - if ( ( g & OutputXYZ.Z ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].z; + if ( ( g & OutputXYZ.X ) != 0 ) buffer[ i++ ] = triplets[ j ].x; + if ( ( g & OutputXYZ.Y ) != 0 ) buffer[ i++ ] = triplets[ j ].y; + if ( ( g & OutputXYZ.Z ) != 0 ) buffer[ i++ ] = triplets[ j ].z; j++; } if ( EnableMagnetometer ) { var m = MagnetometerAttachment.OutputFlags; - if ( ( m & OutputXYZ.X ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].x; - if ( ( m & OutputXYZ.Y ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].y; - if ( ( m & OutputXYZ.Z ) != 0 ) OutputBuffer[ i++ ] = triplets[ j ].z; + if ( ( m & OutputXYZ.X ) != 0 ) buffer[ i++ ] = triplets[ j ].x; + if ( ( m & OutputXYZ.Y ) != 0 ) buffer[ i++ ] = triplets[ j ].y; + if ( ( m & OutputXYZ.Z ) != 0 ) buffer[ i++ ] = triplets[ j ].z; } } - // TODO where do we update runtime values? private void OnPostStepForward() { - GetOutput(Native.getOutputHandler().get(m_outputID)); + GetOutput(Native.getOutputHandler().get(m_outputID), OutputBuffer); if ( Application.isEditor ) { for ( int i = 0; i < OutputBuffer.Length; i++ ) { diff --git a/AGXUnity/Sensor/LidarSensor.cs b/AGXUnity/Sensor/LidarSensor.cs index 87eee6df..2bdbe5dc 100644 --- a/AGXUnity/Sensor/LidarSensor.cs +++ b/AGXUnity/Sensor/LidarSensor.cs @@ -80,7 +80,7 @@ public enum LidarModelPreset } /// - /// WIP component for lidar sensor + /// Lidar Sensor Component /// [DisallowMultipleComponent] [AddComponentMenu( "AGXUnity/Sensors/Lidar Sensor" )] From 886501f975b6b5c72b1929108d646a91dec89abf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Fri, 16 Jan 2026 16:36:24 +0100 Subject: [PATCH 17/63] Bug hunt --- AGXUnity/Sensor/ImuSensor.cs | 36 +++++++++++++++++++++++++----------- 1 file changed, 25 insertions(+), 11 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index e5392207..674a11af 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -29,6 +29,7 @@ public enum ImuAttachmentType /// /// Accelerometer / Gyroscope / Magnetometer /// + [SerializeField] public ImuAttachmentType Type { get; private set; } /// @@ -82,7 +83,7 @@ public enum ImuAttachmentType // Constructor enables us to set different default values per sensor type public ImuAttachment( ImuAttachmentType type, TriaxialRangeData triaxialRange, float crossAxisSensitivity, Vector3 zeroRateBias ) { - this.Type = type; + Type = type; TriaxialRange = triaxialRange; CrossAxisSensitivity = crossAxisSensitivity; ZeroBias = zeroRateBias; @@ -101,7 +102,7 @@ public class ImuSensor : ScriptComponent /// Native instance, created in Start/Initialize. /// public IMU Native { get; private set; } = null; - public IMUModel m_nativeModel = null; + private IMUModel m_nativeModel = null; /// /// When enabled, show configuration for the IMU attachment and create attachment when initializing object @@ -179,7 +180,7 @@ protected override bool Initialize() if (rigidBody == null) { Debug.LogWarning( "No Rigidbody found in this object or parents, IMU will be inactive" ); - return true; + return false; } TrackedRigidBody = rigidBody; @@ -194,7 +195,7 @@ protected override bool Initialize() if ( a.EnableGaussianSpectralNoise ) modifiers.Add(new TriaxialSpectralGaussianNoise(a.GaussianSpectralNoise.ToHandedVec3()) ); if ( a.EnableLinearAccelerationEffects && a.Type == ImuAttachment.ImuAttachmentType.Gyroscope ) - modifiers.Add( new GyroscopeLinearAccelerationEffects(a.GaussianSpectralNoise.ToHandedVec3()) ); + modifiers.Add( new GyroscopeLinearAccelerationEffects(a.LinearAccelerationEffects.ToHandedVec3()) ); return modifiers; }; @@ -244,16 +245,16 @@ protected override bool Initialize() } if ( imu_attachments.Count == 0 ) { - Debug.LogWarning( "No sensor attachments on IMU means the component will be inactive" ); - return true; + Debug.LogWarning( "No sensor attachments, IMU will be inactive" ); + return false; } m_nativeModel = new IMUModel( imu_attachments ); if ( m_nativeModel == null ) { - Debug.LogWarning( "Could not create native imu model, component will be inactive" ); - return true; + Debug.LogWarning( "Could not create native imu model, IMU will be inactive" ); + return false; } PropertySynchronizer.Synchronize( this ); @@ -264,7 +265,8 @@ protected override bool Initialize() var rbFrame = measuredRB.getFrame(); if ( rbFrame == null ) { - Debug.LogWarning( "Could not get rigid body frame" ); + Debug.LogWarning( "Could not get rigid body frame, IMU will be inactive" ); + return false; } Native = new IMU( rbFrame, m_nativeModel ); @@ -289,10 +291,19 @@ protected override bool Initialize() public void GetOutput(IMUOutput output, double[] buffer) { - if ( Native == null ) + if ( Native == null || buffer == null || output == null ) { + Debug.LogError( "Null problem" ); + // output.viewNineDoF()[0]; return; + } + + NineDoFView views = output.viewNineDoF(); + if ( views == null ) { + Debug.LogWarning( "No views" ); + return; + } - NineDoFValue view = output.viewNineDoF()[0]; + NineDoFValue view = views[ 0 ]; // This is all kind of a workaround to use a ninedof buffer with an arbitrary number // of doubles read based on settings. If Native isn't null we have at least one sensor. @@ -331,6 +342,9 @@ public void GetOutput(IMUOutput output, double[] buffer) private void OnPostStepForward() { + if ( !gameObject.activeInHierarchy ) + return; + GetOutput(Native.getOutputHandler().get(m_outputID), OutputBuffer); if ( Application.isEditor ) { From 3c752a0d2b89ebed33b473df81ef037882abe07a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Fri, 16 Jan 2026 16:44:33 +0100 Subject: [PATCH 18/63] Cleanup --- AGXUnity/Sensor/ImuSensor.cs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 674a11af..911cefd6 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -230,11 +230,10 @@ protected override bool Initialize() } // Magnetometer - MagnetometerModel magnetometer = null; if ( EnableMagnetometer ) { var modifiers = buildModifiers(MagnetometerAttachment); - magnetometer = new MagnetometerModel( + var magnetometer = new MagnetometerModel( MagnetometerAttachment.TriaxialRange.GenerateTriaxialRange(), new TriaxialCrossSensitivity( MagnetometerAttachment.CrossAxisSensitivity ), MagnetometerAttachment.ZeroBias.ToHandedVec3(), From 200996cf9da78d6d7033eb47ee0126102d297673 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 20 Jan 2026 11:31:14 +0100 Subject: [PATCH 19/63] Odometer --- AGXUnity/Sensor/OdometerSensor.cs | 241 ++++++++++++++++++ AGXUnity/Sensor/OdometerSensor.cs.meta | 2 + .../AGXUnity+Sensor+OdometerSensorEditor.cs | 9 + ...XUnity+Sensor+OdometerSensorEditor.cs.meta | 2 + 4 files changed, 254 insertions(+) create mode 100644 AGXUnity/Sensor/OdometerSensor.cs create mode 100644 AGXUnity/Sensor/OdometerSensor.cs.meta create mode 100644 Editor/CustomEditors/AGXUnity+Sensor+OdometerSensorEditor.cs create mode 100644 Editor/CustomEditors/AGXUnity+Sensor+OdometerSensorEditor.cs.meta diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs new file mode 100644 index 00000000..03aa0aaa --- /dev/null +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -0,0 +1,241 @@ +using agxSensor; +using AGXUnity.Utils; +using UnityEngine; +using System; + +namespace AGXUnity.Sensor +{ + /// + /// Odometer Sensor Component - measures distance based on constraint value + /// + [DisallowMultipleComponent] + [AddComponentMenu( "AGXUnity/Sensors/Odometer Sensor" )] + [HelpURL( "https://www.algoryx.se/documentation/complete/agx/tags/latest/doc/UserManual/source/agxsensor.html#odometer" )] + public class OdometerSensor : ScriptComponent + { + /// + /// Native instance + /// + public Odometer Native { get; private set; } = null; + private OdometerModel m_nativeModel = null; + + /// + /// Optional: Explicitly assign the constraint component to attach the odometer to. + /// If left empty the component will use the first compatible parent. + /// Compatible with: Hinge, CylindricalJoint, SplineJoint, WheelJoint, SlackHingeJoint, SlackCylindricalJoint + /// + [field: SerializeField] + [Tooltip( "Constraint component to attach odometer to: Hinge, CylindricalJoint, SplineJoint, WheelJoint, slackjoints. If unset, the first compatible parent is used." )] + [DisableInRuntimeInspector] + public Constraint ConstraintComponent { get; set; } = null; + + /// + /// Wheel radius in meters + /// + [field: SerializeField] + [Tooltip( "Wheel radius in meters" )] + [DisableInRuntimeInspector] + [ClampAboveZeroInInspector] + public float WheelRadius { get; set; } = 0.5f; + + [field: SerializeField] + [DisableInRuntimeInspector] + public bool EnableTotalGaussianNoise { get; set; } = false; + /// + /// Noise RMS in meters. + /// + [field: SerializeField] + [Tooltip( "Gaussian noise RMS" )] + [DynamicallyShowInInspector( "EnableTotalGaussianNoise" )] + [DisableInRuntimeInspector] + public float TotalGaussianNoiseRms { get; set; } = 0.0f; + + [field: SerializeField] + [DisableInRuntimeInspector] + public bool EnableSignalResolution { get; set; } = false; + /// + /// Pulses per wheel revolution. + /// + [field: SerializeField] + [Tooltip( "Pulses per wheel revolution" )] + [DynamicallyShowInInspector( "EnableSignalResolution" )] + [DisableInRuntimeInspector] + [ClampAboveZeroInInspector] + public int PulsesPerRevolution { get; set; } = 1024; + + [field: SerializeField] + [DisableInRuntimeInspector] + public bool EnableSignalScaling { get; set; } = false; + /// + /// Constant scaling factor. + /// + [field: SerializeField] + [Tooltip( "Scaling factor applied to the distance signal" )] + [DynamicallyShowInInspector( "EnableSignalScaling" )] + [DisableInRuntimeInspector] + public float SignalScaling { get; set; } = 1.0f; + + [RuntimeValue] public float SensorValue { get; private set; } + + private uint m_outputID = 0; + [HideInInspector] + public double OutputBuffer { get; private set; } + + protected override bool Initialize() + { + SensorEnvironment.Instance.GetInitialized(); + + if ( WheelRadius <= 0.0f ) { + Debug.LogWarning( "Invalid WheelRadius, odometer will be inactive" ); + return false; + } + + // Find a constraint component if not explicitly assigned + if ( ConstraintComponent == null ) { + ConstraintComponent = GetComponentInParent(); + } + + if ( ConstraintComponent == null ) { + Debug.LogWarning( "No constraint component found/assigned, odometer will be inactive" ); + return false; + } + + var modifiers = new IMonoaxialSignalSystemNodeRefVector(); + + if ( EnableTotalGaussianNoise ) + modifiers.Add( new MonoaxialGaussianNoise( (double)TotalGaussianNoiseRms ) ); + + if ( EnableSignalResolution ) + modifiers.Add( new MonoaxialSignalResolution( GetSignalResolution() ) ); + + if ( EnableSignalScaling ) + modifiers.Add( new MonoaxialSignalScaling( (double)SignalScaling ) ); + + m_nativeModel = new OdometerModel( (double)WheelRadius, modifiers ); + + if ( m_nativeModel == null ) { + Debug.LogWarning( "Could not create native odometer model, odometer will be inactive" ); + return false; + } + + PropertySynchronizer.Synchronize( this ); + + var initializedConstraint = ConstraintComponent.GetInitialized(); + if ( initializedConstraint == null ) { + Debug.LogWarning( "Constraint component not initializable, odometer will be inactive" ); + return false; + } + + Native = CreateNativeOdometerFromConstraint( initializedConstraint.Native, m_nativeModel ); + if ( Native == null ) { + Debug.LogWarning( "Unsupported constraint type for odometer, odometer will be inactive" ); + return false; + } + + m_outputID = SensorEnvironment.Instance.GenerateOutputID(); + var output = new OdometerOutputDistance(); + Native.getOutputHandler().add( m_outputID, output ); + + Simulation.Instance.StepCallbacks.PostStepForward += OnPostStepForward; + + SensorEnvironment.Instance.Native.add( Native ); + + return true; + } + + private double GetSignalResolution() + { + return 2.0 * System.Math.PI * WheelRadius / PulsesPerRevolution; + } + + private static Odometer CreateNativeOdometerFromConstraint( agx.Constraint nativeConstraint, OdometerModel model ) + { + if ( nativeConstraint is agx.Hinge hinge ) + return new Odometer( hinge, model ); + + if ( nativeConstraint is agxVehicle.WheelJoint wheel ) + return new Odometer( wheel, model ); + + if ( nativeConstraint is agx.CylindricalJoint cylindrical ) + return new Odometer( cylindrical, model ); + + if ( nativeConstraint is agx.SplineJoint spline ) + return new Odometer( spline, model ); + + if ( nativeConstraint is agx.SlackHingeJoint slackHinge ) + return new Odometer( slackHinge, model ); + + if ( nativeConstraint is agx.SlackCylindricalJoint slackCylindrical ) + return new Odometer( slackCylindrical, model ); + + return null; + } + + public double GetOutput( OdometerOutput output ) + { + if ( Native == null || output == null ) + return 0; + + var view = output.view(); + return view[ 0 ]; + } + + private void OnPostStepForward() + { + if ( !gameObject.activeInHierarchy ) + return; + + OutputBuffer = GetOutput( Native.getOutputHandler().get( m_outputID ) ); + + // Convenience runtime display of output + SensorValue = (float)OutputBuffer; + } + + protected override void OnEnable() + { + Native?.setEnable( true ); + } + + protected override void OnDisable() + { + Native?.setEnable( false ); + } + + protected override void OnDestroy() + { + if ( SensorEnvironment.HasInstance ) + SensorEnvironment.Instance.Native?.remove( Native ); + + if ( Simulation.HasInstance ) + Simulation.Instance.StepCallbacks.PostStepForward -= OnPostStepForward; + + Native?.Dispose(); + Native = null; + + m_nativeModel?.Dispose(); + m_nativeModel = null; + + base.OnDestroy(); + } + + [NonSerialized] + private Mesh m_nodeGizmoMesh = null; + + private void OnDrawGizmosSelected() + { +#if UNITY_EDITOR + if ( m_nodeGizmoMesh == null ) + m_nodeGizmoMesh = Resources.Load( @"Debug/Models/Icosahedron" ); + + if ( m_nodeGizmoMesh == null ) + return; + + Gizmos.color = Color.yellow; + Gizmos.DrawWireMesh( m_nodeGizmoMesh, + transform.position, + transform.rotation, + Vector3.one * 0.2f ); +#endif + } + } +} diff --git a/AGXUnity/Sensor/OdometerSensor.cs.meta b/AGXUnity/Sensor/OdometerSensor.cs.meta new file mode 100644 index 00000000..077cb336 --- /dev/null +++ b/AGXUnity/Sensor/OdometerSensor.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 23f58075b197b564ead6633c7f209368 \ No newline at end of file diff --git a/Editor/CustomEditors/AGXUnity+Sensor+OdometerSensorEditor.cs b/Editor/CustomEditors/AGXUnity+Sensor+OdometerSensorEditor.cs new file mode 100644 index 00000000..a9b4202b --- /dev/null +++ b/Editor/CustomEditors/AGXUnity+Sensor+OdometerSensorEditor.cs @@ -0,0 +1,9 @@ +using UnityEditor; + +namespace AGXUnityEditor.Editors +{ + [CustomEditor( typeof( AGXUnity.Sensor.OdometerSensor ) )] + [CanEditMultipleObjects] + public class AGXUnitySensorOdometerSensorEditor : InspectorEditor + { } +} diff --git a/Editor/CustomEditors/AGXUnity+Sensor+OdometerSensorEditor.cs.meta b/Editor/CustomEditors/AGXUnity+Sensor+OdometerSensorEditor.cs.meta new file mode 100644 index 00000000..e0b7e7b6 --- /dev/null +++ b/Editor/CustomEditors/AGXUnity+Sensor+OdometerSensorEditor.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 6848b7bdc6b8a764cbaaaef2e9e67130 \ No newline at end of file From a216584cb248db357829c6985bbc43dabde80bb7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 20 Jan 2026 11:31:31 +0100 Subject: [PATCH 20/63] Adjustment --- AGXUnity/Sensor/ImuSensor.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 911cefd6..be3edcb7 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -164,7 +164,7 @@ public class ImuSensor : ScriptComponent 0.01f, Vector3.one * 0f ); - [RuntimeValue] public RigidBody TrackedRigidBody; + [RuntimeValue] public RigidBody TrackedRigidBody { get; private set; } [RuntimeValue] public Vector3 OutputRow1; [RuntimeValue] public Vector3 OutputRow2; [RuntimeValue] public Vector3 OutputRow3; From b2556f9e16cad572a6ad7eb09d7434fba6fd0cb2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 20 Jan 2026 14:09:04 +0100 Subject: [PATCH 21/63] Encoder sensor --- AGXUnity/Sensor/EncoderSensor.cs | 345 ++++++++++++++++++ AGXUnity/Sensor/EncoderSensor.cs.meta | 2 + AGXUnity/Sensor/OdometerSensor.cs | 13 +- .../AGXUnity+Sensor+EncoderSensorEditor.cs | 9 + ...GXUnity+Sensor+EncoderSensorEditor.cs.meta | 2 + 5 files changed, 360 insertions(+), 11 deletions(-) create mode 100644 AGXUnity/Sensor/EncoderSensor.cs create mode 100644 AGXUnity/Sensor/EncoderSensor.cs.meta create mode 100644 Editor/CustomEditors/AGXUnity+Sensor+EncoderSensorEditor.cs create mode 100644 Editor/CustomEditors/AGXUnity+Sensor+EncoderSensorEditor.cs.meta diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs new file mode 100644 index 00000000..af264771 --- /dev/null +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -0,0 +1,345 @@ +using agxSensor; +using AGXUnity.Utils; +using UnityEngine; +using System; +using AGXUnity.Model; + +namespace AGXUnity.Sensor +{ + /// + /// Encoder Sensor Component - measures constraint position/speed (on one dof) + /// + [DisallowMultipleComponent] + [AddComponentMenu( "AGXUnity/Sensors/Encoder Sensor" )] + [HelpURL( "https://www.algoryx.se/documentation/complete/agx/tags/latest/doc/UserManual/source/agxsensor.html#encoder" )] + public class EncoderSensor : ScriptComponent + { + /// + /// Native instance + /// + public Encoder Native { get; private set; } = null; + private EncoderModel m_nativeModel = null; + + /// + /// Optional: Explicitly assign the constraint component to attach the encoder to. + /// If left empty the component will use the first compatible parent. + /// Compatible with: Hinge, Prismatic, CylindricalJoint, WheelJoint + /// + [field: SerializeField] + [Tooltip( "Constraint / WheelJoint component to attach encoder to. If unset, the first compatible parent is used" )] + [DisableInRuntimeInspector] + public ScriptComponent ConstraintComponent { get; set; } = null; + + /// + /// Accumulation mode of the encoder + /// INCREMENTAL wraps within the cycle range, ABSOLUTE is single-turn absolute + /// + [field: SerializeField] + [Tooltip( "Encoder accumulation mode. INCREMENTAL wraps within the cycle range; ABSOLUTE is single-turn absolute" )] + [DisableInRuntimeInspector] + public EncoderModel.Mode Mode { get; set; } = EncoderModel.Mode.INCREMENTAL; + + /// + /// Cycle range [min, max] in sensor units. + /// Rotary encoders typically use [0, 2*pi] radians. + /// Linear encoders typically use a range in meters. + /// + [field: SerializeField] + [Tooltip( "Cycle range [min, max] in sensor units. Rotary: radians (commonly [0, 2π]). Linear: meters." )] + [DisableInRuntimeInspector] + public RangeReal MeasurementRange { get; set; } = new RangeReal( float.MinValue, float.MaxValue ); + + public enum TwoDofSample + { + First, + Second + } + + [field: SerializeField] + [Tooltip( "Which DoF to sample for 2-DoF constraints (Cylindrical)" )] + [DisableInRuntimeInspector] + [DynamicallyShowInInspector( "HasTwoDof", true )] + public TwoDofSample SampleTwoDof { get; set; } = TwoDofSample.First; + + public enum WheelJointSample + { + Steering, + WheelAxle, + Suspension + } + + [field: SerializeField] + [Tooltip( "Which secondary constraint to sample for WheelJoint" )] + [DisableInRuntimeInspector] + [DynamicallyShowInInspector( "IsWheelJoint", true )] + public WheelJointSample SampleWheelJoint { get; set; } = WheelJointSample.WheelAxle; + + /// + /// Output selection + /// + [field: SerializeField] + [Tooltip( "Include position in the output" )] + [DisableInRuntimeInspector] + public bool OutputPosition { get; set; } = true; + + [field: SerializeField] + [Tooltip( "Include speed in the output" )] + [DisableInRuntimeInspector] + public bool OutputSpeed { get; set; } = false; + + [field: SerializeField] + [DisableInRuntimeInspector] + public bool EnableTotalGaussianNoise { get; set; } = false; + + /// + /// Noise RMS in sensor units (position units: radians/meters). + /// + [field: SerializeField] + [Tooltip( "Gaussian noise RMS (position units: radians/meters)." )] + [DynamicallyShowInInspector( "EnableTotalGaussianNoise" )] + [DisableInRuntimeInspector] + public float TotalGaussianNoiseRms { get; set; } = 0.0f; + + [field: SerializeField] + [DisableInRuntimeInspector] + public bool EnableSignalResolution { get; set; } = false; + + /// + /// Resolution/bin size in sensor units (position units: radians/meters). + /// + [field: SerializeField] + [Tooltip( "Resolution/bin size (position units: radians/meters)." )] + [DynamicallyShowInInspector( "EnableSignalResolution" )] + [DisableInRuntimeInspector] + [ClampAboveZeroInInspector] + public float SignalResolution { get; set; } = 0.01f; + + [field: SerializeField] + [DisableInRuntimeInspector] + public bool EnableSignalScaling { get; set; } = false; + + /// + /// Constant scaling factor. + /// + [field: SerializeField] + [Tooltip( "Scaling factor applied to encoder outputs." )] + [DynamicallyShowInInspector( "EnableSignalScaling" )] + [DisableInRuntimeInspector] + public float SignalScaling { get; set; } = 1.0f; + + [RuntimeValue] public float PositionValue { get; private set; } + [RuntimeValue] public float SpeedValue { get; private set; } + + private uint m_outputID = 0; + + [HideInInspector] + public double PositionBuffer { get; private set; } + + [HideInInspector] + public double SpeedBuffer { get; private set; } + + public bool IsWheelJoint => ConstraintComponent == null || ConstraintComponent is WheelJoint; + + public bool HasTwoDof => ConstraintComponent == null || (ConstraintComponent is Constraint constraint && constraint.Type == ConstraintType.CylindricalJoint); + + private ScriptComponent FindParentJoint() + { + ScriptComponent component = GetComponentInParent(); + if ( component == null ) + component = GetComponentInParent(); + return component; + } + + // Set constraint on component creation + private void Reset() + { + if (ConstraintComponent == null) + ConstraintComponent = FindParentJoint(); + } + + protected override bool Initialize() + { + SensorEnvironment.Instance.GetInitialized(); + + // Find a constraint component if not explicitly assigned. + if ( ConstraintComponent == null ) { + ConstraintComponent = FindParentJoint(); + } + + if ( ConstraintComponent == null ) { + Debug.LogWarning( "No constraint component found/assigned, encoder will be inactive" ); + return false; + } + + var modifiers = new IMonoaxialSignalSystemNodeRefVector(); + + if ( EnableTotalGaussianNoise ) + modifiers.Add( new MonoaxialGaussianNoise( (double)TotalGaussianNoiseRms ) ); + + if ( EnableSignalResolution ) + modifiers.Add( new MonoaxialSignalResolution( (double)SignalResolution ) ); + + if ( EnableSignalScaling ) + modifiers.Add( new MonoaxialSignalScaling( (double)SignalScaling ) ); + + m_nativeModel = new EncoderModel( Mode, MeasurementRange.Native, modifiers ); + + if ( m_nativeModel == null ) { + Debug.LogWarning( "Could not create native encoder model, encoder will be inactive" ); + return false; + } + + PropertySynchronizer.Synchronize( this ); + + var initializedConstraint = ConstraintComponent.GetInitialized(); + if ( initializedConstraint == null ) { + Debug.LogWarning( "Constraint component not initializable, encoder will be inactive" ); + return false; + } + + Native = CreateNativeEncoderFromConstraint( initializedConstraint.Native, m_nativeModel, SampleTwoDof, SampleWheelJoint ); + if ( Native == null ) { + Debug.LogWarning( "Unsupported constraint type for encoder, encoder will be inactive" ); + return false; + } + + if ( OutputPosition || OutputSpeed ) { + m_outputID = SensorEnvironment.Instance.GenerateOutputID(); + + var output = new EncoderOutput(); + if ( OutputPosition ) + output.add( EncoderOutput.Field.POSITION_F64 ); + if ( OutputSpeed ) + output.add( EncoderOutput.Field.SPEED_F64 ); + + Native.getOutputHandler().add( m_outputID, output ); + + Simulation.Instance.StepCallbacks.PostStepForward += OnPostStepForward; + } + else { + Debug.LogWarning( "No output configured for encoder" ); + } + + + + SensorEnvironment.Instance.Native.add( Native ); + + return true; + } + + private static Encoder CreateNativeEncoderFromConstraint( agx.Constraint nativeConstraint, + EncoderModel model, + TwoDofSample sampleTwoDof, + WheelJointSample sampleWheelJoint ) + { + if ( nativeConstraint == null || model == null ) + return null; + + // Dedicated constructors (preferred). + if ( nativeConstraint is agx.Hinge hinge ) + return new Encoder( hinge, model ); + + if ( nativeConstraint is agx.Prismatic prismatic ) + return new Encoder( prismatic, model ); + + if ( nativeConstraint is agx.CylindricalJoint cylindrical ) + return new Encoder( cylindrical, model, ToConstraint2Dof( sampleTwoDof ) ); + + if ( nativeConstraint is agxVehicle.WheelJoint wheel ) + return new Encoder( wheel, model, ToWheelSecondaryConstraint( sampleWheelJoint ) ); + + return null; + } + + private static agx.Constraint2DOF.DOF ToConstraint2Dof( TwoDofSample value ) + { + return value == TwoDofSample.Second ? agx.Constraint2DOF.DOF.SECOND : agx.Constraint2DOF.DOF.FIRST; + } + + private static agxVehicle.WheelJoint.SecondaryConstraint ToWheelSecondaryConstraint( WheelJointSample value ) + { + switch ( value ) { + case WheelJointSample.Steering: + return agxVehicle.WheelJoint.SecondaryConstraint.STEERING; + case WheelJointSample.Suspension: + return agxVehicle.WheelJoint.SecondaryConstraint.SUSPENSION; + default: + return agxVehicle.WheelJoint.SecondaryConstraint.WHEEL; + } + } + + // Will only run if there is an output + private void OnPostStepForward() + { + if ( !gameObject.activeInHierarchy || Native == null ) + return; + + var output = Native.getOutputHandler().get( m_outputID ); + if ( output == null ) { + PositionBuffer = 0; + SpeedBuffer = 0; + } + else { + var viewPosition = output.viewPosition(); + var viewSpeed = output.viewPosition(); + + if ( viewPosition != null ) + PositionBuffer = viewPosition[ 0 ]; + + if ( viewSpeed != null ) + SpeedBuffer = viewSpeed[ 0 ]; + } + + // Convenience runtime display of output + PositionValue = (float)PositionBuffer; + SpeedValue = (float)SpeedBuffer; + } + + protected override void OnEnable() + { + Native?.setEnable( true ); + } + + protected override void OnDisable() + { + Native?.setEnable( false ); + } + + protected override void OnDestroy() + { + if ( SensorEnvironment.HasInstance ) + SensorEnvironment.Instance.Native?.remove( Native ); + + if ( Simulation.HasInstance ) + Simulation.Instance.StepCallbacks.PostStepForward -= OnPostStepForward; + + Native?.Dispose(); + Native = null; + + m_nativeModel?.Dispose(); + m_nativeModel = null; + + base.OnDestroy(); + } + + [NonSerialized] + private Mesh m_nodeGizmoMesh = null; + + private void OnDrawGizmosSelected() + { +#if UNITY_EDITOR + if ( m_nodeGizmoMesh == null ) + m_nodeGizmoMesh = Resources.Load( @"Debug/Models/Icosahedron" ); + + if ( m_nodeGizmoMesh == null ) + return; + + Gizmos.color = Color.yellow; + Gizmos.DrawWireMesh( m_nodeGizmoMesh, + transform.position, + transform.rotation, + Vector3.one * 0.2f ); +#endif + } + } +} diff --git a/AGXUnity/Sensor/EncoderSensor.cs.meta b/AGXUnity/Sensor/EncoderSensor.cs.meta new file mode 100644 index 00000000..9e11d49e --- /dev/null +++ b/AGXUnity/Sensor/EncoderSensor.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 220d371f3ca346c4eb8c6a9d81192c09 \ No newline at end of file diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs index 03aa0aaa..5903bb12 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -22,10 +22,10 @@ public class OdometerSensor : ScriptComponent /// /// Optional: Explicitly assign the constraint component to attach the odometer to. /// If left empty the component will use the first compatible parent. - /// Compatible with: Hinge, CylindricalJoint, SplineJoint, WheelJoint, SlackHingeJoint, SlackCylindricalJoint + /// Compatible with: Hinge, CylindricalJoint, WheelJoint /// [field: SerializeField] - [Tooltip( "Constraint component to attach odometer to: Hinge, CylindricalJoint, SplineJoint, WheelJoint, slackjoints. If unset, the first compatible parent is used." )] + [Tooltip( "Constraint component to attach odometer to: Hinge, CylindricalJoint, WheelJoint. If unset, the first compatible parent is used." )] [DisableInRuntimeInspector] public Constraint ConstraintComponent { get; set; } = null; @@ -159,15 +159,6 @@ private static Odometer CreateNativeOdometerFromConstraint( agx.Constraint nativ if ( nativeConstraint is agx.CylindricalJoint cylindrical ) return new Odometer( cylindrical, model ); - if ( nativeConstraint is agx.SplineJoint spline ) - return new Odometer( spline, model ); - - if ( nativeConstraint is agx.SlackHingeJoint slackHinge ) - return new Odometer( slackHinge, model ); - - if ( nativeConstraint is agx.SlackCylindricalJoint slackCylindrical ) - return new Odometer( slackCylindrical, model ); - return null; } diff --git a/Editor/CustomEditors/AGXUnity+Sensor+EncoderSensorEditor.cs b/Editor/CustomEditors/AGXUnity+Sensor+EncoderSensorEditor.cs new file mode 100644 index 00000000..fa51edbe --- /dev/null +++ b/Editor/CustomEditors/AGXUnity+Sensor+EncoderSensorEditor.cs @@ -0,0 +1,9 @@ +using UnityEditor; + +namespace AGXUnityEditor.Editors +{ + [CustomEditor( typeof( AGXUnity.Sensor.EncoderSensor ) )] + [CanEditMultipleObjects] + public class AGXUnitySensorEncoderSensorEditor : InspectorEditor + { } +} diff --git a/Editor/CustomEditors/AGXUnity+Sensor+EncoderSensorEditor.cs.meta b/Editor/CustomEditors/AGXUnity+Sensor+EncoderSensorEditor.cs.meta new file mode 100644 index 00000000..a6240025 --- /dev/null +++ b/Editor/CustomEditors/AGXUnity+Sensor+EncoderSensorEditor.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 912be1a7eda29ff4bb0b2c963d845c81 \ No newline at end of file From 993a202bbc3ae8321034b0f46a7bf2de69e48c70 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 20 Jan 2026 14:11:30 +0100 Subject: [PATCH 22/63] Icons --- AGXUnity/Sensor/EncoderSensor.cs.meta | 11 ++++++++++- AGXUnity/Sensor/OdometerSensor.cs.meta | 11 ++++++++++- AGXUnity/Sensor/TriaxialTypes.cs.meta | 11 ++++++++++- 3 files changed, 30 insertions(+), 3 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs.meta b/AGXUnity/Sensor/EncoderSensor.cs.meta index 9e11d49e..fe5225d9 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs.meta +++ b/AGXUnity/Sensor/EncoderSensor.cs.meta @@ -1,2 +1,11 @@ fileFormatVersion: 2 -guid: 220d371f3ca346c4eb8c6a9d81192c09 \ No newline at end of file +guid: 220d371f3ca346c4eb8c6a9d81192c09 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {fileID: 2800000, guid: 291d5afecb12a3747931bd119c01ad37, type: 3} + userData: + assetBundleName: + assetBundleVariant: diff --git a/AGXUnity/Sensor/OdometerSensor.cs.meta b/AGXUnity/Sensor/OdometerSensor.cs.meta index 077cb336..66fa0b9d 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs.meta +++ b/AGXUnity/Sensor/OdometerSensor.cs.meta @@ -1,2 +1,11 @@ fileFormatVersion: 2 -guid: 23f58075b197b564ead6633c7f209368 \ No newline at end of file +guid: 23f58075b197b564ead6633c7f209368 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {fileID: 2800000, guid: 291d5afecb12a3747931bd119c01ad37, type: 3} + userData: + assetBundleName: + assetBundleVariant: diff --git a/AGXUnity/Sensor/TriaxialTypes.cs.meta b/AGXUnity/Sensor/TriaxialTypes.cs.meta index 5d00b93d..46198070 100644 --- a/AGXUnity/Sensor/TriaxialTypes.cs.meta +++ b/AGXUnity/Sensor/TriaxialTypes.cs.meta @@ -1,2 +1,11 @@ fileFormatVersion: 2 -guid: d8282f9366480f04eb6ad8388116cef4 \ No newline at end of file +guid: d8282f9366480f04eb6ad8388116cef4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {fileID: 2800000, guid: 49c5f239479b7b143be0c05568f44faf, type: 3} + userData: + assetBundleName: + assetBundleVariant: From 23005d986f0b555df6d4adb27611fa31d9680190 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Wed, 18 Feb 2026 10:55:15 +0100 Subject: [PATCH 23/63] Work on correct output --- AGXUnity/Sensor/EncoderSensor.cs | 37 ++++++++++++++++---------------- 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs index af264771..67175506 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -37,7 +37,7 @@ public class EncoderSensor : ScriptComponent [field: SerializeField] [Tooltip( "Encoder accumulation mode. INCREMENTAL wraps within the cycle range; ABSOLUTE is single-turn absolute" )] [DisableInRuntimeInspector] - public EncoderModel.Mode Mode { get; set; } = EncoderModel.Mode.INCREMENTAL; + public EncoderModel.Mode Mode { get; set; } = EncoderModel.Mode.ABSOLUTE; /// /// Cycle range [min, max] in sensor units. @@ -87,6 +87,7 @@ public enum WheelJointSample [DisableInRuntimeInspector] public bool OutputSpeed { get; set; } = false; + [InspectorGroupBegin( Name = "Modifiers", DefaultExpanded = true )] [field: SerializeField] [DisableInRuntimeInspector] public bool EnableTotalGaussianNoise { get; set; } = false; @@ -126,6 +127,7 @@ public enum WheelJointSample [DynamicallyShowInInspector( "EnableSignalScaling" )] [DisableInRuntimeInspector] public float SignalScaling { get; set; } = 1.0f; + [InspectorGroupEnd] [RuntimeValue] public float PositionValue { get; private set; } [RuntimeValue] public float SpeedValue { get; private set; } @@ -138,9 +140,11 @@ public enum WheelJointSample [HideInInspector] public double SpeedBuffer { get; private set; } + [HideInInspector] public bool IsWheelJoint => ConstraintComponent == null || ConstraintComponent is WheelJoint; - public bool HasTwoDof => ConstraintComponent == null || (ConstraintComponent is Constraint constraint && constraint.Type == ConstraintType.CylindricalJoint); + [HideInInspector] + public bool HasTwoDof => ConstraintComponent == null || ( ConstraintComponent is Constraint constraint && constraint.Type == ConstraintType.CylindricalJoint ); private ScriptComponent FindParentJoint() { @@ -153,7 +157,7 @@ private ScriptComponent FindParentJoint() // Set constraint on component creation private void Reset() { - if (ConstraintComponent == null) + if ( ConstraintComponent == null ) ConstraintComponent = FindParentJoint(); } @@ -206,11 +210,7 @@ protected override bool Initialize() if ( OutputPosition || OutputSpeed ) { m_outputID = SensorEnvironment.Instance.GenerateOutputID(); - var output = new EncoderOutput(); - if ( OutputPosition ) - output.add( EncoderOutput.Field.POSITION_F64 ); - if ( OutputSpeed ) - output.add( EncoderOutput.Field.SPEED_F64 ); + var output = new EncoderOutputPositionSpeed(); Native.getOutputHandler().add( m_outputID, output ); @@ -235,9 +235,10 @@ private static Encoder CreateNativeEncoderFromConstraint( agx.Constraint nativeC if ( nativeConstraint == null || model == null ) return null; - // Dedicated constructors (preferred). - if ( nativeConstraint is agx.Hinge hinge ) + if ( nativeConstraint is agx.Hinge hinge ) { + Debug.Log( "Creating Hinge encoder" ); return new Encoder( hinge, model ); + } if ( nativeConstraint is agx.Prismatic prismatic ) return new Encoder( prismatic, model ); @@ -280,14 +281,14 @@ private void OnPostStepForward() SpeedBuffer = 0; } else { - var viewPosition = output.viewPosition(); - var viewSpeed = output.viewPosition(); - - if ( viewPosition != null ) - PositionBuffer = viewPosition[ 0 ]; - - if ( viewSpeed != null ) - SpeedBuffer = viewSpeed[ 0 ]; + var viewPosSpeed = output.viewPositionSpeed(); + if ( viewPosSpeed != null && viewPosSpeed.size() > 0 ) { + var first = viewPosSpeed.begin(); + if ( OutputSpeed ) + SpeedBuffer = first.speed; + if ( OutputPosition ) + PositionBuffer = first.position; + } } // Convenience runtime display of output From 43680b87875e6cccd0bee39906f5dd930a045c8a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Wed, 18 Feb 2026 11:28:04 +0100 Subject: [PATCH 24/63] Wheeljoint now working also for odometer --- AGXUnity/Sensor/EncoderSensor.cs | 27 +++++++++++------ AGXUnity/Sensor/ImuSensor.cs | 2 +- AGXUnity/Sensor/OdometerSensor.cs | 49 ++++++++++++++++++++++++++----- 3 files changed, 60 insertions(+), 18 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs index 67175506..2f0167c3 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -154,7 +154,6 @@ private ScriptComponent FindParentJoint() return component; } - // Set constraint on component creation private void Reset() { if ( ConstraintComponent == null ) @@ -195,13 +194,25 @@ protected override bool Initialize() PropertySynchronizer.Synchronize( this ); - var initializedConstraint = ConstraintComponent.GetInitialized(); - if ( initializedConstraint == null ) { - Debug.LogWarning( "Constraint component not initializable, encoder will be inactive" ); - return false; + if ( IsWheelJoint ) { + var initializedWheelJoint = ConstraintComponent.GetInitialized(); + if ( initializedWheelJoint == null ) { + Debug.LogWarning( "Wheel Joint component not initializable, encoder will be inactive" ); + return false; + } + + Native = CreateNativeEncoderFromConstraint( initializedWheelJoint.Native, m_nativeModel, SampleTwoDof, SampleWheelJoint ); + } + else { + var initializedConstraint = ConstraintComponent.GetInitialized(); + if ( initializedConstraint == null ) { + Debug.LogWarning( "Constraint component not initializable, encoder will be inactive" ); + return false; + } + + Native = CreateNativeEncoderFromConstraint( initializedConstraint.Native, m_nativeModel, SampleTwoDof, SampleWheelJoint ); } - Native = CreateNativeEncoderFromConstraint( initializedConstraint.Native, m_nativeModel, SampleTwoDof, SampleWheelJoint ); if ( Native == null ) { Debug.LogWarning( "Unsupported constraint type for encoder, encoder will be inactive" ); return false; @@ -235,10 +246,8 @@ private static Encoder CreateNativeEncoderFromConstraint( agx.Constraint nativeC if ( nativeConstraint == null || model == null ) return null; - if ( nativeConstraint is agx.Hinge hinge ) { - Debug.Log( "Creating Hinge encoder" ); + if ( nativeConstraint is agx.Hinge hinge ) return new Encoder( hinge, model ); - } if ( nativeConstraint is agx.Prismatic prismatic ) return new Encoder( prismatic, model ); diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index be3edcb7..4cdf2d88 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -29,7 +29,7 @@ public enum ImuAttachmentType /// /// Accelerometer / Gyroscope / Magnetometer /// - [SerializeField] + [field: SerializeField] public ImuAttachmentType Type { get; private set; } /// diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs index 5903bb12..550f2b3d 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -2,6 +2,7 @@ using AGXUnity.Utils; using UnityEngine; using System; +using AGXUnity.Model; namespace AGXUnity.Sensor { @@ -25,9 +26,9 @@ public class OdometerSensor : ScriptComponent /// Compatible with: Hinge, CylindricalJoint, WheelJoint /// [field: SerializeField] - [Tooltip( "Constraint component to attach odometer to: Hinge, CylindricalJoint, WheelJoint. If unset, the first compatible parent is used." )] + [Tooltip( "Constraint / WheelJoint component to attach odometer to: Hinge, CylindricalJoint, WheelJoint. If unset, the first compatible parent is used." )] [DisableInRuntimeInspector] - public Constraint ConstraintComponent { get; set; } = null; + public ScriptComponent ConstraintComponent { get; set; } = null; /// /// Wheel radius in meters @@ -38,6 +39,7 @@ public class OdometerSensor : ScriptComponent [ClampAboveZeroInInspector] public float WheelRadius { get; set; } = 0.5f; + [InspectorGroupBegin( Name = "Modifiers", DefaultExpanded = true )] [field: SerializeField] [DisableInRuntimeInspector] public bool EnableTotalGaussianNoise { get; set; } = false; @@ -75,12 +77,31 @@ public class OdometerSensor : ScriptComponent [DisableInRuntimeInspector] public float SignalScaling { get; set; } = 1.0f; + [InspectorGroupEnd] + + [HideInInspector] + public bool IsWheelJoint => ConstraintComponent == null || ConstraintComponent is WheelJoint; + [RuntimeValue] public float SensorValue { get; private set; } private uint m_outputID = 0; [HideInInspector] public double OutputBuffer { get; private set; } + private ScriptComponent FindParentJoint() + { + ScriptComponent component = GetComponentInParent(); + if ( component == null ) + component = GetComponentInParent(); + return component; + } + + private void Reset() + { + if ( ConstraintComponent == null ) + ConstraintComponent = FindParentJoint(); + } + protected override bool Initialize() { SensorEnvironment.Instance.GetInitialized(); @@ -92,7 +113,7 @@ protected override bool Initialize() // Find a constraint component if not explicitly assigned if ( ConstraintComponent == null ) { - ConstraintComponent = GetComponentInParent(); + ConstraintComponent = FindParentJoint(); } if ( ConstraintComponent == null ) { @@ -120,13 +141,25 @@ protected override bool Initialize() PropertySynchronizer.Synchronize( this ); - var initializedConstraint = ConstraintComponent.GetInitialized(); - if ( initializedConstraint == null ) { - Debug.LogWarning( "Constraint component not initializable, odometer will be inactive" ); - return false; + if ( IsWheelJoint ) { + var initializedWheelJoint = ConstraintComponent.GetInitialized(); + if ( initializedWheelJoint == null ) { + Debug.LogWarning( "Wheel Joint component not initializable, encoder will be inactive" ); + return false; + } + + Native = CreateNativeOdometerFromConstraint( initializedWheelJoint.Native, m_nativeModel ); + } + else { + var initializedConstraint = ConstraintComponent.GetInitialized(); + if ( initializedConstraint == null ) { + Debug.LogWarning( "Constraint component not initializable, encoder will be inactive" ); + return false; + } + + Native = CreateNativeOdometerFromConstraint( initializedConstraint.Native, m_nativeModel ); } - Native = CreateNativeOdometerFromConstraint( initializedConstraint.Native, m_nativeModel ); if ( Native == null ) { Debug.LogWarning( "Unsupported constraint type for odometer, odometer will be inactive" ); return false; From a6af5f1ca91776e27902c129bb2578bb2b08b62f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 24 Feb 2026 15:01:34 +0100 Subject: [PATCH 25/63] Adjust default values --- AGXUnity/Sensor/ImuSensor.cs | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 4cdf2d88..284046fc 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -122,7 +122,7 @@ public class ImuSensor : ScriptComponent ImuAttachment.ImuAttachmentType.Accelerometer, new TriaxialRangeData(), 0.01f, - Vector3.one * 260f ); + Vector3.zero ); /// /// When enabled, show configuration for the IMU attachment and create attachment when initializing object @@ -142,7 +142,7 @@ public class ImuSensor : ScriptComponent ImuAttachment.ImuAttachmentType.Gyroscope, new TriaxialRangeData(), 0.01f, - Vector3.one * 3f ); + Vector3.zero ); /// /// When enabled, show configuration for the IMU attachment and create attachment when initializing object @@ -292,7 +292,6 @@ public void GetOutput(IMUOutput output, double[] buffer) { if ( Native == null || buffer == null || output == null ) { Debug.LogError( "Null problem" ); - // output.viewNineDoF()[0]; return; } From 587d904e9d5164ba524bbbb354a663c6326143fd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 24 Feb 2026 15:01:49 +0100 Subject: [PATCH 26/63] Some basic sensor tests --- Tests/Runtime/ImuAndEncoderTests.cs | 175 +++++++++++++++++++++++ Tests/Runtime/ImuAndEncoderTests.cs.meta | 2 + 2 files changed, 177 insertions(+) create mode 100644 Tests/Runtime/ImuAndEncoderTests.cs create mode 100644 Tests/Runtime/ImuAndEncoderTests.cs.meta diff --git a/Tests/Runtime/ImuAndEncoderTests.cs b/Tests/Runtime/ImuAndEncoderTests.cs new file mode 100644 index 00000000..3cb062be --- /dev/null +++ b/Tests/Runtime/ImuAndEncoderTests.cs @@ -0,0 +1,175 @@ +using agx; +using agxSensor; +using AGXUnity; +using AGXUnity.Collide; +using AGXUnity.Sensor; +using NUnit.Framework; +using System.Collections; +using System.Linq; +using System.Text.RegularExpressions; +using UnityEngine; +using UnityEngine.TestTools; + +using GOList = System.Collections.Generic.List; + +namespace AGXUnityTesting.Runtime +{ + public class ImuAndEncoderTests : AGXUnityFixture + { + private GOList m_keep = new GOList(); + + [OneTimeSetUp] + public void SetupSensorScene() + { + Simulation.Instance.PreIntegratePositions = true; + m_keep.Add( Simulation.Instance.gameObject ); + + SensorEnvironment.Instance.FieldType = SensorEnvironment.MagneticFieldType.UNIFORM; + SensorEnvironment.Instance.MagneticFieldVector = Vector3.one; + m_keep.Add( SensorEnvironment.Instance.gameObject ); + } + + [UnityTearDown] + public IEnumerator CleanSensorScene() + { +#if UNITY_2022_2_OR_NEWER + var objects = Object.FindObjectsByType( FindObjectsSortMode.None ); +#else + var objects = Object.FindObjectsOfType( ); +#endif + GOList toDestroy = new GOList(); + + foreach ( var obj in objects ) { + var root = obj.gameObject; + while ( root.transform.parent != null ) + root = root.transform.parent.gameObject; + if ( !m_keep.Contains( root ) ) + toDestroy.Add( root ); + } + + yield return TestUtils.DestroyAndWait( toDestroy.ToArray() ); + } + + [OneTimeTearDown] + public void TearDownSensorScene() + { +#if UNITY_2022_2_OR_NEWER + var geoms = Object.FindObjectsByType( FindObjectsSortMode.None ); +#else + var geoms = Object.FindObjectsOfType( ); +#endif + + foreach ( var g in geoms ) + GameObject.Destroy( g.gameObject ); + + GameObject.Destroy( SensorEnvironment.Instance.gameObject ); + } + + private (AGXUnity.RigidBody, ImuSensor) CreateDefaultTestImu( Vector3 position = default ) + { + var rbGO = new GameObject("RB"); + rbGO.transform.position = position; + var rbComp = rbGO.AddComponent(); + + var imuGO = new GameObject("IMU"); + imuGO.transform.position = position; + imuGO.transform.parent = rbGO.transform; + var imuComp = imuGO.AddComponent(); + + + return (rbComp, imuComp); + } + + private AGXUnity.Constraint CreateTestHinge( Vector3 position = default ) + { + var go1 = Factory.Create< AGXUnity.RigidBody >( Factory.Create() ); + go1.transform.position = new Vector3( 0, 2, 0 ); + go1.GetComponent().MotionControl = agx.RigidBody.MotionControl.KINEMATICS; + var go2 = Factory.Create< AGXUnity.RigidBody >( Factory.Create() ); + var constraintGO = Factory.Create( ConstraintType.Hinge, Vector3.zero, Quaternion.identity, go1.GetComponent(), go2.GetComponent() ); + + return constraintGO.GetComponent(); + } + + + [UnityTest] + public IEnumerator TestCreateImu() + { + var (rb, imu) = CreateDefaultTestImu(); + + yield return TestUtils.Step(); + + Assert.NotNull( imu, "Couldn't create IMU" ); + } + + [UnityTest] + public IEnumerator TestAccelerometerOutput() + { + var (rb, imu) = CreateDefaultTestImu(); + + var g = Simulation.Instance.Gravity.y; + + rb.MotionControl = agx.RigidBody.MotionControl.KINEMATICS; + + yield return TestUtils.Step(); + + Assert.That( imu.OutputBuffer[ 1 ], Is.EqualTo( Mathf.Abs( g ) ).Within( 0.001f ), "Test value should be close to g" ); + } + + [UnityTest] + public IEnumerator TestGyroscopeOutput() + { + var (rb, imu) = CreateDefaultTestImu(); + + rb.MotionControl = agx.RigidBody.MotionControl.KINEMATICS; + rb.AngularVelocity = Vector3.one; + + yield return TestUtils.Step(); + + Assert.That( Mathf.Abs( (float)imu.OutputBuffer[ 5 ] ), Is.EqualTo( 1 ).Within( 0.01f ), "Test value should be 1 like the change in rotation" ); + } + + [UnityTest] + public IEnumerator TestMagnetometerOutput() + { + var (rb, imu) = CreateDefaultTestImu(); + + rb.MotionControl = agx.RigidBody.MotionControl.KINEMATICS; + rb.AngularVelocity = Vector3.one; + + yield return TestUtils.Step(); + + Assert.That( Mathf.Abs( (float)imu.OutputBuffer[ 8 ] ), Is.EqualTo( 1 ).Within( 0.001f ), "Test value should be 1 as the magnetic field was set up to be 1 in each direction" ); + } + + [UnityTest] + public IEnumerator TestEncoderOutput() + { + var constraint = CreateTestHinge(); + var encoder = constraint.gameObject.AddComponent(); + encoder.OutputSpeed = true; + var controller = constraint.GetController(); + controller.Speed = 1; + controller.Enable = true; + + yield return TestUtils.Step(); + + Assert.That( Mathf.Abs( (float)encoder.SpeedBuffer ), Is.EqualTo( 1 ).Within( 0.01f ), "Value should be close to target speed controller speed" ); + } + + [UnityTest] + public IEnumerator TestOdometerOutput() + { + var constraint = CreateTestHinge(); + var odometer = constraint.gameObject.AddComponent(); + var controller = constraint.GetController(); + controller.Speed = 1; + controller.Enable = true; + + yield return TestUtils.Step(); + yield return TestUtils.Step(); + + Assert.That( Mathf.Abs( (float)odometer.OutputBuffer ), Is.EqualTo( 0.02 ).Within( 0.001f ), "Testing" ); + } + } +} diff --git a/Tests/Runtime/ImuAndEncoderTests.cs.meta b/Tests/Runtime/ImuAndEncoderTests.cs.meta new file mode 100644 index 00000000..35f0c91e --- /dev/null +++ b/Tests/Runtime/ImuAndEncoderTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: e39f551326d22b849b05cc0541c22f0a \ No newline at end of file From be794024860359f96422d79f6d1d598ec93d4a76 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 24 Feb 2026 15:03:01 +0100 Subject: [PATCH 27/63] Some formatting --- AGXUnity/Sensor/ImuSensor.cs | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 284046fc..ef383d82 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -177,10 +177,9 @@ protected override bool Initialize() SensorEnvironment.Instance.GetInitialized(); var rigidBody = GetComponentInParent(); - if (rigidBody == null) - { + if ( rigidBody == null ) { Debug.LogWarning( "No Rigidbody found in this object or parents, IMU will be inactive" ); - return false; + return false; } TrackedRigidBody = rigidBody; @@ -250,8 +249,7 @@ protected override bool Initialize() m_nativeModel = new IMUModel( imu_attachments ); - if ( m_nativeModel == null ) - { + if ( m_nativeModel == null ) { Debug.LogWarning( "Could not create native imu model, IMU will be inactive" ); return false; } @@ -262,8 +260,7 @@ protected override bool Initialize() SensorEnvironment.Instance.Native.add( measuredRB ); var rbFrame = measuredRB.getFrame(); - if ( rbFrame == null ) - { + if ( rbFrame == null ) { Debug.LogWarning( "Could not get rigid body frame, IMU will be inactive" ); return false; } @@ -288,7 +285,7 @@ protected override bool Initialize() return true; } - public void GetOutput(IMUOutput output, double[] buffer) + public void GetOutput( IMUOutput output, double[] buffer ) { if ( Native == null || buffer == null || output == null ) { Debug.LogError( "Null problem" ); @@ -343,7 +340,7 @@ private void OnPostStepForward() if ( !gameObject.activeInHierarchy ) return; - GetOutput(Native.getOutputHandler().get(m_outputID), OutputBuffer); + GetOutput( Native.getOutputHandler().get( m_outputID ), OutputBuffer ); if ( Application.isEditor ) { for ( int i = 0; i < OutputBuffer.Length; i++ ) { @@ -394,7 +391,7 @@ private void OnDrawGizmosSelected() #if UNITY_EDITOR if ( m_nodeGizmoMesh == null ) m_nodeGizmoMesh = Resources.Load( @"Debug/Models/HalfSphere" ); - + if ( m_nodeGizmoMesh == null ) return; From 99304ddc442cfc8e461a77d30d12d967c081c8b5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 24 Feb 2026 15:07:47 +0100 Subject: [PATCH 28/63] Format document --- .../InvokeWrapperInspectorDrawer.cs | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs index 820e1fdf..9dd2bd52 100644 --- a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs +++ b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs @@ -1576,27 +1576,27 @@ public static object ImuAttachmentDrawer( object[] objects, InvokeWrapper wrappe if ( InspectorGUI.Foldout( EditorData.Instance.GetData( target, wrapper.Member.Name ), GUI.MakeLabel( "Modifiers", true, "Optional signal output modifiers" ) ) ) { using ( new InspectorGUI.IndentScope() ) { - ( data.EnableTotalGaussianNoise, data.TotalGaussianNoise ) = OptionalVector3GUI( + (data.EnableTotalGaussianNoise, data.TotalGaussianNoise) = OptionalVector3GUI( data.EnableTotalGaussianNoise, data.TotalGaussianNoise, "Total Gaussian Noise", "" ); - ( data.EnableSignalScaling, data.SignalScaling ) = OptionalVector3GUI( + (data.EnableSignalScaling, data.SignalScaling) = OptionalVector3GUI( data.EnableSignalScaling, data.SignalScaling, "Signal Scaling", "" ); - ( data.EnableGaussianSpectralNoise, data.GaussianSpectralNoise ) = OptionalVector3GUI( + (data.EnableGaussianSpectralNoise, data.GaussianSpectralNoise) = OptionalVector3GUI( data.EnableGaussianSpectralNoise, data.GaussianSpectralNoise, "Gaussian Spectral Noise", "" ); if ( data.Type == ImuAttachment.ImuAttachmentType.Gyroscope ) { - ( data.EnableLinearAccelerationEffects, data.LinearAccelerationEffects ) = OptionalVector3GUI( - data.EnableLinearAccelerationEffects, - data.LinearAccelerationEffects, - "Linear Acceleration Effects", - "" ); + (data.EnableLinearAccelerationEffects, data.LinearAccelerationEffects) = OptionalVector3GUI( + data.EnableLinearAccelerationEffects, + data.LinearAccelerationEffects, + "Linear Acceleration Effects", + "" ); } } } @@ -1607,18 +1607,18 @@ public static object ImuAttachmentDrawer( object[] objects, InvokeWrapper wrappe return null; } - private static (bool, Vector3) OptionalVector3GUI(bool toggle, Vector3 value, string label, string tooltip) + private static (bool, Vector3) OptionalVector3GUI( bool toggle, Vector3 value, string label, string tooltip ) { - using (new GUILayout.HorizontalScope() ) { + using ( new GUILayout.HorizontalScope() ) { var rect = EditorGUILayout.GetControlRect(); var xMaxOriginal = rect.xMax; rect.xMax = EditorGUIUtility.labelWidth + 20; //InspectorGUI.MakeLabel( wrapper.Member ); toggle = EditorGUI.ToggleLeft( rect, GUI.MakeLabel( label, false, tooltip ), toggle ); - using ( new GUI.EnabledBlock( UnityEngine.GUI.enabled && toggle)) { + using ( new GUI.EnabledBlock( UnityEngine.GUI.enabled && toggle ) ) { rect.x = rect.xMax - 30; rect.xMax = xMaxOriginal; - value = EditorGUI.Vector3Field(rect, "", value ); + value = EditorGUI.Vector3Field( rect, "", value ); } } return (toggle, value); From 17bbe3594cc93a81fe4b784eeec496c1b8004e25 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 24 Feb 2026 15:08:58 +0100 Subject: [PATCH 29/63] Format document --- Editor/AGXUnityEditor/InspectorEditor.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Editor/AGXUnityEditor/InspectorEditor.cs b/Editor/AGXUnityEditor/InspectorEditor.cs index b452c567..3e4a156b 100644 --- a/Editor/AGXUnityEditor/InspectorEditor.cs +++ b/Editor/AGXUnityEditor/InspectorEditor.cs @@ -104,7 +104,7 @@ public static void DrawMembersGUI( Object[] targets, InspectorGUI.Separator( 1, EditorGUIUtility.singleLineHeight ); var style = GUI.Align( GUI.Skin.label, TextAnchor.MiddleLeft ); GUILayout.Label( GUI.MakeLabel( "Runtime Values", true, "" ), style ); - + using ( new GUI.EnabledBlock( false ) ) { group = InspectorGroupHandler.Create(); foreach ( var wrapper in runtimeValues ) { From a612dc99a6f20694052194f0be5ebc5d7c6c8d5b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 16 Mar 2026 13:20:04 +0100 Subject: [PATCH 30/63] Move updates to sync transforms --- AGXUnity/Sensor/EncoderSensor.cs | 6 +++--- AGXUnity/Sensor/ImuSensor.cs | 6 +++--- AGXUnity/Sensor/OdometerSensor.cs | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs index 2f0167c3..de8f4f64 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -225,7 +225,7 @@ protected override bool Initialize() Native.getOutputHandler().add( m_outputID, output ); - Simulation.Instance.StepCallbacks.PostStepForward += OnPostStepForward; + Simulation.Instance.StepCallbacks.PostSynchronizeTransforms += OnPostSynchronizeTransforms; } else { Debug.LogWarning( "No output configured for encoder" ); @@ -279,7 +279,7 @@ private static agxVehicle.WheelJoint.SecondaryConstraint ToWheelSecondaryConstra } // Will only run if there is an output - private void OnPostStepForward() + private void OnPostSynchronizeTransforms() { if ( !gameObject.activeInHierarchy || Native == null ) return; @@ -321,7 +321,7 @@ protected override void OnDestroy() SensorEnvironment.Instance.Native?.remove( Native ); if ( Simulation.HasInstance ) - Simulation.Instance.StepCallbacks.PostStepForward -= OnPostStepForward; + Simulation.Instance.StepCallbacks.PostSynchronizeTransforms -= OnPostSynchronizeTransforms; Native?.Dispose(); Native = null; diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index ef383d82..de6fdf83 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -278,7 +278,7 @@ protected override bool Initialize() OutputBuffer = new double[ outputCount ]; - Simulation.Instance.StepCallbacks.PostStepForward += OnPostStepForward; + Simulation.Instance.StepCallbacks.PostSynchronizeTransforms += OnPostSynchronizeTransforms; SensorEnvironment.Instance.Native.add( Native ); @@ -335,7 +335,7 @@ public void GetOutput( IMUOutput output, double[] buffer ) } } - private void OnPostStepForward() + private void OnPostSynchronizeTransforms() { if ( !gameObject.activeInHierarchy ) return; @@ -371,7 +371,7 @@ protected override void OnDestroy() SensorEnvironment.Instance.Native?.remove( Native ); if ( Simulation.HasInstance ) { - Simulation.Instance.StepCallbacks.PostStepForward -= OnPostStepForward; + Simulation.Instance.StepCallbacks.PostSynchronizeTransforms -= OnPostSynchronizeTransforms; } Native?.Dispose(); diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs index 550f2b3d..60c06b23 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -169,7 +169,7 @@ protected override bool Initialize() var output = new OdometerOutputDistance(); Native.getOutputHandler().add( m_outputID, output ); - Simulation.Instance.StepCallbacks.PostStepForward += OnPostStepForward; + Simulation.Instance.StepCallbacks.PostSynchronizeTransforms += OnPostSynchronizeTransforms; SensorEnvironment.Instance.Native.add( Native ); @@ -204,7 +204,7 @@ public double GetOutput( OdometerOutput output ) return view[ 0 ]; } - private void OnPostStepForward() + private void OnPostSynchronizeTransforms() { if ( !gameObject.activeInHierarchy ) return; @@ -231,7 +231,7 @@ protected override void OnDestroy() SensorEnvironment.Instance.Native?.remove( Native ); if ( Simulation.HasInstance ) - Simulation.Instance.StepCallbacks.PostStepForward -= OnPostStepForward; + Simulation.Instance.StepCallbacks.PostSynchronizeTransforms -= OnPostSynchronizeTransforms; Native?.Dispose(); Native = null; From d0ef865362419238c959bf8499e372c23d66af59 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 16 Mar 2026 13:28:43 +0100 Subject: [PATCH 31/63] Sync properties later --- AGXUnity/Sensor/EncoderSensor.cs | 4 +--- AGXUnity/Sensor/ImuSensor.cs | 4 ++-- AGXUnity/Sensor/OdometerSensor.cs | 4 ++-- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs index de8f4f64..34414f32 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -192,8 +192,6 @@ protected override bool Initialize() return false; } - PropertySynchronizer.Synchronize( this ); - if ( IsWheelJoint ) { var initializedWheelJoint = ConstraintComponent.GetInitialized(); if ( initializedWheelJoint == null ) { @@ -231,7 +229,7 @@ protected override bool Initialize() Debug.LogWarning( "No output configured for encoder" ); } - + PropertySynchronizer.Synchronize( this ); SensorEnvironment.Instance.Native.add( Native ); diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index de6fdf83..98dd4ea8 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -254,8 +254,6 @@ protected override bool Initialize() return false; } - PropertySynchronizer.Synchronize( this ); - var measuredRB = rigidBody.GetInitialized().Native; SensorEnvironment.Instance.Native.add( measuredRB ); @@ -278,6 +276,8 @@ protected override bool Initialize() OutputBuffer = new double[ outputCount ]; + PropertySynchronizer.Synchronize( this ); + Simulation.Instance.StepCallbacks.PostSynchronizeTransforms += OnPostSynchronizeTransforms; SensorEnvironment.Instance.Native.add( Native ); diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs index 60c06b23..9c4d5b82 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -139,8 +139,6 @@ protected override bool Initialize() return false; } - PropertySynchronizer.Synchronize( this ); - if ( IsWheelJoint ) { var initializedWheelJoint = ConstraintComponent.GetInitialized(); if ( initializedWheelJoint == null ) { @@ -169,6 +167,8 @@ protected override bool Initialize() var output = new OdometerOutputDistance(); Native.getOutputHandler().add( m_outputID, output ); + PropertySynchronizer.Synchronize( this ); + Simulation.Instance.StepCallbacks.PostSynchronizeTransforms += OnPostSynchronizeTransforms; SensorEnvironment.Instance.Native.add( Native ); From b56071d6bf37eec569f783fd20f5793174cc09f0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 16 Mar 2026 13:36:49 +0100 Subject: [PATCH 32/63] Better tooltip and 01 clamping of cross axis sensitivity --- AGXUnity/Sensor/ImuSensor.cs | 4 ++-- Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 98dd4ea8..9c732fe0 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -39,9 +39,9 @@ public enum ImuAttachmentType public TriaxialRangeData TriaxialRange; /// - /// Cross axis sensitivity + /// Cross axis sensitivity - how measurements in one axis affects the other axes. Ratio 0 to 1. /// - [Tooltip("Cross axis sensitivity")] + [Tooltip("Cross axis sensitivity - how measurements in one axis affects the other axes. Ratio 0 to 1.")] public float CrossAxisSensitivity; /// diff --git a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs index 9dd2bd52..e82cd985 100644 --- a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs +++ b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs @@ -1566,7 +1566,7 @@ public static object ImuAttachmentDrawer( object[] objects, InvokeWrapper wrappe var data = wrapper.Get( objects[0] ); using ( new InspectorGUI.IndentScope() ) { data.TriaxialRange = TriaxialRangeDataGUI( data.TriaxialRange ); - data.CrossAxisSensitivity = EditorGUILayout.FloatField( "Cross Axis Sensitivity", data.CrossAxisSensitivity ); + data.CrossAxisSensitivity = Mathf.Clamp01(EditorGUILayout.FloatField( "Cross Axis Sensitivity", data.CrossAxisSensitivity )); data.ZeroBias = EditorGUILayout.Vector3Field( "Zero Rate Bias", data.ZeroBias ); EditorGUI.BeginChangeCheck(); data.OutputFlags = OutputXYZGUI( data.OutputFlags ); From 70129a6956456f34c46dbaaa6d92d77b7ec5005b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 16 Mar 2026 13:40:23 +0100 Subject: [PATCH 33/63] Update AGXUnity/Sensor/OdometerSensor.cs Co-authored-by: Filip Henningsson <114672787+FilipAlg@users.noreply.github.com> --- AGXUnity/Sensor/OdometerSensor.cs | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs index 550f2b3d..c7bc2f4c 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -176,10 +176,7 @@ protected override bool Initialize() return true; } - private double GetSignalResolution() - { - return 2.0 * System.Math.PI * WheelRadius / PulsesPerRevolution; - } + private double SignalResolution => 2.0 * System.Math.PI * WheelRadius / PulsesPerRevolution; private static Odometer CreateNativeOdometerFromConstraint( agx.Constraint nativeConstraint, OdometerModel model ) { From 9e5f967e2a7edcc1959361ddd2ad301c22bddfd6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 16 Mar 2026 13:43:49 +0100 Subject: [PATCH 34/63] make internal methods private --- AGXUnity/Sensor/ImuSensor.cs | 2 +- AGXUnity/Sensor/OdometerSensor.cs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 9c732fe0..f5c783dd 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -285,7 +285,7 @@ protected override bool Initialize() return true; } - public void GetOutput( IMUOutput output, double[] buffer ) + private void GetOutput( IMUOutput output, double[] buffer ) { if ( Native == null || buffer == null || output == null ) { Debug.LogError( "Null problem" ); diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs index 9c4d5b82..213c0f4c 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -195,7 +195,7 @@ private static Odometer CreateNativeOdometerFromConstraint( agx.Constraint nativ return null; } - public double GetOutput( OdometerOutput output ) + private double GetOutput( OdometerOutput output ) { if ( Native == null || output == null ) return 0; From 2ccbc76eb49533221c280a84b7d3e4f0d21a2752 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 16 Mar 2026 14:00:11 +0100 Subject: [PATCH 35/63] Field to property --- AGXUnity/Sensor/ImuSensor.cs | 27 ++++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index f5c783dd..0b71c4cd 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -165,9 +165,9 @@ public class ImuSensor : ScriptComponent Vector3.one * 0f ); [RuntimeValue] public RigidBody TrackedRigidBody { get; private set; } - [RuntimeValue] public Vector3 OutputRow1; - [RuntimeValue] public Vector3 OutputRow2; - [RuntimeValue] public Vector3 OutputRow3; + [RuntimeValue] public Vector3 OutputRow1 { get; private set; } + [RuntimeValue] public Vector3 OutputRow2 { get; private set; } + [RuntimeValue] public Vector3 OutputRow3 { get; private set; } private uint m_outputID = 0; public double[] OutputBuffer { get; private set; } @@ -344,12 +344,21 @@ private void OnPostSynchronizeTransforms() if ( Application.isEditor ) { for ( int i = 0; i < OutputBuffer.Length; i++ ) { - if ( i < 3 ) - OutputRow1[ i ] = (float)OutputBuffer[ i ]; - else if ( i < 6 ) - OutputRow2[ i % 3 ] = (float)OutputBuffer[ i ]; - else - OutputRow3[ i % 6 ] = (float)OutputBuffer[ i ]; + if ( i < 3 ) { + var outputRow1 = OutputRow1; + outputRow1[ i ] = (float)OutputBuffer[ i ]; + OutputRow1 = outputRow1; + } + else if ( i < 6 ) { + var outputRow2 = OutputRow2; + outputRow2[ i % 3 ] = (float)OutputBuffer[ i ]; + OutputRow2 = outputRow2; + } + else { + var outputRow3 = OutputRow3; + outputRow3[ i % 6 ] = (float)OutputBuffer[ i ]; + OutputRow3 = outputRow3; + } } } } From ce3cab5dfaeee2291c15bb8804a55b682a4a77ca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 16 Mar 2026 14:03:05 +0100 Subject: [PATCH 36/63] First / second -> rotational / translational --- AGXUnity/Sensor/EncoderSensor.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs index 34414f32..e05aa7e4 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -51,15 +51,15 @@ public class EncoderSensor : ScriptComponent public enum TwoDofSample { - First, - Second + Rotational, + Translational } [field: SerializeField] [Tooltip( "Which DoF to sample for 2-DoF constraints (Cylindrical)" )] [DisableInRuntimeInspector] [DynamicallyShowInInspector( "HasTwoDof", true )] - public TwoDofSample SampleTwoDof { get; set; } = TwoDofSample.First; + public TwoDofSample SampleTwoDof { get; set; } = TwoDofSample.Rotational; public enum WheelJointSample { @@ -261,7 +261,7 @@ private static Encoder CreateNativeEncoderFromConstraint( agx.Constraint nativeC private static agx.Constraint2DOF.DOF ToConstraint2Dof( TwoDofSample value ) { - return value == TwoDofSample.Second ? agx.Constraint2DOF.DOF.SECOND : agx.Constraint2DOF.DOF.FIRST; + return value == TwoDofSample.Translational ? agx.Constraint2DOF.DOF.SECOND : agx.Constraint2DOF.DOF.FIRST; } private static agxVehicle.WheelJoint.SecondaryConstraint ToWheelSecondaryConstraint( WheelJointSample value ) From c1400dae31f8d4cd07d8e940127eadf6a7b7ac8e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 16 Mar 2026 14:04:54 +0100 Subject: [PATCH 37/63] Pascal --- AGXUnity/Sensor/SensorEnvironment.cs | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/AGXUnity/Sensor/SensorEnvironment.cs b/AGXUnity/Sensor/SensorEnvironment.cs index 9d38f25b..3108bcb1 100644 --- a/AGXUnity/Sensor/SensorEnvironment.cs +++ b/AGXUnity/Sensor/SensorEnvironment.cs @@ -19,9 +19,9 @@ public class SensorEnvironment : UniqueGameObject { public enum MagneticFieldType { - NONE, - UNIFORM, - DIPOLE + None, + Uniform, + Dipole } /// @@ -48,10 +48,10 @@ public enum MagneticFieldType /// [Tooltip("Set type of magnetic field used in the simulation")] [HideInRuntimeInspector] - public MagneticFieldType FieldType = MagneticFieldType.UNIFORM; + public MagneticFieldType FieldType = MagneticFieldType.Uniform; - private bool UsingUniformMagneticField => FieldType == MagneticFieldType.UNIFORM; - private bool UsingDipoleMagneticField => FieldType == MagneticFieldType.DIPOLE; + private bool UsingUniformMagneticField => FieldType == MagneticFieldType.Uniform; + private bool UsingDipoleMagneticField => FieldType == MagneticFieldType.Dipole; /// /// Set the field vector of the uniform magnetic field used in the simulation [in Tesla] @@ -504,10 +504,10 @@ protected override bool Initialize() FindValidComponents( true ).ForEach( c => TrackIfSupported( c ) ); switch ( FieldType ) { - case MagneticFieldType.UNIFORM: + case MagneticFieldType.Uniform: Native.setMagneticField( new UniformMagneticField( MagneticFieldVector.ToHandedVec3() ) ); break; - case MagneticFieldType.DIPOLE: + case MagneticFieldType.Dipole: Native.setMagneticField( new DipoleMagneticField( MagneticMoment.ToHandedVec3(), DipoleCenter.ToHandedVec3() ) ); break; default: From 4265fe3202ebea1e4ee3faa799ed45d5770f0987 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 16 Mar 2026 14:08:40 +0100 Subject: [PATCH 38/63] Update Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs Co-authored-by: Filip Henningsson <114672787+FilipAlg@users.noreply.github.com> --- Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs index 9dd2bd52..18e12fdc 100644 --- a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs +++ b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs @@ -1667,7 +1667,7 @@ private static OutputXYZ OutputXYZGUI( OutputXYZ state ) if ( GUILayout.Toggle( yEnabled, GUI.MakeLabel( "Y", yEnabled, - "Use sensor X value in output" ), + "Use sensor Y value in output" ), skin.GetButton( InspectorGUISkin.ButtonType.Middle ), GUILayout.Width( 76 ) ) != yEnabled ) state ^= OutputXYZ.Y; From 4428933a96a89e3f8a08e7c506b4a66d84bf7df6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 16 Mar 2026 14:09:03 +0100 Subject: [PATCH 39/63] Update Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs Co-authored-by: Filip Henningsson <114672787+FilipAlg@users.noreply.github.com> --- Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs index 18e12fdc..c4cd61f4 100644 --- a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs +++ b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs @@ -1673,7 +1673,7 @@ private static OutputXYZ OutputXYZGUI( OutputXYZ state ) state ^= OutputXYZ.Y; if ( GUILayout.Toggle( zEnabled, GUI.MakeLabel( "Z", - yEnabled, + zEnabled, "Use sensor Z value in output" ), skin.GetButton( InspectorGUISkin.ButtonType.Right ), GUILayout.Width( 76 ) ) != zEnabled ) From 65ba214f6c1b8fbf057ed09380c8d21c557c76ee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 16 Mar 2026 14:10:57 +0100 Subject: [PATCH 40/63] nameof --- AGXUnity/Sensor/EncoderSensor.cs | 10 +++++----- AGXUnity/Sensor/ImuSensor.cs | 6 +++--- AGXUnity/Sensor/OdometerSensor.cs | 6 +++--- AGXUnity/Sensor/SensorEnvironment.cs | 6 +++--- 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs index e05aa7e4..e819b7cb 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -58,7 +58,7 @@ public enum TwoDofSample [field: SerializeField] [Tooltip( "Which DoF to sample for 2-DoF constraints (Cylindrical)" )] [DisableInRuntimeInspector] - [DynamicallyShowInInspector( "HasTwoDof", true )] + [DynamicallyShowInInspector( nameof(HasTwoDof), true )] public TwoDofSample SampleTwoDof { get; set; } = TwoDofSample.Rotational; public enum WheelJointSample @@ -71,7 +71,7 @@ public enum WheelJointSample [field: SerializeField] [Tooltip( "Which secondary constraint to sample for WheelJoint" )] [DisableInRuntimeInspector] - [DynamicallyShowInInspector( "IsWheelJoint", true )] + [DynamicallyShowInInspector( nameof(IsWheelJoint), true )] public WheelJointSample SampleWheelJoint { get; set; } = WheelJointSample.WheelAxle; /// @@ -97,7 +97,7 @@ public enum WheelJointSample /// [field: SerializeField] [Tooltip( "Gaussian noise RMS (position units: radians/meters)." )] - [DynamicallyShowInInspector( "EnableTotalGaussianNoise" )] + [DynamicallyShowInInspector( nameof(EnableTotalGaussianNoise) )] [DisableInRuntimeInspector] public float TotalGaussianNoiseRms { get; set; } = 0.0f; @@ -110,7 +110,7 @@ public enum WheelJointSample /// [field: SerializeField] [Tooltip( "Resolution/bin size (position units: radians/meters)." )] - [DynamicallyShowInInspector( "EnableSignalResolution" )] + [DynamicallyShowInInspector( nameof(EnableSignalResolution) )] [DisableInRuntimeInspector] [ClampAboveZeroInInspector] public float SignalResolution { get; set; } = 0.01f; @@ -124,7 +124,7 @@ public enum WheelJointSample /// [field: SerializeField] [Tooltip( "Scaling factor applied to encoder outputs." )] - [DynamicallyShowInInspector( "EnableSignalScaling" )] + [DynamicallyShowInInspector( nameof(EnableSignalScaling) )] [DisableInRuntimeInspector] public float SignalScaling { get; set; } = 1.0f; [InspectorGroupEnd] diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 0b71c4cd..c982c945 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -116,7 +116,7 @@ public class ImuSensor : ScriptComponent /// Accelerometer sensor attachment /// [field: SerializeField] - [DynamicallyShowInInspector( "EnableAccelerometer" )] + [DynamicallyShowInInspector( nameof(EnableAccelerometer) )] [DisableInRuntimeInspector] public ImuAttachment AccelerometerAttachment { get; private set; } = new ImuAttachment( ImuAttachment.ImuAttachmentType.Accelerometer, @@ -136,7 +136,7 @@ public class ImuSensor : ScriptComponent /// Gyroscope sensor attachment /// [field: SerializeField] - [DynamicallyShowInInspector( "EnableGyroscope" )] + [DynamicallyShowInInspector( nameof(EnableGyroscope) )] [DisableInRuntimeInspector] public ImuAttachment GyroscopeAttachment { get; private set; } = new ImuAttachment( ImuAttachment.ImuAttachmentType.Gyroscope, @@ -156,7 +156,7 @@ public class ImuSensor : ScriptComponent /// Magnetometer sensor attachment /// [field: SerializeField] - [DynamicallyShowInInspector( "EnableMagnetometer" )] + [DynamicallyShowInInspector( nameof(EnableMagnetometer) )] [DisableInRuntimeInspector] public ImuAttachment MagnetometerAttachment { get; private set; } = new ImuAttachment( ImuAttachment.ImuAttachmentType.Magnetometer, diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs index 213c0f4c..cb21314e 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -48,7 +48,7 @@ public class OdometerSensor : ScriptComponent /// [field: SerializeField] [Tooltip( "Gaussian noise RMS" )] - [DynamicallyShowInInspector( "EnableTotalGaussianNoise" )] + [DynamicallyShowInInspector( nameof(EnableTotalGaussianNoise) )] [DisableInRuntimeInspector] public float TotalGaussianNoiseRms { get; set; } = 0.0f; @@ -60,7 +60,7 @@ public class OdometerSensor : ScriptComponent /// [field: SerializeField] [Tooltip( "Pulses per wheel revolution" )] - [DynamicallyShowInInspector( "EnableSignalResolution" )] + [DynamicallyShowInInspector( nameof(EnableSignalResolution) )] [DisableInRuntimeInspector] [ClampAboveZeroInInspector] public int PulsesPerRevolution { get; set; } = 1024; @@ -73,7 +73,7 @@ public class OdometerSensor : ScriptComponent /// [field: SerializeField] [Tooltip( "Scaling factor applied to the distance signal" )] - [DynamicallyShowInInspector( "EnableSignalScaling" )] + [DynamicallyShowInInspector( nameof(EnableSignalScaling) )] [DisableInRuntimeInspector] public float SignalScaling { get; set; } = 1.0f; diff --git a/AGXUnity/Sensor/SensorEnvironment.cs b/AGXUnity/Sensor/SensorEnvironment.cs index 3108bcb1..ded72282 100644 --- a/AGXUnity/Sensor/SensorEnvironment.cs +++ b/AGXUnity/Sensor/SensorEnvironment.cs @@ -58,7 +58,7 @@ public enum MagneticFieldType /// [Tooltip("Set the field vector of the uniform magnetic field used in the simulation [in Tesla]")] [HideInRuntimeInspector] - [DynamicallyShowInInspector( "UsingUniformMagneticField" )] + [DynamicallyShowInInspector( nameof(UsingUniformMagneticField) )] public Vector3 MagneticFieldVector = new Vector3( 19.462e-6f, 44.754e-6f, 7.8426e-6f ); /// @@ -66,7 +66,7 @@ public enum MagneticFieldType /// [Tooltip("Magnetic dipole moment vector [in m^2 * A]")] [HideInRuntimeInspector] - [DynamicallyShowInInspector( "UsingDipoleMagneticField" )] + [DynamicallyShowInInspector( nameof(UsingDipoleMagneticField) )] public Vector3 MagneticMoment = new Vector3( -2.69e19f, -7.65e22f, 1.5e22f ); /// @@ -74,7 +74,7 @@ public enum MagneticFieldType /// [Tooltip("Magnetic dipole center")] [HideInRuntimeInspector] - [DynamicallyShowInInspector( "UsingDipoleMagneticField" )] + [DynamicallyShowInInspector( nameof(UsingDipoleMagneticField) )] public Vector3 DipoleCenter = new Vector3( 1.9e-10f, 20.79e3f, -6.369e6f ); [InspectorGroupEnd] From cb21a0c660b11b90b60f9939fdfbd0125b925dec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 16 Mar 2026 14:15:39 +0100 Subject: [PATCH 41/63] Don't dispose as much --- AGXUnity/Sensor/EncoderSensor.cs | 6 ------ AGXUnity/Sensor/ImuSensor.cs | 5 ----- AGXUnity/Sensor/OdometerSensor.cs | 6 ------ 3 files changed, 17 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs index e819b7cb..ab65f651 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -321,12 +321,6 @@ protected override void OnDestroy() if ( Simulation.HasInstance ) Simulation.Instance.StepCallbacks.PostSynchronizeTransforms -= OnPostSynchronizeTransforms; - Native?.Dispose(); - Native = null; - - m_nativeModel?.Dispose(); - m_nativeModel = null; - base.OnDestroy(); } diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index c982c945..0347024c 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -383,11 +383,6 @@ protected override void OnDestroy() Simulation.Instance.StepCallbacks.PostSynchronizeTransforms -= OnPostSynchronizeTransforms; } - Native?.Dispose(); - Native = null; - m_nativeModel?.Dispose(); - m_nativeModel = null; - base.OnDestroy(); } diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs index cb21314e..7fdb1642 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -233,12 +233,6 @@ protected override void OnDestroy() if ( Simulation.HasInstance ) Simulation.Instance.StepCallbacks.PostSynchronizeTransforms -= OnPostSynchronizeTransforms; - Native?.Dispose(); - Native = null; - - m_nativeModel?.Dispose(); - m_nativeModel = null; - base.OnDestroy(); } From 411784379aa07fc5f6dfc558567d033d9265d9b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 16 Mar 2026 14:17:12 +0100 Subject: [PATCH 42/63] Rename method --- AGXUnity/Sensor/ImuSensor.cs | 6 +++--- AGXUnity/Utils/Math.cs | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 0347024c..edd9fe07 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -267,9 +267,9 @@ protected override bool Initialize() // For SWIG reasons, we will create a ninedof output and use the fields selectively m_outputID = SensorEnvironment.Instance.GenerateOutputID(); uint outputCount = 0; - outputCount += EnableAccelerometer ? Utils.Math.PopCount( (uint)AccelerometerAttachment.OutputFlags ) : 0; - outputCount += EnableGyroscope ? Utils.Math.PopCount( (uint)GyroscopeAttachment.OutputFlags ) : 0; - outputCount += EnableMagnetometer ? Utils.Math.PopCount( (uint)MagnetometerAttachment.OutputFlags ) : 0; + outputCount += EnableAccelerometer ? Utils.Math.CountEnabledBits( (uint)AccelerometerAttachment.OutputFlags ) : 0; + outputCount += EnableGyroscope ? Utils.Math.CountEnabledBits( (uint)GyroscopeAttachment.OutputFlags ) : 0; + outputCount += EnableMagnetometer ? Utils.Math.CountEnabledBits( (uint)MagnetometerAttachment.OutputFlags ) : 0; var output = new IMUOutputNineDoF(); Native.getOutputHandler().add( m_outputID, output ); diff --git a/AGXUnity/Utils/Math.cs b/AGXUnity/Utils/Math.cs index 407f4d5d..5e6e0e7c 100644 --- a/AGXUnity/Utils/Math.cs +++ b/AGXUnity/Utils/Math.cs @@ -63,7 +63,7 @@ public static void Swap( ref T lhs, ref T rhs ) } // Counts enabled bits - public static uint PopCount( uint value ) + public static uint CountEnabledBits( uint value ) { uint c = 0; for ( ; value != 0; c++ ) From ebd121c3701da87d2e87ebbc8dc9c1057e8053ed Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 16 Mar 2026 14:21:54 +0100 Subject: [PATCH 43/63] Remove explicit width --- Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs index e82cd985..f821ac47 100644 --- a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs +++ b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs @@ -1661,22 +1661,19 @@ private static OutputXYZ OutputXYZGUI( OutputXYZ state ) GUI.MakeLabel( "X", xEnabled, "Use sensor X value in output" ), - skin.GetButton( InspectorGUISkin.ButtonType.Left ), - GUILayout.Width( 76 ) ) != xEnabled ) + skin.GetButton( InspectorGUISkin.ButtonType.Left ) ) != xEnabled ) state ^= OutputXYZ.X; if ( GUILayout.Toggle( yEnabled, GUI.MakeLabel( "Y", yEnabled, "Use sensor X value in output" ), - skin.GetButton( InspectorGUISkin.ButtonType.Middle ), - GUILayout.Width( 76 ) ) != yEnabled ) + skin.GetButton( InspectorGUISkin.ButtonType.Middle ) ) != yEnabled ) state ^= OutputXYZ.Y; if ( GUILayout.Toggle( zEnabled, GUI.MakeLabel( "Z", yEnabled, "Use sensor Z value in output" ), - skin.GetButton( InspectorGUISkin.ButtonType.Right ), - GUILayout.Width( 76 ) ) != zEnabled ) + skin.GetButton( InspectorGUISkin.ButtonType.Right ) ) != zEnabled ) state ^= OutputXYZ.Z; } From 23308aad8c06868075003fd3382084e7ca1bd04b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 16 Mar 2026 14:22:35 +0100 Subject: [PATCH 44/63] Format --- AGXUnity/Sensor/EncoderSensor.cs | 10 +++++----- AGXUnity/Sensor/ImuSensor.cs | 6 +++--- Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs | 2 +- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs index ab65f651..c86cfdc3 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -58,7 +58,7 @@ public enum TwoDofSample [field: SerializeField] [Tooltip( "Which DoF to sample for 2-DoF constraints (Cylindrical)" )] [DisableInRuntimeInspector] - [DynamicallyShowInInspector( nameof(HasTwoDof), true )] + [DynamicallyShowInInspector( nameof( HasTwoDof ), true )] public TwoDofSample SampleTwoDof { get; set; } = TwoDofSample.Rotational; public enum WheelJointSample @@ -71,7 +71,7 @@ public enum WheelJointSample [field: SerializeField] [Tooltip( "Which secondary constraint to sample for WheelJoint" )] [DisableInRuntimeInspector] - [DynamicallyShowInInspector( nameof(IsWheelJoint), true )] + [DynamicallyShowInInspector( nameof( IsWheelJoint ), true )] public WheelJointSample SampleWheelJoint { get; set; } = WheelJointSample.WheelAxle; /// @@ -97,7 +97,7 @@ public enum WheelJointSample /// [field: SerializeField] [Tooltip( "Gaussian noise RMS (position units: radians/meters)." )] - [DynamicallyShowInInspector( nameof(EnableTotalGaussianNoise) )] + [DynamicallyShowInInspector( nameof( EnableTotalGaussianNoise ) )] [DisableInRuntimeInspector] public float TotalGaussianNoiseRms { get; set; } = 0.0f; @@ -110,7 +110,7 @@ public enum WheelJointSample /// [field: SerializeField] [Tooltip( "Resolution/bin size (position units: radians/meters)." )] - [DynamicallyShowInInspector( nameof(EnableSignalResolution) )] + [DynamicallyShowInInspector( nameof( EnableSignalResolution ) )] [DisableInRuntimeInspector] [ClampAboveZeroInInspector] public float SignalResolution { get; set; } = 0.01f; @@ -124,7 +124,7 @@ public enum WheelJointSample /// [field: SerializeField] [Tooltip( "Scaling factor applied to encoder outputs." )] - [DynamicallyShowInInspector( nameof(EnableSignalScaling) )] + [DynamicallyShowInInspector( nameof( EnableSignalScaling ) )] [DisableInRuntimeInspector] public float SignalScaling { get; set; } = 1.0f; [InspectorGroupEnd] diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index edd9fe07..09b1d6b5 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -116,7 +116,7 @@ public class ImuSensor : ScriptComponent /// Accelerometer sensor attachment /// [field: SerializeField] - [DynamicallyShowInInspector( nameof(EnableAccelerometer) )] + [DynamicallyShowInInspector( nameof( EnableAccelerometer ) )] [DisableInRuntimeInspector] public ImuAttachment AccelerometerAttachment { get; private set; } = new ImuAttachment( ImuAttachment.ImuAttachmentType.Accelerometer, @@ -136,7 +136,7 @@ public class ImuSensor : ScriptComponent /// Gyroscope sensor attachment /// [field: SerializeField] - [DynamicallyShowInInspector( nameof(EnableGyroscope) )] + [DynamicallyShowInInspector( nameof( EnableGyroscope ) )] [DisableInRuntimeInspector] public ImuAttachment GyroscopeAttachment { get; private set; } = new ImuAttachment( ImuAttachment.ImuAttachmentType.Gyroscope, @@ -156,7 +156,7 @@ public class ImuSensor : ScriptComponent /// Magnetometer sensor attachment /// [field: SerializeField] - [DynamicallyShowInInspector( nameof(EnableMagnetometer) )] + [DynamicallyShowInInspector( nameof( EnableMagnetometer ) )] [DisableInRuntimeInspector] public ImuAttachment MagnetometerAttachment { get; private set; } = new ImuAttachment( ImuAttachment.ImuAttachmentType.Magnetometer, diff --git a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs index f821ac47..505d4aab 100644 --- a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs +++ b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs @@ -1566,7 +1566,7 @@ public static object ImuAttachmentDrawer( object[] objects, InvokeWrapper wrappe var data = wrapper.Get( objects[0] ); using ( new InspectorGUI.IndentScope() ) { data.TriaxialRange = TriaxialRangeDataGUI( data.TriaxialRange ); - data.CrossAxisSensitivity = Mathf.Clamp01(EditorGUILayout.FloatField( "Cross Axis Sensitivity", data.CrossAxisSensitivity )); + data.CrossAxisSensitivity = Mathf.Clamp01( EditorGUILayout.FloatField( "Cross Axis Sensitivity", data.CrossAxisSensitivity ) ); data.ZeroBias = EditorGUILayout.Vector3Field( "Zero Rate Bias", data.ZeroBias ); EditorGUI.BeginChangeCheck(); data.OutputFlags = OutputXYZGUI( data.OutputFlags ); From 963931c05f9a071e73c894299bd4c63c09bfc51f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 17 Mar 2026 10:34:03 +0100 Subject: [PATCH 45/63] private bools for property visibility, fix bug from previous commit --- AGXUnity/Sensor/EncoderSensor.cs | 4 ++-- AGXUnity/Sensor/OdometerSensor.cs | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs index c86cfdc3..6db27668 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -141,10 +141,10 @@ public enum WheelJointSample public double SpeedBuffer { get; private set; } [HideInInspector] - public bool IsWheelJoint => ConstraintComponent == null || ConstraintComponent is WheelJoint; + private bool IsWheelJoint => ConstraintComponent == null || ConstraintComponent is WheelJoint; [HideInInspector] - public bool HasTwoDof => ConstraintComponent == null || ( ConstraintComponent is Constraint constraint && constraint.Type == ConstraintType.CylindricalJoint ); + private bool HasTwoDof => ConstraintComponent == null || ( ConstraintComponent is Constraint constraint && constraint.Type == ConstraintType.CylindricalJoint ); private ScriptComponent FindParentJoint() { diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs index 42909f38..281890ca 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -80,7 +80,7 @@ public class OdometerSensor : ScriptComponent [InspectorGroupEnd] [HideInInspector] - public bool IsWheelJoint => ConstraintComponent == null || ConstraintComponent is WheelJoint; + private bool IsWheelJoint => ConstraintComponent == null || ConstraintComponent is WheelJoint; [RuntimeValue] public float SensorValue { get; private set; } @@ -127,7 +127,7 @@ protected override bool Initialize() modifiers.Add( new MonoaxialGaussianNoise( (double)TotalGaussianNoiseRms ) ); if ( EnableSignalResolution ) - modifiers.Add( new MonoaxialSignalResolution( GetSignalResolution() ) ); + modifiers.Add( new MonoaxialSignalResolution( SignalResolution ) ); if ( EnableSignalScaling ) modifiers.Add( new MonoaxialSignalScaling( (double)SignalScaling ) ); From 37c547d1ebd5e327f630a6821775ac78cb491da2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 17 Mar 2026 10:54:38 +0100 Subject: [PATCH 46/63] Move tooltips to where they can actually be read --- AGXUnity/Sensor/ImuSensor.cs | 4 ---- .../InvokeWrapperInspectorDrawer.cs | 20 ++++++++++++------- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 09b1d6b5..723d47a2 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -35,26 +35,22 @@ public enum ImuAttachmentType /// /// Detectable measurement range, in m/s^2 / radians/s / T /// - [Tooltip("Measurement range - values outside of range will be truncated")] public TriaxialRangeData TriaxialRange; /// /// Cross axis sensitivity - how measurements in one axis affects the other axes. Ratio 0 to 1. /// - [Tooltip("Cross axis sensitivity - how measurements in one axis affects the other axes. Ratio 0 to 1.")] public float CrossAxisSensitivity; /// /// Bias reported in each axis under conditions without externally applied transformation /// - [Tooltip("Bias reported in each axis under conditions without externally applied transformation")] public Vector3 ZeroBias; public bool EnableLinearAccelerationEffects = false; /// /// Applies an offset to the zero rate bias depending on the linear acceleration that the gyroscope is exposed to /// - [Tooltip("Offset to the zero rate bias depending on the linear acceleration")] public Vector3 LinearAccelerationEffects; public bool EnableTotalGaussianNoise = false; diff --git a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs index 350436f1..29ab54ff 100644 --- a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs +++ b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs @@ -1566,8 +1566,12 @@ public static object ImuAttachmentDrawer( object[] objects, InvokeWrapper wrappe var data = wrapper.Get( objects[0] ); using ( new InspectorGUI.IndentScope() ) { data.TriaxialRange = TriaxialRangeDataGUI( data.TriaxialRange ); - data.CrossAxisSensitivity = Mathf.Clamp01( EditorGUILayout.FloatField( "Cross Axis Sensitivity", data.CrossAxisSensitivity ) ); - data.ZeroBias = EditorGUILayout.Vector3Field( "Zero Rate Bias", data.ZeroBias ); + data.CrossAxisSensitivity = Mathf.Clamp01( EditorGUILayout.FloatField( GUI.MakeLabel( + "Cross Axis Sensitivity", false, "How measurements in one axis affects the other axes. Ratio 0 to 1." ), + data.CrossAxisSensitivity ) ); + data.ZeroBias = EditorGUILayout.Vector3Field( GUI.MakeLabel( + "Zero Rate Bias", false, "Value reported per axis when there is no signal input (zero measurement)" ), + data.ZeroBias ); EditorGUI.BeginChangeCheck(); data.OutputFlags = OutputXYZGUI( data.OutputFlags ); if ( EditorGUI.EndChangeCheck() ) @@ -1580,23 +1584,23 @@ public static object ImuAttachmentDrawer( object[] objects, InvokeWrapper wrappe data.EnableTotalGaussianNoise, data.TotalGaussianNoise, "Total Gaussian Noise", - "" ); + "Base level noise in the measurement signal (RMS)" ); (data.EnableSignalScaling, data.SignalScaling) = OptionalVector3GUI( data.EnableSignalScaling, data.SignalScaling, "Signal Scaling", - "" ); + "Linear scaling to each axis of the triaxial signal" ); (data.EnableGaussianSpectralNoise, data.GaussianSpectralNoise) = OptionalVector3GUI( data.EnableGaussianSpectralNoise, data.GaussianSpectralNoise, "Gaussian Spectral Noise", - "" ); + "Gaussian noise dependent on the sample frequency" ); if ( data.Type == ImuAttachment.ImuAttachmentType.Gyroscope ) { (data.EnableLinearAccelerationEffects, data.LinearAccelerationEffects) = OptionalVector3GUI( data.EnableLinearAccelerationEffects, data.LinearAccelerationEffects, "Linear Acceleration Effects", - "" ); + "Offset to the zero rate bias depending on the linear acceleration" ); } } } @@ -1626,7 +1630,9 @@ private static (bool, Vector3) OptionalVector3GUI( bool toggle, Vector3 value, s private static TriaxialRangeData TriaxialRangeDataGUI( TriaxialRangeData data ) { - data.Mode = (TriaxialRangeData.ConfigurationMode)EditorGUILayout.EnumPopup( "Sensor Measurement Range", data.Mode ); + data.Mode = (TriaxialRangeData.ConfigurationMode)EditorGUILayout.EnumPopup( GUI.MakeLabel( + "Sensor Measurement Range", false, "Measurement range - values outside of range will be truncated. Default units m/s^2, radians/s, T" ), + data.Mode ); using ( new InspectorGUI.IndentScope() ) { switch ( data.Mode ) { From 4e69a79b9987f6b2a1336bc3f886e4cc7889124f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 17 Mar 2026 10:55:03 +0100 Subject: [PATCH 47/63] Should have been commited before --- Tests/Runtime/ImuAndEncoderTests.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tests/Runtime/ImuAndEncoderTests.cs b/Tests/Runtime/ImuAndEncoderTests.cs index 3cb062be..fcaca14c 100644 --- a/Tests/Runtime/ImuAndEncoderTests.cs +++ b/Tests/Runtime/ImuAndEncoderTests.cs @@ -24,7 +24,7 @@ public void SetupSensorScene() Simulation.Instance.PreIntegratePositions = true; m_keep.Add( Simulation.Instance.gameObject ); - SensorEnvironment.Instance.FieldType = SensorEnvironment.MagneticFieldType.UNIFORM; + SensorEnvironment.Instance.FieldType = SensorEnvironment.MagneticFieldType.Uniform; SensorEnvironment.Instance.MagneticFieldVector = Vector3.one; m_keep.Add( SensorEnvironment.Instance.gameObject ); } From 49780a6ce821bc83592ca20bc2c5e6c2aff4816a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Fri, 20 Mar 2026 11:30:45 +0100 Subject: [PATCH 48/63] Fix bug on timestep 0, some encoder sensor settings editable runtime --- AGXUnity/Sensor/EncoderSensor.cs | 61 ++++++++++++++++++++++++------- AGXUnity/Sensor/ImuSensor.cs | 6 ++- AGXUnity/Sensor/OdometerSensor.cs | 6 ++- 3 files changed, 58 insertions(+), 15 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs index 6db27668..19e297ea 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -25,7 +25,7 @@ public class EncoderSensor : ScriptComponent /// If left empty the component will use the first compatible parent. /// Compatible with: Hinge, Prismatic, CylindricalJoint, WheelJoint /// - [field: SerializeField] + [SerializeField] [Tooltip( "Constraint / WheelJoint component to attach encoder to. If unset, the first compatible parent is used" )] [DisableInRuntimeInspector] public ScriptComponent ConstraintComponent { get; set; } = null; @@ -34,10 +34,19 @@ public class EncoderSensor : ScriptComponent /// Accumulation mode of the encoder /// INCREMENTAL wraps within the cycle range, ABSOLUTE is single-turn absolute /// - [field: SerializeField] + [SerializeField] + private EncoderModel.Mode m_mode = EncoderModel.Mode.ABSOLUTE; [Tooltip( "Encoder accumulation mode. INCREMENTAL wraps within the cycle range; ABSOLUTE is single-turn absolute" )] - [DisableInRuntimeInspector] - public EncoderModel.Mode Mode { get; set; } = EncoderModel.Mode.ABSOLUTE; + public EncoderModel.Mode Mode + { + get => m_mode; + set + { + m_mode = value; + if ( Native != null ) + Native.getModel().setMode( m_mode ); + } + } /// /// Cycle range [min, max] in sensor units. @@ -46,8 +55,17 @@ public class EncoderSensor : ScriptComponent /// [field: SerializeField] [Tooltip( "Cycle range [min, max] in sensor units. Rotary: radians (commonly [0, 2π]). Linear: meters." )] - [DisableInRuntimeInspector] - public RangeReal MeasurementRange { get; set; } = new RangeReal( float.MinValue, float.MaxValue ); + private RangeReal m_measurementRange = new RangeReal( float.MinValue, float.MaxValue ); + public RangeReal MeasurementRange + { + get => m_measurementRange; + set + { + m_measurementRange = value; + if ( Native != null ) + Native.getModel().setRange( m_measurementRange.Native ); + } + } public enum TwoDofSample { @@ -55,11 +73,20 @@ public enum TwoDofSample Translational } - [field: SerializeField] + [SerializeField] + private TwoDofSample m_sampleTwoDof = TwoDofSample.Rotational; [Tooltip( "Which DoF to sample for 2-DoF constraints (Cylindrical)" )] - [DisableInRuntimeInspector] [DynamicallyShowInInspector( nameof( HasTwoDof ), true )] - public TwoDofSample SampleTwoDof { get; set; } = TwoDofSample.Rotational; + public TwoDofSample SampleTwoDof + { + get => m_sampleTwoDof; + set + { + m_sampleTwoDof = value; + if ( Native != null && Native.getConstraint() is agx.CylindricalJoint ) + Native.setConstraintSampleDof( (ulong)ToConstraint2Dof( m_sampleTwoDof ) ); + } + } public enum WheelJointSample { @@ -68,23 +95,31 @@ public enum WheelJointSample Suspension } - [field: SerializeField] + [SerializeField] + private WheelJointSample m_sampleWheelJoint = WheelJointSample.WheelAxle; [Tooltip( "Which secondary constraint to sample for WheelJoint" )] [DisableInRuntimeInspector] [DynamicallyShowInInspector( nameof( IsWheelJoint ), true )] - public WheelJointSample SampleWheelJoint { get; set; } = WheelJointSample.WheelAxle; + public WheelJointSample SampleWheelJoint + { + get => m_sampleWheelJoint; + set + { + m_sampleWheelJoint = value; + if ( Native != null && Native.getConstraint() is agxVehicle.WheelJoint ) + Native.setConstraintSampleDof( (ulong)ToWheelSecondaryConstraint( m_sampleWheelJoint ) ); + } + } /// /// Output selection /// [field: SerializeField] [Tooltip( "Include position in the output" )] - [DisableInRuntimeInspector] public bool OutputPosition { get; set; } = true; [field: SerializeField] [Tooltip( "Include speed in the output" )] - [DisableInRuntimeInspector] public bool OutputSpeed { get; set; } = false; [InspectorGroupBegin( Name = "Modifiers", DefaultExpanded = true )] diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 723d47a2..02a42182 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -294,6 +294,10 @@ private void GetOutput( IMUOutput output, double[] buffer ) return; } + // Usually only at first timestep + if ( views.size() == 0 ) + return; + NineDoFValue view = views[ 0 ]; // This is all kind of a workaround to use a ninedof buffer with an arbitrary number @@ -333,7 +337,7 @@ private void GetOutput( IMUOutput output, double[] buffer ) private void OnPostSynchronizeTransforms() { - if ( !gameObject.activeInHierarchy ) + if ( !gameObject.activeInHierarchy || Native == null ) return; GetOutput( Native.getOutputHandler().get( m_outputID ), OutputBuffer ); diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs index 281890ca..8f7a6b4c 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -198,7 +198,11 @@ private double GetOutput( OdometerOutput output ) return 0; var view = output.view(); - return view[ 0 ]; + + if ( view.size() > 0 ) + return view[ 0 ]; + else + return 0; } private void OnPostSynchronizeTransforms() From 6505386444ee8c03391a7c5eb891859ca501edb8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Fri, 20 Mar 2026 15:21:14 +0100 Subject: [PATCH 49/63] Fields to properties --- AGXUnity/Sensor/EncoderSensor.cs | 133 ++++++++++++++++++++++++------- 1 file changed, 105 insertions(+), 28 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs index 19e297ea..2b82cb1d 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -14,6 +14,10 @@ namespace AGXUnity.Sensor [HelpURL( "https://www.algoryx.se/documentation/complete/agx/tags/latest/doc/UserManual/source/agxsensor.html#encoder" )] public class EncoderSensor : ScriptComponent { + private const double DisabledTotalGaussianNoiseRms = 0.0; + private const double DisabledSignalResolution = 1e-4; // TODO hard to give this a good default value... + private const double DisabledSignalScaling = 1.0; + /// /// Native instance /// @@ -123,50 +127,103 @@ public WheelJointSample SampleWheelJoint public bool OutputSpeed { get; set; } = false; [InspectorGroupBegin( Name = "Modifiers", DefaultExpanded = true )] - [field: SerializeField] - [DisableInRuntimeInspector] - public bool EnableTotalGaussianNoise { get; set; } = false; + [SerializeField] + private bool m_enableTotalGaussianNoise = false; + public bool EnableTotalGaussianNoise + { + get => m_enableTotalGaussianNoise; + set + { + m_enableTotalGaussianNoise = value; + SynchronizeTotalGaussianNoiseModifier(); + } + } /// /// Noise RMS in sensor units (position units: radians/meters). /// - [field: SerializeField] + [SerializeField] + private float m_totalGaussianNoiseRms = 0.0f; [Tooltip( "Gaussian noise RMS (position units: radians/meters)." )] [DynamicallyShowInInspector( nameof( EnableTotalGaussianNoise ) )] - [DisableInRuntimeInspector] - public float TotalGaussianNoiseRms { get; set; } = 0.0f; + public float TotalGaussianNoiseRms + { + get => m_totalGaussianNoiseRms; + set + { + m_totalGaussianNoiseRms = value; + SynchronizeTotalGaussianNoiseModifier(); + } + } - [field: SerializeField] - [DisableInRuntimeInspector] - public bool EnableSignalResolution { get; set; } = false; + [SerializeField] + private bool m_enableSignalResolution = false; + public bool EnableSignalResolution + { + get => m_enableSignalResolution; + set + { + m_enableSignalResolution = value; + SynchronizeSignalResolutionModifier(); + } + } /// /// Resolution/bin size in sensor units (position units: radians/meters). /// - [field: SerializeField] + [SerializeField] + private float m_signalResolution = 0.01f; [Tooltip( "Resolution/bin size (position units: radians/meters)." )] [DynamicallyShowInInspector( nameof( EnableSignalResolution ) )] - [DisableInRuntimeInspector] [ClampAboveZeroInInspector] - public float SignalResolution { get; set; } = 0.01f; + public float SignalResolution + { + get => m_signalResolution; + set + { + m_signalResolution = value; + SynchronizeSignalResolutionModifier(); + } + } - [field: SerializeField] - [DisableInRuntimeInspector] - public bool EnableSignalScaling { get; set; } = false; + [SerializeField] + private bool m_enableSignalScaling = false; + public bool EnableSignalScaling + { + get => m_enableSignalScaling; + set + { + m_enableSignalScaling = value; + SynchronizeSignalScalingModifier(); + } + } /// /// Constant scaling factor. /// - [field: SerializeField] + [SerializeField] + private float m_signalScaling = 1.0f; [Tooltip( "Scaling factor applied to encoder outputs." )] [DynamicallyShowInInspector( nameof( EnableSignalScaling ) )] - [DisableInRuntimeInspector] - public float SignalScaling { get; set; } = 1.0f; + public float SignalScaling + { + get => m_signalScaling; + set + { + m_signalScaling = value; + SynchronizeSignalScalingModifier(); + } + } [InspectorGroupEnd] [RuntimeValue] public float PositionValue { get; private set; } [RuntimeValue] public float SpeedValue { get; private set; } + private IMonoaxialSignalSystemNodeRefVector m_modifiers = new IMonoaxialSignalSystemNodeRefVector(); + private MonoaxialGaussianNoise m_totalGaussianNoiseModifier = null; + private MonoaxialSignalResolution m_signalResolutionModifier = null; + private MonoaxialSignalScaling m_signalScalingModifier = null; + private uint m_outputID = 0; [HideInInspector] @@ -181,6 +238,28 @@ public WheelJointSample SampleWheelJoint [HideInInspector] private bool HasTwoDof => ConstraintComponent == null || ( ConstraintComponent is Constraint constraint && constraint.Type == ConstraintType.CylindricalJoint ); + private void SynchronizeTotalGaussianNoiseModifier() + { + m_totalGaussianNoiseModifier?.setNoiseRms( GetTotalGaussianNoiseRms() ); + } + + private void SynchronizeSignalResolutionModifier() + { + m_signalResolutionModifier?.setResolution( GetSignalResolutionValue() ); + } + + private void SynchronizeSignalScalingModifier() + { + m_signalScalingModifier?.setScaling( GetSignalScalingValue() ); + } + + private double GetTotalGaussianNoiseRms() => EnableTotalGaussianNoise ? TotalGaussianNoiseRms : DisabledTotalGaussianNoiseRms; + + private double GetSignalResolutionValue() => EnableSignalResolution ? SignalResolution : DisabledSignalResolution; + + private double GetSignalScalingValue() => EnableSignalScaling ? SignalScaling : DisabledSignalScaling; + + private ScriptComponent FindParentJoint() { ScriptComponent component = GetComponentInParent(); @@ -209,18 +288,16 @@ protected override bool Initialize() return false; } - var modifiers = new IMonoaxialSignalSystemNodeRefVector(); - - if ( EnableTotalGaussianNoise ) - modifiers.Add( new MonoaxialGaussianNoise( (double)TotalGaussianNoiseRms ) ); - - if ( EnableSignalResolution ) - modifiers.Add( new MonoaxialSignalResolution( (double)SignalResolution ) ); + m_modifiers = new IMonoaxialSignalSystemNodeRefVector(); + m_totalGaussianNoiseModifier = new MonoaxialGaussianNoise( GetTotalGaussianNoiseRms() ); + m_signalResolutionModifier = new MonoaxialSignalResolution( GetSignalResolutionValue() ); + m_signalScalingModifier = new MonoaxialSignalScaling( GetSignalScalingValue() ); - if ( EnableSignalScaling ) - modifiers.Add( new MonoaxialSignalScaling( (double)SignalScaling ) ); + m_modifiers.Add( m_totalGaussianNoiseModifier ); + m_modifiers.Add( m_signalResolutionModifier ); + m_modifiers.Add( m_signalScalingModifier ); - m_nativeModel = new EncoderModel( Mode, MeasurementRange.Native, modifiers ); + m_nativeModel = new EncoderModel( Mode, MeasurementRange.Native, m_modifiers ); if ( m_nativeModel == null ) { Debug.LogWarning( "Could not create native encoder model, encoder will be inactive" ); From e8ebde2fb4dd1378da58388f5b50de0d00a1d1ff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Fri, 20 Mar 2026 15:26:50 +0100 Subject: [PATCH 50/63] Also odometer --- AGXUnity/Sensor/EncoderSensor.cs | 3 +- AGXUnity/Sensor/OdometerSensor.cs | 153 +++++++++++++++++++++++------- 2 files changed, 121 insertions(+), 35 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs index 2b82cb1d..d6ab543a 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -126,9 +126,9 @@ public WheelJointSample SampleWheelJoint [Tooltip( "Include speed in the output" )] public bool OutputSpeed { get; set; } = false; - [InspectorGroupBegin( Name = "Modifiers", DefaultExpanded = true )] [SerializeField] private bool m_enableTotalGaussianNoise = false; + [InspectorGroupBegin( Name = "Modifiers", DefaultExpanded = true )] public bool EnableTotalGaussianNoise { get => m_enableTotalGaussianNoise; @@ -214,6 +214,7 @@ public float SignalScaling SynchronizeSignalScalingModifier(); } } + [InspectorGroupEnd] [RuntimeValue] public float PositionValue { get; private set; } diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs index 8f7a6b4c..3dfe052b 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -14,6 +14,10 @@ namespace AGXUnity.Sensor [HelpURL( "https://www.algoryx.se/documentation/complete/agx/tags/latest/doc/UserManual/source/agxsensor.html#odometer" )] public class OdometerSensor : ScriptComponent { + private const double DisabledTotalGaussianNoiseRms = 0.0; + private const double DisabledSignalResolution = 1e-4; + private const double DisabledSignalScaling = 1.0; + /// /// Native instance /// @@ -25,57 +29,114 @@ public class OdometerSensor : ScriptComponent /// If left empty the component will use the first compatible parent. /// Compatible with: Hinge, CylindricalJoint, WheelJoint /// - [field: SerializeField] + [SerializeField] [Tooltip( "Constraint / WheelJoint component to attach odometer to: Hinge, CylindricalJoint, WheelJoint. If unset, the first compatible parent is used." )] - [DisableInRuntimeInspector] public ScriptComponent ConstraintComponent { get; set; } = null; /// /// Wheel radius in meters /// - [field: SerializeField] + [SerializeField] + private float m_wheelRadius = 0.5f; [Tooltip( "Wheel radius in meters" )] - [DisableInRuntimeInspector] [ClampAboveZeroInInspector] - public float WheelRadius { get; set; } = 0.5f; + public float WheelRadius + { + get => m_wheelRadius; + set + { + m_wheelRadius = value; + if ( Native != null ) + Native.getModel().setWheelRadius( m_wheelRadius ); + SynchronizeSignalResolutionModifier(); + } + } + [SerializeField] + private bool m_enableTotalGaussianNoise = false; [InspectorGroupBegin( Name = "Modifiers", DefaultExpanded = true )] - [field: SerializeField] - [DisableInRuntimeInspector] - public bool EnableTotalGaussianNoise { get; set; } = false; + public bool EnableTotalGaussianNoise + { + get => m_enableTotalGaussianNoise; + set + { + m_enableTotalGaussianNoise = value; + SynchronizeTotalGaussianNoiseModifier(); + } + } /// /// Noise RMS in meters. /// - [field: SerializeField] + [SerializeField] + private float m_totalGaussianNoiseRms = 0.0f; [Tooltip( "Gaussian noise RMS" )] [DynamicallyShowInInspector( nameof(EnableTotalGaussianNoise) )] - [DisableInRuntimeInspector] - public float TotalGaussianNoiseRms { get; set; } = 0.0f; + public float TotalGaussianNoiseRms + { + get => m_totalGaussianNoiseRms; + set + { + m_totalGaussianNoiseRms = value; + SynchronizeTotalGaussianNoiseModifier(); + } + } - [field: SerializeField] - [DisableInRuntimeInspector] - public bool EnableSignalResolution { get; set; } = false; + [SerializeField] + private bool m_enableSignalResolution = false; + public bool EnableSignalResolution + { + get => m_enableSignalResolution; + set + { + m_enableSignalResolution = value; + SynchronizeSignalResolutionModifier(); + } + } /// /// Pulses per wheel revolution. /// - [field: SerializeField] + [SerializeField] + private int m_pulsesPerRevolution = 1024; [Tooltip( "Pulses per wheel revolution" )] [DynamicallyShowInInspector( nameof(EnableSignalResolution) )] - [DisableInRuntimeInspector] [ClampAboveZeroInInspector] - public int PulsesPerRevolution { get; set; } = 1024; + public int PulsesPerRevolution + { + get => m_pulsesPerRevolution; + set + { + m_pulsesPerRevolution = value; + SynchronizeSignalResolutionModifier(); + } + } - [field: SerializeField] - [DisableInRuntimeInspector] - public bool EnableSignalScaling { get; set; } = false; + [SerializeField] + private bool m_enableSignalScaling = false; + public bool EnableSignalScaling + { + get => m_enableSignalScaling; + set + { + m_enableSignalScaling = value; + SynchronizeSignalScalingModifier(); + } + } /// /// Constant scaling factor. /// - [field: SerializeField] + [SerializeField] + private float m_signalScaling = 1.0f; [Tooltip( "Scaling factor applied to the distance signal" )] [DynamicallyShowInInspector( nameof(EnableSignalScaling) )] - [DisableInRuntimeInspector] - public float SignalScaling { get; set; } = 1.0f; + public float SignalScaling + { + get => m_signalScaling; + set + { + m_signalScaling = value; + SynchronizeSignalScalingModifier(); + } + } [InspectorGroupEnd] @@ -84,6 +145,11 @@ public class OdometerSensor : ScriptComponent [RuntimeValue] public float SensorValue { get; private set; } + private IMonoaxialSignalSystemNodeRefVector m_modifiers = new IMonoaxialSignalSystemNodeRefVector(); + private MonoaxialGaussianNoise m_totalGaussianNoiseModifier = null; + private MonoaxialSignalResolution m_signalResolutionModifier = null; + private MonoaxialSignalScaling m_signalScalingModifier = null; + private uint m_outputID = 0; [HideInInspector] public double OutputBuffer { get; private set; } @@ -121,18 +187,16 @@ protected override bool Initialize() return false; } - var modifiers = new IMonoaxialSignalSystemNodeRefVector(); - - if ( EnableTotalGaussianNoise ) - modifiers.Add( new MonoaxialGaussianNoise( (double)TotalGaussianNoiseRms ) ); + m_modifiers = new IMonoaxialSignalSystemNodeRefVector(); + m_totalGaussianNoiseModifier = new MonoaxialGaussianNoise( GetTotalGaussianNoiseRms() ); + m_signalResolutionModifier = new MonoaxialSignalResolution( GetSignalResolutionValue() ); + m_signalScalingModifier = new MonoaxialSignalScaling( GetSignalScalingValue() ); - if ( EnableSignalResolution ) - modifiers.Add( new MonoaxialSignalResolution( SignalResolution ) ); + m_modifiers.Add( m_totalGaussianNoiseModifier ); + m_modifiers.Add( m_signalResolutionModifier ); + m_modifiers.Add( m_signalScalingModifier ); - if ( EnableSignalScaling ) - modifiers.Add( new MonoaxialSignalScaling( (double)SignalScaling ) ); - - m_nativeModel = new OdometerModel( (double)WheelRadius, modifiers ); + m_nativeModel = new OdometerModel( (double)WheelRadius, m_modifiers ); if ( m_nativeModel == null ) { Debug.LogWarning( "Could not create native odometer model, odometer will be inactive" ); @@ -176,7 +240,28 @@ protected override bool Initialize() return true; } - private double SignalResolution => 2.0 * System.Math.PI * WheelRadius / PulsesPerRevolution; + private double SignalResolutionValue => 2.0 * System.Math.PI * WheelRadius / PulsesPerRevolution; + + private void SynchronizeTotalGaussianNoiseModifier() + { + m_totalGaussianNoiseModifier?.setNoiseRms( GetTotalGaussianNoiseRms() ); + } + + private void SynchronizeSignalResolutionModifier() + { + m_signalResolutionModifier?.setResolution( GetSignalResolutionValue() ); + } + + private void SynchronizeSignalScalingModifier() + { + m_signalScalingModifier?.setScaling( GetSignalScalingValue() ); + } + + private double GetTotalGaussianNoiseRms() => EnableTotalGaussianNoise ? TotalGaussianNoiseRms : DisabledTotalGaussianNoiseRms; + + private double GetSignalResolutionValue() => EnableSignalResolution ? SignalResolutionValue : DisabledSignalResolution; + + private double GetSignalScalingValue() => EnableSignalScaling ? SignalScaling : DisabledSignalScaling; private static Odometer CreateNativeOdometerFromConstraint( agx.Constraint nativeConstraint, OdometerModel model ) { From a1a2e025b8e8a6bf0ce7b89e2455371d9f8d4beb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 23 Mar 2026 09:42:20 +0100 Subject: [PATCH 51/63] Runtime editing of most IMU properties --- AGXUnity/Sensor/ImuSensor.cs | 308 ++++++++++++++++++++++++++----- AGXUnity/Sensor/TriaxialTypes.cs | 67 ++++++- 2 files changed, 326 insertions(+), 49 deletions(-) diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 02a42182..9ed8f564 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -19,6 +19,11 @@ public enum OutputXYZ // Data class to store IMU sensor attachment configuration public class ImuAttachment { + [NonSerialized] + private Action m_onBaseSettingsChanged = null; + [NonSerialized] + private Action m_onModifierSettingsChanged = null; + public enum ImuAttachmentType { Accelerometer, @@ -35,41 +40,152 @@ public enum ImuAttachmentType /// /// Detectable measurement range, in m/s^2 / radians/s / T /// - public TriaxialRangeData TriaxialRange; + [SerializeField] + private TriaxialRangeData m_triaxialRange; + public TriaxialRangeData TriaxialRange + { + get => m_triaxialRange; + set + { + m_triaxialRange = value; + m_triaxialRange?.SetOnChanged( m_onBaseSettingsChanged ); + m_onBaseSettingsChanged?.Invoke(); + } + } /// /// Cross axis sensitivity - how measurements in one axis affects the other axes. Ratio 0 to 1. /// - public float CrossAxisSensitivity; + [SerializeField] + private float m_crossAxisSensitivity; + public float CrossAxisSensitivity + { + get => m_crossAxisSensitivity; + set + { + m_crossAxisSensitivity = value; + m_onBaseSettingsChanged?.Invoke(); + } + } /// /// Bias reported in each axis under conditions without externally applied transformation /// - public Vector3 ZeroBias; + [SerializeField] + private Vector3 m_zeroBias; + public Vector3 ZeroBias + { + get => m_zeroBias; + set + { + m_zeroBias = value; + m_onBaseSettingsChanged?.Invoke(); + } + } - public bool EnableLinearAccelerationEffects = false; + [SerializeField] + private bool m_enableLinearAccelerationEffects = false; + public bool EnableLinearAccelerationEffects + { + get => m_enableLinearAccelerationEffects; + set + { + m_enableLinearAccelerationEffects = value; + m_onModifierSettingsChanged?.Invoke(); + } + } /// /// Applies an offset to the zero rate bias depending on the linear acceleration that the gyroscope is exposed to /// - public Vector3 LinearAccelerationEffects; + [SerializeField] + private Vector3 m_linearAccelerationEffects; + public Vector3 LinearAccelerationEffects + { + get => m_linearAccelerationEffects; + set + { + m_linearAccelerationEffects = value; + m_onModifierSettingsChanged?.Invoke(); + } + } - public bool EnableTotalGaussianNoise = false; + [SerializeField] + private bool m_enableTotalGaussianNoise = false; + public bool EnableTotalGaussianNoise + { + get => m_enableTotalGaussianNoise; + set + { + m_enableTotalGaussianNoise = value; + m_onModifierSettingsChanged?.Invoke(); + } + } /// /// Base level noise in the measurement signal /// - public Vector3 TotalGaussianNoise; + [SerializeField] + private Vector3 m_totalGaussianNoise = Vector3.zero; + public Vector3 TotalGaussianNoise + { + get => m_totalGaussianNoise; + set + { + m_totalGaussianNoise = value; + m_onModifierSettingsChanged?.Invoke(); + } + } - public bool EnableSignalScaling = false; + [SerializeField] + private bool m_enableSignalScaling = false; + public bool EnableSignalScaling + { + get => m_enableSignalScaling; + set + { + m_enableSignalScaling = value; + m_onModifierSettingsChanged?.Invoke(); + } + } /// /// Constant scaling to the triaxial signal /// - public Vector3 SignalScaling = Vector3.one; + [SerializeField] + private Vector3 m_signalScaling = Vector3.one; + public Vector3 SignalScaling + { + get => m_signalScaling; + set + { + m_signalScaling = value; + m_onModifierSettingsChanged?.Invoke(); + } + } - public bool EnableGaussianSpectralNoise = false; + [SerializeField] + private bool m_enableGaussianSpectralNoise = false; + public bool EnableGaussianSpectralNoise + { + get => m_enableGaussianSpectralNoise; + set + { + m_enableGaussianSpectralNoise = value; + m_onModifierSettingsChanged?.Invoke(); + } + } /// /// Sample frequency dependent Gaussian noise /// - public Vector3 GaussianSpectralNoise; + [SerializeField] + private Vector3 m_gaussianSpectralNoise = Vector3.zero; + public Vector3 GaussianSpectralNoise + { + get => m_gaussianSpectralNoise; + set + { + m_gaussianSpectralNoise = value; + m_onModifierSettingsChanged?.Invoke(); + } + } /// /// Output flags - which, if any, of x y z should be used in output view @@ -84,6 +200,13 @@ public ImuAttachment( ImuAttachmentType type, TriaxialRangeData triaxialRange, f CrossAxisSensitivity = crossAxisSensitivity; ZeroBias = zeroRateBias; } + + internal void SetRuntimeCallbacks( Action onBaseSettingsChanged, Action onModifierSettingsChanged ) + { + m_onBaseSettingsChanged = onBaseSettingsChanged; + m_onModifierSettingsChanged = onModifierSettingsChanged; + m_triaxialRange?.SetOnChanged( m_onBaseSettingsChanged ); + } } /// @@ -94,12 +217,38 @@ public ImuAttachment( ImuAttachmentType type, TriaxialRangeData triaxialRange, f [HelpURL( "https://us.download.algoryx.se/AGXUnity/documentation/current/editor_interface.html#simulating-imu-sensors" )] public class ImuSensor : ScriptComponent { + private static readonly Vector3 DisabledTotalGaussianNoise = Vector3.zero; + private static readonly Vector3 DisabledSignalScaling = Vector3.one; + private static readonly Vector3 DisabledGaussianSpectralNoise = Vector3.zero; + private static readonly Vector3 DisabledLinearAccelerationEffects = Vector3.zero; + /// /// Native instance, created in Start/Initialize. /// public IMU Native { get; private set; } = null; private IMUModel m_nativeModel = null; + private AccelerometerModel m_accelerometerModel = null; + private GyroscopeModel m_gyroscopeModel = null; + private MagnetometerModel m_magnetometerModel = null; + + private ITriaxialSignalSystemNodeRefVector m_accelerometerModifiers = null; + private ITriaxialSignalSystemNodeRefVector m_gyroscopeModifiers = null; + private ITriaxialSignalSystemNodeRefVector m_magnetometerModifiers = null; + + private TriaxialGaussianNoise m_accelerometerTotalGaussianNoiseModifier = null; + private TriaxialSignalScaling m_accelerometerSignalScalingModifier = null; + private TriaxialSpectralGaussianNoise m_accelerometerGaussianSpectralNoiseModifier = null; + + private TriaxialGaussianNoise m_gyroscopeTotalGaussianNoiseModifier = null; + private TriaxialSignalScaling m_gyroscopeSignalScalingModifier = null; + private TriaxialSpectralGaussianNoise m_gyroscopeGaussianSpectralNoiseModifier = null; + private GyroscopeLinearAccelerationEffects m_gyroscopeLinearAccelerationEffectsModifier = null; + + private TriaxialGaussianNoise m_magnetometerTotalGaussianNoiseModifier = null; + private TriaxialSignalScaling m_magnetometerSignalScalingModifier = null; + private TriaxialSpectralGaussianNoise m_magnetometerGaussianSpectralNoiseModifier = null; + /// /// When enabled, show configuration for the IMU attachment and create attachment when initializing object /// @@ -113,7 +262,6 @@ public class ImuSensor : ScriptComponent /// [field: SerializeField] [DynamicallyShowInInspector( nameof( EnableAccelerometer ) )] - [DisableInRuntimeInspector] public ImuAttachment AccelerometerAttachment { get; private set; } = new ImuAttachment( ImuAttachment.ImuAttachmentType.Accelerometer, new TriaxialRangeData(), @@ -133,7 +281,6 @@ public class ImuSensor : ScriptComponent /// [field: SerializeField] [DynamicallyShowInInspector( nameof( EnableGyroscope ) )] - [DisableInRuntimeInspector] public ImuAttachment GyroscopeAttachment { get; private set; } = new ImuAttachment( ImuAttachment.ImuAttachmentType.Gyroscope, new TriaxialRangeData(), @@ -153,7 +300,6 @@ public class ImuSensor : ScriptComponent /// [field: SerializeField] [DynamicallyShowInInspector( nameof( EnableMagnetometer ) )] - [DisableInRuntimeInspector] public ImuAttachment MagnetometerAttachment { get; private set; } = new ImuAttachment( ImuAttachment.ImuAttachmentType.Magnetometer, new TriaxialRangeData(), @@ -172,6 +318,10 @@ protected override bool Initialize() { SensorEnvironment.Instance.GetInitialized(); + AccelerometerAttachment.SetRuntimeCallbacks( SynchronizeAccelerometerSettings, SynchronizeAccelerometerModifiers ); + GyroscopeAttachment.SetRuntimeCallbacks( SynchronizeGyroscopeSettings, SynchronizeGyroscopeModifiers ); + MagnetometerAttachment.SetRuntimeCallbacks( SynchronizeMagnetometerSettings, SynchronizeMagnetometerModifiers ); + var rigidBody = GetComponentInParent(); if ( rigidBody == null ) { Debug.LogWarning( "No Rigidbody found in this object or parents, IMU will be inactive" ); @@ -179,63 +329,68 @@ protected override bool Initialize() } TrackedRigidBody = rigidBody; - Func buildModifiers = a => - { - var modifiers = new ITriaxialSignalSystemNodeRefVector(); - - if ( a.EnableTotalGaussianNoise ) - modifiers.Add(new TriaxialGaussianNoise(a.TotalGaussianNoise.ToHandedVec3())); - if ( a.EnableSignalScaling ) - modifiers.Add(new TriaxialSignalScaling(a.SignalScaling.ToHandedVec3())); - if ( a.EnableGaussianSpectralNoise ) - modifiers.Add(new TriaxialSpectralGaussianNoise(a.GaussianSpectralNoise.ToHandedVec3()) ); - if ( a.EnableLinearAccelerationEffects && a.Type == ImuAttachment.ImuAttachmentType.Gyroscope ) - modifiers.Add( new GyroscopeLinearAccelerationEffects(a.LinearAccelerationEffects.ToHandedVec3()) ); - - return modifiers; - }; var imu_attachments = new IMUModelSensorAttachmentRefVector(); // Accelerometer if ( EnableAccelerometer ) { - var modifiers = buildModifiers(AccelerometerAttachment); - - var accelerometer = new AccelerometerModel( + m_accelerometerModifiers = CreateModifiersVectorForAttachment( AccelerometerAttachment ); + m_accelerometerTotalGaussianNoiseModifier = new TriaxialGaussianNoise( GetTotalGaussianNoise( AccelerometerAttachment ).ToHandedVec3() ); + m_accelerometerSignalScalingModifier = new TriaxialSignalScaling( GetSignalScaling( AccelerometerAttachment ).ToHandedVec3() ); + m_accelerometerGaussianSpectralNoiseModifier = new TriaxialSpectralGaussianNoise( GetGaussianSpectralNoise( AccelerometerAttachment ).ToHandedVec3() ); + m_accelerometerModifiers.Add( m_accelerometerTotalGaussianNoiseModifier ); + m_accelerometerModifiers.Add( m_accelerometerSignalScalingModifier ); + m_accelerometerModifiers.Add( m_accelerometerGaussianSpectralNoiseModifier ); + + m_accelerometerModel = new AccelerometerModel( AccelerometerAttachment.TriaxialRange.GenerateTriaxialRange(), new TriaxialCrossSensitivity( AccelerometerAttachment.CrossAxisSensitivity ), AccelerometerAttachment.ZeroBias.ToHandedVec3(), - modifiers + m_accelerometerModifiers ); - imu_attachments.Add( new IMUModelAccelerometerAttachment( AffineMatrix4x4.identity(), accelerometer ) ); + imu_attachments.Add( new IMUModelAccelerometerAttachment( AffineMatrix4x4.identity(), m_accelerometerModel ) ); } // Gyroscope if ( EnableGyroscope ) { - var modifiers = buildModifiers(GyroscopeAttachment); - - var gyroscope = new GyroscopeModel( + m_gyroscopeModifiers = CreateModifiersVectorForAttachment( GyroscopeAttachment ); + m_gyroscopeTotalGaussianNoiseModifier = new TriaxialGaussianNoise( GetTotalGaussianNoise( GyroscopeAttachment ).ToHandedVec3() ); + m_gyroscopeSignalScalingModifier = new TriaxialSignalScaling( GetSignalScaling( GyroscopeAttachment ).ToHandedVec3() ); + m_gyroscopeGaussianSpectralNoiseModifier = new TriaxialSpectralGaussianNoise( GetGaussianSpectralNoise( GyroscopeAttachment ).ToHandedVec3() ); + m_gyroscopeLinearAccelerationEffectsModifier = new GyroscopeLinearAccelerationEffects( GetLinearAccelerationEffects( GyroscopeAttachment ).ToHandedVec3() ); + m_gyroscopeModifiers.Add( m_gyroscopeTotalGaussianNoiseModifier ); + m_gyroscopeModifiers.Add( m_gyroscopeSignalScalingModifier ); + m_gyroscopeModifiers.Add( m_gyroscopeGaussianSpectralNoiseModifier ); + m_gyroscopeModifiers.Add( m_gyroscopeLinearAccelerationEffectsModifier ); + + m_gyroscopeModel = new GyroscopeModel( GyroscopeAttachment.TriaxialRange.GenerateTriaxialRange(), new TriaxialCrossSensitivity( GyroscopeAttachment.CrossAxisSensitivity ), GyroscopeAttachment.ZeroBias.ToHandedVec3(), - modifiers + m_gyroscopeModifiers ); - imu_attachments.Add( new IMUModelGyroscopeAttachment( AffineMatrix4x4.identity(), gyroscope ) ); + imu_attachments.Add( new IMUModelGyroscopeAttachment( AffineMatrix4x4.identity(), m_gyroscopeModel ) ); } // Magnetometer if ( EnableMagnetometer ) { - var modifiers = buildModifiers(MagnetometerAttachment); - - var magnetometer = new MagnetometerModel( + m_magnetometerModifiers = CreateModifiersVectorForAttachment( MagnetometerAttachment ); + m_magnetometerTotalGaussianNoiseModifier = new TriaxialGaussianNoise( GetTotalGaussianNoise( MagnetometerAttachment ).ToHandedVec3() ); + m_magnetometerSignalScalingModifier = new TriaxialSignalScaling( GetSignalScaling( MagnetometerAttachment ).ToHandedVec3() ); + m_magnetometerGaussianSpectralNoiseModifier = new TriaxialSpectralGaussianNoise( GetGaussianSpectralNoise( MagnetometerAttachment ).ToHandedVec3() ); + m_magnetometerModifiers.Add( m_magnetometerTotalGaussianNoiseModifier ); + m_magnetometerModifiers.Add( m_magnetometerSignalScalingModifier ); + m_magnetometerModifiers.Add( m_magnetometerGaussianSpectralNoiseModifier ); + + m_magnetometerModel = new MagnetometerModel( MagnetometerAttachment.TriaxialRange.GenerateTriaxialRange(), new TriaxialCrossSensitivity( MagnetometerAttachment.CrossAxisSensitivity ), MagnetometerAttachment.ZeroBias.ToHandedVec3(), - modifiers + m_magnetometerModifiers ); - imu_attachments.Add( new IMUModelMagnetometerAttachment( AffineMatrix4x4.identity(), magnetometer ) ); + imu_attachments.Add( new IMUModelMagnetometerAttachment( AffineMatrix4x4.identity(), m_magnetometerModel ) ); } if ( imu_attachments.Count == 0 ) { @@ -281,6 +436,71 @@ protected override bool Initialize() return true; } + private static ITriaxialSignalSystemNodeRefVector CreateModifiersVectorForAttachment( ImuAttachment attachment ) + { + return new ITriaxialSignalSystemNodeRefVector(); + } + + private void SynchronizeAccelerometerSettings() + { + if ( m_accelerometerModel == null ) + return; + + m_accelerometerModel.setRange( AccelerometerAttachment.TriaxialRange.GenerateTriaxialRange() ); + m_accelerometerModel.setCrossAxisSensitivity( new TriaxialCrossSensitivity( AccelerometerAttachment.CrossAxisSensitivity ) ); + m_accelerometerModel.setZeroGBias( AccelerometerAttachment.ZeroBias.ToHandedVec3() ); + } + + private void SynchronizeGyroscopeSettings() + { + if ( m_gyroscopeModel == null ) + return; + + m_gyroscopeModel.setRange( GyroscopeAttachment.TriaxialRange.GenerateTriaxialRange() ); + m_gyroscopeModel.setCrossAxisSensitivity( new TriaxialCrossSensitivity( GyroscopeAttachment.CrossAxisSensitivity ) ); + m_gyroscopeModel.setZeroRateBias( GyroscopeAttachment.ZeroBias.ToHandedVec3() ); + } + + private void SynchronizeMagnetometerSettings() + { + if ( m_magnetometerModel == null ) + return; + + m_magnetometerModel.setRange( MagnetometerAttachment.TriaxialRange.GenerateTriaxialRange() ); + m_magnetometerModel.setCrossAxisSensitivity( new TriaxialCrossSensitivity( MagnetometerAttachment.CrossAxisSensitivity ) ); + m_magnetometerModel.setZeroFluxBias( MagnetometerAttachment.ZeroBias.ToHandedVec3() ); + } + + private void SynchronizeAccelerometerModifiers() + { + m_accelerometerTotalGaussianNoiseModifier?.setNoiseRms( GetTotalGaussianNoise( AccelerometerAttachment ).ToHandedVec3() ); + m_accelerometerSignalScalingModifier?.setScaling( GetSignalScaling( AccelerometerAttachment ).ToHandedVec3() ); + m_accelerometerGaussianSpectralNoiseModifier?.setNoiseDensity( GetGaussianSpectralNoise( AccelerometerAttachment ).ToHandedVec3() ); + } + + private void SynchronizeGyroscopeModifiers() + { + m_gyroscopeTotalGaussianNoiseModifier?.setNoiseRms( GetTotalGaussianNoise( GyroscopeAttachment ).ToHandedVec3() ); + m_gyroscopeSignalScalingModifier?.setScaling( GetSignalScaling( GyroscopeAttachment ).ToHandedVec3() ); + m_gyroscopeGaussianSpectralNoiseModifier?.setNoiseDensity( GetGaussianSpectralNoise( GyroscopeAttachment ).ToHandedVec3() ); + m_gyroscopeLinearAccelerationEffectsModifier?.setAccelerationEffects( GetLinearAccelerationEffects( GyroscopeAttachment ).ToHandedVec3() ); + } + + private void SynchronizeMagnetometerModifiers() + { + m_magnetometerTotalGaussianNoiseModifier?.setNoiseRms( GetTotalGaussianNoise( MagnetometerAttachment ).ToHandedVec3() ); + m_magnetometerSignalScalingModifier?.setScaling( GetSignalScaling( MagnetometerAttachment ).ToHandedVec3() ); + m_magnetometerGaussianSpectralNoiseModifier?.setNoiseDensity( GetGaussianSpectralNoise( MagnetometerAttachment ).ToHandedVec3() ); + } + + private static Vector3 GetTotalGaussianNoise( ImuAttachment attachment ) => attachment.EnableTotalGaussianNoise ? attachment.TotalGaussianNoise : DisabledTotalGaussianNoise; + + private static Vector3 GetSignalScaling( ImuAttachment attachment ) => attachment.EnableSignalScaling ? attachment.SignalScaling : DisabledSignalScaling; + + private static Vector3 GetGaussianSpectralNoise( ImuAttachment attachment ) => attachment.EnableGaussianSpectralNoise ? attachment.GaussianSpectralNoise : DisabledGaussianSpectralNoise; + + private static Vector3 GetLinearAccelerationEffects( ImuAttachment attachment ) => attachment.EnableLinearAccelerationEffects ? attachment.LinearAccelerationEffects : DisabledLinearAccelerationEffects; + private void GetOutput( IMUOutput output, double[] buffer ) { if ( Native == null || buffer == null || output == null ) { diff --git a/AGXUnity/Sensor/TriaxialTypes.cs b/AGXUnity/Sensor/TriaxialTypes.cs index 438d3b59..93958874 100644 --- a/AGXUnity/Sensor/TriaxialTypes.cs +++ b/AGXUnity/Sensor/TriaxialTypes.cs @@ -9,6 +9,9 @@ namespace AGXUnity.Sensor [Serializable] public class TriaxialRangeData { + [NonSerialized] + private Action m_onChanged = null; + public enum ConfigurationMode { MaxRange, @@ -17,15 +20,69 @@ public enum ConfigurationMode } [SerializeField] - public ConfigurationMode Mode = ConfigurationMode.MaxRange; + private ConfigurationMode m_mode = ConfigurationMode.MaxRange; + public ConfigurationMode Mode + { + get => m_mode; + set + { + m_mode = value; + m_onChanged?.Invoke(); + } + } + + [SerializeField] + private Vector2 m_equalAxesRange = new( float.MinValue, float.MaxValue ); + public Vector2 EqualAxesRange + { + get => m_equalAxesRange; + set + { + m_equalAxesRange = value; + m_onChanged?.Invoke(); + } + } [SerializeField] - public Vector2 EqualAxesRange = new(float.MinValue, float.MaxValue); + private Vector2 m_rangeX = new( float.MinValue, float.MaxValue ); + public Vector2 RangeX + { + get => m_rangeX; + set + { + m_rangeX = value; + m_onChanged?.Invoke(); + } + } [SerializeField] - public Vector2 RangeX = new(float.MinValue, float.MaxValue); - public Vector2 RangeY = new(float.MinValue, float.MaxValue); - public Vector2 RangeZ = new(float.MinValue, float.MaxValue); + private Vector2 m_rangeY = new( float.MinValue, float.MaxValue ); + public Vector2 RangeY + { + get => m_rangeY; + set + { + m_rangeY = value; + m_onChanged?.Invoke(); + } + } + + [SerializeField] + private Vector2 m_rangeZ = new( float.MinValue, float.MaxValue ); + public Vector2 RangeZ + { + get => m_rangeZ; + set + { + m_rangeZ = value; + m_onChanged?.Invoke(); + } + } + + internal void SetOnChanged( Action onChanged ) + { + m_onChanged = onChanged; + } public agxSensor.TriaxialRange GenerateTriaxialRange() { From b501d1a5b4f06f3eee87c37ea8891bc4ac51aa29 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 23 Mar 2026 10:52:34 +0100 Subject: [PATCH 52/63] Validation fix --- AGXUnity/Sensor/EncoderSensor.cs | 2 +- AGXUnity/Sensor/OdometerSensor.cs | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs index d6ab543a..ef7de446 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -181,7 +181,7 @@ public float SignalResolution get => m_signalResolution; set { - m_signalResolution = value; + m_signalResolution = value > 0 ? value : m_signalResolution; SynchronizeSignalResolutionModifier(); } } diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs index 3dfe052b..5f89d70a 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -45,7 +45,7 @@ public float WheelRadius get => m_wheelRadius; set { - m_wheelRadius = value; + m_wheelRadius = value > 0 ? value : m_wheelRadius; if ( Native != null ) Native.getModel().setWheelRadius( m_wheelRadius ); SynchronizeSignalResolutionModifier(); @@ -105,7 +105,7 @@ public int PulsesPerRevolution get => m_pulsesPerRevolution; set { - m_pulsesPerRevolution = value; + m_pulsesPerRevolution = value > 0 ? value : m_pulsesPerRevolution; SynchronizeSignalResolutionModifier(); } } From 183244f34df0cf6b85519120f2a6ef6e7a83ea02 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 23 Mar 2026 10:59:30 +0100 Subject: [PATCH 53/63] Formatting --- AGXUnity/Sensor/OdometerSensor.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs index 5f89d70a..d077228b 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -70,7 +70,7 @@ public bool EnableTotalGaussianNoise [SerializeField] private float m_totalGaussianNoiseRms = 0.0f; [Tooltip( "Gaussian noise RMS" )] - [DynamicallyShowInInspector( nameof(EnableTotalGaussianNoise) )] + [DynamicallyShowInInspector( nameof( EnableTotalGaussianNoise ) )] public float TotalGaussianNoiseRms { get => m_totalGaussianNoiseRms; @@ -98,7 +98,7 @@ public bool EnableSignalResolution [SerializeField] private int m_pulsesPerRevolution = 1024; [Tooltip( "Pulses per wheel revolution" )] - [DynamicallyShowInInspector( nameof(EnableSignalResolution) )] + [DynamicallyShowInInspector( nameof( EnableSignalResolution ) )] [ClampAboveZeroInInspector] public int PulsesPerRevolution { @@ -127,7 +127,7 @@ public bool EnableSignalScaling [SerializeField] private float m_signalScaling = 1.0f; [Tooltip( "Scaling factor applied to the distance signal" )] - [DynamicallyShowInInspector( nameof(EnableSignalScaling) )] + [DynamicallyShowInInspector( nameof( EnableSignalScaling ) )] public float SignalScaling { get => m_signalScaling; From a1ad768df7eb41e8778f28ebebcde689324c77e7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 23 Mar 2026 11:15:43 +0100 Subject: [PATCH 54/63] Disable and comment --- Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs index 29ab54ff..53b76fdc 100644 --- a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs +++ b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs @@ -1655,8 +1655,10 @@ private static OutputXYZ OutputXYZGUI( OutputXYZ state ) { var skin = InspectorEditor.Skin; + using var _ = new GUI.EnabledBlock( UnityEngine.GUI.enabled && !EditorApplication.isPlayingOrWillChangePlaymode ); + using ( new EditorGUILayout.HorizontalScope() ) { - EditorGUILayout.PrefixLabel( GUI.MakeLabel( "Output values", false ), + EditorGUILayout.PrefixLabel( GUI.MakeLabel( "Output values", false, "Disabled during runtime" ), InspectorEditor.Skin.LabelMiddleLeft ); var xEnabled = state.HasFlag(OutputXYZ.X); From 4ce28d6f6c8865bf64e3895706c158585a2fcf94 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 23 Mar 2026 11:28:17 +0100 Subject: [PATCH 55/63] Remove runtime editing of signal resolution --- AGXUnity/Sensor/EncoderSensor.cs | 4 ++-- AGXUnity/Sensor/OdometerSensor.cs | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs index ef7de446..381c2358 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -15,7 +15,6 @@ namespace AGXUnity.Sensor public class EncoderSensor : ScriptComponent { private const double DisabledTotalGaussianNoiseRms = 0.0; - private const double DisabledSignalResolution = 1e-4; // TODO hard to give this a good default value... private const double DisabledSignalScaling = 1.0; /// @@ -158,6 +157,7 @@ public float TotalGaussianNoiseRms [SerializeField] private bool m_enableSignalResolution = false; + [DisableInRuntimeInspector] public bool EnableSignalResolution { get => m_enableSignalResolution; @@ -256,7 +256,7 @@ private void SynchronizeSignalScalingModifier() private double GetTotalGaussianNoiseRms() => EnableTotalGaussianNoise ? TotalGaussianNoiseRms : DisabledTotalGaussianNoiseRms; - private double GetSignalResolutionValue() => EnableSignalResolution ? SignalResolution : DisabledSignalResolution; + private double GetSignalResolutionValue() => SignalResolution; private double GetSignalScalingValue() => EnableSignalScaling ? SignalScaling : DisabledSignalScaling; diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs index d077228b..aae154f9 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -15,7 +15,6 @@ namespace AGXUnity.Sensor public class OdometerSensor : ScriptComponent { private const double DisabledTotalGaussianNoiseRms = 0.0; - private const double DisabledSignalResolution = 1e-4; private const double DisabledSignalScaling = 1.0; /// @@ -83,6 +82,7 @@ public float TotalGaussianNoiseRms [SerializeField] private bool m_enableSignalResolution = false; + [DisableInRuntimeInspector] public bool EnableSignalResolution { get => m_enableSignalResolution; @@ -259,7 +259,7 @@ private void SynchronizeSignalScalingModifier() private double GetTotalGaussianNoiseRms() => EnableTotalGaussianNoise ? TotalGaussianNoiseRms : DisabledTotalGaussianNoiseRms; - private double GetSignalResolutionValue() => EnableSignalResolution ? SignalResolutionValue : DisabledSignalResolution; + private double GetSignalResolutionValue() => SignalResolutionValue; private double GetSignalScalingValue() => EnableSignalScaling ? SignalScaling : DisabledSignalScaling; From 0bd4fb7de5adec00f5413701560c4c910f0e4b00 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 23 Mar 2026 16:06:25 +0100 Subject: [PATCH 56/63] Removing gizmos per request --- AGXUnity/Sensor/EncoderSensor.cs | 20 -------------------- AGXUnity/Sensor/ImuSensor.cs | 21 --------------------- AGXUnity/Sensor/OdometerSensor.cs | 20 -------------------- 3 files changed, 61 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs index 381c2358..1703193f 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -436,25 +436,5 @@ protected override void OnDestroy() base.OnDestroy(); } - - [NonSerialized] - private Mesh m_nodeGizmoMesh = null; - - private void OnDrawGizmosSelected() - { -#if UNITY_EDITOR - if ( m_nodeGizmoMesh == null ) - m_nodeGizmoMesh = Resources.Load( @"Debug/Models/Icosahedron" ); - - if ( m_nodeGizmoMesh == null ) - return; - - Gizmos.color = Color.yellow; - Gizmos.DrawWireMesh( m_nodeGizmoMesh, - transform.position, - transform.rotation, - Vector3.one * 0.2f ); -#endif - } } } diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 9ed8f564..1c9f5a88 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -605,26 +605,5 @@ protected override void OnDestroy() base.OnDestroy(); } - - - [NonSerialized] - private Mesh m_nodeGizmoMesh = null; - - private void OnDrawGizmosSelected() - { -#if UNITY_EDITOR - if ( m_nodeGizmoMesh == null ) - m_nodeGizmoMesh = Resources.Load( @"Debug/Models/HalfSphere" ); - - if ( m_nodeGizmoMesh == null ) - return; - - Gizmos.color = Color.yellow; - Gizmos.DrawWireMesh( m_nodeGizmoMesh, - transform.position, - transform.rotation, - Vector3.one * 0.2f ); -#endif - } } } diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs index aae154f9..72214e22 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -321,25 +321,5 @@ protected override void OnDestroy() base.OnDestroy(); } - - [NonSerialized] - private Mesh m_nodeGizmoMesh = null; - - private void OnDrawGizmosSelected() - { -#if UNITY_EDITOR - if ( m_nodeGizmoMesh == null ) - m_nodeGizmoMesh = Resources.Load( @"Debug/Models/Icosahedron" ); - - if ( m_nodeGizmoMesh == null ) - return; - - Gizmos.color = Color.yellow; - Gizmos.DrawWireMesh( m_nodeGizmoMesh, - transform.position, - transform.rotation, - Vector3.one * 0.2f ); -#endif - } } } From 7521c79b32446322135168aa38bbe668f00790f3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Mon, 23 Mar 2026 16:09:12 +0100 Subject: [PATCH 57/63] Removing property sync --- AGXUnity/Sensor/EncoderSensor.cs | 2 -- AGXUnity/Sensor/ImuSensor.cs | 2 -- AGXUnity/Sensor/OdometerSensor.cs | 2 -- 3 files changed, 6 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs index 1703193f..3c07fcec 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -342,8 +342,6 @@ protected override bool Initialize() Debug.LogWarning( "No output configured for encoder" ); } - PropertySynchronizer.Synchronize( this ); - SensorEnvironment.Instance.Native.add( Native ); return true; diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs index 1c9f5a88..6187999b 100644 --- a/AGXUnity/Sensor/ImuSensor.cs +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -427,8 +427,6 @@ protected override bool Initialize() OutputBuffer = new double[ outputCount ]; - PropertySynchronizer.Synchronize( this ); - Simulation.Instance.StepCallbacks.PostSynchronizeTransforms += OnPostSynchronizeTransforms; SensorEnvironment.Instance.Native.add( Native ); diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs index 72214e22..19ec9e49 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -231,8 +231,6 @@ protected override bool Initialize() var output = new OdometerOutputDistance(); Native.getOutputHandler().add( m_outputID, output ); - PropertySynchronizer.Synchronize( this ); - Simulation.Instance.StepCallbacks.PostSynchronizeTransforms += OnPostSynchronizeTransforms; SensorEnvironment.Instance.Native.add( Native ); From af4f54609efccc966a6ad24381b5c30becb30dd6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 24 Mar 2026 10:58:02 +0100 Subject: [PATCH 58/63] Foldout for runtime values --- Editor/AGXUnityEditor/InspectorEditor.cs | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/Editor/AGXUnityEditor/InspectorEditor.cs b/Editor/AGXUnityEditor/InspectorEditor.cs index 3e4a156b..9d13aa31 100644 --- a/Editor/AGXUnityEditor/InspectorEditor.cs +++ b/Editor/AGXUnityEditor/InspectorEditor.cs @@ -98,24 +98,25 @@ public static void DrawMembersGUI( Object[] targets, group.Dispose(); // Draw runtime values in one disabled block - // TODO style etc if ( runtimeValues.Count > 0 ) { InspectorGUI.Separator( 1, EditorGUIUtility.singleLineHeight ); - var style = GUI.Align( GUI.Skin.label, TextAnchor.MiddleLeft ); - GUILayout.Label( GUI.MakeLabel( "Runtime Values", true, "" ), style ); - using ( new GUI.EnabledBlock( false ) ) { - group = InspectorGroupHandler.Create(); - foreach ( var wrapper in runtimeValues ) { - group.Update( wrapper, objects[ 0 ] ); + if ( InspectorGUI.Foldout( EditorData.Instance.GetData( targets[ 0 ], targets[ 0 ].name ), + GUI.MakeLabel( "Runtime Values", true, "" ) ) ) { - if ( group.IsHidden ) - continue; + using ( new GUI.EnabledBlock( false ) ) { + group = InspectorGroupHandler.Create(); + foreach ( var wrapper in runtimeValues ) { + group.Update( wrapper, objects[ 0 ] ); - HandleType( wrapper, objects, fallback ); + if ( group.IsHidden ) + continue; + + HandleType( wrapper, objects, fallback ); + } + group.Dispose(); } - group.Dispose(); } } } From a38ff2c8cfe0ebbfae3db9d15ea9e5c89a4529a7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 24 Mar 2026 10:58:17 +0100 Subject: [PATCH 59/63] Remove unused unit property --- AGXUnity/Attributes.cs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/AGXUnity/Attributes.cs b/AGXUnity/Attributes.cs index c5a58def..87e4f083 100644 --- a/AGXUnity/Attributes.cs +++ b/AGXUnity/Attributes.cs @@ -247,8 +247,7 @@ public DynamicallyShowInInspector( string name, bool isMethod = false, bool inve [AttributeUsage( AttributeTargets.Field | AttributeTargets.Property, AllowMultiple = false )] public class RuntimeValue : Attribute { - public string Unit { get; private set; } - public RuntimeValue( string unit = "" ) { this.Unit = unit; } + public RuntimeValue() { } } } From 9ea0d18777ca47e154a406d917c7385ec17432ce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 24 Mar 2026 10:59:37 +0100 Subject: [PATCH 60/63] Remove unused using --- AGXUnity/Sensor/EncoderSensor.cs | 2 -- AGXUnity/Sensor/OdometerSensor.cs | 2 -- 2 files changed, 4 deletions(-) diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs index 3c07fcec..cd1bdee2 100644 --- a/AGXUnity/Sensor/EncoderSensor.cs +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -1,7 +1,5 @@ using agxSensor; -using AGXUnity.Utils; using UnityEngine; -using System; using AGXUnity.Model; namespace AGXUnity.Sensor diff --git a/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs index 19ec9e49..ac78f711 100644 --- a/AGXUnity/Sensor/OdometerSensor.cs +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -1,7 +1,5 @@ using agxSensor; -using AGXUnity.Utils; using UnityEngine; -using System; using AGXUnity.Model; namespace AGXUnity.Sensor From ec4a6154fedfcc690216bf795e0ca927998bceb9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 24 Mar 2026 11:11:15 +0100 Subject: [PATCH 61/63] Add test of disabled component --- Tests/Runtime/ImuAndEncoderTests.cs | 23 ++++++++++++++++++----- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/Tests/Runtime/ImuAndEncoderTests.cs b/Tests/Runtime/ImuAndEncoderTests.cs index fcaca14c..06b2ee6c 100644 --- a/Tests/Runtime/ImuAndEncoderTests.cs +++ b/Tests/Runtime/ImuAndEncoderTests.cs @@ -1,12 +1,8 @@ -using agx; -using agxSensor; using AGXUnity; using AGXUnity.Collide; using AGXUnity.Sensor; using NUnit.Framework; using System.Collections; -using System.Linq; -using System.Text.RegularExpressions; using UnityEngine; using UnityEngine.TestTools; @@ -169,7 +165,24 @@ public IEnumerator TestOdometerOutput() yield return TestUtils.Step(); yield return TestUtils.Step(); - Assert.That( Mathf.Abs( (float)odometer.OutputBuffer ), Is.EqualTo( 0.02 ).Within( 0.001f ), "Testing" ); + Assert.That( Mathf.Abs( (float)odometer.OutputBuffer ), Is.EqualTo( 0.02 ).Within( 0.001f ), "Testing odometer output" ); + } + + + [UnityTest] + public IEnumerator TestOdometerDisable() + { + var constraint = CreateTestHinge(); + var odometer = constraint.gameObject.AddComponent(); + odometer.enabled = false; + var controller = constraint.GetController(); + controller.Speed = 1; + controller.Enable = true; + + yield return TestUtils.Step(); + yield return TestUtils.Step(); + + Assert.That( Mathf.Abs( (float)odometer.OutputBuffer ), Is.EqualTo( 0.0 ), "Should be 0 when disabled" ); } } } From db41331bfa65a47960f1f229de0bdbfac98d26b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 24 Mar 2026 11:16:07 +0100 Subject: [PATCH 62/63] Fix other tests failing due to move to onsynchronizetransforms --- Tests/Runtime/ImuAndEncoderTests.cs | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/Tests/Runtime/ImuAndEncoderTests.cs b/Tests/Runtime/ImuAndEncoderTests.cs index 06b2ee6c..ca12cc08 100644 --- a/Tests/Runtime/ImuAndEncoderTests.cs +++ b/Tests/Runtime/ImuAndEncoderTests.cs @@ -120,6 +120,7 @@ public IEnumerator TestGyroscopeOutput() rb.MotionControl = agx.RigidBody.MotionControl.KINEMATICS; rb.AngularVelocity = Vector3.one; + yield return TestUtils.Step(); yield return TestUtils.Step(); Assert.That( Mathf.Abs( (float)imu.OutputBuffer[ 5 ] ), Is.EqualTo( 1 ).Within( 0.01f ), "Test value should be 1 like the change in rotation" ); @@ -133,6 +134,7 @@ public IEnumerator TestMagnetometerOutput() rb.MotionControl = agx.RigidBody.MotionControl.KINEMATICS; rb.AngularVelocity = Vector3.one; + yield return TestUtils.Step(); yield return TestUtils.Step(); Assert.That( Mathf.Abs( (float)imu.OutputBuffer[ 8 ] ), Is.EqualTo( 1 ).Within( 0.001f ), "Test value should be 1 as the magnetic field was set up to be 1 in each direction" ); @@ -148,6 +150,8 @@ public IEnumerator TestEncoderOutput() controller.Speed = 1; controller.Enable = true; + yield return TestUtils.Step(); // NaN first timestep + yield return TestUtils.Step(); yield return TestUtils.Step(); Assert.That( Mathf.Abs( (float)encoder.SpeedBuffer ), Is.EqualTo( 1 ).Within( 0.01f ), "Value should be close to target speed controller speed" ); @@ -162,10 +166,12 @@ public IEnumerator TestOdometerOutput() controller.Speed = 1; controller.Enable = true; + yield return TestUtils.Step(); // NaN first timestep yield return TestUtils.Step(); yield return TestUtils.Step(); + - Assert.That( Mathf.Abs( (float)odometer.OutputBuffer ), Is.EqualTo( 0.02 ).Within( 0.001f ), "Testing odometer output" ); + Assert.That( Mathf.Abs( (float)odometer.OutputBuffer ), Is.EqualTo( 0.02 ).Within( 0.02f ), "Testing odometer output" ); } From 1a6b2d3c1ec3c038caba55dbfabfac8529636728 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joakim=20M=C3=B6rk?= Date: Tue, 24 Mar 2026 11:21:00 +0100 Subject: [PATCH 63/63] Whitespace --- Tests/Runtime/ImuAndEncoderTests.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tests/Runtime/ImuAndEncoderTests.cs b/Tests/Runtime/ImuAndEncoderTests.cs index ca12cc08..73e10e43 100644 --- a/Tests/Runtime/ImuAndEncoderTests.cs +++ b/Tests/Runtime/ImuAndEncoderTests.cs @@ -169,7 +169,7 @@ public IEnumerator TestOdometerOutput() yield return TestUtils.Step(); // NaN first timestep yield return TestUtils.Step(); yield return TestUtils.Step(); - + Assert.That( Mathf.Abs( (float)odometer.OutputBuffer ), Is.EqualTo( 0.02 ).Within( 0.02f ), "Testing odometer output" ); }