private static void Write2DPlot(string outputFileName, string title, double[] array, int xPoints, int yPoints, double xRange, double yRange)
{
// 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 outputCommand = "set output " + '"' + outputFileName + '"';
sw.WriteLine(outputCommand);
sw.WriteLine("set title \"" + title + "\"");
sw.WriteLine("set xlabel \"x\"");
sw.WriteLine("set ylabel \"v\"");
var inputText = "plot '-' using 1:2:3 with image pixels" + "\n";
//sw.WriteLine("set pm3d");
// sw.WriteLine("set pm3d interpolate 0,0");
//sw.WriteLine("set palette maxcolors 3");
//sw.WriteLine("set palette defined 0 '#000fff', 1 '#90ff70', 2 '#ee0000'");
//var inputText = "splot '-' using 1:2:3 with pm3d" + "\n";
sw.WriteLine(inputText);
var length = xPoints * yPoints;
var stateValues = ArrayEx2.Init(xPoints, yPoints,
(x, y) =>
new double[]
{
StateValueForIndex(xPoints, x, xRange),
StateValueForIndex(yPoints, y, yRange)
}
);
for (int index = 0; index < length; index++)
{
var stateValue = stateValues[index];
var xStateValue = stateValue[0];
var yStateValue = stateValue[1];
var plotValue = array[index];
sw.WriteLine(string.Format("{0} {1} {2}", xStateValue, yStateValue, plotValue));
}
sw.WriteLine(" e "); // terminate data
sw.Flush();
}
}
}
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);
}
}
