BulletSim: reorganize motor step code to separate error computation allowing subclass for PID error correction.

1 files changed, 91 insertions, 51 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index c718228..b57d2c8 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -29,13 +29,14 @@ using System;
 using System.Collections.Generic;
 using System.Text;
 using OpenMetaverse;
+using OpenSim.Framework;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
 public abstract class BSMotor
 {
     // Timescales and other things can be turned off by setting them to 'infinite'.
-    public const float Infinite = 12345f;
+    public const float Infinite = 12345.6f;
     public readonly static Vector3 InfiniteVector = new Vector3(BSMotor.Infinite, BSMotor.Infinite, BSMotor.Infinite);
 
     public BSMotor(string useName)
@@ -62,12 +63,16 @@ public abstract class BSMotor
         }
     }
 }
-// Can all the incremental stepping be replaced with motor classes?
 
 // Motor which moves CurrentValue to TargetValue over TimeScale seconds.
 // The TargetValue decays in TargetValueDecayTimeScale and
 //     the CurrentValue will be held back by FrictionTimeScale.
-// TimeScale and TargetDelayTimeScale may be 'infinite' which means go decay.
+// This motor will "zero itself" over time in that the targetValue will
+//    decay to zero and the currentValue will follow it to that zero.
+//    The overall effect is for the returned correction value to go from large
+//    values (the total difference between current and target minus friction)
+//    to small and eventually zero values.
+// TimeScale and TargetDelayTimeScale may be 'infinite' which means no decay.
 
 // For instance, if something is moving at speed X and the desired speed is Y,
 //    CurrentValue is X and TargetValue is Y. As the motor is stepped, new
@@ -81,13 +86,15 @@ public class BSVMotor : BSMotor
     // public Vector3 FrameOfReference { get; set; }
     // public Vector3 Offset { get; set; }
 
-    public float TimeScale { get; set; }
-    public float TargetValueDecayTimeScale { get; set; }
-    public Vector3 FrictionTimescale { get; set; }
-    public float Efficiency { get; set; }
+    public virtual float TimeScale { get; set; }
+    public virtual float TargetValueDecayTimeScale { get; set; }
+    public virtual Vector3 FrictionTimescale { get; set; }
+    public virtual float Efficiency { get; set; }
+
+    public virtual float ErrorZeroThreshold { get; set; }
 
-    public Vector3 TargetValue { get; private set; }
-    public Vector3 CurrentValue { get; private set; }
+    public virtual Vector3 TargetValue { get; private set; }
+    public virtual Vector3 CurrentValue { get; private set; }
 
     public BSVMotor(string useName)
         : base(useName)
@@ -96,6 +103,7 @@ public class BSVMotor : BSMotor
         Efficiency = 1f;
         FrictionTimescale = BSMotor.InfiniteVector;
         CurrentValue = TargetValue = Vector3.Zero;
+        ErrorZeroThreshold = 0.01f;
     }
     public BSVMotor(string useName, float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency) 
         : this(useName)
@@ -115,24 +123,19 @@ public class BSVMotor : BSMotor
         TargetValue = target;
     }
 
-    // A form of stepping that does not take the time quantum into account.
-    // The caller must do the right thing later.
-    public virtual Vector3 Step()
-    {
-        return Step(1f);
-    }
-
+    // Compute the next step and return the new current value
     public virtual Vector3 Step(float timeStep)
     {
-        Vector3 returnCurrent = Vector3.Zero;
-        if (!CurrentValue.ApproxEquals(TargetValue, 0.01f))
+        Vector3 origTarget = TargetValue;       // DEBUG
+        Vector3 origCurrVal = CurrentValue;     // DEBUG
+
+        Vector3 correction = Vector3.Zero;
+        Vector3 error = TargetValue - CurrentValue;
+        if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
         {
-            Vector3 origTarget = TargetValue;       // DEBUG
-            Vector3 origCurrVal = CurrentValue;   // DEBUG
+            correction = Step(timeStep, error);
 
-            // Addition =  (desiredVector - currentAppliedVector) / secondsItShouldTakeToComplete
-            Vector3 addAmount = (TargetValue - CurrentValue)/TimeScale * timeStep;
-            CurrentValue += addAmount;
+            CurrentValue += correction;
 
             // The desired value reduces to zero which also reduces the difference with current.
             // If the decay time is infinite, don't decay at all.
@@ -143,39 +146,50 @@ public class BSVMotor : BSMotor
                 TargetValue *= (1f - decayFactor);
             }
 
+            // The amount we can correct the error is reduced by the friction
             Vector3 frictionFactor = Vector3.Zero;
             if (FrictionTimescale != BSMotor.InfiniteVector)
             {
                 // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep;
                 // Individual friction components can be 'infinite' so compute each separately.
-                frictionFactor.X = FrictionTimescale.X == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.X) * timeStep;
-                frictionFactor.Y = FrictionTimescale.Y == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.Y) * timeStep;
-                frictionFactor.Z = FrictionTimescale.Z == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.Z) * timeStep;
+                frictionFactor.X = (FrictionTimescale.X == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.X);
+                frictionFactor.Y = (FrictionTimescale.Y == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Y);
+                frictionFactor.Z = (FrictionTimescale.Z == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Z);
+                frictionFactor *= timeStep;
                 CurrentValue *= (Vector3.One - frictionFactor);
             }
 
-            returnCurrent = CurrentValue;
-
-            MDetailLog("{0},  BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},timeScale={5},addAmnt={6},targetDecay={7},decayFact={8},fricTS={9},frictFact={10}",
+            MDetailLog("{0},  BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},error={5},corr={6},targetDecay={6},decayFact={7},frictFac{8},curr={9},target={10},ret={11}",
                                 BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
-                                timeStep, TimeScale, addAmount,
-                                TargetValueDecayTimeScale, decayFactor,
-                                FrictionTimescale, frictionFactor);
-            MDetailLog("{0},  BSVMotor.Step,nonZero,{1},curr={2},target={3},add={4},decay={5},frict={6},ret={7}",
-                                    BSScene.DetailLogZero, UseName, CurrentValue, TargetValue,
-                                    addAmount, decayFactor, frictionFactor, returnCurrent);
+                                timeStep, error, correction,
+                                TargetValueDecayTimeScale, decayFactor, frictionFactor,
+                                CurrentValue, TargetValue, CurrentValue);
         }
         else
         {
             // Difference between what we have and target is small. Motor is done.
             CurrentValue = Vector3.Zero;
             TargetValue = Vector3.Zero;
+            MDetailLog("{0},  BSVMotor.Step,zero,{1},ret={2}",
+                        BSScene.DetailLogZero, UseName, CurrentValue);
+        }
 
-            MDetailLog("{0},  BSVMotor.Step,zero,{1},curr={2},target={3},ret={4}",
-                                    BSScene.DetailLogZero, UseName, TargetValue, CurrentValue, returnCurrent);
+        return CurrentValue;
+    }
+    public virtual Vector3 Step(float timeStep, Vector3 error)
+    {
+        Vector3 returnCorrection = Vector3.Zero;
+        if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
+        {
+            // correction =  error / secondsItShouldTakeToCorrect
+            Vector3 correctionAmount = error / TimeScale * timeStep;
 
+            returnCorrection = correctionAmount;
+            MDetailLog("{0},  BSVMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5},frictTS={6},ret={7}",
+                                    BSScene.DetailLogZero, UseName, timeStep, TimeScale, error,
+                                    correctionAmount, FrictionTimescale, returnCorrection);
         }
-        return returnCurrent;
+        return returnCorrection;
     }
     public override string ToString()
     {
@@ -214,9 +228,14 @@ public class BSFMotor : BSMotor
 // Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors.
 public class BSPIDVMotor : BSVMotor
 {
-    public Vector3 pFactor { get; set; }    // Amount of direct correction of an error (sometimes called 'proportional gain')
-    public Vector3 iFactor { get; set; }    // 
-    public Vector3 dFactor { get; set; }
+    // Larger makes more overshoot, smaller means converge quicker. Range of 0.1 to 10.
+    public Vector3 proportionFactor { get; set; }
+    public Vector3 integralFactor { get; set; }
+    public Vector3 derivFactor { get; set; }
+    // Arbritrary factor range.
+    // EfficiencyHigh means move quickly to the correct number. EfficiencyLow means might over correct.
+    public float EfficiencyHigh = 0.4f;
+    public float EfficiencyLow = 4.0f;
 
     Vector3 IntegralFactor { get; set; }
     Vector3 LastError { get; set; }
@@ -224,17 +243,39 @@ public class BSPIDVMotor : BSVMotor
     public BSPIDVMotor(string useName)
         : base(useName)
     {
-        // larger makes more overshoot, smaller means converge quicker. Range of 0.1 to 10.
-        pFactor = new Vector3(1.00f, 1.00f, 1.00f);
-        iFactor = new Vector3(1.00f, 1.00f, 1.00f);
-        dFactor = new Vector3(1.00f, 1.00f, 1.00f);
+        proportionFactor = new Vector3(1.00f, 1.00f, 1.00f);
+        integralFactor = new Vector3(1.00f, 1.00f, 1.00f);
+        derivFactor = new Vector3(1.00f, 1.00f, 1.00f);
+        IntegralFactor = Vector3.Zero;
+        LastError = Vector3.Zero;
     }
 
-    public override Vector3 Step(float timeStep)
+    public override void Zero()
     {
-        // How far are we from where we should be
-        Vector3 error = TargetValue - CurrentValue;
+        base.Zero();
+    }
+
+    public override float Efficiency
+    {
+        get { return base.Efficiency; }
+        set
+        {
+            base.Efficiency = Util.Clamp(value, 0f, 1f);
+            // Compute factors based on efficiency.
+            // If efficiency is high (1f), use a factor value that moves the error value to zero with little overshoot.
+            // If efficiency is low (0f), use a factor value that overcorrects.
+            // TODO: might want to vary contribution of different factor depending on efficiency.
+            float factor = ((1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow) / 3f;
+            // float factor = (1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow;
+            proportionFactor = new Vector3(factor, factor, factor);
+            integralFactor = new Vector3(factor, factor, factor);
+            derivFactor = new Vector3(factor, factor, factor);
+        }
+    }
 
+    // Ignore Current and Target Values and just advance the PID computation on this error.
+    public Vector3 Step(float timeStep, Vector3 error)
+    {
         // Add up the error so we can integrate over the accumulated errors
         IntegralFactor += error * timeStep;
 
@@ -242,9 +283,8 @@ public class BSPIDVMotor : BSVMotor
         Vector3 derivFactor = (error - LastError) * timeStep;
         LastError = error;
 
-        //                   Proportion                  Integral               Derivitive
-        // Correction = proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError
-        Vector3 ret =        error * pFactor    + IntegralFactor * iFactor + derivFactor * dFactor;
+        // Correction = -(proportionOfPresentError +      accumulationOfPastError    +     rateOfChangeOfError)
+        Vector3 ret   = -(error * proportionFactor + IntegralFactor * integralFactor + derivFactor * derivFactor);
 
         return ret;
     }