aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
diff options
context:
space:
mode:
authorRobert Adams2012-12-20 08:35:36 -0800
committerRobert Adams2012-12-20 08:35:36 -0800
commitb7ad44e3a687041a5a4761f1d0739a4226a901c2 (patch)
tree8586cdf3c08e9a8a955f98c281c73065caec6c6f /OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
parentBulletSim: improve angularVerticalAttraction calculation to compute angular c... (diff)
downloadopensim-SC_OLD-b7ad44e3a687041a5a4761f1d0739a4226a901c2.zip
opensim-SC_OLD-b7ad44e3a687041a5a4761f1d0739a4226a901c2.tar.gz
opensim-SC_OLD-b7ad44e3a687041a5a4761f1d0739a4226a901c2.tar.bz2
opensim-SC_OLD-b7ad44e3a687041a5a4761f1d0739a4226a901c2.tar.xz
BulletSim: reorganize motor step code to separate error computation allowing subclass for PID error correction.
Diffstat (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs')
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSMotors.cs142
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;
29using System.Collections.Generic; 29using System.Collections.Generic;
30using System.Text; 30using System.Text;
31using OpenMetaverse; 31using OpenMetaverse;
32using OpenSim.Framework;
32 33
33namespace OpenSim.Region.Physics.BulletSPlugin 34namespace OpenSim.Region.Physics.BulletSPlugin
34{ 35{
35public abstract class BSMotor 36public abstract class BSMotor
36{ 37{
37 // Timescales and other things can be turned off by setting them to 'infinite'. 38 // Timescales and other things can be turned off by setting them to 'infinite'.
38 public const float Infinite = 12345f; 39 public const float Infinite = 12345.6f;
39 public readonly static Vector3 InfiniteVector = new Vector3(BSMotor.Infinite, BSMotor.Infinite, BSMotor.Infinite); 40 public readonly static Vector3 InfiniteVector = new Vector3(BSMotor.Infinite, BSMotor.Infinite, BSMotor.Infinite);
40 41
41 public BSMotor(string useName) 42 public BSMotor(string useName)
@@ -62,12 +63,16 @@ public abstract class BSMotor
62 } 63 }
63 } 64 }
64} 65}
65// Can all the incremental stepping be replaced with motor classes?
66 66
67// Motor which moves CurrentValue to TargetValue over TimeScale seconds. 67// Motor which moves CurrentValue to TargetValue over TimeScale seconds.
68// The TargetValue decays in TargetValueDecayTimeScale and 68// The TargetValue decays in TargetValueDecayTimeScale and
69// the CurrentValue will be held back by FrictionTimeScale. 69// the CurrentValue will be held back by FrictionTimeScale.
70// TimeScale and TargetDelayTimeScale may be 'infinite' which means go decay. 70// This motor will "zero itself" over time in that the targetValue will
71// decay to zero and the currentValue will follow it to that zero.
72// The overall effect is for the returned correction value to go from large
73// values (the total difference between current and target minus friction)
74// to small and eventually zero values.
75// TimeScale and TargetDelayTimeScale may be 'infinite' which means no decay.
71 76
72// For instance, if something is moving at speed X and the desired speed is Y, 77// For instance, if something is moving at speed X and the desired speed is Y,
73// CurrentValue is X and TargetValue is Y. As the motor is stepped, new 78// CurrentValue is X and TargetValue is Y. As the motor is stepped, new
@@ -81,13 +86,15 @@ public class BSVMotor : BSMotor
81 // public Vector3 FrameOfReference { get; set; } 86 // public Vector3 FrameOfReference { get; set; }
82 // public Vector3 Offset { get; set; } 87 // public Vector3 Offset { get; set; }
83 88
84 public float TimeScale { get; set; } 89 public virtual float TimeScale { get; set; }
85 public float TargetValueDecayTimeScale { get; set; } 90 public virtual float TargetValueDecayTimeScale { get; set; }
86 public Vector3 FrictionTimescale { get; set; } 91 public virtual Vector3 FrictionTimescale { get; set; }
87 public float Efficiency { get; set; } 92 public virtual float Efficiency { get; set; }
93
94 public virtual float ErrorZeroThreshold { get; set; }
88 95
89 public Vector3 TargetValue { get; private set; } 96 public virtual Vector3 TargetValue { get; private set; }
90 public Vector3 CurrentValue { get; private set; } 97 public virtual Vector3 CurrentValue { get; private set; }
91 98
92 public BSVMotor(string useName) 99 public BSVMotor(string useName)
93 : base(useName) 100 : base(useName)
@@ -96,6 +103,7 @@ public class BSVMotor : BSMotor
96 Efficiency = 1f; 103 Efficiency = 1f;
97 FrictionTimescale = BSMotor.InfiniteVector; 104 FrictionTimescale = BSMotor.InfiniteVector;
98 CurrentValue = TargetValue = Vector3.Zero; 105 CurrentValue = TargetValue = Vector3.Zero;
106 ErrorZeroThreshold = 0.01f;
99 } 107 }
100 public BSVMotor(string useName, float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency) 108 public BSVMotor(string useName, float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency)
101 : this(useName) 109 : this(useName)
@@ -115,24 +123,19 @@ public class BSVMotor : BSMotor
115 TargetValue = target; 123 TargetValue = target;
116 } 124 }
117 125
118 // A form of stepping that does not take the time quantum into account. 126 // Compute the next step and return the new current value
119 // The caller must do the right thing later.
120 public virtual Vector3 Step()
121 {
122 return Step(1f);
123 }
124
125 public virtual Vector3 Step(float timeStep) 127 public virtual Vector3 Step(float timeStep)
126 { 128 {
127 Vector3 returnCurrent = Vector3.Zero; 129 Vector3 origTarget = TargetValue; // DEBUG
128 if (!CurrentValue.ApproxEquals(TargetValue, 0.01f)) 130 Vector3 origCurrVal = CurrentValue; // DEBUG
131
132 Vector3 correction = Vector3.Zero;
133 Vector3 error = TargetValue - CurrentValue;
134 if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
129 { 135 {
130 Vector3 origTarget = TargetValue; // DEBUG 136 correction = Step(timeStep, error);
131 Vector3 origCurrVal = CurrentValue; // DEBUG
132 137
133 // Addition = (desiredVector - currentAppliedVector) / secondsItShouldTakeToComplete 138 CurrentValue += correction;
134 Vector3 addAmount = (TargetValue - CurrentValue)/TimeScale * timeStep;
135 CurrentValue += addAmount;
136 139
137 // The desired value reduces to zero which also reduces the difference with current. 140 // The desired value reduces to zero which also reduces the difference with current.
138 // If the decay time is infinite, don't decay at all. 141 // If the decay time is infinite, don't decay at all.
@@ -143,39 +146,50 @@ public class BSVMotor : BSMotor
143 TargetValue *= (1f - decayFactor); 146 TargetValue *= (1f - decayFactor);
144 } 147 }
145 148
149 // The amount we can correct the error is reduced by the friction
146 Vector3 frictionFactor = Vector3.Zero; 150 Vector3 frictionFactor = Vector3.Zero;
147 if (FrictionTimescale != BSMotor.InfiniteVector) 151 if (FrictionTimescale != BSMotor.InfiniteVector)
148 { 152 {
149 // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep; 153 // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep;
150 // Individual friction components can be 'infinite' so compute each separately. 154 // Individual friction components can be 'infinite' so compute each separately.
151 frictionFactor.X = FrictionTimescale.X == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.X) * timeStep; 155 frictionFactor.X = (FrictionTimescale.X == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.X);
152 frictionFactor.Y = FrictionTimescale.Y == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.Y) * timeStep; 156 frictionFactor.Y = (FrictionTimescale.Y == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Y);
153 frictionFactor.Z = FrictionTimescale.Z == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.Z) * timeStep; 157 frictionFactor.Z = (FrictionTimescale.Z == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Z);
158 frictionFactor *= timeStep;
154 CurrentValue *= (Vector3.One - frictionFactor); 159 CurrentValue *= (Vector3.One - frictionFactor);
155 } 160 }
156 161
157 returnCurrent = CurrentValue; 162 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}",
158
159 MDetailLog("{0}, BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},timeScale={5},addAmnt={6},targetDecay={7},decayFact={8},fricTS={9},frictFact={10}",
160 BSScene.DetailLogZero, UseName, origCurrVal, origTarget, 163 BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
161 timeStep, TimeScale, addAmount, 164 timeStep, error, correction,
162 TargetValueDecayTimeScale, decayFactor, 165 TargetValueDecayTimeScale, decayFactor, frictionFactor,
163 FrictionTimescale, frictionFactor); 166 CurrentValue, TargetValue, CurrentValue);
164 MDetailLog("{0}, BSVMotor.Step,nonZero,{1},curr={2},target={3},add={4},decay={5},frict={6},ret={7}",
165 BSScene.DetailLogZero, UseName, CurrentValue, TargetValue,
166 addAmount, decayFactor, frictionFactor, returnCurrent);
167 } 167 }
168 else 168 else
169 { 169 {
170 // Difference between what we have and target is small. Motor is done. 170 // Difference between what we have and target is small. Motor is done.
171 CurrentValue = Vector3.Zero; 171 CurrentValue = Vector3.Zero;
172 TargetValue = Vector3.Zero; 172 TargetValue = Vector3.Zero;
173 MDetailLog("{0}, BSVMotor.Step,zero,{1},ret={2}",
174 BSScene.DetailLogZero, UseName, CurrentValue);
175 }
173 176
174 MDetailLog("{0}, BSVMotor.Step,zero,{1},curr={2},target={3},ret={4}", 177 return CurrentValue;
175 BSScene.DetailLogZero, UseName, TargetValue, CurrentValue, returnCurrent); 178 }
179 public virtual Vector3 Step(float timeStep, Vector3 error)
180 {
181 Vector3 returnCorrection = Vector3.Zero;
182 if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
183 {
184 // correction = error / secondsItShouldTakeToCorrect
185 Vector3 correctionAmount = error / TimeScale * timeStep;
176 186
187 returnCorrection = correctionAmount;
188 MDetailLog("{0}, BSVMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5},frictTS={6},ret={7}",
189 BSScene.DetailLogZero, UseName, timeStep, TimeScale, error,
190 correctionAmount, FrictionTimescale, returnCorrection);
177 } 191 }
178 return returnCurrent; 192 return returnCorrection;
179 } 193 }
180 public override string ToString() 194 public override string ToString()
181 { 195 {
@@ -214,9 +228,14 @@ public class BSFMotor : BSMotor
214// Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors. 228// Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors.
215public class BSPIDVMotor : BSVMotor 229public class BSPIDVMotor : BSVMotor
216{ 230{
217 public Vector3 pFactor { get; set; } // Amount of direct correction of an error (sometimes called 'proportional gain') 231 // Larger makes more overshoot, smaller means converge quicker. Range of 0.1 to 10.
218 public Vector3 iFactor { get; set; } // 232 public Vector3 proportionFactor { get; set; }
219 public Vector3 dFactor { get; set; } 233 public Vector3 integralFactor { get; set; }
234 public Vector3 derivFactor { get; set; }
235 // Arbritrary factor range.
236 // EfficiencyHigh means move quickly to the correct number. EfficiencyLow means might over correct.
237 public float EfficiencyHigh = 0.4f;
238 public float EfficiencyLow = 4.0f;
220 239
221 Vector3 IntegralFactor { get; set; } 240 Vector3 IntegralFactor { get; set; }
222 Vector3 LastError { get; set; } 241 Vector3 LastError { get; set; }
@@ -224,17 +243,39 @@ public class BSPIDVMotor : BSVMotor
224 public BSPIDVMotor(string useName) 243 public BSPIDVMotor(string useName)
225 : base(useName) 244 : base(useName)
226 { 245 {
227 // larger makes more overshoot, smaller means converge quicker. Range of 0.1 to 10. 246 proportionFactor = new Vector3(1.00f, 1.00f, 1.00f);
228 pFactor = new Vector3(1.00f, 1.00f, 1.00f); 247 integralFactor = new Vector3(1.00f, 1.00f, 1.00f);
229 iFactor = new Vector3(1.00f, 1.00f, 1.00f); 248 derivFactor = new Vector3(1.00f, 1.00f, 1.00f);
230 dFactor = new Vector3(1.00f, 1.00f, 1.00f); 249 IntegralFactor = Vector3.Zero;
250 LastError = Vector3.Zero;
231 } 251 }
232 252
233 public override Vector3 Step(float timeStep) 253 public override void Zero()
234 { 254 {
235 // How far are we from where we should be 255 base.Zero();
236 Vector3 error = TargetValue - CurrentValue; 256 }
257
258 public override float Efficiency
259 {
260 get { return base.Efficiency; }
261 set
262 {
263 base.Efficiency = Util.Clamp(value, 0f, 1f);
264 // Compute factors based on efficiency.
265 // If efficiency is high (1f), use a factor value that moves the error value to zero with little overshoot.
266 // If efficiency is low (0f), use a factor value that overcorrects.
267 // TODO: might want to vary contribution of different factor depending on efficiency.
268 float factor = ((1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow) / 3f;
269 // float factor = (1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow;
270 proportionFactor = new Vector3(factor, factor, factor);
271 integralFactor = new Vector3(factor, factor, factor);
272 derivFactor = new Vector3(factor, factor, factor);
273 }
274 }
237 275
276 // Ignore Current and Target Values and just advance the PID computation on this error.
277 public Vector3 Step(float timeStep, Vector3 error)
278 {
238 // Add up the error so we can integrate over the accumulated errors 279 // Add up the error so we can integrate over the accumulated errors
239 IntegralFactor += error * timeStep; 280 IntegralFactor += error * timeStep;
240 281
@@ -242,9 +283,8 @@ public class BSPIDVMotor : BSVMotor
242 Vector3 derivFactor = (error - LastError) * timeStep; 283 Vector3 derivFactor = (error - LastError) * timeStep;
243 LastError = error; 284 LastError = error;
244 285
245 // Proportion Integral Derivitive 286 // Correction = -(proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError)
246 // Correction = proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError 287 Vector3 ret = -(error * proportionFactor + IntegralFactor * integralFactor + derivFactor * derivFactor);
247 Vector3 ret = error * pFactor + IntegralFactor * iFactor + derivFactor * dFactor;
248 288
249 return ret; 289 return ret;
250 } 290 }