예제 #1
0
        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);
            }
        }
예제 #2
0
        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();
                }
            }
        }