Cumulative error avoidance at sleep

tier4 · Dec 11, 2022 · b6ccc8e · b6ccc8e
1 parent e29372c
commit b6ccc8e
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 1 deletion.
diff --git a/Assets/AWSIM/Prefabs/Vehicles/Lexus RX450h 2015 Sample Sensor.prefab b/Assets/AWSIM/Prefabs/Vehicles/Lexus RX450h 2015 Sample Sensor.prefab
@@ -1766,7 +1766,7 @@ MonoBehaviour:
   centerOfMassTransform: {fileID: 2963107001226707149}
   useInertia: 0
   inertia: {x: 0, y: 0, z: 0}
-  sleepVelocityThreshold: 0.04
+  sleepVelocityThreshold: 0.02
   SkiddingCancelRate: 0.606
   frontAxle:
     leftWheel: {fileID: 2963107000584003440}

diff --git a/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs b/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs
@@ -212,6 +212,11 @@ public class Axle
         Vector3 lastPosition;
         Quaternion lastRotation;
 
+        // Sleep position & rotation
+        Vector3 sleepPositon;
+        Quaternion sleepRotation;
+        bool lastSleep;
+
         void Awake()
         {
             m_rigidbody = GetComponent<Rigidbody>();
@@ -254,6 +259,7 @@ void FixedUpdate()
             lastVelocity = m_rigidbody.velocity;
             lastPosition = m_transform.position;
             lastRotation = m_transform.rotation;
+            lastSleep = sleep;
 
             // ----- inner methods -----
 
@@ -340,6 +346,12 @@ bool IsCanSleepInput()
 
             void UpdateVehicleSleep(bool isSleep)
             {
+                if (isSleep == true && lastSleep == false)
+                {
+                    sleepPositon = transform.position;
+                    sleepRotation = transform.rotation;
+                }
+
                 // Vehicle sleep.
                 if (isSleep)
                 {
@@ -348,6 +360,9 @@ void UpdateVehicleSleep(bool isSleep)
 
                     m_rigidbody.Sleep();
                     m_rigidbody.constraints = RigidbodyConstraints.FreezeAll;
+
+                    transform.position = sleepPositon;
+                    transform.rotation = sleepRotation;
                 }
                 else
                 {