OpenSim
/
OpenSimMirror
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

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

0.7.5-pf-bulletsim
Robert Adams 2012-12-20 08:35:36 -08:00
parent a9b9c0f035
commit b7ad44e3a6
1 changed files with 94 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

148
OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
View File

@ -29,13 +29,14 @@ using System;
using System.Collections.Generic; using System.Collections.Generic;
using System.Text; using System.Text;
using OpenMetaverse; using OpenMetaverse;
using OpenSim.Framework;
namespace OpenSim.Region.Physics.BulletSPlugin namespace OpenSim.Region.Physics.BulletSPlugin
{ {
public abstract class BSMotor public abstract class BSMotor
{ {
// Timescales and other things can be turned off by setting them to 'infinite'. // 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 readonly static Vector3 InfiniteVector = new Vector3(BSMotor.Infinite, BSMotor.Infinite, BSMotor.Infinite);
public BSMotor(string useName) 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. // Motor which moves CurrentValue to TargetValue over TimeScale seconds.
// The TargetValue decays in TargetValueDecayTimeScale and // The TargetValue decays in TargetValueDecayTimeScale and
// the CurrentValue will be held back by FrictionTimeScale. // 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, // 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 // 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 FrameOfReference { get; set; }
// public Vector3 Offset { get; set; } // public Vector3 Offset { get; set; }
public float TimeScale { get; set; } public virtual float TimeScale { get; set; }
public float TargetValueDecayTimeScale { get; set; } public virtual float TargetValueDecayTimeScale { get; set; }
public Vector3 FrictionTimescale { get; set; } public virtual Vector3 FrictionTimescale { get; set; }
public float Efficiency { get; set; } public virtual float Efficiency { get; set; }
public Vector3 TargetValue { get; private set; } public virtual float ErrorZeroThreshold { get; 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) public BSVMotor(string useName)
: base(useName) : base(useName)
@ -96,6 +103,7 @@ public class BSVMotor : BSMotor
Efficiency = 1f; Efficiency = 1f;
FrictionTimescale = BSMotor.InfiniteVector; FrictionTimescale = BSMotor.InfiniteVector;
CurrentValue = TargetValue = Vector3.Zero; CurrentValue = TargetValue = Vector3.Zero;
ErrorZeroThreshold = 0.01f;
} }
public BSVMotor(string useName, float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency) public BSVMotor(string useName, float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency)
: this(useName) : this(useName)
@ -115,24 +123,19 @@ public class BSVMotor : BSMotor
TargetValue = target; TargetValue = target;
} }
// A form of stepping that does not take the time quantum into account. // Compute the next step and return the new current value
// The caller must do the right thing later.
public virtual Vector3 Step()
{
return Step(1f);
}
public virtual Vector3 Step(float timeStep) public virtual Vector3 Step(float timeStep)
{ {
Vector3 returnCurrent = Vector3.Zero; Vector3 origTarget = TargetValue; // DEBUG
if (!CurrentValue.ApproxEquals(TargetValue, 0.01f)) Vector3 origCurrVal = CurrentValue; // DEBUG
{
Vector3 origTarget = TargetValue; // DEBUG
Vector3 origCurrVal = CurrentValue; // DEBUG
// Addition = (desiredVector - currentAppliedVector) / secondsItShouldTakeToComplete Vector3 correction = Vector3.Zero;
Vector3 addAmount = (TargetValue - CurrentValue)/TimeScale * timeStep; Vector3 error = TargetValue - CurrentValue;
CurrentValue += addAmount; if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
{
correction = Step(timeStep, error);
CurrentValue += correction;
// The desired value reduces to zero which also reduces the difference with current. // The desired value reduces to zero which also reduces the difference with current.
// If the decay time is infinite, don't decay at all. // If the decay time is infinite, don't decay at all.
@ -143,39 +146,50 @@ public class BSVMotor : BSMotor
TargetValue *= (1f - decayFactor); TargetValue *= (1f - decayFactor);
} }
// The amount we can correct the error is reduced by the friction
Vector3 frictionFactor = Vector3.Zero; Vector3 frictionFactor = Vector3.Zero;
if (FrictionTimescale != BSMotor.InfiniteVector) if (FrictionTimescale != BSMotor.InfiniteVector)
{ {
// frictionFactor = (Vector3.One / FrictionTimescale) * timeStep; // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep;
// Individual friction components can be 'infinite' so compute each separately. // Individual friction components can be 'infinite' so compute each separately.
frictionFactor.X = FrictionTimescale.X == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.X) * timeStep; frictionFactor.X = (FrictionTimescale.X == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.X);
frictionFactor.Y = FrictionTimescale.Y == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.Y) * timeStep; frictionFactor.Y = (FrictionTimescale.Y == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Y);
frictionFactor.Z = FrictionTimescale.Z == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.Z) * timeStep; frictionFactor.Z = (FrictionTimescale.Z == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Z);
frictionFactor *= timeStep;
CurrentValue *= (Vector3.One - frictionFactor); CurrentValue *= (Vector3.One - frictionFactor);
} }
returnCurrent = CurrentValue; 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},timeScale={5},addAmnt={6},targetDecay={7},decayFact={8},fricTS={9},frictFact={10}",
BSScene.DetailLogZero, UseName, origCurrVal, origTarget, BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
timeStep, TimeScale, addAmount, timeStep, error, correction,
TargetValueDecayTimeScale, decayFactor, TargetValueDecayTimeScale, decayFactor, frictionFactor,
FrictionTimescale, frictionFactor); CurrentValue, TargetValue, CurrentValue);
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);
} }
else else
{ {
// Difference between what we have and target is small. Motor is done. // Difference between what we have and target is small. Motor is done.
CurrentValue = Vector3.Zero; CurrentValue = Vector3.Zero;
TargetValue = Vector3.Zero; TargetValue = Vector3.Zero;
MDetailLog("{0}, BSVMotor.Step,zero,{1},ret={2}",
MDetailLog("{0}, BSVMotor.Step,zero,{1},curr={2},target={3},ret={4}", BSScene.DetailLogZero, UseName, CurrentValue);
BSScene.DetailLogZero, UseName, TargetValue, CurrentValue, returnCurrent);
} }
return 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 returnCorrection;
} }
public override string ToString() 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. // Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors.
public class BSPIDVMotor : BSVMotor public class BSPIDVMotor : BSVMotor
{ {
public Vector3 pFactor { get; set; } // Amount of direct correction of an error (sometimes called 'proportional gain') // Larger makes more overshoot, smaller means converge quicker. Range of 0.1 to 10.
public Vector3 iFactor { get; set; } // public Vector3 proportionFactor { get; set; }
public Vector3 dFactor { 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 IntegralFactor { get; set; }
Vector3 LastError { get; set; } Vector3 LastError { get; set; }
@ -224,17 +243,39 @@ public class BSPIDVMotor : BSVMotor
public BSPIDVMotor(string useName) public BSPIDVMotor(string useName)
: base(useName) : base(useName)
{ {
// larger makes more overshoot, smaller means converge quicker. Range of 0.1 to 10. proportionFactor = new Vector3(1.00f, 1.00f, 1.00f);
pFactor = new Vector3(1.00f, 1.00f, 1.00f); integralFactor = new Vector3(1.00f, 1.00f, 1.00f);
iFactor = new Vector3(1.00f, 1.00f, 1.00f); derivFactor = new Vector3(1.00f, 1.00f, 1.00f);
dFactor = 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 base.Zero();
Vector3 error = TargetValue - CurrentValue; }
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 // Add up the error so we can integrate over the accumulated errors
IntegralFactor += error * timeStep; IntegralFactor += error * timeStep;
@ -242,9 +283,8 @@ public class BSPIDVMotor : BSVMotor
Vector3 derivFactor = (error - LastError) * timeStep; Vector3 derivFactor = (error - LastError) * timeStep;
LastError = error; LastError = error;
// Proportion Integral Derivitive // Correction = -(proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError)
// Correction = proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError Vector3 ret = -(error * proportionFactor + IntegralFactor * integralFactor + derivFactor * derivFactor);
Vector3 ret = error * pFactor + IntegralFactor * iFactor + derivFactor * dFactor;
return ret; return ret;
} }