diff --git a/AGXUnity/Attributes.cs b/AGXUnity/Attributes.cs index 2e7b3d6f..87e4f083 100644 --- a/AGXUnity/Attributes.cs +++ b/AGXUnity/Attributes.cs @@ -243,4 +243,11 @@ 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 RuntimeValue() { } + } + } diff --git a/AGXUnity/Sensor/EncoderSensor.cs b/AGXUnity/Sensor/EncoderSensor.cs new file mode 100644 index 00000000..cd1bdee2 --- /dev/null +++ b/AGXUnity/Sensor/EncoderSensor.cs @@ -0,0 +1,436 @@ +using agxSensor; +using UnityEngine; +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 + { + private const double DisabledTotalGaussianNoiseRms = 0.0; + private const double DisabledSignalScaling = 1.0; + + /// + /// 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 + /// + [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 + /// + [SerializeField] + private EncoderModel.Mode m_mode = EncoderModel.Mode.ABSOLUTE; + [Tooltip( "Encoder accumulation mode. INCREMENTAL wraps within the cycle range; ABSOLUTE is single-turn 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. + /// 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." )] + 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 + { + Rotational, + Translational + } + + [SerializeField] + private TwoDofSample m_sampleTwoDof = TwoDofSample.Rotational; + [Tooltip( "Which DoF to sample for 2-DoF constraints (Cylindrical)" )] + [DynamicallyShowInInspector( nameof( HasTwoDof ), true )] + 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 + { + Steering, + WheelAxle, + Suspension + } + + [SerializeField] + private WheelJointSample m_sampleWheelJoint = WheelJointSample.WheelAxle; + [Tooltip( "Which secondary constraint to sample for WheelJoint" )] + [DisableInRuntimeInspector] + [DynamicallyShowInInspector( nameof( IsWheelJoint ), true )] + 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" )] + public bool OutputPosition { get; set; } = true; + + [field: SerializeField] + [Tooltip( "Include speed in the output" )] + public bool OutputSpeed { get; set; } = false; + + [SerializeField] + private bool m_enableTotalGaussianNoise = false; + [InspectorGroupBegin( Name = "Modifiers", DefaultExpanded = true )] + public bool EnableTotalGaussianNoise + { + get => m_enableTotalGaussianNoise; + set + { + m_enableTotalGaussianNoise = value; + SynchronizeTotalGaussianNoiseModifier(); + } + } + + /// + /// Noise RMS in sensor units (position units: radians/meters). + /// + [SerializeField] + private float m_totalGaussianNoiseRms = 0.0f; + [Tooltip( "Gaussian noise RMS (position units: radians/meters)." )] + [DynamicallyShowInInspector( nameof( EnableTotalGaussianNoise ) )] + public float TotalGaussianNoiseRms + { + get => m_totalGaussianNoiseRms; + set + { + m_totalGaussianNoiseRms = value; + SynchronizeTotalGaussianNoiseModifier(); + } + } + + [SerializeField] + private bool m_enableSignalResolution = false; + [DisableInRuntimeInspector] + public bool EnableSignalResolution + { + get => m_enableSignalResolution; + set + { + m_enableSignalResolution = value; + SynchronizeSignalResolutionModifier(); + } + } + + /// + /// Resolution/bin size in sensor units (position units: radians/meters). + /// + [SerializeField] + private float m_signalResolution = 0.01f; + [Tooltip( "Resolution/bin size (position units: radians/meters)." )] + [DynamicallyShowInInspector( nameof( EnableSignalResolution ) )] + [ClampAboveZeroInInspector] + public float SignalResolution + { + get => m_signalResolution; + set + { + m_signalResolution = value > 0 ? value : m_signalResolution; + SynchronizeSignalResolutionModifier(); + } + } + + [SerializeField] + private bool m_enableSignalScaling = false; + public bool EnableSignalScaling + { + get => m_enableSignalScaling; + set + { + m_enableSignalScaling = value; + SynchronizeSignalScalingModifier(); + } + } + + /// + /// Constant scaling factor. + /// + [SerializeField] + private float m_signalScaling = 1.0f; + [Tooltip( "Scaling factor applied to encoder outputs." )] + [DynamicallyShowInInspector( nameof( EnableSignalScaling ) )] + 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] + public double PositionBuffer { get; private set; } + + [HideInInspector] + public double SpeedBuffer { get; private set; } + + [HideInInspector] + private bool IsWheelJoint => ConstraintComponent == null || ConstraintComponent is WheelJoint; + + [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() => SignalResolution; + + private double GetSignalScalingValue() => EnableSignalScaling ? SignalScaling : DisabledSignalScaling; + + + 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(); + + // 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; + } + + m_modifiers = new IMonoaxialSignalSystemNodeRefVector(); + m_totalGaussianNoiseModifier = new MonoaxialGaussianNoise( GetTotalGaussianNoiseRms() ); + m_signalResolutionModifier = new MonoaxialSignalResolution( GetSignalResolutionValue() ); + m_signalScalingModifier = new MonoaxialSignalScaling( GetSignalScalingValue() ); + + m_modifiers.Add( m_totalGaussianNoiseModifier ); + m_modifiers.Add( m_signalResolutionModifier ); + m_modifiers.Add( m_signalScalingModifier ); + + 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" ); + 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 ); + } + + 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 EncoderOutputPositionSpeed(); + + Native.getOutputHandler().add( m_outputID, output ); + + Simulation.Instance.StepCallbacks.PostSynchronizeTransforms += OnPostSynchronizeTransforms; + } + 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; + + 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.Translational ? 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 OnPostSynchronizeTransforms() + { + if ( !gameObject.activeInHierarchy || Native == null ) + return; + + var output = Native.getOutputHandler().get( m_outputID ); + if ( output == null ) { + PositionBuffer = 0; + SpeedBuffer = 0; + } + else { + 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 + 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.PostSynchronizeTransforms -= OnPostSynchronizeTransforms; + + base.OnDestroy(); + } + } +} diff --git a/AGXUnity/Sensor/EncoderSensor.cs.meta b/AGXUnity/Sensor/EncoderSensor.cs.meta new file mode 100644 index 00000000..fe5225d9 --- /dev/null +++ b/AGXUnity/Sensor/EncoderSensor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 220d371f3ca346c4eb8c6a9d81192c09 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {fileID: 2800000, guid: 291d5afecb12a3747931bd119c01ad37, type: 3} + userData: + assetBundleName: + assetBundleVariant: diff --git a/AGXUnity/Sensor/ImuSensor.cs b/AGXUnity/Sensor/ImuSensor.cs new file mode 100644 index 00000000..6187999b --- /dev/null +++ b/AGXUnity/Sensor/ImuSensor.cs @@ -0,0 +1,607 @@ +using agx; +using agxSensor; +using AGXUnity.Utils; +using UnityEngine; +using System; + +namespace AGXUnity.Sensor +{ + [Serializable] + [Flags] + public enum OutputXYZ + { + None = 0, + X = 1 << 0, + Y = 1 << 1, + Z = 1 << 2, + } + [Serializable] + // 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, + Gyroscope, + Magnetometer + } + + /// + /// Accelerometer / Gyroscope / Magnetometer + /// + [field: SerializeField] + public ImuAttachmentType Type { get; private set; } + + /// + /// Detectable measurement range, in m/s^2 / radians/s / T + /// + [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. + /// + [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 + /// + [SerializeField] + private Vector3 m_zeroBias; + public Vector3 ZeroBias + { + get => m_zeroBias; + set + { + m_zeroBias = value; + m_onBaseSettingsChanged?.Invoke(); + } + } + + [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 + /// + [SerializeField] + private Vector3 m_linearAccelerationEffects; + public Vector3 LinearAccelerationEffects + { + get => m_linearAccelerationEffects; + set + { + m_linearAccelerationEffects = value; + m_onModifierSettingsChanged?.Invoke(); + } + } + + [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 + /// + [SerializeField] + private Vector3 m_totalGaussianNoise = Vector3.zero; + public Vector3 TotalGaussianNoise + { + get => m_totalGaussianNoise; + set + { + m_totalGaussianNoise = value; + m_onModifierSettingsChanged?.Invoke(); + } + } + + [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 + /// + [SerializeField] + private Vector3 m_signalScaling = Vector3.one; + public Vector3 SignalScaling + { + get => m_signalScaling; + set + { + m_signalScaling = value; + m_onModifierSettingsChanged?.Invoke(); + } + } + + [SerializeField] + private bool m_enableGaussianSpectralNoise = false; + public bool EnableGaussianSpectralNoise + { + get => m_enableGaussianSpectralNoise; + set + { + m_enableGaussianSpectralNoise = value; + m_onModifierSettingsChanged?.Invoke(); + } + } + /// + /// Sample frequency dependent Gaussian noise + /// + [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 + /// + 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, Vector3 zeroRateBias ) + { + Type = type; + TriaxialRange = triaxialRange; + CrossAxisSensitivity = crossAxisSensitivity; + ZeroBias = zeroRateBias; + } + + internal void SetRuntimeCallbacks( Action onBaseSettingsChanged, Action onModifierSettingsChanged ) + { + m_onBaseSettingsChanged = onBaseSettingsChanged; + m_onModifierSettingsChanged = onModifierSettingsChanged; + m_triaxialRange?.SetOnChanged( m_onBaseSettingsChanged ); + } + } + + /// + /// IMU Sensor Component + /// + [DisallowMultipleComponent] + [AddComponentMenu( "AGXUnity/Sensors/IMU Sensor" )] + [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 + /// + [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 sensor attachment + /// + [field: SerializeField] + [DynamicallyShowInInspector( nameof( EnableAccelerometer ) )] + public ImuAttachment AccelerometerAttachment { get; private set; } = new ImuAttachment( + ImuAttachment.ImuAttachmentType.Accelerometer, + new TriaxialRangeData(), + 0.01f, + Vector3.zero ); + + /// + /// 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 sensor attachment + /// + [field: SerializeField] + [DynamicallyShowInInspector( nameof( EnableGyroscope ) )] + public ImuAttachment GyroscopeAttachment { get; private set; } = new ImuAttachment( + ImuAttachment.ImuAttachmentType.Gyroscope, + new TriaxialRangeData(), + 0.01f, + Vector3.zero ); + + /// + /// 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 sensor attachment + /// + [field: SerializeField] + [DynamicallyShowInInspector( nameof( EnableMagnetometer ) )] + public ImuAttachment MagnetometerAttachment { get; private set; } = new ImuAttachment( + ImuAttachment.ImuAttachmentType.Magnetometer, + new TriaxialRangeData(), + 0.01f, + Vector3.one * 0f ); + + [RuntimeValue] public RigidBody TrackedRigidBody { get; private set; } + [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; } + + 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" ); + return false; + } + TrackedRigidBody = rigidBody; + + var imu_attachments = new IMUModelSensorAttachmentRefVector(); + + // Accelerometer + if ( EnableAccelerometer ) { + 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(), + m_accelerometerModifiers + ); + + imu_attachments.Add( new IMUModelAccelerometerAttachment( AffineMatrix4x4.identity(), m_accelerometerModel ) ); + } + + // Gyroscope + if ( EnableGyroscope ) { + 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(), + m_gyroscopeModifiers + ); + + imu_attachments.Add( new IMUModelGyroscopeAttachment( AffineMatrix4x4.identity(), m_gyroscopeModel ) ); + } + + // Magnetometer + if ( EnableMagnetometer ) { + 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(), + m_magnetometerModifiers + ); + + imu_attachments.Add( new IMUModelMagnetometerAttachment( AffineMatrix4x4.identity(), m_magnetometerModel ) ); + } + + if ( imu_attachments.Count == 0 ) { + 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, IMU will be inactive" ); + return false; + } + + var measuredRB = rigidBody.GetInitialized().Native; + SensorEnvironment.Instance.Native.add( measuredRB ); + + var rbFrame = measuredRB.getFrame(); + if ( rbFrame == null ) { + Debug.LogWarning( "Could not get rigid body frame, IMU will be inactive" ); + return false; + } + Native = new IMU( rbFrame, m_nativeModel ); + + // 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.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 ); + + OutputBuffer = new double[ outputCount ]; + + Simulation.Instance.StepCallbacks.PostSynchronizeTransforms += OnPostSynchronizeTransforms; + + SensorEnvironment.Instance.Native.add( Native ); + + 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 ) { + Debug.LogError( "Null problem" ); + return; + } + + NineDoFView views = output.viewNineDoF(); + if ( views == null ) { + Debug.LogWarning( "No views" ); + 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 + // 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 ) 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 ) 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 ) 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; + } + } + + private void OnPostSynchronizeTransforms() + { + if ( !gameObject.activeInHierarchy || Native == null ) + return; + + GetOutput( Native.getOutputHandler().get( m_outputID ), OutputBuffer ); + + if ( Application.isEditor ) { + for ( int i = 0; i < OutputBuffer.Length; 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; + } + } + } + } + + 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.PostSynchronizeTransforms -= OnPostSynchronizeTransforms; + } + + base.OnDestroy(); + } + } +} 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/AGXUnity/Sensor/OdometerSensor.cs b/AGXUnity/Sensor/OdometerSensor.cs new file mode 100644 index 00000000..ac78f711 --- /dev/null +++ b/AGXUnity/Sensor/OdometerSensor.cs @@ -0,0 +1,321 @@ +using agxSensor; +using UnityEngine; +using AGXUnity.Model; + +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 + { + private const double DisabledTotalGaussianNoiseRms = 0.0; + private const double DisabledSignalScaling = 1.0; + + /// + /// 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, WheelJoint + /// + [SerializeField] + [Tooltip( "Constraint / WheelJoint component to attach odometer to: Hinge, CylindricalJoint, WheelJoint. If unset, the first compatible parent is used." )] + public ScriptComponent ConstraintComponent { get; set; } = null; + + /// + /// Wheel radius in meters + /// + [SerializeField] + private float m_wheelRadius = 0.5f; + [Tooltip( "Wheel radius in meters" )] + [ClampAboveZeroInInspector] + public float WheelRadius + { + get => m_wheelRadius; + set + { + m_wheelRadius = value > 0 ? value : m_wheelRadius; + if ( Native != null ) + Native.getModel().setWheelRadius( m_wheelRadius ); + SynchronizeSignalResolutionModifier(); + } + } + + [SerializeField] + private bool m_enableTotalGaussianNoise = false; + [InspectorGroupBegin( Name = "Modifiers", DefaultExpanded = true )] + public bool EnableTotalGaussianNoise + { + get => m_enableTotalGaussianNoise; + set + { + m_enableTotalGaussianNoise = value; + SynchronizeTotalGaussianNoiseModifier(); + } + } + /// + /// Noise RMS in meters. + /// + [SerializeField] + private float m_totalGaussianNoiseRms = 0.0f; + [Tooltip( "Gaussian noise RMS" )] + [DynamicallyShowInInspector( nameof( EnableTotalGaussianNoise ) )] + public float TotalGaussianNoiseRms + { + get => m_totalGaussianNoiseRms; + set + { + m_totalGaussianNoiseRms = value; + SynchronizeTotalGaussianNoiseModifier(); + } + } + + [SerializeField] + private bool m_enableSignalResolution = false; + [DisableInRuntimeInspector] + public bool EnableSignalResolution + { + get => m_enableSignalResolution; + set + { + m_enableSignalResolution = value; + SynchronizeSignalResolutionModifier(); + } + } + /// + /// Pulses per wheel revolution. + /// + [SerializeField] + private int m_pulsesPerRevolution = 1024; + [Tooltip( "Pulses per wheel revolution" )] + [DynamicallyShowInInspector( nameof( EnableSignalResolution ) )] + [ClampAboveZeroInInspector] + public int PulsesPerRevolution + { + get => m_pulsesPerRevolution; + set + { + m_pulsesPerRevolution = value > 0 ? value : m_pulsesPerRevolution; + SynchronizeSignalResolutionModifier(); + } + } + + [SerializeField] + private bool m_enableSignalScaling = false; + public bool EnableSignalScaling + { + get => m_enableSignalScaling; + set + { + m_enableSignalScaling = value; + SynchronizeSignalScalingModifier(); + } + } + /// + /// Constant scaling factor. + /// + [SerializeField] + private float m_signalScaling = 1.0f; + [Tooltip( "Scaling factor applied to the distance signal" )] + [DynamicallyShowInInspector( nameof( EnableSignalScaling ) )] + public float SignalScaling + { + get => m_signalScaling; + set + { + m_signalScaling = value; + SynchronizeSignalScalingModifier(); + } + } + + [InspectorGroupEnd] + + [HideInInspector] + private bool IsWheelJoint => ConstraintComponent == null || ConstraintComponent is WheelJoint; + + [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; } + + 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(); + + 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 = FindParentJoint(); + } + + if ( ConstraintComponent == null ) { + Debug.LogWarning( "No constraint component found/assigned, odometer will be inactive" ); + return false; + } + + m_modifiers = new IMonoaxialSignalSystemNodeRefVector(); + m_totalGaussianNoiseModifier = new MonoaxialGaussianNoise( GetTotalGaussianNoiseRms() ); + m_signalResolutionModifier = new MonoaxialSignalResolution( GetSignalResolutionValue() ); + m_signalScalingModifier = new MonoaxialSignalScaling( GetSignalScalingValue() ); + + m_modifiers.Add( m_totalGaussianNoiseModifier ); + m_modifiers.Add( m_signalResolutionModifier ); + m_modifiers.Add( m_signalScalingModifier ); + + m_nativeModel = new OdometerModel( (double)WheelRadius, m_modifiers ); + + if ( m_nativeModel == null ) { + Debug.LogWarning( "Could not create native odometer model, 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 ); + } + + 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.PostSynchronizeTransforms += OnPostSynchronizeTransforms; + + SensorEnvironment.Instance.Native.add( Native ); + + return true; + } + + 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() => SignalResolutionValue; + + private double GetSignalScalingValue() => EnableSignalScaling ? SignalScaling : DisabledSignalScaling; + + 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 ); + + return null; + } + + private double GetOutput( OdometerOutput output ) + { + if ( Native == null || output == null ) + return 0; + + var view = output.view(); + + if ( view.size() > 0 ) + return view[ 0 ]; + else + return 0; + } + + private void OnPostSynchronizeTransforms() + { + 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.PostSynchronizeTransforms -= OnPostSynchronizeTransforms; + + base.OnDestroy(); + } + } +} diff --git a/AGXUnity/Sensor/OdometerSensor.cs.meta b/AGXUnity/Sensor/OdometerSensor.cs.meta new file mode 100644 index 00000000..66fa0b9d --- /dev/null +++ b/AGXUnity/Sensor/OdometerSensor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 23f58075b197b564ead6633c7f209368 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {fileID: 2800000, guid: 291d5afecb12a3747931bd119c01ad37, type: 3} + userData: + assetBundleName: + assetBundleVariant: diff --git a/AGXUnity/Sensor/SensorEnvironment.cs b/AGXUnity/Sensor/SensorEnvironment.cs index 2ca911af..ded72282 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. /// @@ -34,6 +41,44 @@ 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( nameof(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( nameof(UsingDipoleMagneticField) )] + public Vector3 MagneticMoment = new Vector3( -2.69e19f, -7.65e22f, 1.5e22f ); + + /// + /// Magnetic dipole center + /// + [Tooltip("Magnetic dipole center")] + [HideInRuntimeInspector] + [DynamicallyShowInInspector( nameof(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; diff --git a/AGXUnity/Sensor/TriaxialTypes.cs b/AGXUnity/Sensor/TriaxialTypes.cs new file mode 100644 index 00000000..93958874 --- /dev/null +++ b/AGXUnity/Sensor/TriaxialTypes.cs @@ -0,0 +1,104 @@ +using System; +using UnityEngine; + +namespace AGXUnity.Sensor +{ + /// + /// Helper class for triaxial range type configurations + /// + [Serializable] + public class TriaxialRangeData + { + [NonSerialized] + private Action m_onChanged = null; + + public enum ConfigurationMode + { + MaxRange, + EqualAxisRanges, + IndividualAxisRanges + } + + [SerializeField] + 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] + private Vector2 m_rangeX = new( float.MinValue, float.MaxValue ); + public Vector2 RangeX + { + get => m_rangeX; + set + { + m_rangeX = value; + m_onChanged?.Invoke(); + } + } + + [SerializeField] + 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() + { + 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..46198070 --- /dev/null +++ b/AGXUnity/Sensor/TriaxialTypes.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d8282f9366480f04eb6ad8388116cef4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {fileID: 2800000, guid: 49c5f239479b7b143be0c05568f44faf, type: 3} + userData: + assetBundleName: + assetBundleVariant: diff --git a/AGXUnity/Utils/Math.cs b/AGXUnity/Utils/Math.cs index 7d4fed57..5e6e0e7c 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 CountEnabledBits( uint value ) + { + uint c = 0; + for ( ; value != 0; c++ ) + value &= value - 1; + return c; + } } } diff --git a/Editor/AGXUnityEditor/InspectorEditor.cs b/Editor/AGXUnityEditor/InspectorEditor.cs index 142dc630..9d13aa31 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,29 @@ public static void DrawMembersGUI( Object[] targets, HandleType( wrapper, objects, fallback ); } group.Dispose(); + + // Draw runtime values in one disabled block + if ( runtimeValues.Count > 0 ) { + + InspectorGUI.Separator( 1, EditorGUIUtility.singleLineHeight ); + + if ( InspectorGUI.Foldout( EditorData.Instance.GetData( targets[ 0 ], targets[ 0 ].name ), + GUI.MakeLabel( "Runtime Values", true, "" ) ) ) { + + 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(); + } + } + } } } diff --git a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs index 94a0bddd..53b76fdc 100644 --- a/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs +++ b/Editor/AGXUnityEditor/InvokeWrapperInspectorDrawer.cs @@ -1552,5 +1552,143 @@ public static object LidarDistanceGaussianNoiseDrawer( object[] objects, InvokeW return null; } + + [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; + } + + var data = wrapper.Get( objects[0] ); + using ( new InspectorGUI.IndentScope() ) { + data.TriaxialRange = TriaxialRangeDataGUI( data.TriaxialRange ); + 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() ) + 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", + "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" ); + } + } + } + } + + InspectorGUI.Separator(); + + return null; + } + + 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( 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 ) { + 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; + } + + 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, "Disabled during runtime" ), + 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 ) ) != xEnabled ) + state ^= OutputXYZ.X; + if ( GUILayout.Toggle( yEnabled, + GUI.MakeLabel( "Y", + yEnabled, + "Use sensor y value in output" ), + skin.GetButton( InspectorGUISkin.ButtonType.Middle ) ) != yEnabled ) + state ^= OutputXYZ.Y; + if ( GUILayout.Toggle( zEnabled, + GUI.MakeLabel( "Z", + zEnabled, + "Use sensor Z value in output" ), + skin.GetButton( InspectorGUISkin.ButtonType.Right ) ) != zEnabled ) + state ^= OutputXYZ.Z; + } + + return state; + } + + } } + 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 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 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 diff --git a/Tests/Runtime/ImuAndEncoderTests.cs b/Tests/Runtime/ImuAndEncoderTests.cs new file mode 100644 index 00000000..73e10e43 --- /dev/null +++ b/Tests/Runtime/ImuAndEncoderTests.cs @@ -0,0 +1,194 @@ +using AGXUnity; +using AGXUnity.Collide; +using AGXUnity.Sensor; +using NUnit.Framework; +using System.Collections; +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(); + 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(); + 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(); // 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" ); + } + + [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(); // 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" ); + } + + + [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" ); + } + } +} 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