<jupyter_start><jupyter_text>Create a file called ref_trajectory.py. Copy the functions from week3 that enabled you to generate a trajectory given a sequence of straight/ turn commands<jupyter_code>import numpy as np
import matplotlib.pyplot as plt
from PIL import Image
import cv2
from ref_trajectory_2 import generate_trajectory as traj
%matplotlib inline<jupyter_output><empty_output><jupyter_text>We will look at 2 ways to make a grid (a) Load an existing circuit (Use MS paint or anything else)(b) Make a custom gridRemember grid resolution is the key parameter to go from grid co-ordinates to world co-ordinates. What is the value of 1 pixel?<jupyter_code>grid_res = 0.05
image = Image.open('./circuit.png').convert("LA") # open the test circuit
grid_data = np.asarray(image)[:,:,0]/255
print(grid_data[40,100])


#find the shape
grid_shape =grid_data.shape
print(grid_shape)
#find the real-world extents of the grid: opposite vertices of the rectangle
x1, y1 = 0, 0
#use shape and grid_res to calculate the top-right vertex
x2, y2 = grid_shape[0]*grid_res,grid_shape[1]*grid_res
print(x2,y2)

#display the image using imshow()
#pay attention to the keywords origin, cmap and extent
#Default origin is top-left, but we should use 'lower'
#how should grid_data be transformed
plt.figure()

#complete
plt.imshow(grid_data,extent=[x1,x2,y1,y2],cmap=plt.cm.gray)

plt.show()
#custom grid
#choose resolution and dimensions
grid_res = 0.05 
grid_span = 50 # square circuit dimensions in m

#calculate grid_shape from grid
# dimensions have to be integers
grid_shape = (int(grid_span/grid_res),int(grid_span/grid_res))
print(grid_shape)
#Initialize 
grid_data = np.zeros(grid_shape)

#Create rectangular obstacles in world co-ordinates
#xmin, xmax, ymin, ymax
obstacles = np.array([[25, 26, 10, 40],
                       [8, 10, 11, 18]]) 
for obs in obstacles:
    #calculate obstacles extent in pixel coords
    xmin, xmax, ymin, ymax =(obs/grid_res).astype('int')
    # mark them as occupied
    grid_data[xmin:xmax,ymin:ymax] = 1

#calculate the extents    
x1, y1 = 0, 0
x2, y2 =grid_span,grid_span 

#imshow() from prior cell
plt.figure()
plt.imshow(1-grid_data.T,origin='lower',cmap=plt.cm.gray,extent=[x1,x2,y1,y2])<jupyter_output>(1000, 1000)<jupyter_text>Plot the reference trajectory on the grid environment<jupyter_code>#pick a suitable starting pose
start_pose = np.array([2, 6, np.pi/2])
#we will use same route as before
route = [("straight", 5),("turn", -90),("straight", 6),("turn", 90)]

#calculate ref_path from traj()
# we want it to be numpy array of dimensions (N, 3)
ref_path = traj(route,start_pose)
ref_path=np.asarray(ref_path)

print(ref_path.shape)


#plot the grid and verify that the path collides with the obstacles
plt.figure
plt.imshow(1-grid_data.T,origin='lower',cmap=plt.cm.gray,extent=[x1,x2,y1,y2])
plt.plot(ref_path[0,:], ref_path[1,:])
plt.plot(start_pose[0], start_pose[1],'r+')<jupyter_output>(3, 121)<jupyter_text>Let us now use circle approximation to perform collision checking. We will use w=0.8, l=1.2m as mule dimensions. Calculate the 4 vertices of the muleCircle parameters are offset and radiiYou would have to plot the mule rectangle and 3 circles<jupyter_code>w=0.8
l=1.2
mule_extents = np.array([[-w/2,-l/2],
                         [w/2,-l/2],
                         [w/2,l/2],
                         [-w/2,l/2],
                        [-w/2,-l/2]])
print(mule_extents)
r =0.5 
l =0.4
circles = [(0, 0, r), (0, l, r), (0, -l, r)]
#plot rectangle or just the 4 vertices
plt.plot(mule_extents[:,0],mule_extents[:,1])
ax=plt.gca()
for x,y, rad in circles:
    #plot circle
    ax.add_patch(plt.Circle([x,y],rad,fill=False))<jupyter_output>[[-0.4 -0.6]
 [ 0.4 -0.6]
 [ 0.4  0.6]
 [-0.4  0.6]
 [-0.4 -0.6]]<jupyter_text>Write a function to find if there is a collision given the grid and the local path1. Rotate the 3 circle vertices along the local path2. Check if $(x-x_c)^2 + (y-y_c)^2 < r$ and grid[x, y] = 1.03. Return True or FalseYou can assume circles variable (approximation of the vehicle dimensions) is assumed as a global variableWhat do you think is a simple unit test?<jupyter_code>grid_res = 0.05
def circle_collision_check(grid, local_traj):
    xmax, ymax = grid.shape
    all_x = np.arange(xmax)
    all_y = np.arange(ymax)
    X, Y = np.meshgrid(all_x, all_y)
    for xl, yl, tl in local_traj.T:
      rot = np.array([[np.sin(tl), -np.cos(tl)],[np.cos(tl), np.sin(tl)]])
      for xc, yc, rc in circles:
        xc_rot, yc_rot = rot @ np.array([xc, yc]) + np.array([xl, yl])
        xc_pix, yc_pix = int(xc_rot/grid_res), int(yc_rot/ grid_res)
        rc_pix = (rc/ grid_res)
        inside_circle = ((X-xc_pix)**2 +(Y-yc_pix)**2 - rc_pix**2 < 0)
        occupied_pt = grid[X, Y] == 1
        if np.sum(np.multiply( inside_circle, occupied_pt)):
          return True
    return False
#The path we chose before was colliding so the function should return True
print(circle_collision_check(grid_data,ref_path))<jupyter_output>True<jupyter_text>Let us move onto the more complicated swath collision checkingWrite a function to trace the footprint of the vehicle along the local path.Input: grid_size, local pathOutput: footprintStart with an empty grid of similar size1. Rotate the 4 vertices of the mule rectangle along the local path2. Collect all the vertices and calculate the convex hull. Remember to typecast them as int32 as cv2..convexHull() expects that3. Use fillConvexPoly() to fill up the hull<jupyter_code>def swath_footprint(local_path,grid_size):
    #initialize image
    img = np.zeros(grid_size)
    vertices = []
    for xl,yl,tl in local_path.T:
        rot = np.array([[np.sin(tl), -np.cos(tl)],[np.cos(tl), np.sin(tl)]])
        for vertex in mule_extents:
            rot_vertex = rot @ vertex + np.array([xl, yl])
            vertices.append(rot_vertex)
    vertices = np.array(vertices,dtype='float32')
    hull_vertices = cv2.convexHull(vertices)
    #change hull vertices to grid coords and typecast as int32
    hull_vertices = (hull_vertices/grid_res).astype('int32')
    
    cv2.fillConvexPoly(img,hull_vertices,1)
    return img.T == 1

footprint = swath_footprint(ref_path[:,60:120],grid_data.shape)
plt.imshow(1-footprint.T,origin='lower',cmap=plt.cm.gray,extent=[x1,x2,y1,y2])
plt.plot(ref_path[0,:], ref_path[1,:],'r')<jupyter_output><empty_output><jupyter_text>Once you know the footprint, it should be easy to see if the footprint coincides with any occupied cells in the original gridTry out some examples to unit test your code for both circle checking and swath checkingIn practice, you can write this as a CollisionChecking class where the mule dimensions are read from a config file and initialized. Global variables are best avoided in production code<jupyter_code>## From the footprint which we received from previous function ,we compare it with our grid_data to find out wether the local path is in collision or not 
collision_check=np.sum(np.multiply(footprint,grid_data)) >= 1
print(collision_check)
#True means the path is in collision<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.11454365919029483, 'fraction_numerical': 0.02694315136126393, 'mean_word_length': 4.298953662182361, 'pattern_counts': {'":': 0, '<': 27, '<?xml version=': 0, '>': 26, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 1, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '7', 'dir': 'jupyter-structured-clean-dedup', 'id': '195529', 'license': 'MIT', 'n_tokens_mistral': 2593, 'n_tokens_neox': 2471, 'n_words': 768, 'path': 'week4/abizer.patanwala98/Q2 - 1/Attempt1_filesubmission_Copy of collision_checking.ipynb', 'repo_name': 'naveenmoto/lablet102', 'size': '7089'}