cs373 ยป

# CS373 Unit 5 - Racetrack

## Racetrack by Daniel Neville

This script runs the hw5-4: Racetrack robot car simulation and outputs a Scalable Vector Graphic file which can be displayed in most browsers. The script is dependent on the public domain vegesvgplot (SVG plotting for Vegetables) Python module.

Example output:

### Code

``````#!/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)

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

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

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

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

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

Result = -Result
return Result

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

RLine = VDiff(Line[1], Line[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))

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

NorthStraight = (
)
SouthStraight = (
)

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

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

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

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(
'    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'

Grid = {
'CanvasMinima': (3.25, 4.25),
'CanvasMaxima': (24.75, 15.75),
'RangeMinima': (0, 0),
'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'
}
}

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