Advertisement
creamygoat

Visualising a Cerebrally Enhanced Greedy Goat

May 17th, 2012
555
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 20.87 KB | None | 0 0
  1. #!/usr/bin/python
  2. # -*- coding: utf-8 -*-
  3. #-------------------------------------------------------------------------------
  4. # Visualising a Cerebrally Enhanced Greedy Goat
  5. #
  6. # unit0516_param_optim_enhanced_svg.py
  7. # http://pastebin.com/e15wbtc0
  8. #
  9. # Custom modules:
  10. #   vegesvgplot.py        http://pastebin.com/6Aek3Exm
  11. #
  12. #-------------------------------------------------------------------------------
  13.  
  14.  
  15. '''Visualisation of two optimisation methods in regard to Unit 5-16
  16.  
  17. Description:
  18.  This Python program runs two variants of the PID parameter optimiser
  19.  for the robot car presented in unit 5-16 (the car with badly misaligned
  20.  steering). The output is written to a Scalable Vector
  21.  Graphic file named “output.svg”.
  22.  
  23. Author(s):
  24.  Daniel Neville
  25.  Prof. Sebastian Thrun, udacity (original robot simulation)
  26.  
  27. Copyright, Licence:
  28.  Code by Daniel Neville: Public domain
  29.  Code snarfed from udacity: See http://www.udacity.com/legal/
  30.  
  31. Platform:
  32.  Python 2.5
  33.  
  34.  
  35. INDEX
  36.  
  37.  
  38. Imports
  39.  
  40. Fun stuff:
  41.  
  42.  Snarfed and modified robot code
  43.  RenderToSVG(Data)
  44.  TwiddleAndPlot(InitialCTE, Tolerance, YScale, ErrFnIx, Data)
  45.  
  46. Main:
  47.  
  48.  Main()
  49.  
  50. '''
  51.  
  52.  
  53. #-------------------------------------------------------------------------------
  54.  
  55.  
  56. import math
  57.  
  58. from math import (
  59.   pi, sqrt, hypot, sin, cos, tan, asin, acos, atan, atan2, radians, degrees,
  60.   floor, ceil
  61. )
  62.  
  63. import random
  64.  
  65. # The SVG Plotting for Vegetables module can be found at
  66. # http://pastebin.com/6Aek3Exm
  67.  
  68. from vegesvgplot import (
  69.  
  70.   # Shape constants
  71.   Pt_Break, Pt_Anchor, Pt_Control,
  72.   PtCmdWithCoordsSet, PtCmdSet,
  73.  
  74.   # Indent tracker class
  75.   tIndentTracker,
  76.  
  77.   # Affine matrix class
  78.   tAffineMtx,
  79.  
  80.   # Affine matrix creation functions
  81.   AffineMtxTS, AffineMtxTRS2D, Affine2DMatrices,
  82.  
  83.   # Utility functions
  84.   ValidatedRange, MergedDictionary, Save,
  85.   ArrayDimensions, NewMDArray, CopyArray, At, SetAt,
  86.  
  87.   # Basic vector functions
  88.   VZeros, VOnes, VStdBasis, VDim, VAug, VMajorAxis,
  89.   VNeg, VSum, VDiff, VSchur, VDot,
  90.   VLengthSquared, VLength, VManhattan,
  91.   VScaled, VNormalised,
  92.   VPerp, VCrossProduct, VCrossProduct4D,
  93.   VScalarTripleProduct, VVectorTripleProduct,
  94.   VProjectionOnto,
  95.   VTransposedMAV,
  96.   VRectToPol, VPolToRect,
  97.   VLerp,
  98.  
  99.   # Shape functions
  100.   ShapeFromVertices, ShapePoints, ShapeSubpathRanges, ShapeCurveRanges,
  101.   ShapeLength, LineToShapeIntersections, TransformedShape, PiecewiseArc,
  102.  
  103.   # Output formatting functions
  104.   MaxDP, GFListStr, GFTupleStr, HTMLEscaped, AttrMarkup, ProgressColourStr,
  105.  
  106.   # SVG functions
  107.   SVGStart, SVGEnd, SVGPathDataSegments, SVGPath, SVGText,
  108.   SVGGroup, SVGGroupEnd, SVGGrid
  109.  
  110. )
  111.  
  112.  
  113. #-------------------------------------------------------------------------------
  114. # Fun stuff
  115. #-------------------------------------------------------------------------------
  116.  
  117.  
  118. def RunUnitCode(PIDParams, InitialCTE=1.0, ErrFnIx=0, ShowProgress=False):
  119.  
  120.   #-----------------------------------------------------------------------------
  121.   # Code snarfed from udacity and modified
  122.   #-----------------------------------------------------------------------------
  123.  
  124.   # ------------------------------------------------
  125.   #
  126.   # this is the robot class
  127.   #
  128.  
  129.   class robot:
  130.  
  131.       # --------
  132.       # init:
  133.       #    creates robot and initializes location/orientation to 0, 0, 0
  134.       #
  135.  
  136.       def __init__(self, length = 20.0):
  137.           self.x = 0.0
  138.           self.y = 0.0
  139.           self.orientation = 0.0
  140.           self.length = length
  141.           self.steering_noise = 0.0
  142.           self.distance_noise = 0.0
  143.           self.steering_drift = 0.0
  144.  
  145.       # --------
  146.       # set:
  147.       # sets a robot coordinate
  148.       #
  149.  
  150.       def set(self, new_x, new_y, new_orientation):
  151.  
  152.           self.x = float(new_x)
  153.           self.y = float(new_y)
  154.           self.orientation = float(new_orientation) % (2.0 * pi)
  155.  
  156.  
  157.       # --------
  158.       # set_noise:
  159.       # sets the noise parameters
  160.       #
  161.  
  162.       def set_noise(self, new_s_noise, new_d_noise):
  163.           # makes it possible to change the noise parameters
  164.           # this is often useful in particle filters
  165.           self.steering_noise = float(new_s_noise)
  166.           self.distance_noise = float(new_d_noise)
  167.  
  168.       # --------
  169.       # set_steering_drift:
  170.       # sets the systematical steering drift parameter
  171.       #
  172.  
  173.       def set_steering_drift(self, drift):
  174.           self.steering_drift = drift
  175.  
  176.       # --------
  177.       # move:
  178.       #    steering = front wheel steering angle, limited by max_steering_angle
  179.       #    distance = total distance driven, most be non-negative
  180.  
  181.       def move(self, steering, distance,
  182.                tolerance = 0.001, max_steering_angle = pi / 4.0):
  183.  
  184.           if steering > max_steering_angle:
  185.               steering = max_steering_angle
  186.           if steering < -max_steering_angle:
  187.               steering = -max_steering_angle
  188.           if distance < 0.0:
  189.               distance = 0.0
  190.  
  191.  
  192.           # make a new copy
  193.           res = robot()
  194.           res.length         = self.length
  195.           res.steering_noise = self.steering_noise
  196.           res.distance_noise = self.distance_noise
  197.           res.steering_drift = self.steering_drift
  198.  
  199.           # apply noise
  200.           steering2 = random.gauss(steering, self.steering_noise)
  201.           distance2 = random.gauss(distance, self.distance_noise)
  202.  
  203.           # apply steering drift
  204.           steering2 += self.steering_drift
  205.  
  206.           # Execute motion
  207.           turn = tan(steering2) * distance2 / res.length
  208.  
  209.           if abs(turn) < tolerance:
  210.  
  211.               # approximate by straight line motion
  212.  
  213.               res.x = self.x + (distance2 * cos(self.orientation))
  214.               res.y = self.y + (distance2 * sin(self.orientation))
  215.               res.orientation = (self.orientation + turn) % (2.0 * pi)
  216.  
  217.           else:
  218.  
  219.               # approximate bicycle model for motion
  220.  
  221.               radius = distance2 / turn
  222.               cx = self.x - (sin(self.orientation) * radius)
  223.               cy = self.y + (cos(self.orientation) * radius)
  224.               res.orientation = (self.orientation + turn) % (2.0 * pi)
  225.               res.x = cx + (sin(res.orientation) * radius)
  226.               res.y = cy - (cos(res.orientation) * radius)
  227.  
  228.           return res
  229.  
  230.  
  231.  
  232.  
  233.       def __repr__(self):
  234.           return '[x=%.5f y=%.5f orient=%.5f]'  % (self.x, self.y, self.orientation)
  235.  
  236.  
  237.   # ------------------------------------------------------------------------
  238.   #
  239.   # run - does a single control run.
  240.  
  241.  
  242.   def run(params, InitialCTE, ErrFnIx, printflag = False):
  243.  
  244.       myrobot = robot()
  245.       myrobot.set(0.0, InitialCTE, 0.0)
  246.       speed = 1.0
  247.       err = 0.0
  248.       N = 100
  249.       # myrobot.set_noise(0.1, 0.0)
  250.       myrobot.set_steering_drift(10.0 / 180.0 * pi) # 10 degree steering error
  251.  
  252.       Path = [(myrobot.x, myrobot.y)]
  253.  
  254.       LastCTE = myrobot.y
  255.       LastDeltaCTE = 0.0
  256.       iCTE = 0.0
  257.  
  258.       for i in range(N * 2):
  259.  
  260.           CTE = myrobot.y
  261.           dCTE = CTE - LastCTE
  262.           ddCTE = dCTE - LastDeltaCTE
  263.           iCTE += CTE
  264.  
  265.           steer = - params[0] * CTE  \
  266.               - params[1] * dCTE \
  267.               - iCTE * params[2]
  268.           myrobot = myrobot.move(steer, speed)
  269.  
  270.           if ErrFnIx == 0:
  271.             if i >= N:
  272.                 err += (CTE ** 2)
  273.           elif ErrFnIx == 1:
  274.             #err += CTE ** 2 + 1.0 * (dCTE**2 / (0.01 + CTE**2))
  275.             err += (CTE + 4.0 * dCTE) ** 2 + (20.0 * ddCTE ** 2)
  276.           else:
  277.             raise Error('Unhandled error function index %s.' % str(ErrFnIx))
  278.  
  279.           LastCTE = CTE
  280.           LastDeltaCTE = dCTE
  281.  
  282.           if printflag:
  283.               print myrobot, steer
  284.           Path.append((myrobot.x, myrobot.y)) #<<<<
  285.  
  286.       return err / float(N), Path
  287.  
  288.   #-----------------------------------------------------------------------------
  289.  
  290.   Err, Path = run(PIDParams, InitialCTE, ErrFnIx, ShowProgress)
  291.  
  292.   return Err, Path
  293.  
  294.  
  295. #-------------------------------------------------------------------------------
  296.  
  297.  
  298. def RenderToSVG(Data):
  299.  
  300.   '''Return data rendered to an SVG file in a string.
  301.  
  302.  Grid is a dictionary with the keys:
  303.  
  304.    Title: This title is rendered and is embedded in the SVG header.
  305.    Plots: Multiple plots with grid, clipping and shape data.
  306.      ClipMinima: x0, y0 of clipping region
  307.      ClipMaxima: x1, y1 of clipping region
  308.      Grid: See SVGGrid().
  309.      Paths: A list of Shapes to be rendered in red-to-indigo rainbow colours.
  310.      BadPaths: A list of Shapes to be rendered in a faded colour.
  311.  
  312.  '''
  313.  
  314.   #-----------------------------------------------------------------------------
  315.  
  316.   def Field(Name, Default):
  317.     return Data[Name] if Name in Data else Default
  318.  
  319.   #-----------------------------------------------------------------------------
  320.  
  321.   def DField(OtherData, Name, Default):
  322.     return OtherData[Name] if Name in OtherData else Default
  323.  
  324.   #-----------------------------------------------------------------------------
  325.  
  326.   IT = tIndentTracker('  ')
  327.   Result = ''
  328.  
  329.   Title = Field('Title', '(Untitled)')
  330.  
  331.   Result += SVGStart(IT, Title, {
  332.     'width': '28cm',
  333.     'height': '19cm',
  334.     'viewBox': '0 0 28 19',
  335.   })
  336.  
  337.   Result += IT('<defs>')
  338.   IT.StepIn()
  339.   Result += IT(
  340.     '<marker id="ArrowHead"',
  341.     '    viewBox="0 0 10 10" refX="0" refY="5"',
  342.     '    markerUnits="strokeWidth"',
  343.     '    markerWidth="16" markerHeight="12"',
  344.     '    orient="auto">'
  345.     '  <path d="M 0,0  L 10,5  L 0,10  z"/>',
  346.     '</marker>'
  347.   )
  348.   # More marker, symbol and gradient definitions can go here.
  349.   IT.StepOut()
  350.   Result += IT('</defs>')
  351.  
  352.   # Background
  353.  
  354.   Result += IT(
  355.     '<!-- Background -->',
  356.     '<rect x="0" y="0" width="28" height="19" stroke="none" fill="white"/>'
  357.   )
  358.  
  359.   # Outer group
  360.  
  361.   Result += IT('<!-- Outer group -->')
  362.   Result += SVGGroup(IT, {'stroke': 'black', 'stroke-width': '0.025'})
  363.  
  364.   for PlotIx, Plot in enumerate(Data['Plots']):
  365.  
  366.     # Clipping region
  367.  
  368.     ClipMinima = DField(Plot, 'ClipMinima', None)
  369.     ClipMaxima = DField(Plot, 'ClipMaxima', None)
  370.  
  371.     GClipAttr = {}
  372.  
  373.     if (ClipMinima is not None) and (ClipMaxima is not None):
  374.  
  375.       PlotClipID = 'plotclip' + str(PlotIx)
  376.       ClipSpan = VDiff(ClipMaxima, ClipMinima)
  377.  
  378.       Result += IT(
  379.         '<clipPath id="' + HTMLEscaped(PlotClipID) + '">',
  380.         '  <rect x="%g" y="%g" width="%g" height="%g"/>' %
  381.             (ClipMinima[0], ClipMinima[1], ClipSpan[0], ClipSpan[1]),
  382.         '</clipPath>'
  383.       )
  384.  
  385.       GClipAttr['clip-path'] = 'url(#' + HTMLEscaped(PlotClipID) + ')'
  386.  
  387.     # Plot group
  388.  
  389.     Result += IT('<!-- Plot %d -->' % (PlotIx))
  390.     Result += SVGGroup(IT, GClipAttr)
  391.  
  392.     # Plots of both rejected and tentatively accepted paths
  393.  
  394.     Result += IT('<!-- Grid -->')
  395.     Result += SVGGrid(IT, Plot['Grid'])
  396.  
  397.     # Rejected paths
  398.  
  399.     BadPaths = DField(Plot, 'BadPaths', None)
  400.     if BadPaths is not None:
  401.  
  402.       Result += IT('<!-- Rejected paths -->')
  403.       Result += SVGGroup(IT, {
  404.         'opacity': '0.10', 'stroke': '#ff0099'
  405.       })
  406.  
  407.       NumBadPaths = len(BadPaths)
  408.  
  409.       for PathIx, Path in enumerate(BadPaths):
  410.         Result += SVGPath(IT, Path)
  411.  
  412.       Result += SVGGroupEnd(IT)
  413.  
  414.     # Axes
  415.  
  416.     Result += IT('<!-- Axes -->')
  417.     RangeMin = Plot['Grid']['RangeMinima']
  418.     RangeMax = Plot['Grid']['RangeMaxima']
  419.     Result += SVGGroup(IT, {
  420.       'stroke': 'black',
  421.       'stroke-width': '0.05',
  422.       'stroke-linecap': 'square',
  423.       'marker-end': 'url(#ArrowHead)'
  424.     })
  425.     Result += SVGPath(IT,
  426.       [(Pt_Anchor, (RangeMin[0], 0.0)), (Pt_Anchor, (RangeMax[0] + 0.1, 0.0))]
  427.     )
  428.     Result += SVGPath(IT,
  429.       [(Pt_Anchor, (0.0, RangeMin[1])), (Pt_Anchor, (0.0, RangeMax[1] + 0.1))]
  430.     )
  431.     Result += SVGGroupEnd(IT)
  432.  
  433.     # Paths in rainbow colours
  434.  
  435.     Paths = DField(Plot, 'Paths', None)
  436.     if Paths is not None:
  437.  
  438.       NumPaths = len(Paths)
  439.  
  440.       Result += IT('<!-- Paths in rainbow colours -->')
  441.       for PathIx, Path in enumerate(Paths):
  442.         if NumPaths >= 2:
  443.           Progress = float(PathIx) / float(NumPaths - 1)
  444.         else:
  445.           Progress = 1.0
  446.         Opacity = 1.0 if Progress in [0.0, 1.0] else 0.60 + 0.00 * Progress
  447.         ColourStr = ProgressColourStr(Progress, Opacity)
  448.         Result += IT('<!-- Path %d, (%.1f%%) -->' % (PathIx, 100.0 * Progress))
  449.         Result += SVGPath(IT, Path, {"stroke": ColourStr})
  450.  
  451.     # Plot title and Axis labels
  452.  
  453.     # Placing text on a transformed coordinate space can be vary
  454.     # tricky, especially when the Y axis may be flipped.
  455.  
  456.     Result += IT('<!-- Plot title and axis labels -->')
  457.  
  458.     Result += SVGGroup(IT, {
  459.       'font-family': 'serif',
  460.       'font-size': '1.2',
  461.       'font-weight': 'normal',
  462.       'font-style': 'italic',
  463.       'fill': 'black',
  464.       'stroke': 'none'
  465.     })
  466.  
  467.     YSign = -1 if DField(Plot['Grid'], 'YIsUp', False) else 1
  468.     TXAttr = {'transform': 'scale(1, -1)'} if YSign < 0 else {}
  469.  
  470.     PlotTitle = DField(Plot, 'Title', '')
  471.  
  472.     if len(PlotTitle) > 0:
  473.       x = 0.5 * (RangeMin[0] + RangeMax[0])
  474.       y = -1.0 + (RangeMin[1] if YSign > 0 else -RangeMax[1])
  475.       Result += SVGText(IT,
  476.         (x, y),
  477.         PlotTitle,
  478.         MergedDictionary(
  479.           TXAttr,
  480.           {
  481.             'text-anchor': 'middle',
  482.             'font-family': 'sans-serif',
  483.             'font-size': '1.5',
  484.             'font-weight': 'bold',
  485.             'font-style': 'normal',
  486.           }
  487.         )
  488.       )
  489.  
  490.     Result += SVGText(IT,
  491.       (RangeMax[0] + 0.5, -0.3),
  492.       'x',
  493.       MergedDictionary(TXAttr, {'font-size': '1.5'})
  494.     )
  495.  
  496.     x = RangeMax[1] + 0.75
  497.     y = 0.1
  498.  
  499.     if YSign < 0:
  500.       GXAttr = {'transform': 'rotate(90)'}
  501.       TAAttr = {}
  502.     else:
  503.       GXAttr = {'transform': 'rotate(-90)'}
  504.       TAAttr = {'text-anchor': 'end'}
  505.       x, y = -x, -y
  506.  
  507.     Result += SVGGroup(IT, GXAttr)
  508.     Result += SVGText(IT, (x, y), 'CTE', MergedDictionary(TXAttr, TAAttr))
  509.     Result += SVGGroupEnd(IT)
  510.  
  511.     Result += SVGGroupEnd(IT)
  512.  
  513.     # End of plot
  514.  
  515.     Result += SVGGroupEnd(IT)
  516.  
  517.     # End of plot group
  518.  
  519.     Result += SVGGroupEnd(IT)
  520.  
  521.   # Title and legend
  522.  
  523.   Result += IT('<!-- Title background -->')
  524.   Result += IT(
  525.     '<rect x="0" y="0" width="28" height="1.1" stroke="none" fill="white"/>'
  526.   )
  527.  
  528.   Result += IT('<!-- Title group -->')
  529.   Result += SVGGroup(IT, {
  530.     'font-family': 'sans-serif',
  531.     'font-size': '0.36',
  532.     'font-weight': 'normal',
  533.     'fill': 'black',
  534.     'stroke': 'none'
  535.   })
  536.  
  537.   Result += IT('<!-- Title -->')
  538.   Result += SVGText(IT, (0.5, 0.82), Title, {
  539.     'font-size': '0.72',
  540.     'font-weight': 'bold'
  541.   })
  542.  
  543.   Result += IT('<!-- Legend line labels-->')
  544.   Result += SVGText(IT, (23.5, 0.82), 'Initial')
  545.   Result += SVGText(IT, (26.0, 0.82), 'Final')
  546.  
  547.   Result += IT('<!-- Legend lines -->')
  548.   Result += SVGGroup(IT, {
  549.     'fill': 'none',
  550.     'stroke-width': '0.1',
  551.     'stroke-linecap': 'round'
  552.   })
  553.  
  554.   Result += SVGPath(IT,
  555.     [(Pt_Anchor, (22.5, 0.7)), (Pt_Anchor, (23.3, 0.7))],
  556.     {'stroke': ProgressColourStr(0.0)}
  557.   )
  558.  
  559.   Result += SVGPath(IT,
  560.     [(Pt_Anchor, (25.0, 0.7)), (Pt_Anchor, (25.8, 0.7))],
  561.     {'stroke': ProgressColourStr(1.0)}
  562.   )
  563.  
  564.   Result += SVGGroupEnd(IT)
  565.  
  566.   # End of title group
  567.  
  568.   Result += SVGGroupEnd(IT)
  569.  
  570.   # End of outer group
  571.  
  572.   Result += SVGGroupEnd(IT)
  573.  
  574.   Result += SVGEnd(IT)
  575.  
  576.   return Result
  577.  
  578.  
  579. #-------------------------------------------------------------------------------
  580.  
  581.  
  582. def TwiddleAndPlot(InitialCTE, Tolerance, YScale, ErrFnIx, PlotBadPaths, Data):
  583.  
  584.   '''Find the best PID values for the robot car, graphing the results to Data.
  585.  
  586.  The optimal PID values (Proportional, Differential and Integral) are
  587.  returned as a tuple.
  588.  
  589.  InitialCTE is the initial cross-track error of the car in metres.
  590.  Tolerance is the sum of the adjustments required for the greedy goat
  591.    to consider the PID parameters optimal.
  592.  YScale is the vertical magnification used in the output plot.
  593.  ErrFnIx, the error function index selects the fitness function to use.
  594.  PlotBadPaths, if true, adds rejected paths to Data['BadPaths'].
  595.  Data is a dictionary to which the output is written.
  596.  
  597.  '''
  598.  
  599.   #-----------------------------------------------------------------------------
  600.  
  601.   # Adaptor function
  602.  
  603.   def Evaluate(P, OtherParams):
  604.     return RunUnitCode(*((P,) + OtherParams))
  605.  
  606.   #-----------------------------------------------------------------------------
  607.  
  608.   # Conditional y-scaling
  609.  
  610.   def PathShape(Path):
  611.     Result = ShapeFromVertices(Path, 1)
  612.     if YScale != 1.0:
  613.       Result = TransformedShape(AM, Result)
  614.     return Result
  615.  
  616.   #-----------------------------------------------------------------------------
  617.  
  618.   AM = AffineMtxTS((0.0, 0.0), (1.0, YScale))
  619.  
  620.   # Additional simulation paramters
  621.   OtherParams = (InitialCTE, ErrFnIx)
  622.  
  623.   P = [0.0, 0.0, 0.0]
  624.   DeltaP = [1.0, 1.0, 1.0]
  625.  
  626.   Paths = []
  627.   BadPaths = []
  628.  
  629.   BestErr, Path = Evaluate(P, OtherParams)
  630.   Paths.append(PathShape(Path))
  631.  
  632.   while sum(DeltaP) > Tolerance:
  633.  
  634.     for i in range(len(P)):
  635.  
  636.       # Try positive delta
  637.  
  638.       P[i] += DeltaP[i]
  639.       Err, Path = Evaluate(P, OtherParams)
  640.  
  641.       if Err < BestErr:
  642.  
  643.         # Positive was good.
  644.  
  645.         BestErr = Err
  646.         DeltaP[i] *= 1.1
  647.         Paths.append(PathShape(Path))
  648.  
  649.       else:
  650.  
  651.         # Positive delta was bad.
  652.  
  653.         if PlotBadPaths:
  654.           BadPaths.append(PathShape(Path))
  655.  
  656.         # Try negative delta instead
  657.  
  658.         P[i] -= 2.0 * DeltaP[i]
  659.         Err, Path = Evaluate(P, OtherParams)
  660.  
  661.         if Err < BestErr:
  662.  
  663.           # Negative was good.
  664.  
  665.           BestErr = Err
  666.           DeltaP[i] *= 1.1
  667.           Paths.append(PathShape(Path))
  668.  
  669.         else:
  670.  
  671.           # Neither positive nor negative was good.
  672.  
  673.           if PlotBadPaths:
  674.             BadPaths.append(PathShape(Path))
  675.  
  676.           # Try a smaller delta next time.
  677.  
  678.           P[i] += DeltaP[i]
  679.           DeltaP[i] *= 0.9
  680.  
  681.     print "P = %s, |∆P| = %s" % (GFListStr(P), GFListStr(DeltaP))
  682.     #print BestErr
  683.  
  684.   Data['Paths'] = Paths
  685.  
  686.   if PlotBadPaths:
  687.     Data['BadPaths'] = BadPaths
  688.  
  689.   return P
  690.  
  691.  
  692. #-------------------------------------------------------------------------------
  693. # Main
  694. #-------------------------------------------------------------------------------
  695.  
  696.  
  697. def Main():
  698.  
  699.   ERRFNIX_STANDARD = 0
  700.   ERRFNIX_ENHANCED = 1
  701.  
  702.   #-----------------------------------------------------------------------------
  703.  
  704.   Variations = [
  705.     (
  706.       'Standard',
  707.       ERRFNIX_STANDARD,
  708.       {
  709.         'ClipMinima': (0, 1.05),
  710.         'ClipMaxima': (28, 9.95),
  711.       }, {
  712.         'CanvasMinima': (0.5, 1.5),
  713.         'CanvasMaxima': (27.5, 9.5),
  714.       }
  715.     ),
  716.     (
  717.       'Enhanced',
  718.       ERRFNIX_ENHANCED,
  719.       {
  720.         'ClipMinima': (0, 10.05),
  721.         'ClipMaxima': (28, 19.0),
  722.       }, {
  723.         'CanvasMinima': (0.5, 10.5),
  724.         'CanvasMaxima': (27.5, 18.5),
  725.       }
  726.     ),
  727.   ]
  728.  
  729.   InitialCTE = 1.0
  730.   Tolerance = 0.001
  731.   YScale = 10.0
  732.   PlotBadPaths = False
  733.  
  734.   ErrFnIx = ERRFNIX_STANDARD
  735.   OutputFileName = 'output.svg'
  736.  
  737.   Grid = {
  738.     'CanvasMinima': (0.5, 1.5),
  739.     'CanvasMaxima': (27.5, 18.5),
  740.     'RangeMinima': (0, -10),
  741.     'RangeMaxima': (100, 10),
  742.     'YIsUp': True,
  743.     'Transpose': False,
  744.     'SquareAlignment': 'Corner',
  745.     'DrawGrid': True,
  746.     'DrawUnitAxes': False,
  747.     'GridLineAttributes': {
  748.       'stroke-width': '0.075', 'stroke': 'rgba(0, 192, 255, 0.5)'
  749.     },
  750.     'GeneralAttributes': {
  751.       'stroke-width': '0.15', 'stroke': 'red'
  752.     }
  753.   }
  754.  
  755.   Data = {}
  756.  
  757.   Title = 'Unit5-16: Enhanced PID Optimisation'
  758.  
  759.   if YScale != 1.0:
  760.     Title += u' (y × %g)' % (YScale)
  761.  
  762.   Data['Title'] = Title
  763.  
  764.   Plots = []
  765.  
  766.   for VIx, Variation in enumerate(Variations):
  767.  
  768.     VName, ErrFnIx, VDataPatch, VGridPatch = Variation
  769.  
  770.     print 'Variation %d: %s' % (VIx, VName)
  771.     print 'Initial cross-track error = %g' % (InitialCTE)
  772.     print 'Paramter tolerance = %g' % (Tolerance)
  773.  
  774.     VData = {}
  775.     VData = MergedDictionary(VData, VDataPatch)
  776.  
  777.     VGrid = MergedDictionary(Grid, VGridPatch)
  778.     VData['Grid'] = VGrid
  779.  
  780.     Tau = TwiddleAndPlot(
  781.       InitialCTE, Tolerance, YScale, ErrFnIx, PlotBadPaths, VData
  782.     )
  783.  
  784.     print 'Best PID paramters:\n' + \
  785.       '  Pprop = %g, Pdiff = %g Pint = %g' % (Tau[0], Tau[1], Tau[2])
  786.  
  787.     VData['Title'] = VName + u': τ → ' + GFListStr(Tau)
  788.  
  789.     Plots.append(VData)
  790.  
  791.   Data['Plots'] = Plots
  792.  
  793.   print 'Rendering SVG...'
  794.   SVG = RenderToSVG(Data)
  795.   print 'Done.'
  796.  
  797.   print 'Saving SVG to "' + OutputFileName + '"...'
  798.   Save(SVG.encode('utf_8'), OutputFileName)
  799.   print 'Done.'
  800.  
  801.  
  802. #-------------------------------------------------------------------------------
  803. # Command line trigger
  804. #-------------------------------------------------------------------------------
  805.  
  806.  
  807. if __name__ == '__main__':
  808.   Main()
  809.  
  810.  
  811. #-------------------------------------------------------------------------------
  812. # End
  813. #-------------------------------------------------------------------------------
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement