Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #!/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
- #-------------------------------------------------------------------------------
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement