aboutsummaryrefslogtreecommitdiffstatshomepage
diff options
context:
space:
mode:
authorRobert Adams2012-12-20 16:05:33 -0800
committerRobert Adams2012-12-20 16:05:33 -0800
commite522bdb96a86ed1789e1438e4aac1ed522db70b8 (patch)
tree9ae6fdeabc5a2fc14313af6da0d805e44853b849
parentBulletSim: replace use of funky nullable values for vehicle property update c... (diff)
downloadopensim-SC-e522bdb96a86ed1789e1438e4aac1ed522db70b8.zip
opensim-SC-e522bdb96a86ed1789e1438e4aac1ed522db70b8.tar.gz
opensim-SC-e522bdb96a86ed1789e1438e4aac1ed522db70b8.tar.bz2
opensim-SC-e522bdb96a86ed1789e1438e4aac1ed522db70b8.tar.xz
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.
Diffstat (limited to '')
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSMotors.cs52
1 files changed, 43 insertions, 9 deletions
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
46 } 46 }
47 public virtual void Reset() { } 47 public virtual void Reset() { }
48 public virtual void Zero() { } 48 public virtual void Zero() { }
49 public virtual void GenerateTestOutput(float timeStep) { }
49 50
50 // A name passed at motor creation for easily identifyable debugging messages. 51 // A name passed at motor creation for easily identifyable debugging messages.
51 public string UseName { get; private set; } 52 public string UseName { get; private set; }
@@ -93,8 +94,9 @@ public class BSVMotor : BSMotor
93 94
94 public virtual float ErrorZeroThreshold { get; set; } 95 public virtual float ErrorZeroThreshold { get; set; }
95 96
96 public virtual Vector3 TargetValue { get; private set; } 97 public virtual Vector3 TargetValue { get; protected set; }
97 public virtual Vector3 CurrentValue { get; private set; } 98 public virtual Vector3 CurrentValue { get; protected set; }
99 public virtual Vector3 LastError { get; protected set; }
98 100
99 public BSVMotor(string useName) 101 public BSVMotor(string useName)
100 : base(useName) 102 : base(useName)
@@ -122,6 +124,11 @@ public class BSVMotor : BSMotor
122 { 124 {
123 TargetValue = target; 125 TargetValue = target;
124 } 126 }
127 public override void Zero()
128 {
129 base.Zero();
130 CurrentValue = TargetValue = Vector3.Zero;
131 }
125 132
126 // Compute the next step and return the new current value 133 // Compute the next step and return the new current value
127 public virtual Vector3 Step(float timeStep) 134 public virtual Vector3 Step(float timeStep)
@@ -168,21 +175,25 @@ public class BSVMotor : BSMotor
168 else 175 else
169 { 176 {
170 // Difference between what we have and target is small. Motor is done. 177 // Difference between what we have and target is small. Motor is done.
171 CurrentValue = Vector3.Zero; 178 CurrentValue = TargetValue;
172 TargetValue = Vector3.Zero; 179 MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={2}",
173 MDetailLog("{0}, BSVMotor.Step,zero,{1},ret={2}", 180 BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
174 BSScene.DetailLogZero, UseName, CurrentValue);
175 } 181 }
176 182
177 return CurrentValue; 183 return CurrentValue;
178 } 184 }
179 public virtual Vector3 Step(float timeStep, Vector3 error) 185 public virtual Vector3 Step(float timeStep, Vector3 error)
180 { 186 {
187 LastError = error;
181 Vector3 returnCorrection = Vector3.Zero; 188 Vector3 returnCorrection = Vector3.Zero;
182 if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)) 189 if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
183 { 190 {
184 // correction = error / secondsItShouldTakeToCorrect 191 // correction = error / secondsItShouldTakeToCorrect
185 Vector3 correctionAmount = error / TimeScale * timeStep; 192 Vector3 correctionAmount;
193 if (TimeScale == 0f || TimeScale == BSMotor.Infinite)
194 correctionAmount = error * timeStep;
195 else
196 correctionAmount = error / TimeScale * timeStep;
186 197
187 returnCorrection = correctionAmount; 198 returnCorrection = correctionAmount;
188 MDetailLog("{0}, BSVMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5},frictTS={6},ret={7}", 199 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
191 } 202 }
192 return returnCorrection; 203 return returnCorrection;
193 } 204 }
205
206 // The user sets all the parameters and calls this which outputs values until error is zero.
207 public override void GenerateTestOutput(float timeStep)
208 {
209 // maximum number of outputs to generate.
210 int maxOutput = 50;
211 MDetailLog("{0},BSVMotor.Test,{1},===================================== BEGIN Test Output", BSScene.DetailLogZero, UseName);
212 MDetailLog("{0},BSVMotor.Test,{1},timeScale={2},targDlyTS={3},frictTS={4},eff={5},curr={6},tgt={7}",
213 BSScene.DetailLogZero, UseName,
214 TimeScale, TargetValueDecayTimeScale, FrictionTimescale, Efficiency,
215 CurrentValue, TargetValue);
216
217 LastError = BSMotor.InfiniteVector;
218 while (maxOutput-- > 0 && !LastError.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
219 {
220 Vector3 lastStep = Step(timeStep);
221 MDetailLog("{0},BSVMotor.Test,{1},cur={2},tgt={3},lastError={4},lastStep={5}",
222 BSScene.DetailLogZero, UseName, CurrentValue, TargetValue, LastError, lastStep);
223 }
224 MDetailLog("{0},BSVMotor.Test,{1},===================================== END Test Output", BSScene.DetailLogZero, UseName);
225
226
227 }
228
194 public override string ToString() 229 public override string ToString()
195 { 230 {
196 return String.Format("<{0},curr={1},targ={2},decayTS={3},frictTS={4}>", 231 return String.Format("<{0},curr={1},targ={2},decayTS={3},frictTS={4}>",
@@ -238,7 +273,6 @@ public class BSPIDVMotor : BSVMotor
238 public float EfficiencyLow = 4.0f; 273 public float EfficiencyLow = 4.0f;
239 274
240 Vector3 IntegralFactor { get; set; } 275 Vector3 IntegralFactor { get; set; }
241 Vector3 LastError { get; set; }
242 276
243 public BSPIDVMotor(string useName) 277 public BSPIDVMotor(string useName)
244 : base(useName) 278 : base(useName)
@@ -274,7 +308,7 @@ public class BSPIDVMotor : BSVMotor
274 } 308 }
275 309
276 // Ignore Current and Target Values and just advance the PID computation on this error. 310 // Ignore Current and Target Values and just advance the PID computation on this error.
277 public Vector3 Step(float timeStep, Vector3 error) 311 public override Vector3 Step(float timeStep, Vector3 error)
278 { 312 {
279 // Add up the error so we can integrate over the accumulated errors 313 // Add up the error so we can integrate over the accumulated errors
280 IntegralFactor += error * timeStep; 314 IntegralFactor += error * timeStep;