Viewing Program

Program Data

Program thumbnail for "{props.program.title}"

Title	Plan, Smooth, Control
ID	4812667164426240
Created	10 years ago, 1430638008787
Updated	10 years ago, 1430906299774
Code	/** * This program finds the shortest path through a series of obstacles using A* search, * smooths the path, then uses PID control to drive a robot to the goal. */ // You can play around with these constants--- var NUM_BLOCKS = 20; var OBSTACLE_PROBABILITY = 0.2; var MOVE_NOISE = 0.1; var STEERING_NOISE = 0.1; var ROBOT_SPEED = 0.5; var MAX_STEERING_ANGLE = Math.PI/4.0; var tP = 2.0; var tI = 0.0001; var tD = 16.0; var weightData = 0.1; var weightSmooth = 0.1; // Search types var A_STAR = 0; var GREEDY = 1; var BREADTH_FIRST = 2; var DEPTH_FIRST = 3; var SEARCH_MODE = A_STAR; // -------------------------------------------- var BLOCK_SIZE = width/NUM_BLOCKS; var START = 2; var GOAL_X = NUM_BLOCKS-1; var GOAL_Y = NUM_BLOCKS-1; var GOAL = 3; var START_COLOR = color(23, 33, 176); var GOAL_COLOR = color(199, 188, 68); var FRONTIER_COLOR = color(105, 179, 105); var EXPLORED_COLOR = color(117, 117, 117, 100); var PATH_COLOR = color(166, 53, 53); var SMOOTH_PATH_COLOR = color(53, 68, 166); var PLAN = 0; var SMOOTH = 1; var CALIBRATE = 2; var NAVIGATE = 3; var DELTA = [[-1, 0], [1, 0], [0, -1], [0, 1]]; var frontier = [[0, 0, 0, 0]]; var explored = []; var predecessors = []; var path = []; var smoothPath = []; var mode = PLAN; var index = 0; var cte = 0; var sigma = 0; angleMode = "radians"; frameRate(60); // Initialize world var world = []; for (var i = 0; i < NUM_BLOCKS; i++) { var row = []; for (var j = 0; j < NUM_BLOCKS; j++) { var r = random(); if (r < OBSTACLE_PROBABILITY) { row.push(1); } else { row.push(0); } } world.push(row); } world[0][0] = START; world[GOAL_Y][GOAL_X] = GOAL; for (var i = 0; i < NUM_BLOCKS; i++) { var row = []; for (var j = 0; j < NUM_BLOCKS; j++) { row.push(false); } explored.push(row); } explored[0][0] = true; for (var i = 0; i < NUM_BLOCKS; i++) { var row = []; for (var j = 0; j < NUM_BLOCKS; j++) { row.push(null); } predecessors.push(row); } var Robot = function(x, y, size) { this.x = x; this.y = y; this.vx = 0; this.vy = 0; this.orientation = 0; this.size = size; // Thanks to Sebastian Thrun for this code: // https://www.udacity.com/course/viewer#!/c-cs373/l-48696626/e-48403941/m-48729137 // My code for this movement can be found at: // https://www.khanacademy.org/computer-programming/bicycle-model/5496951953031168 this.move = function(d, theta) { var generator = new Random(millis()); var moveDistance = (generator.nextGaussian() MOVE_NOISE) + d; var turnAngle = (generator.nextGaussian() * STEERING_NOISE) + theta; turnAngle = min(turnAngle, MAX_STEERING_ANGLE); turnAngle = max(turnAngle, -MAX_STEERING_ANGLE); var turn = tan(turnAngle)moveDistance/this.size; // Approximately straight motion if (abs(turn) < 0.001) { this.x += moveDistancecos(this.orientation); this.y += moveDistancesin(this.orientation); this.orientation = (this.orientation+turn)%(2.0Math.PI); } // Move using the bicyle model else { var radius = moveDistance/turn; var cx = this.x-(sin(this.orientation)radius); var cy = this.y+(cos(this.orientation)radius); this.orientation = (this.orientation+turn)%(2.0Math.PI); this.x = cx + (sin(this.orientation)radius); this.y = cy - (cos(this.orientation)radius); } }; this.draw = function() { pushMatrix(); translate(this.x, this.y); rotate(this.orientation); fill(128, 27, 27); stroke(255, 0, 0); rect(-this.size/2, -(this.size0.750.5), this.size, this.size0.75); popMatrix(); }; }; var robot; var addToFrontier = function(node) { // Insert the node into the frontier // Order by lowest cost var i = frontier.length-1; if (SEARCH_MODE === A_STAR) { while (i > 0 && node[2]+node[3] < frontier[i][2]+frontier[i][3]) { i--; } } else if (SEARCH_MODE === GREEDY) { while (i > 0 && node[3] < frontier[i][3]) { i--; } } else if (SEARCH_MODE === BREADTH_FIRST) { frontier.push(node); } else if (SEARCH_MODE === DEPTH_FIRST) { frontier.splice(0, 0, node); } frontier.splice(i+1, 0, node); }; var distance = function(x1, y1, x2, y2) { return sqrt(pow(x1-x2, 2) + pow(y1-y2, 2)); }; var manhattanDistance = function(x1, y1, x2, y2) { return abs(x1-x2) + abs(y1-y2); }; var drawWorld = function() { background(255, 255, 255); for (var i = 0; i < world.length; i++) { for (var j = 0; j < world[0].length; j++) { if (world[i][j] === 1) { stroke(0, 0, 0); fill(0, 0, 0); } else if (world[i][j] === START) { fill(START_COLOR); stroke(START_COLOR); } else if (world[i][j] === GOAL) { fill(GOAL_COLOR); stroke(GOAL_COLOR); } else { fill(255, 255, 255); noStroke(); } rect(jBLOCK_SIZE, iBLOCK_SIZE, BLOCK_SIZE, BLOCK_SIZE); } } }; var simulate = function(steps) { var error = 0; var crosstrackError = 0; var r = new Robot(0, 1, BLOCK_SIZE/2); var sigma = 0; var diff = 0; for (var i = 0; i < steps2; i++) { // Compute cte diff = r.y-crosstrackError; crosstrackError = r.y; sigma += crosstrackError; if (i > steps) { error += pow(crosstrackError, 2); } // Update robot r.move(ROBOT_SPEED, -tPcrosstrackError - tDdiff - tIsigma); } return error; }; draw = function() { drawWorld(); // Use A* to find a path to the goal if (mode === PLAN) { // Draw explored fill(EXPLORED_COLOR); noStroke(); for (var i = 0; i < explored.length; i++) { for (var j = 0; j < explored[0].length; j++) { if (explored[i][j]) { rect(jBLOCK_SIZE, iBLOCK_SIZE, BLOCK_SIZE, BLOCK_SIZE); } } } // Draw frontier fill(FRONTIER_COLOR); noStroke(); for (var i = 0; i < frontier.length; i++) { rect(frontier[i][0]BLOCK_SIZE, frontier[i][1]BLOCK_SIZE, BLOCK_SIZE, BLOCK_SIZE); } // A* if (frontier.length > 0) { // Remove a node from the frontier var x = frontier[0][0]; var y = frontier[0][1]; var cost = frontier[0][2]; //println(cost + ", " + frontier[0][3]); frontier.splice(0, 1); // Goal check if (world[y][x] === GOAL) { mode = SMOOTH; } else { // Add all adjacent unexplored nodes for (var i = 0; i < DELTA.length; i++) { // If the new position is in the world var x2 = x + DELTA[i][0]; var y2 = y + DELTA[i][1]; if (x2 >= 0 && x2 < world[0].length && y2 >= 0 && y2 < world.length) { // If the position is unexplored if (!explored[y2][x2] && world[y2][x2] !== 1) { explored[y2][x2] = true; predecessors[y2][x2] = [x, y]; addToFrontier([x2, y2, cost+1, dist(x2, y2, GOAL_X, GOAL_Y)]); } } } } } else { mode = -1; println("No possible path to goal."); } } // Smooth the path else if (mode === SMOOTH) { // Build path if (path.length === 0) { var x = GOAL_X; var y = GOAL_Y; path.splice(0, 0, [x, y]); smoothPath.splice(0, 0, [x, y]); while (x !== 0 \|\| y !== 0) { var newX = predecessors[y][x][0]; var newY = predecessors[y][x][1]; x = newX; y = newY; path.splice(0, 0, [x, y]); smoothPath.splice(0, 0, [x, y]); } } // Draw the original path stroke(PATH_COLOR); strokeWeight(5); for (var i = 0; i < path.length; i++) { point(BLOCK_SIZEpath[i][0]+BLOCK_SIZE/2, BLOCK_SIZEpath[i][1]+BLOCK_SIZE/2); } strokeWeight(1); for (var i = 0; i < path.length-1; i++) { line(BLOCK_SIZEpath[i][0]+BLOCK_SIZE/2, BLOCK_SIZEpath[i][1]+BLOCK_SIZE/2, BLOCK_SIZEpath[i+1][0]+BLOCK_SIZE/2, BLOCK_SIZEpath[i+1][1]+BLOCK_SIZE/2); } // Draw the new path stroke(SMOOTH_PATH_COLOR); strokeWeight(5); for (var i = 0; i < smoothPath.length; i++) { point(BLOCK_SIZEsmoothPath[i][0]+BLOCK_SIZE/2, BLOCK_SIZEsmoothPath[i][1]+BLOCK_SIZE/2); } strokeWeight(1); for (var i = 0; i < smoothPath.length-1; i++) { line(BLOCK_SIZEsmoothPath[i][0]+BLOCK_SIZE/2, BLOCK_SIZEsmoothPath[i][1]+BLOCK_SIZE/2, BLOCK_SIZEsmoothPath[i+1][0]+BLOCK_SIZE/2, BLOCK_SIZEsmoothPath[i+1][1]+BLOCK_SIZE/2); } // Perform gradient descent var update; var diff = 0; for (var i = 1; i < smoothPath.length-1; i++) { update = [0, 0]; for (var j = 0; j < smoothPath[0].length; j++) { update[j] += weightData * (path[i][j] - smoothPath[i][j]); update[j] += weightSmooth * (smoothPath[(i+1)%smoothPath.length][j] + smoothPath[(i-1+smoothPath.length)%smoothPath.length][j] - 2smoothPath[i][j]); } // Simulataneous update for (var j = 0; j < smoothPath[0].length; j++) { smoothPath[i][j] += update[j]; diff += abs(update[j]); } } if (diff < 0.000001) { robot = new Robot(BLOCK_SIZE/2, BLOCK_SIZE/2, BLOCK_SIZE/2); mode = NAVIGATE; } } else if (mode === CALIBRATE) { var steps = 100; var error = simulate(steps); var dp = [1.0, 1.0, 1.0]; var params = [0, 0, 0]; if (error/steps > 0.04) { for (var i = 0; i < dp.length; i++) { params[i] += dp[i]; var newError = simulate(steps); if (newError < error) { error = newError; dp[i] = 1.1; } else { params[i] -= 2dp[i]; newError = simulate(steps); if (newError < error) { error = newError; dp[i] = -1.1; } else { params[i] += dp[i]; dp[i] = 0.9; } } } tP = params[0]; tD = params[1]; tI = params[2]; println(error/steps); } else { println(params); tP = params[0]; tD = params[1]; tI = params[2]; mode = NAVIGATE; } } // Use PID control to follow the path else if (mode === NAVIGATE) { // Draw path stroke(SMOOTH_PATH_COLOR); strokeWeight(5); for (var i = 0; i < smoothPath.length; i++) { point(BLOCK_SIZEsmoothPath[i][0]+BLOCK_SIZE/2, BLOCK_SIZEsmoothPath[i][1]+BLOCK_SIZE/2); } strokeWeight(1); for (var i = 0; i < smoothPath.length-1; i++) { line(BLOCK_SIZEsmoothPath[i][0]+BLOCK_SIZE/2, BLOCK_SIZEsmoothPath[i][1]+BLOCK_SIZE/2, BLOCK_SIZEsmoothPath[i+1][0]+BLOCK_SIZE/2, BLOCK_SIZEsmoothPath[i+1][1]+BLOCK_SIZE/2); } // Draw robot robot.draw(); // Compute cte var diff = -cte; var x1 = smoothPath[index][0]BLOCK_SIZE+BLOCK_SIZE/2; var y1 = smoothPath[index][1]BLOCK_SIZE+BLOCK_SIZE/2; var x2 = smoothPath[index+1][0]BLOCK_SIZE+BLOCK_SIZE/2; var y2 = smoothPath[index+1][1]BLOCK_SIZE+BLOCK_SIZE/2; var dx = x2-x1; var dy = y2-y1; var d = sqrt(pow(dx, 2) + pow(dy, 2)); var u = ((robot.x-x1)dx + (robot.y-y1)dy)/(pow(dx, 2) + pow(dy, 2)); var Px = x1 + ducos(atan2(dy, dx)); var Py = y1 + dusin(atan2(dy, dx)); cte = ((robot.y-y1)dx-(robot.x-x1)dy)/(pow(dx, 2) + pow(dy, 2)); sigma += cte; diff += cte; if (u > 1) { index++; index = min(index, smoothPath.length-2); } // Update robot robot.move(ROBOT_SPEED, -tPcte - tDdiff - tIsigma); //println(index); } };
Folds	83,89,91,142,145,166,168,170,176,199
Fork	false
Key	ag5zfmtoYW4tYWNhZGVteXIXCxIKU2NyYXRjaHBhZBiAgIimhqPGCAw
Votes	590
Spin-Offs	33
Type	PJS
Width	400
Height	400
User Flagged	false
Origin Scratchpad	null
Hidden From Hotlist	false
Restricted Posting	false
By Child	false
Author Nick	jdsutton
Author Name	jdsutton
Author ID	kaid_216263246392057096030602
Author Profile Access	COACH

Archive Data

Added	5 months ago, 1739370251300
Updated	3 months ago, 1744820744284
Source Deleted	unknown