From f6460cbb1b30cc5d47f91f96b386d39f64512193 Mon Sep 17 00:00:00 2001
From: Markus Quaritsch <markus.quaritsch@tugraz.at>
Date: Fri, 29 Apr 2016 09:15:55 +0200
Subject: [PATCH] added operating point to response, introduce abort criterion
for search, coasting handles DistanceExceeded response (i.e. vehicle stops to
early) and issues braking action
---
.../Exceptions/VectoSimulationException.cs | 5 +
VectoCommon/VectoCommon/Models/IResponse.cs | 3 +
.../Models/Connector/Ports/Impl/Response.cs | 3 +
.../Impl/DefaultDriverStrategy.cs | 7 ++
.../Models/SimulationComponent/Impl/Driver.cs | 40 +++++---
VectoCore/VectoCore/Utils/SearchAlgorithm.cs | 96 ++++++++++++++++---
6 files changed, 127 insertions(+), 27 deletions(-)
diff --git a/VectoCommon/VectoCommon/Exceptions/VectoSimulationException.cs b/VectoCommon/VectoCommon/Exceptions/VectoSimulationException.cs
index 53c5a6bbae..4ad924d871 100644
--- a/VectoCommon/VectoCommon/Exceptions/VectoSimulationException.cs
+++ b/VectoCommon/VectoCommon/Exceptions/VectoSimulationException.cs
@@ -71,4 +71,9 @@ namespace TUGraz.VectoCommon.Exceptions
{
public VectoSearchFailedException(string message, params object[] args) : base(message, args) {}
}
+
+ public class VectoSearchAbortedException : VectoException
+ {
+ public VectoSearchAbortedException(string message, params object[] args) : base(message, args) { }
+ }
}
\ No newline at end of file
diff --git a/VectoCommon/VectoCommon/Models/IResponse.cs b/VectoCommon/VectoCommon/Models/IResponse.cs
index fffb0b55fb..30a1afe1c4 100644
--- a/VectoCommon/VectoCommon/Models/IResponse.cs
+++ b/VectoCommon/VectoCommon/Models/IResponse.cs
@@ -30,6 +30,7 @@
*/
using TUGraz.VectoCommon.Utils;
+using TUGraz.VectoCore.Models.SimulationComponent.Impl;
namespace TUGraz.VectoCommon.Models
{
@@ -63,5 +64,7 @@ namespace TUGraz.VectoCommon.Models
PerSecond EngineSpeed { get; set; }
Second AbsTime { get; set; }
+
+ Driver.OperatingPoint OperatingPoint { get; set; }
}
}
\ No newline at end of file
diff --git a/VectoCore/VectoCore/Models/Connector/Ports/Impl/Response.cs b/VectoCore/VectoCore/Models/Connector/Ports/Impl/Response.cs
index f594056130..9555b7c5f3 100644
--- a/VectoCore/VectoCore/Models/Connector/Ports/Impl/Response.cs
+++ b/VectoCore/VectoCore/Models/Connector/Ports/Impl/Response.cs
@@ -32,6 +32,7 @@
using System.Linq;
using TUGraz.VectoCommon.Models;
using TUGraz.VectoCommon.Utils;
+using TUGraz.VectoCore.Models.SimulationComponent.Impl;
namespace TUGraz.VectoCore.Models.Connector.Ports.Impl
{
@@ -61,6 +62,8 @@ namespace TUGraz.VectoCore.Models.Connector.Ports.Impl
public Second AbsTime { get; set; }
+ public Driver.OperatingPoint OperatingPoint { get; set; }
+
public PerSecond EngineSpeed { get; set; }
public object Source { get; set; }
diff --git a/VectoCore/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs b/VectoCore/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs
index b0bb6295c7..0bfdc0b6f9 100644
--- a/VectoCore/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs
+++ b/VectoCore/VectoCore/Models/SimulationComponent/Impl/DefaultDriverStrategy.cs
@@ -540,6 +540,13 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
}
//Phase = BrakingPhase.Brake;
}).
+ Case<ResponseDrivingCycleDistanceExceeded>(r => {
+ if (!ds.IsEqual(r.MaxDistance)) {
+ // distance has been reduced due to vehicle stop in coast/roll action => use brake action to get exactly to the stop-distance
+ // TODO: what if no gear is enaged (and we need driveline power to get to the stop-distance?
+ response = Driver.DrivingActionBrake(absTime, ds, DriverStrategy.BrakeTrigger.NextTargetSpeed, gradient);
+ }
+ }).
Case<ResponseGearShift>(r => { response = Driver.DrivingActionRoll(absTime, ds, targetVelocity, gradient); });
// handle the SpeedLimitExceeded Response separately in case it occurs in one of the requests in the second try
response.Switch().
diff --git a/VectoCore/VectoCore/Models/SimulationComponent/Impl/Driver.cs b/VectoCore/VectoCore/Models/SimulationComponent/Impl/Driver.cs
index 6aca5126b2..4f5bac080b 100644
--- a/VectoCore/VectoCore/Models/SimulationComponent/Impl/Driver.cs
+++ b/VectoCore/VectoCore/Models/SimulationComponent/Impl/Driver.cs
@@ -358,7 +358,8 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
var nextAcceleration = DriverData.AccelerationCurve.Lookup(v2).Deceleration;
var tmp = ComputeTimeInterval(VectoMath.Min(operatingPoint.Acceleration, nextAcceleration),
operatingPoint.SimulationDistance);
- if (!operatingPoint.Acceleration.IsEqual(nextAcceleration) && operatingPoint.SimulationDistance.IsEqual(tmp.SimulationDistance)) {
+ if (!operatingPoint.Acceleration.IsEqual(nextAcceleration) &&
+ operatingPoint.SimulationDistance.IsEqual(tmp.SimulationDistance)) {
// only adjust operating point if the acceleration is different but the simulation distance is not modified
// i.e., braking to the next sample point (but a little bit slower)
Log.Debug("adjusting acceleration from {0} to {1}", operatingPoint.Acceleration, tmp.Acceleration);
@@ -570,7 +571,9 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
retVal.SimulationInterval = tmp.SimulationInterval;
retVal.SimulationDistance = tmp.SimulationDistance;
}
- return NextComponent.Request(absTime, retVal.SimulationInterval, acc, gradient, true);
+ var response = NextComponent.Request(absTime, retVal.SimulationInterval, acc, gradient, true);
+ response.OperatingPoint = retVal;
+ return response;
},
criterion: response => {
if (response is ResponseEngineSpeedTooLow) {
@@ -583,19 +586,29 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
var r = (ResponseDryRun)response;
delta = actionRoll ? r.GearboxPowerRequest : (coasting ? r.DeltaDragLoad : r.DeltaFullLoad);
return delta.Value();
- });
-
- if (
- !retVal.Acceleration.IsBetween(DriverData.AccelerationCurve.MaxDeceleration(),
- DriverData.AccelerationCurve.MaxAcceleration())) {
- Log.Info("Operating Point outside driver acceleration limits: a: {0}", retVal.Acceleration);
- }
+ },
+ abortCriterion:
+ (response, cnt) => {
+ var r = (ResponseDryRun)response;
+ if (r == null) {
+ return false;
+ }
- return ComputeTimeInterval(retVal.Acceleration, retVal.SimulationDistance);
+ return coasting && !ds.IsEqual(r.OperatingPoint.SimulationDistance);
+ });
+ } catch (VectoSearchAbortedException) {
+ // search aborted, try to go ahead with the last acceleration
} catch (Exception) {
Log.Error("Failed to find operating point! absTime: {0}", absTime);
throw;
}
+
+ if (!retVal.Acceleration.IsBetween(DriverData.AccelerationCurve.MaxDeceleration(),
+ DriverData.AccelerationCurve.MaxAcceleration())) {
+ Log.Info("Operating Point outside driver acceleration limits: a: {0}", retVal.Acceleration);
+ }
+
+ return ComputeTimeInterval(retVal.Acceleration, retVal.SimulationDistance);
}
/// <summary>
@@ -734,8 +747,11 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
public IResponse DrivingActionHalt(Second absTime, Second dt, MeterPerSecond targetVelocity, Radian gradient)
{
if (!targetVelocity.IsEqual(0) || !DataBus.VehicleSpeed.IsEqual(0, 1e-3)) {
- Log.Error("TargetVelocity ({0}) and VehicleVelocity ({1}) must be zero when vehicle is halting!", targetVelocity, DataBus.VehicleSpeed);
- throw new VectoSimulationException("TargetVelocity ({0}) and VehicleVelocity ({1}) must be zero when vehicle is halting!", targetVelocity, DataBus.VehicleSpeed);
+ Log.Error("TargetVelocity ({0}) and VehicleVelocity ({1}) must be zero when vehicle is halting!", targetVelocity,
+ DataBus.VehicleSpeed);
+ throw new VectoSimulationException(
+ "TargetVelocity ({0}) and VehicleVelocity ({1}) must be zero when vehicle is halting!", targetVelocity,
+ DataBus.VehicleSpeed);
}
var retVal = NextComponent.Request(absTime, dt, 0.SI<MeterPerSquareSecond>(), gradient);
diff --git a/VectoCore/VectoCore/Utils/SearchAlgorithm.cs b/VectoCore/VectoCore/Utils/SearchAlgorithm.cs
index cfedec5064..2ef327137c 100644
--- a/VectoCore/VectoCore/Utils/SearchAlgorithm.cs
+++ b/VectoCore/VectoCore/Utils/SearchAlgorithm.cs
@@ -30,10 +30,13 @@
*/
using System;
+using System.Data;
+using System.Linq;
using TUGraz.VectoCore.Configuration;
using TUGraz.VectoCommon.Exceptions;
using TUGraz.VectoCommon.Models;
using TUGraz.VectoCommon.Utils;
+using TUGraz.VectoCore.Models.Connector.Ports.Impl;
namespace TUGraz.VectoCore.Utils
{
@@ -52,7 +55,7 @@ namespace TUGraz.VectoCore.Utils
Func<object, double> criterion) where T : SIBase<T>
{
var iterationCount = 0;
- return Search(x, y, interval, getYValue, evaluateFunction, criterion, ref iterationCount);
+ return Search(x, y, interval, getYValue, evaluateFunction, criterion, null, ref iterationCount);
}
/// <summary>
@@ -62,18 +65,38 @@ namespace TUGraz.VectoCore.Utils
/// getYValue: result => ((ResponseDryRun)result).Delta,
/// evaluateFunction: x => NextComponent.Request(absTime, dt, x, gradient, true),
/// criterion: result => ((ResponseDryRun)result).Delta);
+ /// abortCriterion: result => true/false
+ /// </code>
+ /// </summary>
+ public static T Search<T>(T x, SI y, T interval, Func<object, SI> getYValue, Func<T, object> evaluateFunction,
+ Func<object, double> criterion, Func<object, int, bool> abortCriterion) where T : SIBase<T>
+ {
+ var iterationCount = 0;
+ return Search(x, y, interval, getYValue, evaluateFunction, criterion, abortCriterion, ref iterationCount);
+ }
+
+ /// <summary>
+ /// Applies a numerical search over the evaluateFunction until the criterion reaches approximately 0.
+ /// <code>
+ /// SearchAlgorithm.Search(firstAcceleration, firstDelta, secondAccelerationInterval,
+ /// getYValue: result => ((ResponseDryRun)result).Delta,
+ /// evaluateFunction: x => NextComponent.Request(absTime, dt, x, gradient, true),
+ /// criterion: result => ((ResponseDryRun)result).Delta);
+ /// abortCriterion: result => true/false
/// </code>
/// </summary>
public static T Search<T>(T x, SI y, T interval, Func<object, SI> getYValue,
- Func<T, object> evaluateFunction, Func<object, double> criterion, ref int iterationCount) where T : SIBase<T>
+ Func<T, object> evaluateFunction, Func<object, double> criterion, Func<object, int, bool> abortCriterion,
+ ref int iterationCount) where T : SIBase<T>
{
T result;
try {
- result = InterpolateLinear(x, y, interval, getYValue, evaluateFunction, criterion, ref iterationCount);
+ result = InterpolateLinear(x, y, interval, getYValue, evaluateFunction, criterion, abortCriterion,
+ ref iterationCount);
} catch (VectoException ex) {
var log = LogManager.GetLogger(typeof(SearchAlgorithm).FullName);
log.Debug("Falling back to LineSearch. InterpolationSearch failed: " + ex.Message);
- result = LineSearch(x, y, interval, getYValue, evaluateFunction, criterion, ref iterationCount);
+ result = LineSearch(x, y, interval, getYValue, evaluateFunction, criterion, abortCriterion, ref iterationCount);
}
return result;
}
@@ -84,8 +107,8 @@ namespace TUGraz.VectoCore.Utils
/// Phase 1: Linear Bracketing: Search iterative for the area of interest (with fixed step size).
/// Phase 2: Binary Sectioning: Binary search in the area of interest.
/// </summary>
- private static T LineSearch<T>(T x, SI y, T interval, Func<object, SI> getYValue,
- Func<T, object> evaluateFunction, Func<object, double> criterion, ref int iterationCount) where T : SIBase<T>
+ private static T LineSearch<T>(T x, SI y, T interval, Func<object, SI> getYValue, Func<T, object> evaluateFunction,
+ Func<object, double> criterion, Func<object, int, bool> abortCriterion, ref int iterationCount) where T : SIBase<T>
{
var log = LogManager.GetLogger(typeof(SearchAlgorithm).FullName);
@@ -97,8 +120,6 @@ namespace TUGraz.VectoCore.Utils
LogManager.DisableLogging();
try {
for (var count = 1; count < 100; count++) {
- debug.Add(new { x, y });
-
if (origY.Sign() != y.Sign()) {
intervalFactor = 0.5;
}
@@ -107,6 +128,7 @@ namespace TUGraz.VectoCore.Utils
x += interval * -y.Sign();
var result = evaluateFunction(x);
+ debug.Add(new { x, y, delta = criterion(result), result });
if (criterion(result).IsEqual(0, Constants.SimulationSettings.LineSearchTolerance)) {
LogManager.EnableLogging();
log.Debug("LineSearch found an operating point after {0} function calls.", count);
@@ -114,6 +136,12 @@ namespace TUGraz.VectoCore.Utils
LogManager.DisableLogging();
return x;
}
+ if (abortCriterion != null && abortCriterion(result, iterationCount)) {
+ LogManager.EnableLogging();
+ log.Debug("LineSearch aborted due to abortCriterion: {0}", result);
+ LogManager.DisableLogging();
+ throw new VectoSearchAbortedException("LineSearch");
+ }
y = getYValue(result);
}
} finally {
@@ -124,6 +152,8 @@ namespace TUGraz.VectoCore.Utils
log.Debug("LineSearch could not find an operating point.");
log.Error("Exceeded max iterations when searching for operating point!");
log.Error("debug: {0}", debug);
+
+ WriteSerach(debug, "LineSearch.csv");
throw new VectoSearchFailedException("Failed to find operating point! points: {0}", debug);
}
@@ -132,11 +162,12 @@ namespace TUGraz.VectoCore.Utils
/// Calculates linear equation of 2 points and jumps directly to root-point.
/// </summary>
private static T InterpolateLinear<T>(T x1, SI y1, T interval, Func<object, SI> getYValue,
- Func<T, object> evaluateFunction, Func<object, double> criterion, ref int iterationCount) where T : SIBase<T>
+ Func<T, object> evaluateFunction, Func<object, double> criterion, Func<object, int, bool> abortCriterion,
+ ref int iterationCount) where T : SIBase<T>
{
var log = LogManager.GetLogger(typeof(SearchAlgorithm).FullName);
- var debug = new DebugData();
- debug.Add(new { x = x1, y = y1 } );
+ var debug = new DebugData();
+ debug.Add(new { x = x1, y = y1 });
log.Debug("Log Disabled during Search InterpolateLinear.");
LogManager.DisableLogging();
try {
@@ -152,7 +183,7 @@ namespace TUGraz.VectoCore.Utils
for (var count = 2; count < 30; count++) {
var y2 = getYValue(result);
- debug.Add(new { x = x2, y = y2 });
+ debug.Add(new { x = x2, y = y2, delta = criterion(result), result });
try {
var k = (y2 - y1) / (x2 - x1);
@@ -163,7 +194,7 @@ namespace TUGraz.VectoCore.Utils
if (!(ex.InnerException is DivideByZeroException)) {
throw;
}
- debug.Add(new { x = x2, y = getYValue(result) });
+ debug.Add(new { x = x2, y = getYValue(result), delta = criterion(result), result });
LogManager.EnableLogging();
log.Debug("InterpolateLinear could not get more exact. Aborting after {0} function calls.", count);
LogManager.DisableLogging();
@@ -173,14 +204,19 @@ namespace TUGraz.VectoCore.Utils
result = evaluateFunction(x2);
if (criterion(result).IsEqual(0, Constants.SimulationSettings.InterpolateSearchTolerance)) {
- debug.Add(new { x = x2, y = getYValue(result) });
+ debug.Add(new { x = x2, y = getYValue(result), delta = criterion(result), result });
LogManager.EnableLogging();
log.Debug("InterpolateLinear found an operating point after {0} function calls.", count);
LogManager.DisableLogging();
iterationCount += count;
return x2;
}
-
+ if (abortCriterion != null && abortCriterion(result, iterationCount)) {
+ LogManager.EnableLogging();
+ log.Debug("LineSearch aborted due to abortCriterion: {0}", result);
+ LogManager.DisableLogging();
+ throw new VectoSearchAbortedException("InterpolateLinearSearch");
+ }
y1 = y2;
}
} finally {
@@ -191,7 +227,37 @@ namespace TUGraz.VectoCore.Utils
log.Debug("InterpolateLinear could not find an operating point.");
log.Error("Exceeded max iterations when searching for operating point!");
log.Error("debug: {0}", debug);
+
+ WriteSerach(debug, "InterpolateSearch.csv");
throw new VectoSearchFailedException("Failed to find operating point! points: {0}", debug);
}
+
+ private static void WriteSerach(DebugData debug, string filename)
+ {
+ var table = new DataTable();
+ table.Columns.Add("x", typeof(double));
+ table.Columns.Add("y", typeof(double));
+ table.Columns.Add("delta", typeof(double));
+ table.Columns.Add("AuxPower", typeof(double));
+ table.Columns.Add("engineSpeed", typeof(double));
+ table.Columns.Add("enginePower", typeof(double));
+
+ foreach (var entry in debug.Data.Skip(1)) {
+ var response = entry.result as ResponseDryRun;
+ if (response == null) {
+ continue;
+ }
+ var row = table.NewRow();
+ row["x"] = entry.x.Value();
+ row["y"] = entry.y.Value();
+ row["delta"] = entry.delta;
+ row["AuxPower"] = response.AuxiliariesPowerDemand.Value();
+ row["engineSpeed"] = response.EngineSpeed.Value();
+ row["enginePower"] = response.EnginePowerRequest.Value();
+
+ table.Rows.Add(row);
+ }
+ VectoCSVFile.Write(filename, table);
+ }
}
}
\ No newline at end of file
--
GitLab