From b7ad44e3a687041a5a4761f1d0739a4226a901c2 Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Thu, 20 Dec 2012 08:35:36 -0800 Subject: BulletSim: reorganize motor step code to separate error computation allowing subclass for PID error correction. --- OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs | 142 +++++++++++++++-------- 1 file 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; } -- cgit v1.1