public Mission() { Start = new Pose(0f, 0f, 0f); Goal = new Pose(10f, 10f, 0f); AStarEpsilon = 1.5f; AStarGridResolution = 1f; Environment = new Environment(); }
public PIDController(ArrayList<Pose> path, Pose goal) : base(path, goal) { CheckpointReached = false; MaxSpeed = 2.2352f; PGain = 0.5f; DGain = 1f; }
private Environment buildEnvironment(int start, int goal, out Pose startPose, out Pose goalPose) { startPose = new Pose(); goalPose = new Pose(); Environment e = new Environment(); e.GridWidth = 100; e.GridHeight = 100; e.GridResolution = 0.5f; for (int i = -1; i < numSpacesInRow + 1; i++) { if (i == start) { Vector2 pos = firstRowOffset + new Vector2(0f, distanceBetweenSpaces * i) - new Vector2(Car.WHEEL_POS_LR.X, 0f); startPose = new Pose(pos, MathHelper.Pi); } else { e.Obstacles.Add(new BoxObstacle(world, carHeight, carWidth, firstRowOffset + new Vector2(0f, distanceBetweenSpaces * i))); } e.Obstacles.Add(new BoxObstacle(world, carHeight, carWidth, firstRowOffset - new Vector2(distanceBetweenRows, 0f) + new Vector2(0f, distanceBetweenSpaces * i))); } for (int i = -1; i < numSpacesInRow + 1; i += i == 0 || i == 6 ? 3 : 1) { e.Obstacles.Add(new BoxObstacle(world, carHeight, carWidth, secondRowOffset + new Vector2(0f, distanceBetweenSpaces * i))); e.Obstacles.Add(new BoxObstacle(world, carHeight, carWidth, secondRowOffset - new Vector2(distanceBetweenRows, 0f) + new Vector2(0f, distanceBetweenSpaces * i))); } /*for (int i = -2 + numSpacesInRow / 2; i < numSpacesInRow / 2 + 2; i++) { e.Obstacles.Add(new BoxObstacle(world, carHeight, carWidth, secondRowOffset + new Vector2(0f, distanceBetweenSpaces * i))); e.Obstacles.Add(new BoxObstacle(world, carHeight, carWidth, secondRowOffset - new Vector2(distanceBetweenRows, 0f) + new Vector2(0f, distanceBetweenSpaces * i))); }*/ for (int i = -1; i < numSpacesInRow + 1; i++) { e.Obstacles.Add(new BoxObstacle(world, carHeight, carWidth, thirdRowOffset + new Vector2(0f, distanceBetweenSpaces * i))); if (i == goal) { Vector2 pos = thirdRowOffset - new Vector2(distanceBetweenRows, 0f) + new Vector2(0f, distanceBetweenSpaces * i) - new Vector2(Car.WHEEL_POS_LR.X, 0f); goalPose = new Pose(pos, MathHelper.Pi); } else { e.Obstacles.Add(new BoxObstacle(world, carHeight, carWidth, thirdRowOffset - new Vector2(distanceBetweenRows, 0f) + new Vector2(0f, distanceBetweenSpaces * i))); } } return e; }
public static Pose NextPose(Pose current, float steerAngle, float velocity, float dt) { float length = velocity * dt; float turnRadius = AxleDistance / (float)Math.Tan(steerAngle); float phi = length / turnRadius; float phiover2 = phi / 2; float sinPhi = (float)Math.Sin(phiover2); float L = 2 * sinPhi * turnRadius; float x = L * (float)Math.Cos(phiover2); float y = L * sinPhi; Vector2 pos = new Vector2(x, y); pos = Vector2.Transform(pos, Matrix.CreateRotationZ(current.Orientation)); return new Pose(current.Position + pos, current.Orientation + phi); }
public static Pose NextPose(Pose current, Steer steer, Gear gear, float speed, float dt, float turnRadius, out float length) { length = speed * dt; float x, y, phi; if (steer == Steer.Straight) { x = length; y = 0; phi = 0; } else { phi = length / turnRadius; float phiover2 = phi / 2; float sinPhi = (float)Math.Sin(phiover2); float L = 2 * sinPhi * turnRadius; x = L * (float)Math.Cos(phiover2); y = L * sinPhi; } if (steer == Steer.Right) { y = -y; phi = -phi; } if (gear == Gear.Backward) { x = -x; phi = -phi; } Vector2 pos = new Vector2(x, y); pos = Vector2.Transform(pos, Matrix.CreateRotationZ(current.Orientation)); return new Pose(current.Position + pos, current.Orientation + phi); }
public static Vector2 GetCenterPosition(Pose pose) { return new Vector2(pose.X + (float)Math.Cos(pose.Orientation) * -WHEEL_POS_LR.X, pose.Y + (float)Math.Sin(pose.Orientation) * -WHEEL_POS_LR.X); }
public Pose(Pose copy) : this(copy.Position, copy.Orientation, copy.WheelAngle) { }
private void calcRS() { maxLength = float.MinValue; paths = new ArrayList<ArrayList<Pose>>(); actions = new ArrayList<ReedsSheppActionSet>(); for (int x = -cells / 2; x <= cells / 2; x++) for (int y = -cells / 2; y <= cells / 2; y++) { Pose start = new Pose(x * cellSize, y * cellSize, orientation); ReedsSheppActionSet actionSet = ReedsSheppSolver.Solve(start, goal, VehicleModel.TurnRadius); actionSet.Length = actionSet.CalculateCost(VehicleModel.TurnRadius, HybridAStar.reverseFactor, HybridAStar.switchPenalty); if (actionSet.Length > maxLength) maxLength = actionSet.Length; actions.Add(actionSet); paths.Add(ReedsSheppDriver.Discretize(start, actionSet, VehicleModel.TurnRadius, 1f)); } }
public static Pose RandomContinuousAction(Pose current, float dt) { return NextPose(current, ((float)r.NextDouble() * 2 - 1) * MaxTurnAngle, SlowVelocity, dt); }
public CarController(ArrayList<Pose> path, Pose goal) { Path = path; MaxSpeed = 0.5f; this.goal = goal; }
public CarControls MissionComplete(Pose currentPose, float wheelAngle, float speed, GameTime gameTime) { return new CarControls(); }
public CarControls ForwardDrive(Pose currentPose, float wheelAngle, float speed, GameTime gameTime) { int prevIndex, nextIndex; localize(currentPose.Position, speed, out prevIndex, out nextIndex); InReverse = FrontPath[prevIndex].Gear == Gear.Backward; bool comingToStop = stoppingPoints.Contains(nextIndex); float cte; float howFar = MathHelper.Clamp(howFarAlong(currentPose.Position, Path[prevIndex].Position, Path[nextIndex].Position, out cte), 0f, 1f); DebugInfo = String.Format("How far: {0:0.000}\n", howFar); updateInfo(cte, wheelAngle, speed, gameTime); ArrayList<Pose> thePath = InReverse ? ReverseFrontPath : FrontPath; NextWaypointIndex = nextIndex; PrevWaypointIndex = prevIndex; Vector2 next = thePath[nextIndex].Position; Vector2 prev = thePath[prevIndex].Position; FakeFrontAxle = Car.GetFakeFrontAxlePosition(currentPose); Vector2 frontAxle = InReverse ? FakeFrontAxle : Car.GetFrontAxlePosition(currentPose); ClosestPoint = findClosestPoint(frontAxle, prev, next); Vector2 heading = prevIndex > 0 ? next - thePath[prevIndex - 1].Position : next - prev; float desiredHeading = (float)Math.Atan2(heading.Y, heading.X); float nextHeading; if (comingToStop) { nextHeading = Path[nextIndex].Orientation + (InReverse ? MathHelper.Pi : 0); } else { Vector2 nextHeadingVec = thePath[nextIndex + 1].Position - prev; nextHeading = (float)Math.Atan2(nextHeadingVec.Y, nextHeadingVec.X); } // lerp based on how far you are along a segment // use WrapAngle to avoid pi - (-pi) issues desiredHeading += MathHelper.WrapAngle(nextHeading - desiredHeading) * howFar; DebugInfo += String.Format("Orientation: {0:0.000}\n", desiredHeading); // Is the path to our left or right? Vector2 norm = next - prev; norm.Normalize(); Left = Vector2.Transform(norm, Matrix.CreateRotationZ(MathHelper.PiOver2)) * 2 + ClosestPoint; Right = Vector2.Transform(norm, Matrix.CreateRotationZ(-MathHelper.PiOver2)) * 2 + ClosestPoint; float dir = 1; if ((Left - frontAxle).LengthSquared() < (Right - frontAxle).LengthSquared()) dir = -1; float k = 1.5f; float dist = (frontAxle - ClosestPoint).Length(); float dtheta = MathHelper.WrapAngle(currentPose.Orientation - desiredHeading + (InReverse ? MathHelper.Pi : 0)); if (InReverse) { dtheta = -dtheta; dir = -dir; } float phi; if (comingToStop) phi = (float)Math.Atan(4f * k * dir * dist); else phi = -dtheta + (float)Math.Atan(k * dir * dist); float phiError = phi - wheelAngle; float dPhiError = (phiError - prevPhiError) / (float)gameTime.TotalGameTime.TotalSeconds; float steer = phiError * 8f + dPhiError * 0.5f; // Velocity control float vp = 0.5f; float vi = 0f;// 0.00005f; float gas = 0f; float brake = 0f; float verror = speed - MathHelper.Lerp(Velocities[prevIndex], Velocities[nextIndex], howFar); DebugInfo += String.Format("Speed: {0:0.000}\n", speed); DebugInfo += String.Format("Target speed: {0:0.000}\n", MathHelper.Lerp(Velocities[prevIndex], Velocities[nextIndex], howFar)); DebugInfo += String.Format("From {0:0.000} to {1:0.000}", Velocities[prevIndex], Velocities[nextIndex]); if (nextIndex + 1 < Path.Count) DebugInfo += String.Format(" then {0:0.000}", Velocities[nextIndex + 1]); vpasterror += verror * (float)gameTime.TotalGameTime.TotalSeconds; float vtotalerror = vp * verror + vi * vpasterror; if (vtotalerror > 0f) brake = vtotalerror; else if (vtotalerror < 0f) gas = -vtotalerror; if (InReverse) gas = -gas; if (comingToStop && howFar == 1f) { lastCusp = nextIndex; if (nextIndex < Path.Count - 1) { NextWaypointIndex++; PrevWaypointIndex++; } else { NextWaypointIndex = PrevWaypointIndex = Path.Count - 1; } State = ControllerState.Stopped; return new CarControls(0f, 1f, steer); } return new CarControls(gas, brake, steer); }
public void InitSim(bool autoStart, bool smoothingOn, Mission mission) { gridDone = false; pathDone = false; pathSmoothDone = false; pathSearching = false; pathSearchingDone = false; autoDrive = true; run = false; isCollided = false; currentControls = new CarControls(0f, 0f, 0f); if (camera == null) { camera = new Camera(MathHelper.PiOver4, GraphicsDevice.Viewport.AspectRatio, 0.1f, 1000f); camera.Position = new Vector3(75f, 75f, 180f); } startPose = mission.Start; goalPose = mission.Goal; HybridAStar.Epsilon = mission.AStarEpsilon; HybridAStar.GridResolution = mission.AStarGridResolution; HybridAStar.SafetyFactor = 1.5f; HybridAStar.Reset(); car = new Car(world, startPose); grid = new ObstacleGrid(world, mission.Environment); car.Body.OnCollision += new OnCollisionEventHandler(OnCollision); gridDone = false; pathDone = false; pathSearchingDone = false; bg = new Thread(() => { DateTime now = DateTime.Now; grid.BuildGVD(); console.WriteLine("GVD Generation Time: " + Math.Round((DateTime.Now - now).TotalMilliseconds) + " ms"); gridDone = true; now = DateTime.Now; pathSearching = true; astar = HybridAStar.FindPath(grid, startPose, goalPose); TimeSpan astarTime = DateTime.Now - now; poses = astar.Path; pathSearching = false; pathSearchingDone = true; if (astar.Path.Count > 0) pathDone = true; now = DateTime.Now; if (smoothingOn) smoothedPath = Smoother.Smooth(astar.Path, grid); else smoothedPath = astar.Path; TimeSpan smoothingTime = DateTime.Now - now; int numUnsafe = Smoother.UnsafeIndices != null ? Smoother.UnsafeIndices.Count : 0; console.WriteLine("A*: Total Planning Time: " + Math.Round((astarTime + smoothingTime).TotalMilliseconds) + " ms"); console.WriteLine(" Heuristic Time: " + Math.Round(astar.HeuristicInitTime.TotalMilliseconds) + " ms"); console.WriteLine(" Searching Time: " + Math.Round((astarTime - astar.HeuristicInitTime).TotalMilliseconds) + " ms"); console.WriteLine(" Smoothing Time: " + Math.Round(smoothingTime.TotalMilliseconds) + " ms (" + Smoother.NumIterations + " iterations, " + Smoother.Change + "m, " + numUnsafe + " unsafe points)"); console.WriteLine(" " + astar.Discovered.Count + " nodes discovered"); console.WriteLine(" " + astar.Expanded.Count + " nodes expanded"); controller = new StanleyFSMController(smoothedPath, goalPose); pathSmoothDone = true; if (autoStart) run = true; }); bg.IsBackground = true; bg.Priority = ThreadPriority.Lowest; bg.Start(); }
public static Pose NextPose(Pose current, Steer steer, Gear gear, float dt, out float length) { return NextPose(current, steer, gear, SlowVelocity, dt, TurnRadius, out length); }
public static Vector2 GetFrontAxlePosition(Pose pose) { return new Vector2(pose.X + (float)Math.Cos(pose.Orientation) * WHEEL_BASE, pose.Y + (float)Math.Sin(pose.Orientation) * WHEEL_BASE); }
public CarControls ReverseDrive(Pose currentPose, float wheelAngle, float speed, GameTime gameTime) { return ForwardDrive(currentPose, wheelAngle, speed, gameTime); }
public Car(World world, Pose pose) : this(world, pose.Position, pose.Orientation) { }
public CarControls Stopped(Pose currentPose, float wheelAngle, float speed, GameTime gameTime) { float phiError; float desiredHeading; ControllerState nextState; if (NextWaypointIndex == Path.Count - 1 && PrevWaypointIndex == Path.Count - 1) { desiredHeading = currentPose.Orientation; nextState = ControllerState.MissionComplete; } else { ArrayList<Pose> thePath = FrontPath[PrevWaypointIndex].Gear == Gear.Forward ? FrontPath : ReverseFrontPath; Vector2 next = thePath[NextWaypointIndex].Position; Vector2 prev = thePath[PrevWaypointIndex].Position; Vector2 heading = next - prev; desiredHeading = (float)Math.Atan2(heading.Y, heading.X); nextState = thePath[PrevWaypointIndex].Gear == Gear.Forward ? ControllerState.ForwardDrive : ControllerState.ReverseDrive; } phiError = -MathHelper.WrapAngle(currentPose.Orientation - desiredHeading) - wheelAngle; if (Math.Abs(phiError) < 0.001f || Math.Abs(Math.Abs(wheelAngle) - Car.MAX_WHEEL_DEFLECTION) < 0.1f) { State = nextState; Velocities[PrevWaypointIndex] = Velocities[NextWaypointIndex]; } return new CarControls(0f, 1f, phiError * 8f); }
public abstract CarControls Update(Pose currentPose, float wheelAngle, float speed, GameTime gameTime);
public override CarControls Update(Pose currentPose, float wheelAngle, float speed, GameTime gameTime) { lastPose = currentPose; if (Path.Count == 0) return new CarControls(); switch (State) { case ControllerState.MissionStart: return MissionStart(); case ControllerState.Stopped: return Stopped(currentPose, wheelAngle, speed, gameTime); case ControllerState.ForwardDrive: return ForwardDrive(currentPose, wheelAngle, speed, gameTime); case ControllerState.ReverseDrive: return ReverseDrive(currentPose, wheelAngle, speed, gameTime); } return new CarControls(); }
public override CarControls Update(Pose currentPose, float wheelAngle, float speed, GameTime gameTime) { if ((Path.Last.Position - currentPose.Position).LengthSquared() < 2f || stop) { stop = true; if (speed == 0f) CheckpointReached = true; return new CarControls(0f, 0.1f, 0f); } // Find closest path point to rear axle Vector2 rearAxle = currentPose.Position; float frontbestd = float.MaxValue; int frontbesti = 0; for (int i = 0; i < Path.Count - 1; i++) { float d = (rearAxle - Path[i].Position).LengthSquared(); if (d < frontbestd) { frontbestd = d; frontbesti = i; } } Vector2 next = Path[frontbesti].Position; Vector2 prev; if (frontbesti + 1 >= Path.Count) prev = Path[frontbesti - 1].Position; else if (frontbesti - 1 < 0) { prev = next; next = Path[frontbesti + 1].Position; } else { Vector2 prevmaybe = Path[frontbesti - 1].Position; Vector2 nextmaybe = Path[frontbesti + 1].Position; if ((rearAxle - prevmaybe).LengthSquared() < (rearAxle - nextmaybe).LengthSquared()) prev = prevmaybe; else { prev = next; next = nextmaybe; } } /*float Rx = rearAxle.X - prev.X; float Ry = rearAxle.Y - prev.Y; float dx = next.X - prev.X; float dy = next.Y - prev.Y; float cte = (Ry * dx + Rx * dy) / (dx * dx + dy * dy);*/ // Find the closest point to the front axle on the line defined by the points next and prev float x, y; if (next.X == prev.X) // Avoid division by zero { x = next.X; y = rearAxle.Y; } else { float m = (next.Y - prev.Y) / (next.X - prev.X); float b = next.Y - m * next.X; x = (m * rearAxle.Y + rearAxle.X - m * b) / (m * m + 1); y = (m * m * rearAxle.Y + m * rearAxle.X + b) / (m * m + 1); } ClosestPoint = new Vector2(x, y); CrossTrackError = (rearAxle - ClosestPoint).Length(); Vector2 norm = next - prev; norm.Normalize(); left = Vector2.Transform(norm, Matrix.CreateRotationZ(MathHelper.PiOver2)) * 2 + ClosestPoint; right = Vector2.Transform(norm, Matrix.CreateRotationZ(-MathHelper.PiOver2)) * 2 + ClosestPoint; float dir = 1; if ((left - rearAxle).LengthSquared() < (right - rearAxle).LengthSquared()) dir = -1; float alpha = PGain * CrossTrackError + DGain * (CrossTrackError - previouscte) / (float)gameTime.ElapsedGameTime.TotalSeconds; previouscte = CrossTrackError; float gas = 0; if (speed < MaxSpeed) gas = 0.5f; float steer = alpha * dir - wheelAngle; return new CarControls(gas, 0f, steer); }
public StanleyFSMController(ArrayList<Pose> path, Pose goal) : base(path, goal) { MaxSpeed = 100f;// 4.4704f; MaxReverseSpeed = MaxSpeed / 4; State = ControllerState.MissionStart; stoppingPoints = new LinkedList<int>(); DebugInfo = ""; lastPose = new Pose(); totalCte = 0f; totalLateralAccel = 0f; maxLateralAccel = 0f; lastLateralAccel = 0f; countInfo = 0; if (Path.Count == 0) return; MissionStart(); }
public static Mission CreateMission(MissionType type, World world) { switch (type) { case MissionType.Junkyard: Pose start = new Pose(10f, 10f, 0f); Pose goal = new Pose(140f, 140f, MathHelper.PiOver2); return new Mission { Start = start, Goal = goal, Environment = new RandomEnvironment(world, 200, start.Position, goal.Position), AStarEpsilon = 1.1f }; case MissionType.ParkingLot: return new Mission { //Start = new Pose(20f, 10f, 0f), //Goal = new Pose(90f, 83.6f, MathHelper.PiOver2), Start = new Pose(140f, 45f, -MathHelper.Pi), Goal = new Pose(90f, 86.34f, -MathHelper.PiOver2), Environment = new ParkingEnvironment(world), AStarEpsilon = 2f }; } return new Mission(); }
public static Pose RandomAction(Pose current, float dt) { float l; return NextPose(current, (Steer)r.Next(3), Gear.Forward, dt, out l); }