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); } }