Example #1
0
 public Mission()
 {
     Start = new Pose(0f, 0f, 0f);
     Goal = new Pose(10f, 10f, 0f);
     AStarEpsilon = 1.5f;
     AStarGridResolution = 1f;
     Environment = new Environment();
 }
Example #2
0
 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;
        }
Example #4
0
        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);
        }
Example #5
0
        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);
        }
Example #6
0
 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);
 }
Example #7
0
 public Pose(Pose copy)
     : this(copy.Position, copy.Orientation, copy.WheelAngle)
 {
 }
Example #8
0
 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));
         }
 }
Example #9
0
 public static Pose RandomContinuousAction(Pose current, float dt)
 {
     return NextPose(current, ((float)r.NextDouble() * 2 - 1) * MaxTurnAngle, SlowVelocity, dt);
 }
Example #10
0
 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);
        }
Example #13
0
        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();
        }
Example #14
0
 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);
 }
Example #15
0
 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);
 }
Example #17
0
 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);
        }
Example #19
0
 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();
        }
Example #21
0
        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();
        }
Example #23
0
        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();
        }
Example #24
0
 public static Pose RandomAction(Pose current, float dt)
 {
     float l;
     return NextPose(current, (Steer)r.Next(3), Gear.Forward, dt, out l);
 }