private static void AnimateValueIteration()
{
double[] finalCost = GetFinalCost(xPoints, yPoints, xRange, yRange);
var discretisedControl = new double[] { -1.0, 0.0, 1.0 };
Func <double[], double, double, double[]> stateEquation = (state, u, t) => Systems.DoubleIntegrator(state, u, t);
// run the ValueIterate loop manually, for plotting
var stateValues = ArrayEx2.Init(xPoints, yPoints,
(x, y) =>
new double[]
{
StateValueForIndex(xPoints, x, xRange),
StateValueForIndex(yPoints, y, yRange)
}
);
var updatedStatesPerControl = discretisedControl
.Select(u => stateValues.Select(state => stateEquation(state, 0.01, u)).ToArray())
.ToArray();
var size = xPoints * yPoints;
var currentCost = finalCost.ToArray();
var newCosts = new double[size];
var newControl = new double[size];
var gamma = 0.999;
var plotEveryN = 10;
var xTolerance = xRange / xPoints;
var yTolerance = yRange / yPoints;
// Minimum time control. U is not used (would be for LQR step)
Func <double[], double, double> costPerStepFunc =
(s, u) => Math.Abs(s[0]) < xTolerance && Math.Abs(s[1]) < yTolerance
? 0.0 : 1.0;
for (var count = 0; count < 1000; ++count)
{
if (count % plotEveryN == 0)
{
var filenameNumberString = (count / plotEveryN).ToString("D3");
var controlPath = string.Format("E:/temp/plot/control/{0}.png", filenameNumberString);
var costPath = string.Format("E:/temp/plot/cost/{0}.png", filenameNumberString);
Write2DPlot(controlPath, "control", newControl, xPoints, yPoints, xRange, yRange);
Write2DPlot(costPath, "cost", currentCost, xPoints, yPoints, xRange, yRange);
}
ValueIteration.ValueIterationStep(finalCost, currentCost, newCosts, newControl,
updatedStatesPerControl, discretisedControl, gamma,
costPerStepFunc,
xPoints, yPoints, xRange, yRange);
if (count % plotEveryN == 0)
{
var maxDiff = newCosts.Zip(currentCost, (x, y) => Math.Abs(x - y)).Max();
Console.WriteLine(count.ToString("D3") + " " + maxDiff.ToString());
}
Array.Copy(newCosts, currentCost, newCosts.Length);
}
}
private static void PlotTrajectory()
{
double[] finalCost = GetFinalCost(xPoints, yPoints, xRange, yRange);
var discretisedControl = new double[] { -1.0, 0.0, 1.0 };
Func <double[], double, double, double[]> stateEquation = (s, u, t) => Systems.DoubleIntegrator(s, u, t);
var result = ValueIteration.ValueIterate(finalCost,
discretisedControl,
stateEquation,
xPoints,
yPoints,
xRange,
yRange,
10000);
var currentCost = result.Item1;
var controls = result.Item2;
var count = result.Item3;
Console.WriteLine("Converged in {0} iterations", count);
// initial state at x = -2.0, heading in -ve x direction with v = -1.5 units/sec
var initialState = new double[] { -2.0, -1.5 };
var state = initialState.ToArray();
const int numIterations = 2000;
List <double[]> statesAndControls = new List <double[]>();
for (var iteration = 0; iteration < numIterations; iteration++)
{
var control = MathHelper.FindInterpolatedCost(state, controls, xPoints, xRange, yPoints, yRange);
// var control = AnalyticalDoubleIntegratorBangBangControl(state);
state = Systems.DoubleIntegrator(state, 0.01, control);
if (iteration % 10 == 0)
{
statesAndControls.Add(new[] { state[0], state[1], control });
}
}
// liuxingguang.blogspot.co.uk/how-to-call-gnuplot-from-c-wpf.html
// msdn.microsoft.com/en-us/library/hh297126(v=vs.100).aspx
using (var process = new Process())
{
process.StartInfo.FileName = _pathToGnuPlot;
process.StartInfo.CreateNoWindow = true;
process.StartInfo.UseShellExecute = false;
process.StartInfo.RedirectStandardInput = true;
process.Start();
using (StreamWriter sw = process.StandardInput)
{
sw.WriteLine("set view map");
// sw.WriteLine("set dgrid3d");
sw.WriteLine("set terminal png size 640,480");
var outputFileName = "E:/temp/plot/trajectory.png";
var title = "trajectory";
var outputCommand = "set output " + '"' + outputFileName + '"';
sw.WriteLine(outputCommand);
sw.WriteLine("set title \"" + title + "\"");
sw.WriteLine("set xlabel \"x\"");
sw.WriteLine("set ylabel \"v\"");
sw.WriteLine("set style data lines");
// with lines lc rgb 'blue'
// var inputText = "splot '-' using 1:2:3 with lines ls 1" + "\n";
var inputText = "splot '-' using 1:2:3 with lines palette" + "\n";
sw.WriteLine(inputText);
foreach (var stateAndControl in statesAndControls)
{
var xStateValue = stateAndControl[0];
var vStateValue = stateAndControl[1];
var control = stateAndControl[2];
sw.WriteLine(string.Format("{0} {1} {2}", xStateValue, vStateValue, control));
}
sw.WriteLine(" e "); // terminate data
sw.Flush();
}
}
}
