Visualising the Racetrack for udacity CS373 hw5-4

#!/usr/bin/python
# -*- coding: utf-8 -*-
#-------------------------------------------------------------------------------
# Racetrack following with PID control for hw5-4
#
# hw0504_racetrack_svg.py
# http://pastebin.com/4nTNtLmm
#
# Custom modules:
#   vegesvgplot.py        http://pastebin.com/6Aek3Exm
#
#-------------------------------------------------------------------------------


'''Racetrack following with PID control for hw5-4

Description:
  This Python script demonstrates the udacity CS373 robot car following
  a racetrack. 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:

  RunUnitCode(PIDParams, Radius, [ShowProgress], [PathLog])
    (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, Radius, ShowProgress=False, PathLog=None):

  if PathLog is None:
    PathLog = []

  #-----------------------------------------------------------------------------
  # 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)


  ############## ONLY ADD / MODIFY CODE BELOW THIS LINE ####################

      def cte(self, radius):

          '''Return the cross-track error from the racetrack path.

          The racetrack is parametised by the given radius of its semicircular
          sections at each end. The length of the straight sections connecting
          the semicircles is twice the radius.

          The car may be travelling around the track in either direction.

          '''

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

          def VSum(A, B):
            return tuple(x + y for x, y in zip(A, B))

          def VDiff(A, B):
            return tuple(x - y for x, y in zip(A, B))

          def VDot(A, B):
            return sum(x * y for x, y in zip(A, B))

          def VLengthSquared(A):
            return sum(x * x for x in A)

          def VLength(A):
            return sqrt(VLengthSquared(A))

          def VScaled(A, Scale):
            return tuple(x * Scale for x in A)

          def VNeg(A):
            return tuple(-x for x in A)

          def VNormalised(A):
            return VScaled(A, 1.0 / VLength(A))

          def VPerp(A):
            return (-A[1], A[0])

          def VProjectionOnto(Axis, Vector):
            Result = VScaled(
              Axis,
              VDot(Vector, Axis) / (1.0 * LengthSquared(Axis))
            )
            return Result

          def VLerp(A, B, Progress):
            return VSum(VScaled(A, 1.0 - Progress), VScaled(B, Progress))

          def VRectToPol(A):
            Distance = VLength(A)
            Angle = atan2(A[1], A[0])
            return (Distance, Angle)

          def VPolToRect(B):
            c = cos(B[1])
            s = sin(B[1])
            return (B[0] * c, B[0] * s)

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

          def CTEFromCircle(Pos, Heading, Centre, Radius):
            Sinister = VPerp(Heading)
            Radial = VDiff(P, Centre)
            Result = VLength(Radial) - Radius
            if VDot(Sinister, Radial) < 0.0:
              Result = -Result
            return Result

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

          def CTEFromLine(Pos, Heading, Line):
            RLine = VDiff(Line[1], Line[0])
            if VDot(Heading, RLine) >= 0:
              RPos = VDiff(Pos, Line[0])
            else:
              RLine = VNeg(RLine)
              RPos = VDiff(Pos, Line[1])
            LineSinister = VNormalised(VPerp(RLine))
            Result = VDot(RPos, LineSinister)
            return Result

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

          P = (self.x, self.y)
          H = VPolToRect((1.0, self.orientation))

          StraightLength = 2.0 * radius

          CWest = (radius, radius)
          CEast = (CWest[0] + StraightLength, CWest[1])
          Middle = VLerp(CWest, CEast, 0.5)

          NorthStraight = (
            (CWest[0], CWest[1] + radius),
            (CEast[0], CEast[1] + radius)
          )
          SouthStraight = (
            (CWest[0], CWest[1] - radius),
            (CEast[0], CEast[1] - radius)
          )

          if P[0] < CWest[0]:
            cte = CTEFromCircle(P, H, CWest, radius)
          elif P[0] > CEast[0]:
            cte = CTEFromCircle(P, H, CEast, radius)
          elif P[1] > Middle[1]:
            cte = CTEFromLine(P, H, NorthStraight)
          else:
            cte = CTEFromLine(P, H, SouthStraight)

          return cte

  ############## ONLY ADD / MODIFY CODE ABOVE THIS LINE ####################




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


  def run(params, radius, printflag = False):
      myrobot = robot()
      myrobot.set(0.0, radius, pi / 2.0)
      speed = 1.0 # motion distance is equal to speed (we assume time = 1)
      err = 0.0
      int_crosstrack_error = 0.0
      N = 200
      PathLog.append((myrobot.x, myrobot.y)) #<<< SVG-ONLY

      crosstrack_error = myrobot.cte(radius) # You need to define the cte function!

      for i in range(N*2):
          diff_crosstrack_error = - crosstrack_error
          crosstrack_error = myrobot.cte(radius)
          diff_crosstrack_error += crosstrack_error
          int_crosstrack_error += crosstrack_error
          steer = - params[0] * crosstrack_error \
                  - params[1] * diff_crosstrack_error \
                  - params[2] * int_crosstrack_error
          myrobot = myrobot.move(steer, speed)
          if i >= N:
              err += crosstrack_error ** 2
          if printflag:
              print myrobot
          PathLog.append((myrobot.x, myrobot.y)) #<<< SVG-ONLY
      return err / float(N)

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

  return run(PIDParams, Radius, ShowProgress)


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


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.
    Grid: See SVGGrid().
    Paths: A list of Shapes to be rendered in red-to-indigo rainbow colours.

  '''

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


def RenderToSVG(Data):

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

  Data is a dictionary with the keys:

    Title: This title is rendered and is embedded in the SVG header.
    Grid: See SVGGrid().
    Paths: A list of Shapes to be rendered in red-to-indigo rainbow colours.

  '''

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

  # Shorthand

  A = Pt_Anchor
  C = Pt_Control
  B = (Pt_Break, None)

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

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

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

  IT = tIndentTracker('  ')

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

  Result = SVGStart(IT, Title)

  Result += IT('<defs>')
  IT.StepIn()
  Result += IT(
    '<marker id="ArrowHead"',
    '    viewBox="0 0 10 10" refX="0" refY="5"',
    '    markerUnits="strokeWidth"',
    '    markerWidth="8" markerHeight="6"',
    '    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'
  })

  # Plot with grid

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

  # Track

  Track = Field('Track', None)
  if Track is not None:

    Result += IT('<!-- Track -->')
    Result += SVGGroup(IT, {'fill': 'none'})

    Result += IT('<!-- Black edge -->')
    Result += SVGPath(IT, Track, {
      'stroke-width': '1.35',
      'stroke': 'black'
    })
    Result += IT(u'<!-- Shoulder – White -->')
    Result += SVGPath(IT, Track, {
      'stroke-width': '1.3',
      'stroke': 'white'
    })
    Result += IT(u'<!-- Shoulder – Red -->')
    Result += SVGPath(IT, Track, {
      'stroke-width': '1.3',
      'stroke': '#d00',
      'stroke-dasharray': '0.5 0.5',
      'stroke-linecap': 'butt'
    })
    Result += IT('<!-- The track proper -->')
    Result += SVGPath(IT, Track, {
      'stroke-width': '1.0',
      'stroke': '#ccc'
    })
    Result += IT('<!-- Centreline -->')
    Result += SVGPath(IT, Track, {
      'stroke-width': '0.1',
      'stroke': 'white',
      'stroke-dasharray': '1 1',
      'stroke-linecap': 'butt'
    })

    Result += SVGGroupEnd(IT)

  # Paths in rainbow colours

  Paths = Field('Paths', None)
  if Paths is not None and len(Paths) > 0:

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

  # End of plot

  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 DoFunStuff(Data):

  '''Demonstrate a PID-controlled car following a racetrack.

  The output is written to the Data (a Python dictionary) to keep the
  rendering details separate.

  '''

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

  Data['Title'] = 'HW5-4: Racetrack'

  Radius = 25.0

  Grid = {
    'CanvasMinima': (3.25, 4.25),
    'CanvasMaxima': (24.75, 15.75),
    'RangeMinima': (0, 0),
    'RangeMaxima': (int(ceil(4 * Radius)), int(ceil(2 * Radius))),
    'YIsUp': True,
    'SquareAlignment': 'Corner',
    'DrawGrid': True,
    'GridLineAttributes': {
      'stroke-width': '0.02', 'stroke': 'rgba(0, 192, 255, 0.5)'
    },
    'GeneralAttributes': {
      'stroke-width': '0.05', 'stroke': 'red'
    }
  }

  r = Radius
  Track = []
  Track += PiecewiseArc((r, r), r, (1.5 * pi, 0.5 * pi), 8)
  Track += PiecewiseArc((3.0 * r, r), r, (0.5 * pi, -0.5 * pi), 8)
  Track.append(Track[0])

  Paths = []
  PathLog = []

  PIDParams = [10.0, 15.0, 0.0]
  Err = RunUnitCode(PIDParams, Radius, True, PathLog)
  print '\nFinal parameters:', GFListStr(PIDParams)
  print 'Error score:', Err

  Paths.append(ShapeFromVertices(PathLog, 1))

  Data['Grid'] = Grid
  Data['Paths'] = Paths
  Data['Track'] = Track


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


def Main():

  OutputFileName = 'output.svg'

  print 'Smoothing a loop with knots...'

  Data = {}
  DoFunStuff(Data)

  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
#-------------------------------------------------------------------------------