Omni-Directional Wheeled Robot Simulation in Python

In this post, I will show you a computer simulation I developed of an omni-directional vehicle with Mecanum wheels. This project was developed during my graduate studies at Johns Hopkins University. My goal was to develop a 2D representation of this type of robot so that I could implement the kinematic equations and control algorithms from scratch, using nothing but Python. 

mecanum_wheel_robot_simulation

After I developed the source code, I converted the entire Python file (.py) into an executable (.exe file …. i.e. a program that can run on any machine) using a special software program called PyInstaller.

Table of Contents

Prerequisites

  • Python 3.6 (or higher)

Return to Table of Contents

Requirements

Here are the requirements for this project:

  • Build a simulation of an omni-directional wheeled robot from scratch.
  • Create an executable so that the simulation can be launched and interfaced with on a normal Windows machine – OS: XP or higher.
  • Show the kinematic equations and control algorithm used (see next section).
  • Robot’s dimensions must be 4 feet and 2 feet – that is, wheel plane to wheel plane, and axle to axle. 
  • The simulated environment must be a flat, high friction (i.e., assume no slippage) ‘infinite’ area, with a viewable area (on screen) of 30 feet long by 15 feet wide.
  • The environment view is a plan (aerial) view.
  • The environment area must have a 6inch by 6inch visible grid underlaid on it (i.e., on its surface but under the vehicle) 
    • This grid below is the simulation environment that appears when the program is launched.
1-environment
  • The viewable area must stay fixed until such time as the vehicle reference point travels within 3 feet of the viewable edge, at which point the viewable area shall be repositioned so that the vehicle is at its center.
  • An interface is required, to the side of the viewable area.
2-interface-required
  • Autonomous control of the vehicle is required. Maximum speed of the vehicle is 15 ft/sec.
  • The vehicle image must move on screen in agreement with the controlled movement.
  • The radius of the wheels on the robot must be 0.5 feet.
  • Simulation must have a reset capability that when executed, resets the vehicle to the origin at rest, with no commands executed.
3-reset-capability

The robot must have the following modes:

Autonomous Control 1: The direction (θP), speed, and rate of rotation of the vehicle reference point can be specified and executed. In this case, the vehicle will keep executing given command(s) until a ‘stop’ command is provided.

4-autonomous-control-mode

Autonomous Control 2: The user shall be able to specify the rotation rate of each wheel and the vehicle will keep executing the given commands until a ‘stop’ command. In this mode alone, any mouse or keyboard button push will be considered a ‘stop’ command.

5-autonomous-control-mode-2

Point Execution: A straight line path may be specified by an end point (XF,YF), including a desired end orientation (θF). The vehicle’s current pose will be considered its starting point and orientation.

6-point-execution

Circle Execution: A circle may be defined (instead of points) and executed, with user-defined radius and inclination, θP (direction of circle center from current vehicle position).

7-circle-execution

Rectangle Execution: A rectangle may be defined (instead of points) and executed, with user-defined side lengths and inclination, θP, of diagonally opposite vertex from current vehicle position. Vehicle will start at a vertex.

8-rectangle-execution

Figure 8 Execution: A figure 8 may be defined (instead of points) and executed, consisting of two circles with two user-defined, possibly different, radii, and inclination, θP (direction of circle centers from current vehicle position).

9-figure-8-execution

Waypoint Execution: Waypoints (points between start and end points), during Point Execution, can be specified (minimum 3 waypoints – although none are required) and executed.

10-waypoint-execution

Other Requirements

  • Time taken to get from current to end points can be specified for all path executions above except for the open-ended path of the Autonomous Control Mode. If allotted time requires the vehicle to travel faster than its maximum speed, an error is shown on the interface.
11-error-log
  • Both the user-defined path to be executed and the path traveled by the vehicle reference point must be displayed on screen in two different colors that are easily discernible from each other.
12-easily-discernible
  • Simulation must provide: Vehicle position display, Wheel rate displays, Robot x- and y- velocity displays.
13-simulation-must-provide-display

Return to Table of Contents

List of Kinematic Equations Used

mecanum-wheel-robot

Forward Kinematic Equations

14-forward-kinematic-equations

Where:

  • R = wheel radius (e.g. feet)
  • Vx = Velocity in the x direction in the robot (local) reference frame (e.g. feet/second)
  • Vy = Velocity in the y direction in the robot (local) reference frame (e.g. feet/second)
  • ω = rotation rate of the vehicle reference point (e.g. in degrees/second)
  • Ψ = wheel rotation rate (e.g. in degrees per second)

Inverse Kinematic Equations

15-inverse-kinematic-equations

Return to Table of Contents

List of Control Algorithms Used

The control algorithms are contained inside omnidirectional_robot.py. Here is the list:

  1. control_robot_body(direction, speed, rotation_rate)
  2. control_robot_wheels(rot1, rot2, rot3, rot4)
  3. point_execution_no_wp(x_final, y_final, orientation_final, deadline):
  4. point_execution_with_wp(x_wp1, y_wp1, x_wp2, y_wp2, x_wp3, y_wp3, x_final, y_final, orientation_final, deadline)
  5. move_in_a_circle(radius, direction, deadline)
  6. move_in_a_rectangle(length_in_ft, direction_of_opp_vertex, deadline)
  7. move_in_a_figure_eight(circle1_radius, circle2_radius, direction, deadline)

Return to Table of Contents

List of Math Equations Used

Calculate the interior angle in degrees between diagonal of rectangle and the side length

  • interior_angle = acos(length_in_ft/diagonal_length))

Converting local velocity in the robot frame to global velocity in the global (inertial reference frame)

  • v_x_global = (v_x_local * cos(vehicle_angle_ref)) – (v_y_local * sin(vehicle_angle_ref))
  • v_y_global = (v_x_local * sin(vehicle_angle_ref)) + (v_y_local * cos(vehicle_angle_ref))

Calculating the global velocities form the direction/heading angle (yaw)

  • v_x_global = magnitude(v) * cos(angle for yaw)
  • v_y_global = magnitude(v) * sin(angle for yaw)

Converting global velocity in the global (inertial reference frame) to local velocity in the robot frame

  • v_x_local = (v_x_global * cos(vehicle_angle_ref)) + (v_y_global * sin(vehicle_angle_ref))
  • v_y_local = (-v_x_global * sin(vehicle_angle_ref)) + (v_y_global * cos(vehicle_angle_ref))

Calculating the direction of travel of the vehicle reference point in degrees

  • direction_global = atan2((y_final – y_pos_ref),(x_final – x_pos_ref))

Calculating the distance between two points in feet

  • distance = square_root((x2-x1)2 + (y2-y1)2)

Calculating the speed of the vehicle using the distance and time

  • speed = distance / time

Convert direction (local) into direction (global) for the rectangle implementation

  • direction_global = direction_of_opp_vertex + (vehicle_angle_ref)

Determining the new x-coordinate of the vehicle reference point

  • x_pos_ref = x_initial + (v_x_global * dt)

Determining the new y-coordinate of the vehicle reference point

  • y_pos_ref = y_initial + (v_y_global * dt)

Determining the new orientation of the vehicle reference point

  • vehicle_angle_ref = vehicle_angle_ref_initial + (rotation_rate * dt)

Equation for a circle centered at point (H, K)

  • x = rcos(Ɵ) + H
  • y = rsin(Ɵ) + K

Get the length of the diagonal of the rectangle

  • diagonal_length = abs(length_in_ft/cos(direction_of_opp_vertex))

Get the width of the diagonal of the rectangle

  • width_in_ft = abs(diagonal_length *sin(direction_of_opp_vertex))

Return to Table of Contents

Source Code

Here is the source code for the program. Make sure you have the Numpy and Matplotlib libraries installed on your system before you run the program. The typical syntax for installing these libraries if you are using something like Anaconda for your Python framework is:

pip install numpy
pip install matplotlib

Other than that, you don’t need anything else to run the program. Just copy and paste this code into your favorite place to run Python programs, and off you go!

import math
import matplotlib.pyplot as plt
import matplotlib.patches as patches
import numpy as np
import time
import tkinter as tk

# Project: Omnidirectional Mecanum-Wheeled Robot
# Author: Addison Sears-Collins
# Date created: 02/22/2020

################### Initialize Constants ##################
# Set the robot vehicle's dimensions
# Robot is 2 feet in width and 4 feet in length
ROBOT_WIDTH = 2.0
ROBOT_LENGTH = 4.0

# Maximum speed of the robot in feet/second
MAX_SPEED = 15.0

# Robot vehicle's wheel radius in feet
WHEEL_RADIUS = 0.5

# Length and width of the viewable area on screen
# The grid is 30 feet long by 15 feet wide
ENV_LENGTH = 30.0
ENV_WIDTH = 15.0

# The distance between ticks on the x and y-axis in feet
# Each square on the grid is 6inch by 6inch 
X_TICK_SPACING = 0.5
Y_TICK_SPACING = 0.5

# Boundary in feet. Viewable area will stay fixed until 
# such time as the vehicle reference point travels within
# 3 feet of the viewable edge.
BOUNDARY = 3.0

################### Global Variables ######################
# The robot vehicle object (which will be a rectangle)
rect = None

# The coordinates of the vehicle reference point
x_pos_ref = None # in feet
y_pos_ref = None # in feet
vehicle_angle_ref = None # in degrees

# The grid specifications
x_axis_min = None
x_axis_max = None
y_axis_min = None
y_axis_max = None

# Store historical x and y-values in order to track
# the path traveled by the vehicle reference point
hist_x_vals = []
hist_y_vals = []

# The circle that pertains to the move_in_a_circle() method
# and the figure 8 method
circle1 = None

# The rectangle that pertains to the move_in_a_rectangle()
# method
this_rect = None

# Create a window using the Tk class
window = tk.Tk()

# Global flag
running_control_robot_body = False
running_control_robot_wheels = False

##################### Helper Methods ######################
def convert_local_velocity_to_global(v_x_local, v_y_local):
    """
    Convert the velocities in the x and y-directions in 
    the local reference frame to velocities in the global
    reference frame.
    @param v_x_local float: Velocity in the x-direction 
        in the local reference frame in ft/s
    @param v_y_local float: Velocity in the y-direction 
        in the local reference frame in ft/s
    @return v_x_global float: Velocity in the x-direction 
        in the global reference frame in ft/s
    @return v_y_global float: Velocity in the y-direction 
        in the global reference frame in ft/s
    """
    v_x_global = (v_x_local * math.cos(math.radians(
        vehicle_angle_ref))) - (v_y_local * math.sin(
        math.radians(vehicle_angle_ref)))

    v_y_global = (v_x_local * math.sin(math.radians(
        vehicle_angle_ref))) + (v_y_local * math.cos(
        math.radians(vehicle_angle_ref)))

    return v_x_global, v_y_global 

# Method to close the Tkinter window
def close_window(): 
    window.destroy()

def get_distance_btw_points(x1,y1,x2,y2):
    """
    Calculate the distance between two points in feet.
    @param x1: The x-coordinate of point 1
    @param y1: The y-coordinate of point 1
    @param x2: The x-coordinate of point 2
    @param y2: The y-coordinate of point 2
    @return distance: float: in feet
    """
    distance = math.sqrt((x2-x1)**2 + (y2-y1)**2)
    return distance

def get_robot_motion_values(rot1, rot2, rot3, rot4):
    """
    Calculate the velocities in the x and y-directions in 
    the local reference frame as well as the rotation rate
    of the vehicle reference point.
    @param rot1: Rotation rate of wheel 1 in degrees/sec
    @param rot2: Rotation rate of wheel 2 in degrees/sec
    @param rot3: Rotation rate of wheel 3 in degrees/sec
    @param rot4: Rotation rate of wheel 4 in degrees/sec
    @return v_x_local float: Velocity in the x-direction 
        in the local reference frame in ft/s
    @return v_y_local float: Velocity in the y-direction 
        in the local reference frame in ft/s
    @return rotation_rate: Rotation rate of the vehicle
        reference point in degrees per second in the 
        counter-clockwise direction
    """
    v_x_local = (WHEEL_RADIUS/4) * (
        rot1 - rot2 - rot3 + rot4)
    v_y_local = (WHEEL_RADIUS/4) * (
        rot1 + rot2 + rot3 + rot4)
    rotation_rate = (WHEEL_RADIUS/(4*(
        ROBOT_LENGTH + ROBOT_WIDTH)))*(
        -rot1 + rot2 - rot3 + rot4)

    return v_x_local, v_y_local, rotation_rate

def get_wheel_rot_rates(v_x_local, v_y_local, 
        rotation_rate):
    """
    Calculate the wheel rotation rates.
    @param v_x_local: Velocity in the x-direction 
        in the local reference frame in ft/s
    @param v_y_local: Velocity in the y-direction 
        in the local reference frame in ft/s
    @param rotation_rate: Rotation rate in degrees per 
        second in the counter-clockwise direction (vehicle
        reference point)
    @return rot1, rot2, rot3, rot4: float: Wheel rotation 
        rates in degrees per second
    """
    rot1 = (1/WHEEL_RADIUS)*(v_y_local + v_x_local - (
        ROBOT_LENGTH + ROBOT_WIDTH) * rotation_rate)
    rot2 = (1/WHEEL_RADIUS)*(v_y_local - v_x_local + (
        ROBOT_LENGTH + ROBOT_WIDTH) * rotation_rate)
    rot3 = (1/WHEEL_RADIUS)*(v_y_local - v_x_local - (
        ROBOT_LENGTH + ROBOT_WIDTH) * rotation_rate)
    rot4 = (1/WHEEL_RADIUS)*(v_y_local + v_x_local + (
        ROBOT_LENGTH + ROBOT_WIDTH) * rotation_rate)

    return rot1, rot2, rot3, rot4

def get_speed(distance, time):
    """
    Calculate the speed of the vehicle using the distance 
    and time.
    @param distance: The distance the robot vehicle must 
        travel between 2 or more points in feet
    @param time: Total travel time in seconds
    @return speed: float
    """
    speed = distance / time
    return speed

def get_velocity_magnitude(velocity_x, velocity_y):
    """
    Calculate the speed of the vehicle using the x and y
    components of the velocity.
    @param velocity_x: Velocity in x-direction in ft/sec
    @param velocity_y: Velocity in y-direction in ft/sec
    @return speed: float
    """
    speed = math.sqrt(((velocity_x)**2) + ((velocity_y)**2))
    return speed

def is_close_to_edge(x, y):
    """
    The viewable area will stay fixed until such time as 
    the vehicle reference point travels within 3 feet of 
    the viewable edge. Check if the viewable area needs to
    be repositioned.
    @param x: x-coordinate of the vehicle reference point
    @param y: y-coordinate of the vehicle reference point
    @return bool
    """
    if (x - x_axis_max) > -BOUNDARY:
        return True
    elif (x - x_axis_min) < BOUNDARY:
        return True
    elif (y - y_axis_max) > -BOUNDARY:
        return True
    elif (y - y_axis_min) < BOUNDARY:
        return True
    else:
        return False

def is_too_fast(speed):
    """
    The maximum speed of the robot vehicle is 15 ft/second
    Check if the user input requires the vehicle to travel
    faster than its maximum speed of 15 ft/second
    @param speed: The magnitude of the velocity in ft/sec
    @return bool
    """
    if speed > 15.0:
        return True
    else:
        return False

def plot_arrow(x, y, orientation):
    """
    Plot the arrow on the top of the robot. Arrow points 
    to +y-direction of the robot (i.e. towards the front 
    center part of the robot). It is centered on the 
    vehicle reference point.
    @param x: x-coordinate of the vehicle reference point
    @param y: y-coordinate of the vehicle reference point
    @param orientation: orientation of the vehicle 
        reference point in radians
    """
    # Clear datapoints if they exist
    try:
        for datapoints in ax.get_lines():
            datapoints.remove()
    except:
        pass

    # Plot the arrow
    plt.plot(x, y, marker=(3, 0, math.degrees(
        orientation)), markersize=20, color="black")

def plot_grid(x, y):
    """
    Plot the grid. 
    @param x: x-coordinate of the center of the grid.
    @param y: y-coordinate of the center of the grid
    """
    global x_axis_min, x_axis_max, y_axis_min, y_axis_max

    # Set the x and y limits of the grid.
    x_axis_max = x + (ENV_WIDTH / 2.0) + X_TICK_SPACING
    x_axis_min = x - (ENV_WIDTH / 2.0)
    y_axis_max = y + (ENV_LENGTH / 2.0) + Y_TICK_SPACING
    y_axis_min = y - (ENV_LENGTH / 2.0)
    ax.set(xlim=(x_axis_min, x_axis_max), ylim=(y_axis_min, 
        y_axis_max))

    # Each square on the grid is 6inch by 
    # 6inch (6inch = 0.5 feet)
    ax.set_xticks(np.arange(x_axis_min, x_axis_max, 
        X_TICK_SPACING))
    ax.set_yticks(np.arange(y_axis_min, y_axis_max, 
        Y_TICK_SPACING))
    ax.grid(True)

    turn_off_tick_labels()

def plot_line(x1, y1, direction):
    """
    Show the user defined path as a red line
    @param x1: x-coordinate of the start point of the line
    @param y1: y-coordinate of the start point of the line
    @direction: Direction of travel of the vehicle
        reference point in radians
    """
    # Arbitrary-chosen line length
    line_length = 2.0 * math.sqrt(2.0)

    x2 = (line_length * math.cos(direction))
    x2 = x1 + x2
    y2 = (line_length * math.sin(direction))
    y2 = y1 + y2

    plt.plot([x1, x2], [y1, y2], color='red', 
             linestyle='-', linewidth=2)  

def plot_path_traveled(x_values, y_values):
    """
    Show the path traveled by the robot.
    @param x_values list: List of x values
    @param y_values list: List of y values
    """
    plt.plot(x_values, y_values, color='green', 
             linestyle='-', linewidth=2)    

def plot_robot(x, y, orientation):
    """
    Plot the robot on the grid.    
    Rotate lower left coordinate of the robot based on 
    vehicle reference point's orientation.
    This equation gives the lower left coordinate's new 
        position when rotated around the origin (x=0,y=0):
        X = x*cos(θ) - y*sin(θ)
        Y = x*sin(θ) + y*cos(θ)
    @param x: x-coordinate of the vehicle reference point
    @param y: y-coordinate of the vehicle reference point
    @param orientation: orientation of the vehicle 
        reference point in radians
    """    
    global rect

    # Remove the existing rectangle if it exists
    try:
        rect.remove()
    except:
        pass

    rect_x_pos = ((-ROBOT_WIDTH/2.0) * math.cos(
        orientation)) - ((-ROBOT_LENGTH/2.0) * math.sin(
        orientation))
    rect_y_pos = ((-ROBOT_WIDTH/2.0) * math.sin(
        orientation)) + ((-ROBOT_LENGTH/2.0) * math.cos(
        orientation))

    # Translate lower left coordinate of the robot so it 
    #   is relative to the vehicle reference point
    rect_x_pos = rect_x_pos + x
    rect_y_pos = rect_y_pos + y

    # Update the robot's position and orientation
    rect = patches.Rectangle((rect_x_pos,rect_y_pos),
        ROBOT_WIDTH,ROBOT_LENGTH, math.degrees(
        orientation),lw=3,ec='black', fc='orange')

    # Add the rectangle to the Axes
    ax.add_patch(rect)

def reset():
    """
    This method resets the robot and grid to the origin
    """
    global hist_x_vals, hist_y_vals
    global x_pos_ref, y_pos_ref, vehicle_angle_ref
    global circle1, this_rect

    plot_grid(0,0)
    plot_robot(0, 0, math.radians(0))
    plot_arrow(0, 0, math.radians(0))

    hist_x_vals.clear()
    hist_y_vals.clear()

    x_pos_ref = 0.0
    y_pos_ref = 0.0
    vehicle_angle_ref = 0.0

    hist_x_vals.append(0)
    hist_y_vals.append(0)

    # Remove circle1 if it exists on the plot
    try:
        circle1.set_radius(0)
    except:
        pass

    # Remove this_rect if it exists on the plot
    try:
        this_rect.remove()
    except:
        pass

    # Vehicle position display
    veh_pos_label = tk.Label(window, 
        text = ("Vehicle Position"))
    veh_pos_label.grid(row=2, column=4, padx=5)

    # Add new position values to display window
    veh_x_pos_label = tk.Label(window, 
        text = ("X: " + str(round(0.0,3)) + " feet"))
    veh_x_pos_label.grid(row=3, column=4, padx=5)

    veh_y_pos_label = tk.Label(window, 
        text = ("Y: " + str(round(0.0,3)) + " feet"))
    veh_y_pos_label.grid(row=4, column=4, padx=5)

    veh_orientation_label = tk.Label(window, 
        text = ("Orientation: " + str(
        round(0.0,3)) + " degrees"))
    veh_orientation_label.grid(row=5, column=4, padx=5)

    # Wheel Rotation Rates display
    wheel_rot_rates_label = tk.Label(window, 
        text = ("Wheel Rotation Rates"))
    wheel_rot_rates_label.grid(row=6, column=4, padx=5)

    # Add new rotation rates to display window
    wheel_1_rot_label = tk.Label(window, 
        text = ("Wheel 1: " + str(round(0.0,3)) + " degrees/s"))
    wheel_1_rot_label.grid(row=7, column=4, padx=5)

    wheel_2_rot_label = tk.Label(window, 
        text = ("Wheel 2: " + str(round(0.0,3)) + " degrees/s"))
    wheel_2_rot_label.grid(row=8, column=4, padx=5)

    wheel_3_rot_label = tk.Label(window, 
        text = ("Wheel 3: " + str(round(0.0,3)) + " degrees/s"))
    wheel_3_rot_label.grid(row=9, column=4, padx=5)

    wheel_4_rot_label = tk.Label(window, 
        text = ("Wheel 4: " + str(round(0.0,3)) + " degrees/s"))
    wheel_4_rot_label.grid(row=10, column=4, padx=5)

    # Robot Velocity Display
    robot_velocity_label = tk.Label(window, 
        text = ("Robot Velocity (Local)"))
    robot_velocity_label.grid(row=11, column=4, padx=5)

    robot_velocity_x_label = tk.Label(window, 
        text = ("Velocity X: " + str(round(0.0,3)) + " ft/s"))
    robot_velocity_x_label.grid(row=12, column=4, padx=5)

    robot_velocity_y_label = tk.Label(window, 
        text = ("Velocity Y: " + str(round(0.0,3)) + " ft/s"))
    robot_velocity_y_label.grid(row=13, column=4, padx=5)

def stop():
    """
    This method stops the current processes.
    """
    global running_control_robot_body
    global running_control_robot_wheels
    running_control_robot_body = False
    running_control_robot_wheels = False

def turn_off_tick_labels():
    """
    Turn off the tick labels if desired.
    """
    ax.set_yticklabels([])
    ax.set_xticklabels([])

##################### Control Methods #####################
def start_control_robot_body():
    """
    This method starts the control_robot_body method
    """
    global running_control_robot_body
    running_control_robot_body = True

def control_robot_body(direction = 45.0, speed = 2.5, 
                       rotation_rate = 0.0):
    """
    The user can specify the direction, speed, and rotation
    rate of the vehicle reference point.
    @param direction: The direction of the vehicle 
        reference point in degrees, measured from the 
        vehicle's positive x-axis
    @param speed: The magnitude of the velocity in ft/sec
    @param rotation_rate: Rotation rate in degrees/sec, 
        going in the counter-clockwise direction
    """
    global hist_x_vals, hist_y_vals
    global x_pos_ref, y_pos_ref, vehicle_angle_ref
 
    if(running_control_robot_body):
        # Time interval in seconds
        dt = 0.25

        # Method will not run if speed entered is >15 ft/s
        if (is_too_fast(speed)):
            # Error Messages
            print("Error: Maximum speed is 15 " +
	            "ft/s.\nPlease increase the deadline.\n\n" +
	        "Speed = " + str(speed) + " ft/s")

            error_label = tk.Label(window, 
                text = ("Error Log\n ("+ str(time.strftime(
                "%Y-%m-%d %H:%M:%S", time.localtime(
                ))) + ")\n Maximum speed is 15 " +
                "ft/s.\nPlease reduce the speed.\n" +
                "Speed = " + str(speed) + " ft/s"))
            error_label.grid(row=14, column=4, 
                padx=5, pady=25)
            stop()

    if(running_control_robot_body):

        x_initial = x_pos_ref
        y_initial = y_pos_ref
        vehicle_angle_ref_initial = vehicle_angle_ref

        # Convert direction (local) into direction (global)
        direction_global = direction + vehicle_angle_ref

        # Calculate velocity in the x-direction in the
        # global reference frame
        v_x_global = (speed) * math.cos(
            math.radians(direction_global))

        # Calculate velocity in the y-direction in the
        # global reference frame
        v_y_global = (speed) * math.sin(
            math.radians(direction_global))

        # Determine the new x-coordinate of the vehicle
        # reference point
        x_pos_ref = x_initial + (v_x_global * dt)

        # Determine the new y-coordinate of the vehicle
        # reference point
        y_pos_ref = y_initial + (v_y_global * dt)

        # Determine the new orientation of the vehicle
        # reference point
        vehicle_angle_ref = vehicle_angle_ref_initial + (
            rotation_rate * dt)

        # Reposition grid if we are close to the edge
        if (is_close_to_edge(x_pos_ref, y_pos_ref)):
            plot_grid(x_pos_ref, y_pos_ref)

        # Update user-defined path
        plot_line(x_initial, 
            y_initial, math.radians(direction_global))
        plt.pause(dt)

        # Move robot to new position
        plot_robot(x_pos_ref,y_pos_ref,math.radians(
            vehicle_angle_ref))
        plot_arrow(x_pos_ref,y_pos_ref,math.radians(
            vehicle_angle_ref))

        # Update path traveled by robot
        hist_x_vals.append(x_pos_ref)
        hist_y_vals.append(y_pos_ref)
        plot_path_traveled(hist_x_vals, hist_y_vals)

        # Calculate velocity in the x-direction in the
        # local reference frame
        v_x_local = (speed) * math.cos(
            math.radians(direction))

        # Calculate velocity in the y-direction in the
        # local reference frame
        v_y_local = (speed) * math.sin(
            math.radians(direction))

        # Update wheel rotation rates
        rot1, rot2, rot3, rot4 = get_wheel_rot_rates(
            v_x_local, v_y_local, rotation_rate)

        # Vehicle position display
        print("VEHICLE POSITION")
        print("X: " + str(x_pos_ref) + " feet")
        print("Y: " + str(y_pos_ref) + " feet")
        print("Orientation: " + str(vehicle_angle_ref) +
                      " degrees\n")

        # Wheel rate display
        print("WHEEL ROTATION RATES")
        print("Wheel 1: " + str(rot1) + " degrees/s")
        print("Wheel 2: " + str(rot2) + " degrees/s")
        print("Wheel 3: " + str(rot3) + " degrees/s")
        print("Wheel 4: " + str(rot4) + " degrees/s\n")

        # Robot velocity display
        print("ROBOT VELOCITY (LOCAL)")
        print("V_X: " + str(v_x_local) + " ft/s")
        print("V_Y: " + str(v_y_local) + " ft/s\n\n")


        # Vehicle position display
        veh_pos_label = tk.Label(window, 
            text = ("Vehicle Position"))
        veh_pos_label.grid(row=2, column=4, padx=5)

        # Add new position values to display window
        veh_x_pos_label = tk.Label(window, 
            text = ("X: " + str(round(x_pos_ref,3)) + " feet"))
        veh_x_pos_label.grid(row=3, column=4, padx=5)

        veh_y_pos_label = tk.Label(window, 
            text = ("Y: " + str(round(y_pos_ref,3)) + " feet"))
        veh_y_pos_label.grid(row=4, column=4, padx=5)

        veh_orientation_label = tk.Label(window, 
            text = ("Orientation: " + str(
            round(vehicle_angle_ref,3)) + " degrees"))
        veh_orientation_label.grid(row=5, column=4, padx=5)

        # Wheel Rotation Rates display
        wheel_rot_rates_label = tk.Label(window, 
            text = ("Wheel Rotation Rates"))
        wheel_rot_rates_label.grid(row=6, column=4, padx=5)

        # Add new rotation rates to display window
        wheel_1_rot_label = tk.Label(window, 
            text = ("Wheel 1: " + str(round(rot1,3)) + " degrees/s"))
        wheel_1_rot_label.grid(row=7, column=4, padx=5)

        wheel_2_rot_label = tk.Label(window, 
            text = ("Wheel 2: " + str(round(rot2,3)) + " degrees/s"))
        wheel_2_rot_label.grid(row=8, column=4, padx=5)

        wheel_3_rot_label = tk.Label(window, 
            text = ("Wheel 3: " + str(round(rot3,3)) + " degrees/s"))
        wheel_3_rot_label.grid(row=9, column=4, padx=5)

        wheel_4_rot_label = tk.Label(window, 
            text = ("Wheel 4: " + str(round(rot4,3)) + " degrees/s"))
        wheel_4_rot_label.grid(row=10, column=4, padx=5)

        # Robot Velocity Display
        robot_velocity_label = tk.Label(window, 
            text = ("Robot Velocity (Local)"))
        robot_velocity_label.grid(row=11, column=4, padx=5)

        robot_velocity_x_label = tk.Label(window, 
            text = ("Velocity X: " + str(round(v_x_local,3)) + " ft/s"))
        robot_velocity_x_label.grid(row=12, column=4, padx=5)

        robot_velocity_y_label = tk.Label(window, 
            text = ("Velocity Y: " + str(round(v_y_local,3)) + " ft/s"))
        robot_velocity_y_label.grid(row=13, column=4, padx=5)

        plt.pause(dt)    
  
    window.after(1, lambda: control_robot_body(
        float(direction_entry.get()),
        float(speed_entry.get()),
        float(rot_rate_entry.get())))

def start_control_robot_wheels():
    """
    This method starts the control_robot_wheels method
    """
    global running_control_robot_wheels
    running_control_robot_wheels = True

def stop_control_robot_wheels(event):
    """
    This method stops the control_robot_wheels process
    upon either a mouse click on the plot or a 
    keyboard button push.
    """
    global running_control_robot_wheels
    running_control_robot_wheels = False
        
def control_robot_wheels(rot1 = -7.0, rot2 = 0.0, 
    rot3 = 0.0, rot4 = -7.0):
    """
    The user shall be able to specify the rotation rate of
    each wheel.
    @param rot1: Rotation rate of wheel 1 in degrees/sec
    @param rot2: Rotation rate of wheel 2 in degrees/sec
    @param rot3: Rotation rate of wheel 3 in degrees/sec
    @param rot4: Rotation rate of wheel 4 in degrees/sec
    """
    global hist_x_vals, hist_y_vals
    global x_pos_ref, y_pos_ref, vehicle_angle_ref

    # In this mode alone, any mouse or keyboard button push
    # will be considered a stop command.
    # Must click directly on the plot.
    fig.canvas.mpl_connect('button_press_event', 
        stop_control_robot_wheels)
    fig.canvas.mpl_connect('key_press_event', 
        stop_control_robot_wheels)

    if(running_control_robot_wheels):

        # Time interval in seconds
        dt = 0.25

        # Get current robot motion
        v_x_local, v_y_local, rotation_rate = (
            get_robot_motion_values(rot1, rot2, rot3, rot4))

        speed = get_velocity_magnitude(v_x_local, v_y_local)

        # Method will not run if speed entered is >15 ft/s
        if (is_too_fast(speed)):
            # Error Messages
            print("Error: Maximum speed is 15 " +
	            "ft/s.\nPlease increase the deadline.\n\n" +
	            "Speed = " + str(speed) + " ft/s")
            error_label = tk.Label(window, 
                text = ("Error Log\n ("+ str(time.strftime(
                "%Y-%m-%d %H:%M:%S", time.localtime(
                ))) + ")\n Maximum speed is 15 " +
                "ft/s.\nPlease reduce the wheel rotation rates.\n" +
                "Speed = " + str(speed) + " ft/s"))
            error_label.grid(row=14, column=4, 
                padx=5, pady=25)
            stop()

    if(running_control_robot_wheels):

        v_x_global, v_y_global = (
            convert_local_velocity_to_global(
            v_x_local, v_y_local))

        # Calculate the direction of travel of the vehicle 
        # reference point
        direction_global = math.degrees(math.atan2(
            v_y_global,v_x_global))

        x_initial = x_pos_ref
        y_initial = y_pos_ref
        vehicle_angle_ref_initial = vehicle_angle_ref

        # Determine the new x-coordinate of the vehicle
        # reference point
        x_pos_ref = x_initial + (v_x_global * dt)

        # Determine the new y-coordinate of the vehicle
        # reference point
        y_pos_ref = y_initial + (v_y_global * dt)

        # Determine the new orientation of the vehicle
        # reference point
        vehicle_angle_ref = vehicle_angle_ref_initial + (
            rotation_rate * dt)

        # Reposition grid if we are close to the edge
        if (is_close_to_edge(x_pos_ref, y_pos_ref)):
            plot_grid(x_pos_ref, y_pos_ref)

        # Update user-defined path
        plot_line(x_initial, y_initial, math.radians(
            direction_global))
        plt.pause(dt)

        # Move robot to new position
        plot_robot(x_pos_ref,y_pos_ref,math.radians(
            vehicle_angle_ref))
        plot_arrow(x_pos_ref,y_pos_ref,math.radians(
            vehicle_angle_ref))

        # Update path traveled by robot
        hist_x_vals.append(x_pos_ref)
        hist_y_vals.append(y_pos_ref)
        plot_path_traveled(hist_x_vals, hist_y_vals)

        # Vehicle position display
        print("VEHICLE POSITION")
        print("X: " + str(x_pos_ref) + " feet")
        print("Y: " + str(y_pos_ref) + " feet")
        print("Orientation: " + str(vehicle_angle_ref) +
                      " degrees\n")

        # Wheel rate display
        print("WHEEL ROTATION RATES")
        print("Wheel 1: " + str(rot1) + " degrees/s")
        print("Wheel 2: " + str(rot2) + " degrees/s")
        print("Wheel 3: " + str(rot3) + " degrees/s")
        print("Wheel 4: " + str(rot4) + " degrees/s\n")

        # Robot velocity display
        print("ROBOT VELOCITY (LOCAL)")
        print("V_X: " + str(v_x_local) + " ft/s")
        print("V_Y: " + str(v_y_local) + " ft/s\n\n")

        # Vehicle position display
        veh_pos_label = tk.Label(window, 
            text = ("Vehicle Position"))
        veh_pos_label.grid(row=2, column=4, padx=5)

        # Add new position values to display window
        veh_x_pos_label = tk.Label(window, 
            text = ("X: " + str(round(x_pos_ref,3)) + " feet"))
        veh_x_pos_label.grid(row=3, column=4, padx=5)

        veh_y_pos_label = tk.Label(window, 
            text = ("Y: " + str(round(y_pos_ref,3)) + " feet"))
        veh_y_pos_label.grid(row=4, column=4, padx=5)

        veh_orientation_label = tk.Label(window, 
            text = ("Orientation: " + str(
            round(vehicle_angle_ref,3)) + " degrees"))
        veh_orientation_label.grid(row=5, column=4, padx=5)

        # Wheel Rotation Rates display
        wheel_rot_rates_label = tk.Label(window, 
            text = ("Wheel Rotation Rates"))
        wheel_rot_rates_label.grid(row=6, column=4, padx=5)

        # Add new rotation rates to display window
        wheel_1_rot_label = tk.Label(window, 
            text = ("Wheel 1: " + str(round(rot1,3)) + " degrees/s"))
        wheel_1_rot_label.grid(row=7, column=4, padx=5)

        wheel_2_rot_label = tk.Label(window, 
            text = ("Wheel 2: " + str(round(rot2,3)) + " degrees/s"))
        wheel_2_rot_label.grid(row=8, column=4, padx=5)

        wheel_3_rot_label = tk.Label(window, 
            text = ("Wheel 3: " + str(round(rot3,3)) + " degrees/s"))
        wheel_3_rot_label.grid(row=9, column=4, padx=5)

        wheel_4_rot_label = tk.Label(window, 
            text = ("Wheel 4: " + str(round(rot4,3)) + " degrees/s"))
        wheel_4_rot_label.grid(row=10, column=4, padx=5)

        # Robot Velocity Display
        robot_velocity_label = tk.Label(window, 
            text = ("Robot Velocity (Local)"))
        robot_velocity_label.grid(row=11, column=4, padx=5)

        robot_velocity_x_label = tk.Label(window, 
            text = ("Velocity X: " + str(round(v_x_local,3)) + " ft/s"))
        robot_velocity_x_label.grid(row=12, column=4, padx=5)

        robot_velocity_y_label = tk.Label(window, 
            text = ("Velocity Y: " + str(round(v_y_local,3)) + " ft/s"))
        robot_velocity_y_label.grid(row=13, column=4, padx=5)

        plt.pause(dt)  

    window.after(1, lambda: control_robot_wheels(
        float(rot_rate1_entry.get()), 
        float(rot_rate2_entry.get()), 
        float(rot_rate3_entry.get()), 
        float(rot_rate4_entry.get())))

def point_execution_no_wp(x_final = 5.0, y_final = 5.0, 
        orientation_final = -45.0, deadline =  5.0):
    """
    The user can specify a straight line path to an
    end point, including a desired end orientation.
    No waypoints between start and endpoints.
    @param x_final: x-coordinate of the desired end point
    @param y_final: y-coordinate of the desired end point
    @param orientation_final: Desired end orientation in
        degrees, going counter-clockwise from the x-axis
        of the global reference frame
    @param deadline: Time taken to get to end point in
        seconds
    """
    global hist_x_vals, hist_y_vals
    global x_pos_ref, y_pos_ref, vehicle_angle_ref

    distance = get_distance_btw_points(x_pos_ref,y_pos_ref,
        x_final,y_final)

    speed = get_speed(distance,deadline)

    # Method will not run if speed entered is >15 ft/s
    if (is_too_fast(speed)):
        # Error Messages
        print("Error: Maximum speed is 15 " +
	        "ft/s.\nPlease increase the deadline.\n\n" +
	        "Speed = " + str(speed) + " ft/s")
        error_label = tk.Label(window, 
            text = ("Error Log\n ("+ str(time.strftime(
            "%Y-%m-%d %H:%M:%S", time.localtime(
            ))) + ")\n Maximum speed is 15 " +
            "ft/s.\nPlease increase the deadline.\n" +
            "Speed = " + str(speed) + " ft/s"))
        error_label.grid(row=14, column=4, 
            padx=5, pady=25)
        stop()

    # Calculate rotation rate in degrees/sec
    rotation_rate = (orientation_final - (
        vehicle_angle_ref))/(deadline)

    # Number of frames to reach deadline
    no_of_frames = 5

    # Time interval in seconds
    dt = (deadline / no_of_frames)

    # Calculate the direction of travel of the vehicle 
    # reference point in degrees
    direction_global = math.degrees(math.atan2(
        (y_final - y_pos_ref),(x_final - x_pos_ref)))

    # Plot user-defined path
    plt.plot([x_pos_ref, x_final], [y_pos_ref, y_final], 
             color='red', linestyle='-', linewidth=2)  

    plt.pause(0.25)

    # Calculate velocity in the x-direction in the
    # global reference frame
    v_x_global = (speed) * math.cos(
        math.radians(direction_global))

    # Calculate velocity in the y-direction in the
    # global reference frame
    v_y_global = (speed) * math.sin(
        math.radians(direction_global))

    for num in range(no_of_frames):

        x_initial = x_pos_ref
        y_initial = y_pos_ref
        vehicle_angle_ref_initial = vehicle_angle_ref

        # Determine the new x-coordinate of the vehicle
        # reference point
        x_pos_ref = x_initial + (v_x_global * dt)

        # Determine the new y-coordinate of the vehicle
        # reference point
        y_pos_ref = y_initial + (v_y_global * dt)

        # Determine the new orientation of the vehicle
        # reference point
        vehicle_angle_ref = vehicle_angle_ref_initial + (
            rotation_rate * dt)

        plt.pause(dt)
       
        # Reposition grid if we are close to the edge
        if (is_close_to_edge(x_pos_ref, y_pos_ref)):
            plot_grid(x_pos_ref, y_pos_ref)

        # Move robot to new position
        plot_robot(x_pos_ref,y_pos_ref,math.radians(
            vehicle_angle_ref))
        plot_arrow(x_pos_ref,y_pos_ref,math.radians(
            vehicle_angle_ref))

        # Update path traveled by robot
        hist_x_vals.append(x_pos_ref)
        hist_y_vals.append(y_pos_ref)
        plot_path_traveled(hist_x_vals, hist_y_vals)

        # Convert direction (global) into direction (local)
        direction = direction_global - vehicle_angle_ref
        
        # Calculate velocity in the x-direction in the
        # local reference frame
        v_x_local = (speed) * math.cos(
            math.radians(direction))

        # Calculate velocity in the y-direction in the
        # local reference frame
        v_y_local = (speed) * math.sin(
            math.radians(direction))

        # Update wheel rotation rates
        rot1, rot2, rot3, rot4 = get_wheel_rot_rates(
            v_x_local, v_y_local, rotation_rate)

        # Update user-defined path
        plt.plot([x_pos_ref, x_final], [y_pos_ref,y_final], 
             color='red', linestyle='-', linewidth=2)

        # Vehicle position display
        print("VEHICLE POSITION")
        print("X: " + str(x_pos_ref) + " feet")
        print("Y: " + str(y_pos_ref) + " feet")
        print("Orientation: " + str(vehicle_angle_ref) +
                      " degrees\n")

        # Wheel rate display
        print("WHEEL ROTATION RATES")
        print("Wheel 1: " + str(rot1) + " degrees/s")
        print("Wheel 2: " + str(rot2) + " degrees/s")
        print("Wheel 3: " + str(rot3) + " degrees/s")
        print("Wheel 4: " + str(rot4) + " degrees/s\n")

        # Robot velocity display
        print("ROBOT VELOCITY (LOCAL)")
        print("V_X: " + str(v_x_local) + " ft/s")
        print("V_Y: " + str(v_y_local) + " ft/s\n\n")


        # Vehicle position display
        veh_pos_label = tk.Label(window, 
            text = ("Vehicle Position"))
        veh_pos_label.grid(row=2, column=4, padx=5)

        # Add new position values to display window
        veh_x_pos_label = tk.Label(window, 
            text = ("X: " + str(round(x_pos_ref,3)) + " feet"))
        veh_x_pos_label.grid(row=3, column=4, padx=5)

        veh_y_pos_label = tk.Label(window, 
            text = ("Y: " + str(round(y_pos_ref,3)) + " feet"))
        veh_y_pos_label.grid(row=4, column=4, padx=5)

        veh_orientation_label = tk.Label(window, 
            text = ("Orientation: " + str(
            round(vehicle_angle_ref,3)) + " degrees"))
        veh_orientation_label.grid(row=5, column=4, padx=5)

        # Wheel Rotation Rates display
        wheel_rot_rates_label = tk.Label(window, 
            text = ("Wheel Rotation Rates"))
        wheel_rot_rates_label.grid(row=6, column=4, padx=5)

        # Add new rotation rates to display window
        wheel_1_rot_label = tk.Label(window, 
            text = ("Wheel 1: " + str(round(rot1,3)) + " degrees/s"))
        wheel_1_rot_label.grid(row=7, column=4, padx=5)

        wheel_2_rot_label = tk.Label(window, 
            text = ("Wheel 2: " + str(round(rot2,3)) + " degrees/s"))
        wheel_2_rot_label.grid(row=8, column=4, padx=5)

        wheel_3_rot_label = tk.Label(window, 
            text = ("Wheel 3: " + str(round(rot3,3)) + " degrees/s"))
        wheel_3_rot_label.grid(row=9, column=4, padx=5)

        wheel_4_rot_label = tk.Label(window, 
            text = ("Wheel 4: " + str(round(rot4,3)) + " degrees/s"))
        wheel_4_rot_label.grid(row=10, column=4, padx=5)

        # Robot Velocity Display
        robot_velocity_label = tk.Label(window, 
            text = ("Robot Velocity (Local)"))
        robot_velocity_label.grid(row=11, column=4, padx=5)

        robot_velocity_x_label = tk.Label(window, 
            text = ("Velocity X: " + str(round(v_x_local,3)) + " ft/s"))
        robot_velocity_x_label.grid(row=12, column=4, padx=5)

        robot_velocity_y_label = tk.Label(window, 
            text = ("Velocity Y: " + str(round(v_y_local,3)) + " ft/s"))
        robot_velocity_y_label.grid(row=13, column=4, padx=5)

def point_execution_with_wp(x_wp1 = 5.0, y_wp1 = 5.0, 
        x_wp2 = 5.0, y_wp2 = -5.0, 
        x_wp3 = -5.0, y_wp3 = 5.0,
        x_final = -5.0, y_final = -5.0, 
        orientation_final = 90.0, deadline = 5.0):
    """
    The user can specify a straight line path to an
    end point, including a desired end orientation.
    Waypoints between start and endpoints can be specified.
    @param x_wp1: x-coordinate of waypoint 1
    @param y_wp1: y-coordinate of waypoint 1
    @param x_wp2: x-coordinate of waypoint 2
    @param y_wp2: y-coordinate of waypoint 2
    @param x_wp3: x-coordinate of waypoint 3
    @param y_wp3: y-coordinate of waypoint 3
    @param x_final: x-coordinate of end point
    @param y_final: y-coordinate of end point
    @param orientation_final: Desired end orientation in 
        degrees.
    @param deadline float: Time taken to get from current 
        point to end point in seconds.
    """
    global hist_x_vals, hist_y_vals
    global x_pos_ref, y_pos_ref, vehicle_angle_ref

    # List of all x and y values along the path
    x_list = [x_pos_ref, x_wp1, x_wp2, x_wp3, x_final]
    y_list = [y_pos_ref, y_wp1, y_wp2, y_wp3, y_final]

    # Number of legs on the path
    no_of_legs = len(x_list) - 1

    # Keep track of the distances of each leg of the trip
    distance_list = []

    # Calculate the distance of each leg of the trip in ft
    for idx in range(no_of_legs):
        distance_list.append(get_distance_btw_points(
            x_list[idx],y_list[idx],x_list[idx + 1],y_list[
            idx + 1]))

    total_distance = sum(distance_list)

    speed = get_speed(total_distance,deadline)

    # Method will not run if speed entered is >15 ft/s
    if (is_too_fast(speed)):
        print("Error: Maximum speed is 15 " +
	        "ft/s.\nPlease increase the deadline.\n\n" +
	        "Speed = " + str(speed) + " ft/s")
        error_label = tk.Label(window, 
            text = ("Error Log\n ("+ str(time.strftime(
            "%Y-%m-%d %H:%M:%S", time.localtime(
            ))) + ")\n Maximum speed is 15 " +
            "ft/s.\nPlease increase the deadline.\n" +
            "Speed = " + str(speed) + " ft/s"))
        error_label.grid(row=14, column=4, 
            padx=5, pady=25)
        stop()

    # Calculate rotation rate in degrees/sec
    rotation_rate = (orientation_final - (
        vehicle_angle_ref))/(deadline)

    # Calculate the duration of each leg in seconds
    deadline_list = []

    for idx in range(no_of_legs):
        deadline_list.append((distance_list[
            idx]/total_distance) * deadline)

    # Number of frames per leg
    no_of_frames_per_leg = 5

    # Time intervals in seconds for each leg
    dt_list = []

    for idx in range(no_of_legs):
        dt_list.append(deadline_list[idx]/(
            no_of_frames_per_leg))

    # Move through each leg of the trip
    for idx in range(no_of_legs):

        # Number of frames per leg
        no_of_frames_per_leg = 5

        # Calculate the direction of travel of the vehicle 
        # reference point in degrees
        direction_global = math.degrees(math.atan2(
            (y_list[idx + 1] - y_list[idx]),(x_list[
            idx + 1] - x_list[idx])))

        # Calculate velocity in the x-direction in the
        # global reference frame
        v_x_global = (speed) * math.cos(
            math.radians(direction_global))

        # Calculate velocity in the y-direction in the
        # global reference frame
        v_y_global = (speed) * math.sin(
            math.radians(direction_global))

        for num in range(no_of_frames_per_leg):

            x_initial = x_pos_ref
            y_initial = y_pos_ref
            vehicle_angle_ref_initial = vehicle_angle_ref

            # Determine the new x-coordinate of the vehicle
            # reference point
            x_pos_ref = x_initial + (v_x_global * dt_list[
                idx])

            # Determine the new y-coordinate of the vehicle
            # reference point
            y_pos_ref = y_initial + (v_y_global * dt_list[
                idx])

            # Determine the new orientation of the vehicle
            # reference point
            vehicle_angle_ref = (
                vehicle_angle_ref_initial + (
                rotation_rate * dt_list[idx]))

            # Reposition grid if we are close to the edge
            if (is_close_to_edge(x_pos_ref, y_pos_ref)):
                plot_grid(x_pos_ref, y_pos_ref)

            # Move robot to new position
            plot_robot(x_pos_ref,y_pos_ref,math.radians(
                vehicle_angle_ref))
            plot_arrow(x_pos_ref,y_pos_ref,math.radians(
                vehicle_angle_ref))

            # Update path traveled by robot
            hist_x_vals.append(x_pos_ref)
            hist_y_vals.append(y_pos_ref)
            plot_path_traveled(hist_x_vals, hist_y_vals)

            # Convert direction (global) into direction (local)
            direction = direction_global - vehicle_angle_ref

            # Calculate velocity in the x-direction in the
            # local reference frame
            v_x_local = (speed) * math.cos(
                math.radians(direction))

            # Calculate velocity in the y-direction in the
            # local reference frame
            v_y_local = (speed) * math.sin(
                math.radians(direction))

            # Update wheel rotation rates
            rot1, rot2, rot3, rot4 = get_wheel_rot_rates(
                v_x_local, v_y_local, rotation_rate)

            # Vehicle position display
            print("VEHICLE POSITION")
            print("X: " + str(x_pos_ref) + " feet")
            print("Y: " + str(y_pos_ref) + " feet")
            print("Orientation: " + str(vehicle_angle_ref) +
                          " degrees\n")

            # Wheel rate display
            print("WHEEL ROTATION RATES")
            print("Wheel 1: " + str(rot1) + " degrees/s")
            print("Wheel 2: " + str(rot2) + " degrees/s")
            print("Wheel 3: " + str(rot3) + " degrees/s")
            print("Wheel 4: " + str(rot4) + " degrees/s\n")

            # Robot velocity display
            print("ROBOT VELOCITY (LOCAL)")
            print("V_X: " + str(v_x_local) + " ft/s")
            print("V_Y: " + str(v_y_local) + " ft/s\n\n")

            # Vehicle position display
            veh_pos_label = tk.Label(window, 
                text = ("Vehicle Position"))
            veh_pos_label.grid(row=2, column=4, padx=5)

            # Add new position values to display window
            veh_x_pos_label = tk.Label(window, 
                text = ("X: " + str(round(x_pos_ref,3)) + " feet"))
            veh_x_pos_label.grid(row=3, column=4, padx=5)

            veh_y_pos_label = tk.Label(window, 
                text = ("Y: " + str(round(y_pos_ref,3)) + " feet"))
            veh_y_pos_label.grid(row=4, column=4, padx=5)

            veh_orientation_label = tk.Label(window, 
                text = ("Orientation: " + str(
                round(vehicle_angle_ref,3)) + " degrees"))
            veh_orientation_label.grid(row=5, column=4, padx=5)

            # Wheel Rotation Rates display
            wheel_rot_rates_label = tk.Label(window, 
                text = ("Wheel Rotation Rates"))
            wheel_rot_rates_label.grid(row=6, column=4, padx=5)

            # Add new rotation rates to display window
            wheel_1_rot_label = tk.Label(window, 
                text = ("Wheel 1: " + str(round(rot1,3)) + " degrees/s"))
            wheel_1_rot_label.grid(row=7, column=4, padx=5)

            wheel_2_rot_label = tk.Label(window, 
                text = ("Wheel 2: " + str(round(rot2,3)) + " degrees/s"))
            wheel_2_rot_label.grid(row=8, column=4, padx=5)

            wheel_3_rot_label = tk.Label(window, 
                text = ("Wheel 3: " + str(round(rot3,3)) + " degrees/s"))
            wheel_3_rot_label.grid(row=9, column=4, padx=5)

            wheel_4_rot_label = tk.Label(window, 
                text = ("Wheel 4: " + str(round(rot4,3)) + " degrees/s"))
            wheel_4_rot_label.grid(row=10, column=4, padx=5)

            # Robot Velocity Display
            robot_velocity_label = tk.Label(window, 
                text = ("Robot Velocity (Local)"))
            robot_velocity_label.grid(row=11, column=4, padx=5)

            robot_velocity_x_label = tk.Label(window, 
                text = ("Velocity X: " + str(round(v_x_local,3)) + " ft/s"))
            robot_velocity_x_label.grid(row=12, column=4, padx=5)

            robot_velocity_y_label = tk.Label(window, 
                text = ("Velocity Y: " + str(round(v_y_local,3)) + " ft/s"))
            robot_velocity_y_label.grid(row=13, column=4, padx=5)

            # Update user-defined path
            plt.plot([x_pos_ref, x_list[idx + 1]], [
                y_pos_ref,y_list[idx + 1]], color='red', 
                linestyle='-', linewidth=2)  

            plt.pause(dt_list[idx])  

def move_in_a_circle(radius = 2.0, direction = 45.0, 
        deadline = 5.0):
    """
    Vehicle can move in a circle with user-defined 
    radius and inclination (direction of circle center 
    from current vehicle position). 
    @param radius: Radius of the circle in feet
    @param direction: Direction of circle center
        from the vehicle reference point, measured
        from the vehicle's positive x-axis in degrees
    @param deadline float: Time taken to get from current 
        point to end point in seconds.
    """
    global hist_x_vals, hist_y_vals
    global x_pos_ref, y_pos_ref, vehicle_angle_ref
    global circle1

    # Rotation rate of vehicle reference point
    rotation_rate = 0.0

    # Convert direction (local) into direction (global)
    direction_global = direction + vehicle_angle_ref

    # x-coordinate of the center of the circle
    x_center = x_pos_ref + (radius) * math.cos(
        math.radians(direction_global))

    # y-coordinate of the center of the circle
    y_center = y_pos_ref + (radius) * math.sin(
        math.radians(direction_global))

    # Plot the circle path
    circle1 = plt.Circle((x_center, y_center), 
        radius, color="red", fill=False, lw=2)
    ax.add_artist(circle1)

    # Record the angle at which the vehicle reference point
    # is from the center of the circle in degrees
    starting_angle = math.degrees(math.atan2((
        y_pos_ref - y_center),(x_pos_ref - x_center)))

    # Convert negative angles to positive angles
    if starting_angle < 0:
        starting_angle += 360            

    # Calculate the total distance of travel in feet
    total_distance = 2 * math.pi * radius

    speed = get_speed(total_distance,deadline)

    # Method will not run if speed entered is >15 ft/s
    if (is_too_fast(speed)):
        print("Error: Maximum speed is 15 " +
	        "ft/s.\nPlease increase the deadline.\n\n" +
	        "Speed = " + str(speed) + " ft/s")
        error_label = tk.Label(window, 
            text = ("Error Log\n ("+ str(time.strftime(
            "%Y-%m-%d %H:%M:%S", time.localtime(
            ))) + ")\n Maximum speed is 15 " +
            "ft/s.\nPlease increase the deadline.\n" +
            "Speed = " + str(speed) + " ft/s"))
        error_label.grid(row=14, column=4, 
            padx=5, pady=25)
        stop() 

    # Number of individual movements along the circle path
    no_of_circle_legs = 25

    # Number of degrees per circle leg
    no_of_degrees_per_circle_leg = 360.0/no_of_circle_legs

    # Number of seconds per leg
    dt = deadline/no_of_circle_legs

    # Create a list of the angles at which the vehicle 
    # reference point is from the center of the circle
    # for a complete circle path (in degrees)
    angles_list = []

    for angle_count in range(no_of_circle_legs):
        starting_angle += no_of_degrees_per_circle_leg
        angles_list.append(starting_angle)

    # List of all x and y values along the path
    x_list = [x_pos_ref]
    y_list = [y_pos_ref]
    for element in angles_list:
        x_temp = radius * math.cos(math.radians(element))+ (
            x_center)
        y_temp = radius * math.sin(math.radians(element))+ (
            y_center)
        x_list.append(x_temp)
        y_list.append(y_temp)

    ax.add_artist(circle1)
    plt.pause(dt)

    # Move through each leg of the trip
    for idx in range(no_of_circle_legs):

        # Calculate the direction of travel of the vehicle 
        # reference point in degrees
        direction_global = math.degrees(math.atan2(
            (y_list[idx + 1] - y_list[idx]),(x_list[
            idx + 1] - x_list[idx])))
        
        # Calculate velocity in the x-direction in the
        # global reference frame
        v_x_global = (speed) * math.cos(
            math.radians(direction_global))

        # Calculate velocity in the y-direction in the
        # global reference frame
        v_y_global = (speed) * math.sin(
            math.radians(direction_global))

        x_initial = x_pos_ref
        y_initial = y_pos_ref
        vehicle_angle_ref_initial = vehicle_angle_ref

        # Determine the new x-coordinate of the vehicle
        # reference point
        x_pos_ref = x_initial + (v_x_global * dt)

        # Determine the new y-coordinate of the vehicle
        # reference point
        y_pos_ref = y_initial + (v_y_global * dt)

        # Reposition grid if we are close to the edge
        if (is_close_to_edge(x_pos_ref, y_pos_ref)):
            plot_grid(x_pos_ref, y_pos_ref)

        # Move robot to new position
        plot_robot(x_pos_ref,y_pos_ref,math.radians(
            vehicle_angle_ref))
        plot_arrow(x_pos_ref,y_pos_ref,math.radians(
            vehicle_angle_ref))

        # Update path traveled by robot
        hist_x_vals.append(x_pos_ref)
        hist_y_vals.append(y_pos_ref)
        plot_path_traveled(hist_x_vals, hist_y_vals)

        # Convert direction (global) into direction (local)
        direction = direction_global - vehicle_angle_ref

        # Calculate velocity in the x-direction in the
        # local reference frame
        v_x_local = (speed) * math.cos(
            math.radians(direction))

        # Calculate velocity in the y-direction in the
        # local reference frame
        v_y_local = (speed) * math.sin(
            math.radians(direction))

        # Update wheel rotation rates
        rot1, rot2, rot3, rot4 = get_wheel_rot_rates(
            v_x_local, v_y_local, rotation_rate)

        # Vehicle position display
        print("VEHICLE POSITION")
        print("X: " + str(x_pos_ref) + " feet")
        print("Y: " + str(y_pos_ref) + " feet")
        print("Orientation: " + str(vehicle_angle_ref) +
                      " degrees\n")

        # Wheel rate display
        print("WHEEL ROTATION RATES")
        print("Wheel 1: " + str(rot1) + " degrees/s")
        print("Wheel 2: " + str(rot2) + " degrees/s")
        print("Wheel 3: " + str(rot3) + " degrees/s")
        print("Wheel 4: " + str(rot4) + " degrees/s\n")

        # Robot velocity display
        print("ROBOT VELOCITY (LOCAL)")
        print("V_X: " + str(v_x_local) + " ft/s")
        print("V_Y: " + str(v_y_local) + " ft/s\n\n")

        # Vehicle position display
        veh_pos_label = tk.Label(window, 
            text = ("Vehicle Position"))
        veh_pos_label.grid(row=2, column=4, padx=5)

        # Add new position values to display window
        veh_x_pos_label = tk.Label(window, 
            text = ("X: " + str(round(x_pos_ref,3)) + " feet"))
        veh_x_pos_label.grid(row=3, column=4, padx=5)

        veh_y_pos_label = tk.Label(window, 
            text = ("Y: " + str(round(y_pos_ref,3)) + " feet"))
        veh_y_pos_label.grid(row=4, column=4, padx=5)

        veh_orientation_label = tk.Label(window, 
            text = ("Orientation: " + str(
            round(vehicle_angle_ref,3)) + " degrees"))
        veh_orientation_label.grid(row=5, column=4, padx=5)

        # Wheel Rotation Rates display
        wheel_rot_rates_label = tk.Label(window, 
            text = ("Wheel Rotation Rates"))
        wheel_rot_rates_label.grid(row=6, column=4, padx=5)

        # Add new rotation rates to display window
        wheel_1_rot_label = tk.Label(window, 
            text = ("Wheel 1: " + str(round(rot1,3)) + " degrees/s"))
        wheel_1_rot_label.grid(row=7, column=4, padx=5)

        wheel_2_rot_label = tk.Label(window, 
            text = ("Wheel 2: " + str(round(rot2,3)) + " degrees/s"))
        wheel_2_rot_label.grid(row=8, column=4, padx=5)

        wheel_3_rot_label = tk.Label(window, 
            text = ("Wheel 3: " + str(round(rot3,3)) + " degrees/s"))
        wheel_3_rot_label.grid(row=9, column=4, padx=5)

        wheel_4_rot_label = tk.Label(window, 
            text = ("Wheel 4: " + str(round(rot4,3)) + " degrees/s"))
        wheel_4_rot_label.grid(row=10, column=4, padx=5)

        # Robot Velocity Display
        robot_velocity_label = tk.Label(window, 
            text = ("Robot Velocity (Local)"))
        robot_velocity_label.grid(row=11, column=4, padx=5)

        robot_velocity_x_label = tk.Label(window, 
            text = ("Velocity X: " + str(round(v_x_local,3)) + " ft/s"))
        robot_velocity_x_label.grid(row=12, column=4, padx=5)

        robot_velocity_y_label = tk.Label(window, 
            text = ("Velocity Y: " + str(round(v_y_local,3)) + " ft/s"))
        robot_velocity_y_label.grid(row=13, column=4, padx=5)

        plt.pause(dt)  

def move_in_a_rectangle(length_in_ft = 5.0, 
        direction_of_opp_vertex = 25.0, deadline = 5.0):
    """
    Vehicle can move in a rectangle shape consisting of
    user-defined side lengths and inclination (direction of
    opposite vertex from current vehicle position). Vehicle
    will start at the vertex.
    @param length_in_ft: Length of the rectangle in feet
    @param direction_of_opp_vertex: Direction of diagonally
        opposite vertex from current vehicle position in 
        degrees, going counter-clockwise from the vehicle's
        positive x-axis.
    @param deadline float: Time taken to get from current 
        point to end point in seconds.
    """
    global this_rect

    # Get the length of the diagonal of the rectangle
    diagonal_length = abs(length_in_ft/math.cos(
        math.radians(direction_of_opp_vertex)))

    # Get the width of the diagonal of the rectangle
    width_in_ft = abs(diagonal_length * math.sin(
        math.radians(direction_of_opp_vertex)))

    # Convert direction (local) into direction (global)
    direction_global = direction_of_opp_vertex + (
        vehicle_angle_ref)

    # Calculate the interior angle in degrees between 
    # diagonal of rectangle and the side length
    interior_angle = math.degrees(math.acos(
        length_in_ft/diagonal_length))

    # Calculate coordinates for waypoint 1
    x_wp1 = x_pos_ref + (length_in_ft * math.cos(
        math.radians(direction_global - interior_angle)))
    y_wp1 = y_pos_ref + (length_in_ft * math.sin(
        math.radians(direction_global - interior_angle)))
   
    # Calculate coordinates for waypoint 2
    x_wp2 = x_pos_ref + (diagonal_length * math.cos(
        math.radians(direction_global)))
    y_wp2 = y_pos_ref + (diagonal_length * math.sin(
        math.radians(direction_global)))

    # Angle to wp3 as measured counter-clockwise from the
    # global positive x-axis
    wp3_angle = direction_global + (90.0-interior_angle)

    # Calculate coordinates for waypoint 3
    x_wp3 = x_pos_ref + (width_in_ft * math.cos(
        math.radians(wp3_angle)))
    y_wp3 = y_pos_ref + (width_in_ft * math.sin(
        math.radians(wp3_angle)))

    # Plot the rectangle
    this_rect = patches.Rectangle((x_pos_ref,y_pos_ref),
        length_in_ft,width_in_ft, direction_global - interior_angle,
        lw=2,ec='red', fill=False)

    # Add the rectangle to the Axes
    ax.add_patch(this_rect)

    point_execution_with_wp(x_wp1, y_wp1, x_wp2, y_wp2, 
        x_wp3, y_wp3, x_pos_ref, y_pos_ref, 0.0, deadline)

def move_in_a_figure_eight(circle1_radius = 2.0, 
        circle2_radius = 4.0, direction = 90.0, 
        deadline = 20.0):
    """
    Vehicle can move in a figure 8 consisting of two 
    circles with two user-defined, possibly different
    radii and inclination (direction of circle center 
    from current vehicle position). 
    @param circle1_radius: Radius of the 1st circle in feet
    @param circle2_radius: Radius of the 2nd circle in feet
    @param direction: Direction of circle1 center
        from the vehicle reference point, measured
        from the vehicle's positive x-axis in degrees
    @param deadline float: Time taken to get from current 
        point to end point in seconds.
    """
    # Set the deadlines for the completion of each circle
    deadline1 = (circle1_radius/(
        circle1_radius + circle2_radius)) * deadline
    deadline2 = (circle2_radius/(
        circle1_radius + circle2_radius)) * deadline

    # Set the directions of each circle from the vehicle
    # reference point
    direction1 = direction
    direction2 = direction + 180.0

    # Move in a figure 8 pattern - 1st circle
    move_in_a_circle(circle1_radius,direction1,deadline1)

    # Clear out the first circle from the plot
    circle1.set_radius(0)

    # Move in a figure 8 pattern - 2nd circle
    move_in_a_circle(circle2_radius,direction2,deadline2)

######################## Main Code ########################
# Initiate the simulation environment (i.e. grid)
plt.ion() # Turn on interactive mode
fig = plt.figure() # Create a new blank canvas
ax = fig.gca() # Get the current axes
reset()

####################### Tkinter GUI #######################
# Set the window title
window.title("Omnidirectional Mecanum-Wheeled Robot " +
            "(Author: Addison Sears-Collins)")

# Create user-interface
stop_button = tk.Button(window, text = "Stop", command=stop)
stop_button.grid(row=0, column=1, columnspan=2, pady=3, 
    ipadx=100)

reset_button = tk.Button(window, text = "Reset", command=reset) 
reset_button.grid(row=1, column=1, columnspan=2, pady=3, 
    ipadx=100)

control_robot_body_button = tk.Button(window, 
    text = "Control Robot Body", 
    command=start_control_robot_body) 
control_robot_body_button.grid(row=2, column=0, 
    columnspan=2, padx=5, pady=5)
window.after(1, lambda: control_robot_body(
    float(direction_entry.get()),
    float(speed_entry.get()),
    float(rot_rate_entry.get())))

direction_label = tk.Label(window, 
    text = "Direction (degrees)")
direction_label.grid(row=3, column=0, padx=5)
direction_entry = tk.Entry(window)
direction_entry.grid(row=3, column=1, padx=5)
direction_entry.insert(0, 45.0)

speed_label = tk.Label(window, text = "Speed (ft/s)")
speed_label.grid(row=4, column=0, padx=5)
speed_entry = tk.Entry(window)
speed_entry.grid(row=4, column=1, padx=5)
speed_entry.insert(0, 2.5)

rot_rate_label = tk.Label(window, 
    text = "Rotation Rate (degrees/s)")
rot_rate_label.grid(row=5, column=0, padx=5)
rot_rate_entry = tk.Entry(window)
rot_rate_entry.grid(row=5, column=1, padx=5)
rot_rate_entry.insert(0, 0.0)

control_robot_wheels_button = tk.Button(window, 
    text = "Control Robot Wheels",
    command=start_control_robot_wheels) 
control_robot_wheels_button.grid(row=6, column=0, 
    columnspan=2, padx=5, pady=5)
window.after(1, lambda: control_robot_wheels(
    float(rot_rate1_entry.get()), 
    float(rot_rate2_entry.get()), 
    float(rot_rate3_entry.get()), 
    float(rot_rate4_entry.get())))

rot_rate1_label = tk.Label(window, 
    text = "Rotation Rate 1 (degrees/s)")
rot_rate1_label.grid(row=7, column=0, padx=5)
rot_rate1_entry = tk.Entry(window)
rot_rate1_entry.grid(row=7, column=1, padx=5)
rot_rate1_entry.insert(0, -7.0)

rot_rate2_label = tk.Label(window, 
    text = "Rotation Rate 2 (degrees/s)")
rot_rate2_label.grid(row=8, column=0, padx=5)
rot_rate2_entry = tk.Entry(window)
rot_rate2_entry.grid(row=8, column=1, padx=5)
rot_rate2_entry.insert(0, 0.0)

rot_rate3_label = tk.Label(window, 
    text = "Rotation Rate 3 (degrees/s)")
rot_rate3_label.grid(row=9, column=0, padx=5)
rot_rate3_entry = tk.Entry(window)
rot_rate3_entry.grid(row=9, column=1, padx=5)
rot_rate3_entry.insert(0, 0.0)

rot_rate4_label = tk.Label(window, 
    text = "Rotation Rate 4 (degrees/s)")
rot_rate4_label.grid(row=10, column=0, padx=5)
rot_rate4_entry = tk.Entry(window)
rot_rate4_entry.grid(row=10, column=1, padx=5)
rot_rate4_entry.insert(0, -7.0)

point_exec_no_wp_button = tk.Button(window, 
    text = "Point Execution (No Waypoints)",
    command=lambda: point_execution_no_wp(
        float(destination_x_entry.get()), 
        float(destination_y_entry.get()), 
        float(destination_orientation_entry.get()), 
        float(deadline_entry.get()))) 
point_exec_no_wp_button.grid(row=11, 
    column=0, columnspan=2, padx=5, pady=5)

destination_x_label = tk.Label(window, 
    text = "Destination X (ft)")
destination_x_label.grid(row=12, column=0, padx=5)
destination_x_entry = tk.Entry(window)
destination_x_entry.grid(row=12, column=1, padx=5)
destination_x_entry.insert(0, 5.0)

destination_y_label = tk.Label(window, 
    text = "Destination Y (ft)")
destination_y_label.grid(row=13, column=0, padx=5)
destination_y_entry = tk.Entry(window)
destination_y_entry.grid(row=13, column=1, padx=5)
destination_y_entry.insert(0, 5.0)

destination_orientation_label = tk.Label(window, 
    text = "Destination Orientation (degrees)")
destination_orientation_label.grid(row=14, column=0,padx=5)
destination_orientation_entry = tk.Entry(window)
destination_orientation_entry.grid(row=14, column=1,padx=5)
destination_orientation_entry.insert(0, -45.0)

deadline_label = tk.Label(window, 
    text = "Deadline (seconds)")
deadline_label.grid(row=15, column=0, padx=5)
deadline_entry = tk.Entry(window)
deadline_entry.grid(row=15, column=1, padx=5)
deadline_entry.insert(0, 5.0)

point_exec_with_wp_button = tk.Button(window, 
    text = "Point Execution (With Waypoints)",
    command=lambda: point_execution_with_wp(
        float(wp_x1_entry.get()), float(wp_y1_entry.get()), 
        float(wp_x2_entry.get()), float(wp_y2_entry.get()), 
        float(wp_x3_entry.get()), float(wp_y3_entry.get()),
        float(wp_xfinal_entry.get()), float(wp_yfinal_entry.get()), 
        float(destination_orientationwp_final_entry.get()), 
        float(deadlinewp_entry.get()))) 
point_exec_with_wp_button.grid(row=16, column=0, 
    columnspan=4, padx=5, pady=5)

wp_x1_label = tk.Label(window, text = "Waypoint X1 (ft)")
wp_x1_label.grid(row=17, column=0, padx=5)
wp_x1_entry = tk.Entry(window)
wp_x1_entry.grid(row=17, column=1, padx=5)
wp_x1_entry.insert(0, 5.0)

wp_y1_label = tk.Label(window, text = "Waypoint Y1 (ft)")
wp_y1_label.grid(row=18, column=0, padx=5)
wp_y1_entry = tk.Entry(window)
wp_y1_entry.grid(row=18, column=1, padx=5)
wp_y1_entry.insert(0, 5.0)

wp_x2_label = tk.Label(window, text = "Waypoint X2 (ft)")
wp_x2_label.grid(row=19, column=0, padx=5)
wp_x2_entry = tk.Entry(window)
wp_x2_entry.grid(row=19, column=1, padx=5)
wp_x2_entry.insert(0, 5.0)

wp_y2_label = tk.Label(window, text = "Waypoint Y2 (ft)")
wp_y2_label.grid(row=20, column=0, padx=5)
wp_y2_entry = tk.Entry(window)
wp_y2_entry.grid(row=20, column=1, padx=5)
wp_y2_entry.insert(0, -5.0)

wp_x3_label = tk.Label(window, text = "Waypoint X3 (ft)")
wp_x3_label.grid(row=21, column=0, padx=5)
wp_x3_entry = tk.Entry(window)
wp_x3_entry.grid(row=21, column=1, padx=5)
wp_x3_entry.insert(0, -5.0)

wp_y3_label = tk.Label(window, text = "Waypoint Y3 (ft)")
wp_y3_label.grid(row=22, column=0, padx=5)
wp_y3_entry = tk.Entry(window)
wp_y3_entry.grid(row=22, column=1, padx=5)
wp_y3_entry.insert(0, 5.0)

wp_xfinal_label = tk.Label(window, 
    text = "Destination X Final (ft)")
wp_xfinal_label.grid(row=17, column=2, padx=5)
wp_xfinal_entry = tk.Entry(window)
wp_xfinal_entry.grid(row=17, column=3, padx=5)
wp_xfinal_entry.insert(0, -5.0)

wp_yfinal_label = tk.Label(window, 
    text = "Destination Y Final (ft)")
wp_yfinal_label.grid(row=18, column=2, padx=5)
wp_yfinal_entry = tk.Entry(window)
wp_yfinal_entry.grid(row=18, column=3, padx=5)
wp_yfinal_entry.insert(0, -5.0)

destination_orientationwp_final_label = tk.Label(
    window, text = "Destination Orientation (degrees)")
destination_orientationwp_final_label.grid(row=19, 
    column=2, padx=5)
destination_orientationwp_final_entry = tk.Entry(window)
destination_orientationwp_final_entry.grid(row=19, 
    column=3, padx=5)
destination_orientationwp_final_entry.insert(0, 90.0)

deadlinewp_label = tk.Label(window, 
    text = "Deadline (seconds)")
deadlinewp_label.grid(row=20, column=2, padx=5)
deadlinewp_entry = tk.Entry(window)
deadlinewp_entry.grid(row=20, column=3, padx=5)
deadlinewp_entry.insert(0, 5.0)

move_in_a_circle_button = tk.Button(window, text = "Move in a Circle",
    command=lambda: move_in_a_circle(
        float(radius_entry.get()), 
        float(cir_direction_entry.get()), 
        float(cir_deadline_entry.get()))) 
move_in_a_circle_button.grid(row=2, column=2, columnspan=2, padx=5, pady=5)

radius_label = tk.Label(window, text = "Radius (ft)")
radius_label.grid(row=3, column=2, padx=5)
radius_entry = tk.Entry(window)
radius_entry.grid(row=3, column=3, padx=5)
radius_entry.insert(0, 2.0)

cir_direction_label = tk.Label(window, text = "Circle Center Direction (degrees)")
cir_direction_label.grid(row=4, column=2, padx=5)
cir_direction_entry = tk.Entry(window)
cir_direction_entry.grid(row=4, column=3, padx=5)
cir_direction_entry.insert(0, 45.0)

cir_deadline_label = tk.Label(window, text = "Deadline (seconds)")
cir_deadline_label.grid(row=5, column=2, padx=5)
cir_deadline_entry = tk.Entry(window)
cir_deadline_entry.grid(row=5, column=3, padx=5)
cir_deadline_entry.insert(0, 5.0)

move_in_a_rectangle_button = tk.Button(window, 
    text = "Move in a Rectangle",
    command=lambda: move_in_a_rectangle(
        float(length_entry.get()), 
        float(direc_opp_vrtx_entry.get()), 
        float(rect_deadline_entry.get()))) 
move_in_a_rectangle_button.grid(row=6, column=2, columnspan=2, padx=5, pady=5)

length_label = tk.Label(window, text = "Length (ft)")
length_label.grid(row=7, column=2, padx=5)
length_entry = tk.Entry(window)
length_entry.grid(row=7, column=3, padx=5)
length_entry.insert(0, 5.0)

direc_opp_vrtx_label = tk.Label(window, text = "Direction of Opposite Vertex (degrees)")
direc_opp_vrtx_label.grid(row=8, column=2, padx=5)
direc_opp_vrtx_entry = tk.Entry(window)
direc_opp_vrtx_entry.grid(row=8, column=3, padx=5)
direc_opp_vrtx_entry.insert(0, 25.0)

rect_deadline_label = tk.Label(window, text = "Deadline (seconds)")
rect_deadline_label.grid(row=9, column=2, padx=5)
rect_deadline_entry = tk.Entry(window)
rect_deadline_entry.grid(row=9, column=3, padx=5)
rect_deadline_entry.insert(0, 5.0)

fig8_button = tk.Button(window, 
    text = "Move in a Figure 8",
    command=lambda: move_in_a_figure_eight(
        float(circle1_radius_entry.get()), 
        float(circle2_radius_entry.get()), 
        float(direction_circle1_entry.get()), 
        float(fig_8_deadline_entry.get()))) 
fig8_button.grid(row=10, column=2, columnspan=2, padx=5, pady=5)

circle1_radius_label = tk.Label(window, text = "Circle 1 Radius (ft)")
circle1_radius_label.grid(row=11, column=2, padx=5)
circle1_radius_entry = tk.Entry(window)
circle1_radius_entry.grid(row=11, column=3, padx=5)
circle1_radius_entry.insert(0, 2.0)

circle2_radius_label = tk.Label(window, text = "Circle 2 Radius (ft)")
circle2_radius_label.grid(row=12, column=2, padx=5)
circle2_radius_entry = tk.Entry(window)
circle2_radius_entry.grid(row=12, column=3, padx=5)
circle2_radius_entry.insert(0, 4.0)

direction_circle1_label = tk.Label(window, text = "Direction of Circle 1 (degrees)")
direction_circle1_label.grid(row=13, column=2, padx=5)
direction_circle1_entry = tk.Entry(window)
direction_circle1_entry.grid(row=13, column=3, padx=5)
direction_circle1_entry.insert(0, 90.0)

fig_8_deadline_label = tk.Label(window, text = "Deadline (seconds)")
fig_8_deadline_label.grid(row=14, column=2, padx=5)
fig_8_deadline_entry = tk.Entry(window)
fig_8_deadline_entry.grid(row=14, column=3, padx=5)
fig_8_deadline_entry.insert(0, 20.0)

# Vehicle Position Display
veh_pos_label = tk.Label(window, 
    text = ("Vehicle Position"))
veh_pos_label.grid(row=2, column=4, padx=5)

veh_x_pos_label = tk.Label(window, 
    text = ("X: " + "0.0" + " feet"))
veh_x_pos_label.grid(row=3, column=4, padx=5)

veh_y_pos_label = tk.Label(window, 
    text = ("Y: " + "0.0" + " feet"))
veh_y_pos_label.grid(row=4, column=4, padx=5)

veh_orientation_label = tk.Label(window, 
    text = ("Orientation: " + "0.0" + " degrees"))
veh_orientation_label.grid(row=5, column=4, padx=5)

# Wheel Rotation Rates display
wheel_rot_rates_label = tk.Label(window, 
    text = ("Wheel Rotation Rates"))
wheel_rot_rates_label.grid(row=6, column=4, padx=5)

# Add new rotation rates to display window
wheel_1_rot_label = tk.Label(window, 
    text = ("Wheel 1: " + "0.0" + " degrees/s"))
wheel_1_rot_label.grid(row=7, column=4, padx=5)

wheel_2_rot_label = tk.Label(window, 
    text = ("Wheel 2: " + "0.0" + " degrees/s"))
wheel_2_rot_label.grid(row=8, column=4, padx=5)

wheel_3_rot_label = tk.Label(window, 
    text = ("Wheel 3: " + "0.0" + " degrees/s"))
wheel_3_rot_label.grid(row=9, column=4, padx=5)

wheel_4_rot_label = tk.Label(window, 
    text = ("Wheel 4: " + "0.0" + " degrees/s"))
wheel_4_rot_label.grid(row=10, column=4, padx=5)

# Robot Velocity Display
robot_velocity_label = tk.Label(window, 
    text = ("Robot Velocity (Local)"))
robot_velocity_label.grid(row=11, column=4, padx=5)

robot_velocity_x_label = tk.Label(window, 
    text = ("Velocity X: " + "0.0" + " ft/s"))
robot_velocity_x_label.grid(row=12, column=4, padx=5)

robot_velocity_y_label = tk.Label(window, 
    text = ("Velocity Y: " + "0.0" + " ft/s"))
robot_velocity_y_label.grid(row=13, column=4, padx=5)

# Error Messages
error_label = tk.Label(window, 
    text = ("Error Log"))
error_label.grid(row=14, column=4, 
    padx=5, pady=25) 

# Quit button
exit_button = tk.Button(window, text = "Exit", 
    command=close_window) 
exit_button.grid(row=0, column=4, columnspan=2, 
    padx=5, pady=5, ipadx=50)

window.mainloop()

Return to Table of Contents

Play Flappy Bird Using Imitation Learning

In this tutorial, we will learn the fundamentals of imitation learning by playing a video game. We will teach an agent how to play Flappy Bird, one of the most popular video games in the world way back in 2014.

flappy_bird_imitation_learning

The objective of the game is to get a bird to move through a sequence of pipes without touching them. If the bird touches them, he falls out of the sky.

Table of Contents

You Will Need

Return to Table of Contents

What is Imitation Learning?

In order to understand what imitation learning is, it is first helpful to understand what reinforcement learning is all about.

Reinforcement learning is a machine learning method that involves training a software “agent” (i.e. algorithm) to learn through trial and error. The agent is rewarded by the environment when it does something good and punished when it does something bad. “Good” and “bad” behavior in this case are defined beforehand by the programmer. 

The idea behind reinforcement learning is that the agent learns over time the best actions it must take in different situations (i.e. states of the environment) in order to maximize its rewards and minimize its punishment. That stuff in bold in the previous sentence is formally known as the agent’s optimal policy.

Check out my reinforcement learning post for an example of how reinforcement learning works.

One of the problems with reinforcement learning is that it takes a long time for an agent to learn through trial and error. Knowing the optimal action to take in any given state of the environment means that the agent has to explore a lot of different random actions in different states of the environment in order to fully learn. 

You can imagine how infeasible this gets as the environment gets more complex. Imagine a robot learning how to make a breakfast of bacon and eggs. Assume the robot is in your kitchen and has no clue what bacon and eggs are. It has no clue how to use a stove. It has no clue how to use a spatula. It doesn’t even know how to use a frying pan.

eggs_bacon_breakfast

The only thing the robot has is the feedback it receives when it takes an action that is in accordance with the goal of getting it to make bacon and eggs.

You could imagine that it could take decades for a robot to learn how to cook bacon and eggs this way, through trial and error. As humans, we don’t have that much time, so we need to use some other machine learning method that is more efficient.

And this brings us to imitation learning. Imitation learning helps address the shortcomings of reinforcement learning. It is also known as learning from demonstrations.

In imitation learning, during the training phase, the agent first observes the actions of an expert (e.g. a human). The agent keeps a record of what actions the expert took in different states of the environment. The agent uses this data to create a policy that attempts to mimic the expert. 

play_flappy_bird_using_imitation_learning
Diagram of How Imitation Learning Works

Babies and little kids learn this way. They imitate the actions of their parents. For example, I remember learning how to make my bed by watching my mom and then trying to imitate what she did.

Now, at runtime, the environment won’t be exactly similar as it was when the agent was in the training phase observing the expert. The agent will try to approximate the best action to take in any given state based on what it learned while watching the expert during the training phase.

Imitation learning is a powerful technique that works well when:

  • The number of states of the environment is large (e.g. in a kitchen, where you can have near infinite ways a kitchen can be arranged)
  • The rewards are sparse (e.g. there are only a few actions a robot can take that will bring him closer to the goal of making a bacon and egg breakfast) 
  • An expert is available.

Going back to our bacon and eggs example, a robotic agent doing imitation learning would observe a human making bacon and eggs from scratch. The robot would then try to imitate that process.

In the case of reinforcement learning, the robotic agent will take random actions and will receive a reward every time it takes an action that is consistent with the goal of cooking bacon and eggs. 

For example, if the robot cracks an egg into a frying pan, it would receive a reward. If the robot cracks an egg on the floor, it would not receive a reward.

After a considerable amount of time (maybe years!), the robot would finally be smart enough (based on performance metrics established by the programmer) to know how to cook bacon and eggs.

So you see that the runtime behavior of imitation learning and reinforcement learning are the same. The difference is in how the agent learns during the training phase. 

In reinforcement learning, the agent learns through trial and error by receiving varying rewards from the environment in response to its action. 

In imitation learning, the agent learns by watching an expert. It observes the action the expert took in various states.

Because there is no explicit reward in the case of imitation learning, the agent only becomes as good as the teacher (but never better). Thus, if you have a bad expert, you will have a bad agent.

The agent doesn’t know the reward (consequences) of taking a specific action in a certain state. The graphic below further illustrates the difference between reinforcement learning and imitation learning.

imitation_vs_reinforcement_learning

So with that background, let’s dive into implementing an imitation learning algorithm to teach an agent to play Flappy Bird.

Return to Table of Contents

Directions

Install Flappy Bird

You can find the instructions for installing FlappyBird at this link, but let’s run through the process as it can be a bit tricky. Go slow, so you make sure you get all the necessary libraries.

Open up a terminal window.

Install all the dependencies for Python 3.X listed here. I’ll copy and paste all that below, but if you have any issues, check out the link. You can copy and paste the command below into your terminal.

sudo apt-get install git python3-dev python3-setuptools python3-numpy python3-opengl \
    libsdl-image1.2-dev libsdl-mixer1.2-dev libsdl-ttf2.0-dev libsmpeg-dev \
    libsdl1.2-dev libportmidi-dev libswscale-dev libavformat-dev libavcodec-dev \
    libtiff5-dev libx11-6 libx11-dev fluid-soundfont-gm timgm6mb-soundfont \
    xfonts-base xfonts-100dpi xfonts-75dpi xfonts-cyrillic fontconfig fonts-freefont-ttf libfreetype6-dev

You don’t have to know what each library does, but in case you’re interested, you can take a look at the Ubuntu packages page to get a complete description.

Make sure you have pip installed.

Now you need to install pygame.

pip3 install pygame
1-install-pygameJPG

We’re not done yet with all the installations. Let’s install the PyGame Learning Environment (PLE) now by copying the entire repository to our computer. It already includes Flappy Bird.

git clone https://github.com/ntasfi/PyGame-Learning-Environment
2-clone-from-githubJPG

You will be asked to login to GitHub. Type in your username and password.

Type the following command to see if it installed properly. You should see the PyGame-Learning-Environment folder.

ls

Now let’s switch to that folder.

cd PyGame-Learning-Environment

Complete the install, by typing the following command (be sure to include the period).

sudo pip3 install -e .

Now Flappy Bird is all set up.

Test to see if everything is working, type the following command to run the Aliens program

python3 -m pygame.examples.aliens
3-aliens_pygameJPG

Return to Table of Contents

Launch Flappy Bird

Let’s write a Python script in order to use the game environment. 

Open a new terminal window.

Go to the PyGame-Learning-Environment directory.

cd PyGame-Learning-Environment

We will create the script flappybird1.py. Open the text editor to create a new file.

gedit flappybird1.py

Here is the full code. You can find a full explanation of the methods at the PyGameLearning Environment website. The function getScreenRGB retrieves the state of the environment in the form of red/green/blue image pixels:

# Import Flappy Bird from games library in
# Python Learning Environment (PLE)
from ple.games.flappybird import FlappyBird 

# Import PyGame Learning Environment
from ple import PLE 

# Create a game instance
game = FlappyBird() 

# Pass the game instance to the PLE
p = PLE(game, fps=30, display_screen=True, force_fps=False)

# Initialize the environment
p.init()


actions = p.getActionSet()
print(actions) # 119 to flap wings, or None to do nothing
action_dict = {0: actions[1], 1:actions[0]}

reward = 0.0

for i in range(10000):
  if p.game_over():
    p.reset_game()

  state = p.getScreenRGB()
  action = 1
  reward = p.act(action_dict[action])

Now, run the program using the following command:

python3 flappybird1.py

Here is what you should see:

4-flappy-bird

Here is flappybird2.py. The agent takes a random action at each iteration of the game.

# Import Flappy Bird from games library in
# Python Learning Environment (PLE)
from ple.games.flappybird import FlappyBird 

# Import PyGame Learning Environment
from ple import PLE 

# Import Numpy
import numpy as np

class NaiveAgent():
  """
  This is a naive agent that just picks actions at random.
  """
  def __init__(self,actions):
    self.actions = actions

  def pickAction(self, reward, obs):
    return self.actions[np.random.randint(0, len(self.actions))]

# Create a game instance
game = FlappyBird() 

# Pass the game instance to the PLE
p = PLE(game)

# Create the agent
agent = NaiveAgent(p.getActionSet())

# Initialize the environment
p.init()

actions = p.getActionSet()
action_dict = {0: actions[1], 1:actions[0]}

reward = 0.0

for f in range(15000):

  # If the game is over
  if p.game_over():
    p.reset_game()

  action = agent.pickAction(reward, p.getScreenRGB())
  reward = p.act(action)

  if f > 1000:
    p.display_screen = True
    p.force_fps = False # Slow screen

Return to Table of Contents

Implement the Dataset Aggregation Algorithm (DAgger)

How the DAgger Algorithm Works

In this section, we will implement the Dataset Aggregation Algorithm (DAgger) and apply it to FlappyBird. 

The DAgger algorithm is a type of imitation learning algorithm that is intended to help address the problem of the agent “memorizing” the actions of the expert in different states. It is time-consuming to watch the expert do every combination of possible states of the environment, so we need the expert to give the agent some guidance on what to do in the event the agent makes errors.

In the first phase, we first let the expert take action in the environment and keep track of what action the expert took in a variety of states of the environment. This phase creates an initial “policy”, a collection of state-expert action pairs.

Then for the second phase, we add a twist to the situation. The expert continues to act, and we record the expert’s actions. However, the environment doesn’t change according to the expert’s actions. Rather it changes in response to the policy from phase 1. The environment is “ignoring” the actions of the expert. 

In the meantime, a new policy is created that maps the state to the expert’s actions.

You might now wonder, which policy would the agent choose? The policy from phase 1 or phase 2. One method is to pick one at random. Another method (and the one often preferred in practice) is to hold out a subset of states of the environment and test which one performs the best.

You can find the Dagger paper here to learn more.

Install TensorFlow

We now need to install TensorFlow, the popular machine learning library.

Open a new terminal window, and type the following command (wait a bit as it will take some time to download).

pip3 install tensorflow

Run FlappyBird

Now we need to run FlappyBird with the imitation learning algorithm. Since the game has no real expert, we have to have our algorithm load a special file (included in the GitHub folder that we will clone in a second) that has the expert’s pre-prepared policy. 

Clone the code at this GitHub page here into the file.

git clone https://github.com/PacktPublishing/Reinforcement-Learning-Algorithms-with-Python.git

To run the code, migrate to the “Chapter10” file. You want to find the file called Dagger.py.

gedit Dagger.py

To see FlappyBird in action, change this line:

env = PLE(env, fps=30, display_screen=False)

To this:

env = PLE(env, fps=30, display_screen=True, force_fps=False)

To run the code, type the following command:

python3 Dagger.py

Eventually, you should see the results of each game spit out, with the bird’s score on the far right. Notice that the agent’s score gets above 100 after only a few minutes of training.

5-scores-of-imitation-learning

That’s it for imitation learning. If you want to learn more, I highly suggest you check out Andrea Lonza’s book, Reinforcement Learning Algorithms with Python, that does a great job teaching imitation learning.

Return to Table of Contents

References

Return to Table of Contents

How to Install Pip on Ubuntu 20.04

In this project, we will install pip. Pip makes it easy to manage software packages written in Python.

You Will Need

Directions

Log in to your Ubuntu Linux machine.

Click on the nine white dots in the lower left of the desktop.

1-six-dots

Type “terminal” in the search blank, and then click on the Terminal icon.

2-terminal-search-blank

Before we install anything, let’s make sure we have the latest list of available packages. Let’s also upgrade the system if necessary.

Type the following command in the terminal window:

sudo apt update

You’ll need to type your password and then press Enter.

Now, let’s upgrade our system.

sudo apt -y upgrade

The -y flag means that we agree to upgrade our system to the latest and greatest software packages.

Now, we need to install pip. Type the following command and press Enter:

sudo apt install -y python3-pip

Pip should take a minute or two to install.

If at any time you want to install a Python package, you use the following command:

pip3 install package_name

You’ll need to replace package_name above with the name of the library or package you want to have installed.

For example, let’s install some mathematical packages. We will install numpy (scientific computing library), scipy (scientific computing library), matplotlib (graph plotting library), and sympy (computer algebra library).

Here is the command you need to type into the Terminal:

pip3 install numpy scipy matplotlib sympy
4-install-math-packagesJPG

That’s it!