From 151c8ee0ac23dab389d784ecdcaa252879f295c0 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Mon, 25 Nov 2024 17:25:10 +0900 Subject: [PATCH 1/4] create FirstOrderLaggedFloat.cs Signed-off-by: Autumn60 --- .../Scripts/Vehicles/FirstOrderLaggedFloat.cs | 59 +++++++++++++++++++ .../Vehicles/FirstOrderLaggedFloat.cs.meta | 11 ++++ 2 files changed, 70 insertions(+) create mode 100644 Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs create mode 100644 Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs.meta diff --git a/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs b/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs new file mode 100644 index 000000000..76de4b9b2 --- /dev/null +++ b/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs @@ -0,0 +1,59 @@ +using System; +using UnityEngine; + +namespace AWSIM +{ + public class FirstOrderLaggedFloat + { + private float timeConstant; + + private float desiredValue; + private float currentValue; + private float lastTime; + + public float Value + { + get + { + Update(); + return currentValue; + } + } + public float DesiredValue + { + set + { + Update(); + desiredValue = value; + } + get + { + return desiredValue; + } + } + + public FirstOrderLaggedFloat(float timeConstant, float initialValue) + { + this.timeConstant = timeConstant; + + desiredValue = currentValue = initialValue; + lastTime = Time.time; + } + + private void Update() + { + float dt = Time.time - lastTime; + + if (timeConstant == 0.0f) + { + currentValue = desiredValue; + } + else + { + currentValue += (dt / timeConstant) * (desiredValue - currentValue); + } + + lastTime = Time.time; + } + } +} \ No newline at end of file diff --git a/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs.meta b/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs.meta new file mode 100644 index 000000000..ec3daa55e --- /dev/null +++ b/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 963ca01e17396605cbc15c9a3f20e472 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: From 2082917d7bd5c65fbd637a5f11f60cecb1ba9092 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Mon, 25 Nov 2024 17:29:09 +0900 Subject: [PATCH 2/4] apply first order lag to SteerAngleInput Signed-off-by: Autumn60 --- Assets/AWSIM/Scripts/Vehicles/Vehicle.cs | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs b/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs index d39655933..e69e7d79f 100644 --- a/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs +++ b/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs @@ -196,8 +196,14 @@ public float SidewaySlipMultipler /// Vehicle steering input. Tire angle (degree) /// Negative is left, positive is right turn tire angle. /// - // TODO: Compute first order lag - public float SteerAngleInput; + public float SteerAngleInput + { + get => steerAngle.DesiredValue; + set => steerAngle.DesiredValue = Mathf.Clamp(value, -MaxSteerAngleInput, MaxSteerAngleInput); + } + private FirstOrderLaggedFloat steerAngle; + [SerializeField, Min(0.0f), Tooltip("Set 0 to disable the lag")] + private float steerAngleTimeConstant = 0.0f; /// /// Vehicle turn signal input. NONE, LEFT, RIGHT, HAZARD. @@ -217,7 +223,7 @@ public float SidewaySlipMultipler /// /// Vehicle steering angle (degree) /// - public float SteerAngle => SteerAngleInput; + public float SteerAngle => steerAngle.Value; public float SteerAngleNormalized => SteerAngle / MaxSteerAngleInput; @@ -296,6 +302,9 @@ void Awake() // Initialize with non-slip value ForwardSlipMultipler = 1f; SidewaySlipMultipler = 1f; + + // Initialize steer angle + steerAngle = new FirstOrderLaggedFloat(steerAngleTimeConstant, 0.0f); } // GroundSlipMultiplier changes the slip rate. @@ -324,7 +333,6 @@ void FixedUpdate() { // Clamp input values. AccelerationInput = Mathf.Clamp(AccelerationInput, -MaxAccelerationInput, MaxAccelerationInput); - SteerAngleInput = Mathf.Clamp(SteerAngleInput, -MaxSteerAngleInput, MaxSteerAngleInput); // Compute vehicle infomation. ComputeVehicleState(); From 0d3f32686fb19bfebead2107ba4e4e4498874eb7 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Mon, 25 Nov 2024 17:38:06 +0900 Subject: [PATCH 3/4] apply first order lag to acceleration Signed-off-by: Autumn60 --- Assets/AWSIM/Scripts/Vehicles/Vehicle.cs | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs b/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs index e69e7d79f..280d0ad89 100644 --- a/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs +++ b/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs @@ -189,8 +189,14 @@ public float SidewaySlipMultipler /// In the plane, output the force that will result in this acceleration. /// On a slope, it is affected by the slope resistance, so it does not match the input. /// - // TODO: Compute first order lag - public float AccelerationInput; + public float AccelerationInput + { + get => acceleration.DesiredValue; + set => acceleration.DesiredValue = Mathf.Clamp(value, -MaxAccelerationInput, MaxAccelerationInput); + } + private FirstOrderLaggedFloat acceleration; + [SerializeField, Min(0.0f), Tooltip("Set 0 to disable the lag")] + private float accelerationTimeConstant = 0.0f; /// /// Vehicle steering input. Tire angle (degree) @@ -215,6 +221,11 @@ public float SteerAngleInput /// public Vector3 LocalAcceleration { get; private set; } + /// + /// Vehicle acceleration (m/s^2) + /// + public float Acceleration => acceleration.Value; + /// /// Vehicle speed (m/s) /// @@ -303,6 +314,9 @@ void Awake() ForwardSlipMultipler = 1f; SidewaySlipMultipler = 1f; + // Initialize acceleration + acceleration = new FirstOrderLaggedFloat(accelerationTimeConstant, 0.0f); + // Initialize steer angle steerAngle = new FirstOrderLaggedFloat(steerAngleTimeConstant, 0.0f); } @@ -331,9 +345,6 @@ private void OnTriggerExit(Collider other) void FixedUpdate() { - // Clamp input values. - AccelerationInput = Mathf.Clamp(AccelerationInput, -MaxAccelerationInput, MaxAccelerationInput); - // Compute vehicle infomation. ComputeVehicleState(); @@ -347,8 +358,7 @@ void FixedUpdate() if (sleep == false) { // Update wheel force. - var acceleration = AccelerationInput; - UpdateWheelsForce(acceleration); + UpdateWheelsForce(Acceleration); } // cache value for next frame. From 5075a25095f480404f12cc29b717b639a7bbf9d7 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Mon, 25 Nov 2024 17:50:35 +0900 Subject: [PATCH 4/4] change param order Signed-off-by: Autumn60 --- Assets/AWSIM/Scripts/Vehicles/Vehicle.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs b/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs index 280d0ad89..8ff3418e9 100644 --- a/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs +++ b/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs @@ -171,11 +171,15 @@ public float SidewaySlipMultipler // -MaxSteerAngleInput <= SteerAngleInput <= MaxSteerAngleInput. [Range(0.01f, 80)] public float MaxSteerAngleInput = 25f; + [SerializeField, Min(0.0f), Tooltip("Set 0 to disable the lag")] + private float steerAngleTimeConstant = 0.0f; // Set value to clamp AccelerationInput (m/s^2). // -MaxAccelerationInput <= AccelerationInput <= MaxAccelerationInput. [Range(0.01f, 50)] public float MaxAccelerationInput = 10; + [SerializeField, Min(0.0f), Tooltip("Set 0 to disable the lag")] + private float accelerationTimeConstant = 0.0f; [Header("Inputs")] @@ -195,8 +199,6 @@ public float AccelerationInput set => acceleration.DesiredValue = Mathf.Clamp(value, -MaxAccelerationInput, MaxAccelerationInput); } private FirstOrderLaggedFloat acceleration; - [SerializeField, Min(0.0f), Tooltip("Set 0 to disable the lag")] - private float accelerationTimeConstant = 0.0f; /// /// Vehicle steering input. Tire angle (degree) @@ -208,8 +210,6 @@ public float SteerAngleInput set => steerAngle.DesiredValue = Mathf.Clamp(value, -MaxSteerAngleInput, MaxSteerAngleInput); } private FirstOrderLaggedFloat steerAngle; - [SerializeField, Min(0.0f), Tooltip("Set 0 to disable the lag")] - private float steerAngleTimeConstant = 0.0f; /// /// Vehicle turn signal input. NONE, LEFT, RIGHT, HAZARD.