From 7b84bcfbb8817feca25f33db85bee6c1fc7f0b98 Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Tue, 18 Dec 2012 22:59:59 -0800 Subject: BulletSim: initial implementation of a PID motor. Not hooked up yet. --- OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs | 44 ++++++++++++++++++++---- 1 file changed, 38 insertions(+), 6 deletions(-) (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs') diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs index e0faf4e..c718228 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs @@ -117,12 +117,12 @@ public class BSVMotor : BSMotor // A form of stepping that does not take the time quantum into account. // The caller must do the right thing later. - public Vector3 Step() + public virtual Vector3 Step() { return Step(1f); } - public Vector3 Step(float timeStep) + public virtual Vector3 Step(float timeStep) { Vector3 returnCurrent = Vector3.Zero; if (!CurrentValue.ApproxEquals(TargetValue, 0.01f)) @@ -204,17 +204,49 @@ public class BSFMotor : BSMotor public void SetTarget(float target) { } - public float Step(float timeStep) + public virtual float Step(float timeStep) { return 0f; } } -public class BSPIDMotor : BSMotor + +// Proportional, Integral, Derivitive Motor +// Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors. +public class BSPIDVMotor : BSVMotor { - // TODO: write and use this one - public BSPIDMotor(string useName) + 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; } + + Vector3 IntegralFactor { get; set; } + Vector3 LastError { get; set; } + + 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); + } + + public override Vector3 Step(float timeStep) + { + // How far are we from where we should be + Vector3 error = TargetValue - CurrentValue; + + // Add up the error so we can integrate over the accumulated errors + IntegralFactor += error * timeStep; + + // A simple derivitive is the rate of change from the last error. + Vector3 derivFactor = (error - LastError) * timeStep; + LastError = error; + + // Proportion Integral Derivitive + // Correction = proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError + Vector3 ret = error * pFactor + IntegralFactor * iFactor + derivFactor * dFactor; + + return ret; } } } -- cgit v1.1 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(-) (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs') 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 From e522bdb96a86ed1789e1438e4aac1ed522db70b8 Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Thu, 20 Dec 2012 16:05:33 -0800 Subject: BulletSim: Check for unspecified TimeScale in BSVMotor and don't scale if not specified. Add test dump routine. Don'e zero current and target values when error goes to zero as the values could be used externally to store the actual target values, etc. --- OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs | 52 ++++++++++++++++++++---- 1 file changed, 43 insertions(+), 9 deletions(-) (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs') diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs index b57d2c8..a6af40d 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs @@ -46,6 +46,7 @@ public abstract class BSMotor } public virtual void Reset() { } public virtual void Zero() { } + public virtual void GenerateTestOutput(float timeStep) { } // A name passed at motor creation for easily identifyable debugging messages. public string UseName { get; private set; } @@ -93,8 +94,9 @@ public class BSVMotor : BSMotor public virtual float ErrorZeroThreshold { get; set; } - public virtual Vector3 TargetValue { get; private set; } - public virtual Vector3 CurrentValue { get; private set; } + public virtual Vector3 TargetValue { get; protected set; } + public virtual Vector3 CurrentValue { get; protected set; } + public virtual Vector3 LastError { get; protected set; } public BSVMotor(string useName) : base(useName) @@ -122,6 +124,11 @@ public class BSVMotor : BSMotor { TargetValue = target; } + public override void Zero() + { + base.Zero(); + CurrentValue = TargetValue = Vector3.Zero; + } // Compute the next step and return the new current value public virtual Vector3 Step(float timeStep) @@ -168,21 +175,25 @@ public class BSVMotor : BSMotor 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); + CurrentValue = TargetValue; + MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={2}", + BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue); } return CurrentValue; } public virtual Vector3 Step(float timeStep, Vector3 error) { + LastError = error; Vector3 returnCorrection = Vector3.Zero; if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)) { // correction = error / secondsItShouldTakeToCorrect - Vector3 correctionAmount = error / TimeScale * timeStep; + Vector3 correctionAmount; + if (TimeScale == 0f || TimeScale == BSMotor.Infinite) + correctionAmount = error * timeStep; + else + correctionAmount = error / TimeScale * timeStep; returnCorrection = correctionAmount; MDetailLog("{0}, BSVMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5},frictTS={6},ret={7}", @@ -191,6 +202,30 @@ public class BSVMotor : BSMotor } return returnCorrection; } + + // The user sets all the parameters and calls this which outputs values until error is zero. + public override void GenerateTestOutput(float timeStep) + { + // maximum number of outputs to generate. + int maxOutput = 50; + MDetailLog("{0},BSVMotor.Test,{1},===================================== BEGIN Test Output", BSScene.DetailLogZero, UseName); + MDetailLog("{0},BSVMotor.Test,{1},timeScale={2},targDlyTS={3},frictTS={4},eff={5},curr={6},tgt={7}", + BSScene.DetailLogZero, UseName, + TimeScale, TargetValueDecayTimeScale, FrictionTimescale, Efficiency, + CurrentValue, TargetValue); + + LastError = BSMotor.InfiniteVector; + while (maxOutput-- > 0 && !LastError.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)) + { + Vector3 lastStep = Step(timeStep); + MDetailLog("{0},BSVMotor.Test,{1},cur={2},tgt={3},lastError={4},lastStep={5}", + BSScene.DetailLogZero, UseName, CurrentValue, TargetValue, LastError, lastStep); + } + MDetailLog("{0},BSVMotor.Test,{1},===================================== END Test Output", BSScene.DetailLogZero, UseName); + + + } + public override string ToString() { return String.Format("<{0},curr={1},targ={2},decayTS={3},frictTS={4}>", @@ -238,7 +273,6 @@ public class BSPIDVMotor : BSVMotor public float EfficiencyLow = 4.0f; Vector3 IntegralFactor { get; set; } - Vector3 LastError { get; set; } public BSPIDVMotor(string useName) : base(useName) @@ -274,7 +308,7 @@ public class BSPIDVMotor : BSVMotor } // Ignore Current and Target Values and just advance the PID computation on this error. - public Vector3 Step(float timeStep, Vector3 error) + public override Vector3 Step(float timeStep, Vector3 error) { // Add up the error so we can integrate over the accumulated errors IntegralFactor += error * timeStep; -- cgit v1.1 From b4f8a05e9a30373942c3a0511e43dcce3df11184 Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Fri, 21 Dec 2012 09:56:31 -0800 Subject: BulletSim: Better detail logging of VMotor actions. --- OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs') diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs index a6af40d..34a87c6 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs @@ -166,11 +166,13 @@ public class BSVMotor : BSMotor CurrentValue *= (Vector3.One - frictionFactor); } - 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}", + MDetailLog("{0}, BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}", BSScene.DetailLogZero, UseName, origCurrVal, origTarget, - timeStep, error, correction, - TargetValueDecayTimeScale, decayFactor, frictionFactor, - CurrentValue, TargetValue, CurrentValue); + timeStep, error, correction); + MDetailLog("{0}, BSVMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},frictTS={4},frictFact={5},tgt={6},curr={7}", + BSScene.DetailLogZero, UseName, + TargetValueDecayTimeScale, decayFactor, FrictionTimescale, frictionFactor, + TargetValue, CurrentValue); } else { @@ -196,9 +198,8 @@ public class BSVMotor : BSMotor 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); + MDetailLog("{0}, BSVMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}", + BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount); } return returnCorrection; } -- cgit v1.1