Visualising a Cerebrally Enhanced Greedy Goat

#!/usr/bin/python
# -*- coding: utf-8 -*-
#-------------------------------------------------------------------------------
# Visualising a Cerebrally Enhanced Greedy Goat
#
# unit0516_param_optim_enhanced_svg.py
# http://pastebin.com/e15wbtc0
#
# Custom modules:
#   vegesvgplot.py        http://pastebin.com/6Aek3Exm
#
#-------------------------------------------------------------------------------


'''Visualisation of two optimisation methods in regard to Unit 5-16

Description:
  This Python program runs two variants of the PID parameter optimiser
  for the robot car presented in unit 5-16 (the car with badly misaligned
  steering). The output is written to a Scalable Vector
  Graphic file named “output.svg”.

Author(s):
  Daniel Neville
  Prof. Sebastian Thrun, udacity (original robot simulation)

Copyright, Licence:
  Code by Daniel Neville: Public domain
  Code snarfed from udacity: See http://www.udacity.com/legal/

Platform:
  Python 2.5


INDEX


Imports

Fun stuff:

  Snarfed and modified robot code
  RenderToSVG(Data)
  TwiddleAndPlot(InitialCTE, Tolerance, YScale, ErrFnIx, Data)

Main:

  Main()

'''


#-------------------------------------------------------------------------------


import math

from math import (
  pi, sqrt, hypot, sin, cos, tan, asin, acos, atan, atan2, radians, degrees,
  floor, ceil
)

import random

# The SVG Plotting for Vegetables module can be found at
# http://pastebin.com/6Aek3Exm

from vegesvgplot import (

  # Shape constants
  Pt_Break, Pt_Anchor, Pt_Control,
  PtCmdWithCoordsSet, PtCmdSet,

  # Indent tracker class
  tIndentTracker,

  # Affine matrix class
  tAffineMtx,

  # Affine matrix creation functions
  AffineMtxTS, AffineMtxTRS2D, Affine2DMatrices,

  # Utility functions
  ValidatedRange, MergedDictionary, Save,
  ArrayDimensions, NewMDArray, CopyArray, At, SetAt,

  # Basic vector functions
  VZeros, VOnes, VStdBasis, VDim, VAug, VMajorAxis,
  VNeg, VSum, VDiff, VSchur, VDot,
  VLengthSquared, VLength, VManhattan,
  VScaled, VNormalised,
  VPerp, VCrossProduct, VCrossProduct4D,
  VScalarTripleProduct, VVectorTripleProduct,
  VProjectionOnto,
  VTransposedMAV,
  VRectToPol, VPolToRect,
  VLerp,

  # Shape functions
  ShapeFromVertices, ShapePoints, ShapeSubpathRanges, ShapeCurveRanges,
  ShapeLength, LineToShapeIntersections, TransformedShape, PiecewiseArc,

  # Output formatting functions
  MaxDP, GFListStr, GFTupleStr, HTMLEscaped, AttrMarkup, ProgressColourStr,

  # SVG functions
  SVGStart, SVGEnd, SVGPathDataSegments, SVGPath, SVGText,
  SVGGroup, SVGGroupEnd, SVGGrid

)


#-------------------------------------------------------------------------------
# Fun stuff
#-------------------------------------------------------------------------------


def RunUnitCode(PIDParams, InitialCTE=1.0, ErrFnIx=0, ShowProgress=False):

  #-----------------------------------------------------------------------------
  # Code snarfed from udacity and modified
  #-----------------------------------------------------------------------------

  # ------------------------------------------------
  #
  # this is the robot class
  #

  class robot:

      # --------
      # init:
      #    creates robot and initializes location/orientation to 0, 0, 0
      #

      def __init__(self, length = 20.0):
          self.x = 0.0
          self.y = 0.0
          self.orientation = 0.0
          self.length = length
          self.steering_noise = 0.0
          self.distance_noise = 0.0
          self.steering_drift = 0.0

      # --------
      # set:
      # sets a robot coordinate
      #

      def set(self, new_x, new_y, new_orientation):

          self.x = float(new_x)
          self.y = float(new_y)
          self.orientation = float(new_orientation) % (2.0 * pi)


      # --------
      # set_noise:
      # sets the noise parameters
      #

      def set_noise(self, new_s_noise, new_d_noise):
          # makes it possible to change the noise parameters
          # this is often useful in particle filters
          self.steering_noise = float(new_s_noise)
          self.distance_noise = float(new_d_noise)

      # --------
      # set_steering_drift:
      # sets the systematical steering drift parameter
      #

      def set_steering_drift(self, drift):
          self.steering_drift = drift

      # --------
      # move:
      #    steering = front wheel steering angle, limited by max_steering_angle
      #    distance = total distance driven, most be non-negative

      def move(self, steering, distance,
               tolerance = 0.001, max_steering_angle = pi / 4.0):

          if steering > max_steering_angle:
              steering = max_steering_angle
          if steering < -max_steering_angle:
              steering = -max_steering_angle
          if distance < 0.0:
              distance = 0.0


          # make a new copy
          res = robot()
          res.length         = self.length
          res.steering_noise = self.steering_noise
          res.distance_noise = self.distance_noise
          res.steering_drift = self.steering_drift

          # apply noise
          steering2 = random.gauss(steering, self.steering_noise)
          distance2 = random.gauss(distance, self.distance_noise)

          # apply steering drift
          steering2 += self.steering_drift

          # Execute motion
          turn = tan(steering2) * distance2 / res.length

          if abs(turn) < tolerance:

              # approximate by straight line motion

              res.x = self.x + (distance2 * cos(self.orientation))
              res.y = self.y + (distance2 * sin(self.orientation))
              res.orientation = (self.orientation + turn) % (2.0 * pi)

          else:

              # approximate bicycle model for motion

              radius = distance2 / turn
              cx = self.x - (sin(self.orientation) * radius)
              cy = self.y + (cos(self.orientation) * radius)
              res.orientation = (self.orientation + turn) % (2.0 * pi)
              res.x = cx + (sin(res.orientation) * radius)
              res.y = cy - (cos(res.orientation) * radius)

          return res




      def __repr__(self):
          return '[x=%.5f y=%.5f orient=%.5f]'  % (self.x, self.y, self.orientation)


  # ------------------------------------------------------------------------
  #
  # run - does a single control run.


  def run(params, InitialCTE, ErrFnIx, printflag = False):

      myrobot = robot()
      myrobot.set(0.0, InitialCTE, 0.0)
      speed = 1.0
      err = 0.0
      N = 100
      # myrobot.set_noise(0.1, 0.0)
      myrobot.set_steering_drift(10.0 / 180.0 * pi) # 10 degree steering error

      Path = [(myrobot.x, myrobot.y)]

      LastCTE = myrobot.y
      LastDeltaCTE = 0.0
      iCTE = 0.0

      for i in range(N * 2):

          CTE = myrobot.y
          dCTE = CTE - LastCTE
          ddCTE = dCTE - LastDeltaCTE
          iCTE += CTE

          steer = - params[0] * CTE  \
              - params[1] * dCTE \
              - iCTE * params[2]
          myrobot = myrobot.move(steer, speed)

          if ErrFnIx == 0:
            if i >= N:
                err += (CTE ** 2)
          elif ErrFnIx == 1:
            #err += CTE ** 2 + 1.0 * (dCTE**2 / (0.01 + CTE**2))
            err += (CTE + 4.0 * dCTE) ** 2 + (20.0 * ddCTE ** 2)
          else:
            raise Error('Unhandled error function index %s.' % str(ErrFnIx))

          LastCTE = CTE
          LastDeltaCTE = dCTE

          if printflag:
              print myrobot, steer
          Path.append((myrobot.x, myrobot.y)) #<<<<

      return err / float(N), Path

  #-----------------------------------------------------------------------------

  Err, Path = run(PIDParams, InitialCTE, ErrFnIx, ShowProgress)

  return Err, Path


#-------------------------------------------------------------------------------


def RenderToSVG(Data):

  '''Return data rendered to an SVG file in a string.

  Grid is a dictionary with the keys:

    Title: This title is rendered and is embedded in the SVG header.
    Plots: Multiple plots with grid, clipping and shape data.
      ClipMinima: x0, y0 of clipping region
      ClipMaxima: x1, y1 of clipping region
      Grid: See SVGGrid().
      Paths: A list of Shapes to be rendered in red-to-indigo rainbow colours.
      BadPaths: A list of Shapes to be rendered in a faded colour.

  '''

  #-----------------------------------------------------------------------------

  def Field(Name, Default):
    return Data[Name] if Name in Data else Default

  #-----------------------------------------------------------------------------

  def DField(OtherData, Name, Default):
    return OtherData[Name] if Name in OtherData else Default

  #-----------------------------------------------------------------------------

  IT = tIndentTracker('  ')
  Result = ''

  Title = Field('Title', '(Untitled)')

  Result += SVGStart(IT, Title, {
    'width': '28cm',
    'height': '19cm',
    'viewBox': '0 0 28 19',
  })

  Result += IT('<defs>')
  IT.StepIn()
  Result += IT(
    '<marker id="ArrowHead"',
    '    viewBox="0 0 10 10" refX="0" refY="5"',
    '    markerUnits="strokeWidth"',
    '    markerWidth="16" markerHeight="12"',
    '    orient="auto">'
    '  <path d="M 0,0  L 10,5  L 0,10  z"/>',
    '</marker>'
  )
  # More marker, symbol and gradient definitions can go here.
  IT.StepOut()
  Result += IT('</defs>')

  # Background

  Result += IT(
    '<!-- Background -->',
    '<rect x="0" y="0" width="28" height="19" stroke="none" fill="white"/>'
  )

  # Outer group

  Result += IT('<!-- Outer group -->')
  Result += SVGGroup(IT, {'stroke': 'black', 'stroke-width': '0.025'})

  for PlotIx, Plot in enumerate(Data['Plots']):

    # Clipping region

    ClipMinima = DField(Plot, 'ClipMinima', None)
    ClipMaxima = DField(Plot, 'ClipMaxima', None)

    GClipAttr = {}

    if (ClipMinima is not None) and (ClipMaxima is not None):

      PlotClipID = 'plotclip' + str(PlotIx)
      ClipSpan = VDiff(ClipMaxima, ClipMinima)

      Result += IT(
        '<clipPath id="' + HTMLEscaped(PlotClipID) + '">',
        '  <rect x="%g" y="%g" width="%g" height="%g"/>' %
            (ClipMinima[0], ClipMinima[1], ClipSpan[0], ClipSpan[1]),
        '</clipPath>'
      )

      GClipAttr['clip-path'] = 'url(#' + HTMLEscaped(PlotClipID) + ')'

    # Plot group

    Result += IT('<!-- Plot %d -->' % (PlotIx))
    Result += SVGGroup(IT, GClipAttr)

    # Plots of both rejected and tentatively accepted paths

    Result += IT('<!-- Grid -->')
    Result += SVGGrid(IT, Plot['Grid'])

    # Rejected paths

    BadPaths = DField(Plot, 'BadPaths', None)
    if BadPaths is not None:

      Result += IT('<!-- Rejected paths -->')
      Result += SVGGroup(IT, {
        'opacity': '0.10', 'stroke': '#ff0099'
      })

      NumBadPaths = len(BadPaths)

      for PathIx, Path in enumerate(BadPaths):
        Result += SVGPath(IT, Path)

      Result += SVGGroupEnd(IT)

    # Axes

    Result += IT('<!-- Axes -->')
    RangeMin = Plot['Grid']['RangeMinima']
    RangeMax = Plot['Grid']['RangeMaxima']
    Result += SVGGroup(IT, {
      'stroke': 'black',
      'stroke-width': '0.05',
      'stroke-linecap': 'square',
      'marker-end': 'url(#ArrowHead)'
    })
    Result += SVGPath(IT,
      [(Pt_Anchor, (RangeMin[0], 0.0)), (Pt_Anchor, (RangeMax[0] + 0.1, 0.0))]
    )
    Result += SVGPath(IT,
      [(Pt_Anchor, (0.0, RangeMin[1])), (Pt_Anchor, (0.0, RangeMax[1] + 0.1))]
    )
    Result += SVGGroupEnd(IT)

    # Paths in rainbow colours

    Paths = DField(Plot, 'Paths', None)
    if Paths is not None:

      NumPaths = len(Paths)

      Result += IT('<!-- Paths in rainbow colours -->')
      for PathIx, Path in enumerate(Paths):
        if NumPaths >= 2:
          Progress = float(PathIx) / float(NumPaths - 1)
        else:
          Progress = 1.0
        Opacity = 1.0 if Progress in [0.0, 1.0] else 0.60 + 0.00 * Progress
        ColourStr = ProgressColourStr(Progress, Opacity)
        Result += IT('<!-- Path %d, (%.1f%%) -->' % (PathIx, 100.0 * Progress))
        Result += SVGPath(IT, Path, {"stroke": ColourStr})

    # Plot title and Axis labels

    # Placing text on a transformed coordinate space can be vary
    # tricky, especially when the Y axis may be flipped.

    Result += IT('<!-- Plot title and axis labels -->')

    Result += SVGGroup(IT, {
      'font-family': 'serif',
      'font-size': '1.2',
      'font-weight': 'normal',
      'font-style': 'italic',
      'fill': 'black',
      'stroke': 'none'
    })

    YSign = -1 if DField(Plot['Grid'], 'YIsUp', False) else 1
    TXAttr = {'transform': 'scale(1, -1)'} if YSign < 0 else {}

    PlotTitle = DField(Plot, 'Title', '')

    if len(PlotTitle) > 0:
      x = 0.5 * (RangeMin[0] + RangeMax[0])
      y = -1.0 + (RangeMin[1] if YSign > 0 else -RangeMax[1])
      Result += SVGText(IT,
        (x, y),
        PlotTitle,
        MergedDictionary(
          TXAttr,
          {
            'text-anchor': 'middle',
            'font-family': 'sans-serif',
            'font-size': '1.5',
            'font-weight': 'bold',
            'font-style': 'normal',
          }
        )
      )

    Result += SVGText(IT,
      (RangeMax[0] + 0.5, -0.3),
      'x',
      MergedDictionary(TXAttr, {'font-size': '1.5'})
    )

    x = RangeMax[1] + 0.75
    y = 0.1

    if YSign < 0:
      GXAttr = {'transform': 'rotate(90)'}
      TAAttr = {}
    else:
      GXAttr = {'transform': 'rotate(-90)'}
      TAAttr = {'text-anchor': 'end'}
      x, y = -x, -y

    Result += SVGGroup(IT, GXAttr)
    Result += SVGText(IT, (x, y), 'CTE', MergedDictionary(TXAttr, TAAttr))
    Result += SVGGroupEnd(IT)

    Result += SVGGroupEnd(IT)

    # End of plot

    Result += SVGGroupEnd(IT)

    # End of plot group

    Result += SVGGroupEnd(IT)

  # Title and legend

  Result += IT('<!-- Title background -->')
  Result += IT(
    '<rect x="0" y="0" width="28" height="1.1" stroke="none" fill="white"/>'
  )

  Result += IT('<!-- Title group -->')
  Result += SVGGroup(IT, {
    'font-family': 'sans-serif',
    'font-size': '0.36',
    'font-weight': 'normal',
    'fill': 'black',
    'stroke': 'none'
  })

  Result += IT('<!-- Title -->')
  Result += SVGText(IT, (0.5, 0.82), Title, {
    'font-size': '0.72',
    'font-weight': 'bold'
  })

  Result += IT('<!-- Legend line labels-->')
  Result += SVGText(IT, (23.5, 0.82), 'Initial')
  Result += SVGText(IT, (26.0, 0.82), 'Final')

  Result += IT('<!-- Legend lines -->')
  Result += SVGGroup(IT, {
    'fill': 'none',
    'stroke-width': '0.1',
    'stroke-linecap': 'round'
  })

  Result += SVGPath(IT,
    [(Pt_Anchor, (22.5, 0.7)), (Pt_Anchor, (23.3, 0.7))],
    {'stroke': ProgressColourStr(0.0)}
  )

  Result += SVGPath(IT,
    [(Pt_Anchor, (25.0, 0.7)), (Pt_Anchor, (25.8, 0.7))],
    {'stroke': ProgressColourStr(1.0)}
  )

  Result += SVGGroupEnd(IT)

  # End of title group

  Result += SVGGroupEnd(IT)

  # End of outer group

  Result += SVGGroupEnd(IT)

  Result += SVGEnd(IT)

  return Result


#-------------------------------------------------------------------------------


def TwiddleAndPlot(InitialCTE, Tolerance, YScale, ErrFnIx, PlotBadPaths, Data):

  '''Find the best PID values for the robot car, graphing the results to Data.

  The optimal PID values (Proportional, Differential and Integral) are
  returned as a tuple.

  InitialCTE is the initial cross-track error of the car in metres.
  Tolerance is the sum of the adjustments required for the greedy goat
    to consider the PID parameters optimal.
  YScale is the vertical magnification used in the output plot.
  ErrFnIx, the error function index selects the fitness function to use.
  PlotBadPaths, if true, adds rejected paths to Data['BadPaths'].
  Data is a dictionary to which the output is written.

  '''

  #-----------------------------------------------------------------------------

  # Adaptor function

  def Evaluate(P, OtherParams):
    return RunUnitCode(*((P,) + OtherParams))

  #-----------------------------------------------------------------------------

  # Conditional y-scaling

  def PathShape(Path):
    Result = ShapeFromVertices(Path, 1)
    if YScale != 1.0:
      Result = TransformedShape(AM, Result)
    return Result

  #-----------------------------------------------------------------------------

  AM = AffineMtxTS((0.0, 0.0), (1.0, YScale))

  # Additional simulation paramters
  OtherParams = (InitialCTE, ErrFnIx)

  P = [0.0, 0.0, 0.0]
  DeltaP = [1.0, 1.0, 1.0]

  Paths = []
  BadPaths = []

  BestErr, Path = Evaluate(P, OtherParams)
  Paths.append(PathShape(Path))

  while sum(DeltaP) > Tolerance:

    for i in range(len(P)):

      # Try positive delta

      P[i] += DeltaP[i]
      Err, Path = Evaluate(P, OtherParams)

      if Err < BestErr:

        # Positive was good.

        BestErr = Err
        DeltaP[i] *= 1.1
        Paths.append(PathShape(Path))

      else:

        # Positive delta was bad.

        if PlotBadPaths:
          BadPaths.append(PathShape(Path))

        # Try negative delta instead

        P[i] -= 2.0 * DeltaP[i]
        Err, Path = Evaluate(P, OtherParams)

        if Err < BestErr:

          # Negative was good.

          BestErr = Err
          DeltaP[i] *= 1.1
          Paths.append(PathShape(Path))

        else:

          # Neither positive nor negative was good.

          if PlotBadPaths:
            BadPaths.append(PathShape(Path))

          # Try a smaller delta next time.

          P[i] += DeltaP[i]
          DeltaP[i] *= 0.9

    print "P = %s, |∆P| = %s" % (GFListStr(P), GFListStr(DeltaP))
    #print BestErr

  Data['Paths'] = Paths

  if PlotBadPaths:
    Data['BadPaths'] = BadPaths

  return P


#-------------------------------------------------------------------------------
# Main
#-------------------------------------------------------------------------------


def Main():

  ERRFNIX_STANDARD = 0
  ERRFNIX_ENHANCED = 1

  #-----------------------------------------------------------------------------

  Variations = [
    (
      'Standard',
      ERRFNIX_STANDARD,
      {
        'ClipMinima': (0, 1.05),
        'ClipMaxima': (28, 9.95),
      }, {
        'CanvasMinima': (0.5, 1.5),
        'CanvasMaxima': (27.5, 9.5),
      }
    ),
    (
      'Enhanced',
      ERRFNIX_ENHANCED,
      {
        'ClipMinima': (0, 10.05),
        'ClipMaxima': (28, 19.0),
      }, {
        'CanvasMinima': (0.5, 10.5),
        'CanvasMaxima': (27.5, 18.5),
      }
    ),
  ]

  InitialCTE = 1.0
  Tolerance = 0.001
  YScale = 10.0
  PlotBadPaths = False

  ErrFnIx = ERRFNIX_STANDARD
  OutputFileName = 'output.svg'

  Grid = {
    'CanvasMinima': (0.5, 1.5),
    'CanvasMaxima': (27.5, 18.5),
    'RangeMinima': (0, -10),
    'RangeMaxima': (100, 10),
    'YIsUp': True,
    'Transpose': False,
    'SquareAlignment': 'Corner',
    'DrawGrid': True,
    'DrawUnitAxes': False,
    'GridLineAttributes': {
      'stroke-width': '0.075', 'stroke': 'rgba(0, 192, 255, 0.5)'
    },
    'GeneralAttributes': {
      'stroke-width': '0.15', 'stroke': 'red'
    }
  }

  Data = {}

  Title = 'Unit5-16: Enhanced PID Optimisation'

  if YScale != 1.0:
    Title += u' (y × %g)' % (YScale)

  Data['Title'] = Title

  Plots = []

  for VIx, Variation in enumerate(Variations):

    VName, ErrFnIx, VDataPatch, VGridPatch = Variation

    print 'Variation %d: %s' % (VIx, VName)
    print 'Initial cross-track error = %g' % (InitialCTE)
    print 'Paramter tolerance = %g' % (Tolerance)

    VData = {}
    VData = MergedDictionary(VData, VDataPatch)

    VGrid = MergedDictionary(Grid, VGridPatch)
    VData['Grid'] = VGrid

    Tau = TwiddleAndPlot(
      InitialCTE, Tolerance, YScale, ErrFnIx, PlotBadPaths, VData
    )

    print 'Best PID paramters:\n' + \
      '  Pprop = %g, Pdiff = %g Pint = %g' % (Tau[0], Tau[1], Tau[2])

    VData['Title'] = VName + u': τ → ' + GFListStr(Tau)

    Plots.append(VData)

  Data['Plots'] = Plots

  print 'Rendering SVG...'
  SVG = RenderToSVG(Data)
  print 'Done.'

  print 'Saving SVG to "' + OutputFileName + '"...'
  Save(SVG.encode('utf_8'), OutputFileName)
  print 'Done.'


#-------------------------------------------------------------------------------
# Command line trigger
#-------------------------------------------------------------------------------


if __name__ == '__main__':
  Main()


#-------------------------------------------------------------------------------
# End
#-------------------------------------------------------------------------------