diff --git a/Dockerfile b/Dockerfile index beb06ae..7ac5b24 100644 --- a/Dockerfile +++ b/Dockerfile @@ -146,12 +146,13 @@ RUN mkdir -p ~/catkin_ws/src ENV OsqpEigen_DIR=/osqp-eigen RUN apt-get install -y ros-melodic-teb-local-planner && apt-get install -y ros-melodic-rviz +#install pip +RUN apt-get install -y software-properties-common && add-apt-repository main && apt-add-repository universe && add-apt-repository restricted && add-apt-repository multiverse && apt-get update -y && sudo apt install python2.7 && apt-get -y install python-pip - -RUN apt-get install -y wget && wget https://ompl.kavrakilab.org/install-ompl-ubuntu.sh && chmod u+x install-ompl-ubuntu.sh && ./install-ompl-ubuntu.sh --python +RUN apt-get install -y wget && wget https://raw.githubusercontent.com/pmusau17/Planning-and-MPC/main/learning_ompl/install-ompl-ubuntu.sh && chmod u+x install-ompl-ubuntu.sh && /bin/bash -c "./install-ompl-ubuntu.sh --python" WORKDIR catkin_ws/ -# small details they often don't tell you I'm tirrrrreeeeeeed +# # small details they often don't tell you I'm tirrrrreeeeeeed ENV PYTHONPATH="/ompl-1.5.2/py-bindings" -CMD source /opt/ros/melodic/setup.bash \ No newline at end of file +# CMD source /opt/ros/melodic/setup.bash \ No newline at end of file diff --git a/kinodynamic_rrt/README.md b/kinodynamic_rrt/README.md new file mode 100644 index 0000000..2e21c47 --- /dev/null +++ b/kinodynamic_rrt/README.md @@ -0,0 +1 @@ +### Kinodynamic RRT \ No newline at end of file diff --git a/kinodynamic_rrt/bicycle_model.py b/kinodynamic_rrt/bicycle_model.py new file mode 100644 index 0000000..66c4918 --- /dev/null +++ b/kinodynamic_rrt/bicycle_model.py @@ -0,0 +1,141 @@ +""" +Bicycle Model Class +author Patrick Musau +""" + +import numpy as np +import copy +import math + +""" """ +### Parameters for Bicycle Model Obtained through system Identification +ca = 1.9569 +cm = 0.0342 +ch = -37.1967 + +# distance from back to rear wheels +lf = 0.225 +lr = 0.225 +beta = 0 + +class State: + + """ + dt: time step used for euler simulation + T: simulation_time + x: xs[0] pos (m) + y: xs[1] pos (m) + yaw: xs[2] radians + v: xs[3] velocity m/s + """ + def __init__(self,xs=[0,0,0,0]): + self.x = xs + self.beta=0 + """ + euler step + x = x + (x' * dt) + """ + + """ + Use euler simulation to simulate the trajectory forward + returns 4d array of simulation + U = control input [throttle, steering angle] + T = Simulation Horizion (s) + """ + def simulate_forward(self,u,T,dt=0.01): + steps = int(T / dt) + trace = [self.x] + for i in range(steps+1): + trace.append(self.update(u,dt)) + return np.asarray(trace) + + + """ + Simulate trajectories with an underlying speed and a set of steering angles + + throttle: speed we want the car to move along the trajectory + num_primitives: how many unique trajectories that are considered as motion primitives + steer_min, steer_max = maximum turning action considered for the motion primitives + T : predictionhorizon + + """ + def simulate_motion_primitives(self,throttle=0.3,num_primitives=5,steer_min=-0.61,steer_max=0.61,T=0.5,dt=0.01,plot=False): + steering_angles = np.linspace(steer_min,steer_max,num_primitives) + traces = [] + start = self.x[:] + for i in range(num_primitives): + u = [throttle,steering_angles[i]] + tr = self.simulate_forward(u,T,dt) + traces.append(tr) + self.x = start[:] + return traces + + + + + def update(self, uc,dt=0.01): + u = uc[0] + delta = uc[1] + self.x[0] = self.x[0] + (self.x[3] * math.cos(self.x[2] + self.beta)) * dt + self.x[1] = self.x[1] + (self.x[3] * math.sin(self.x[2] + self.beta)) * dt + self.x[3] = self.x[3] + (-ca * self.x[3] + ca*cm*(u - ch)) * dt + self.x[2] = self.x[2] + (self.x[3] * (math.cos(beta)/(lf+lr)) * math.tan(delta)) * dt + return [self.x[0],self.x[1],self.x[2],self.x[3]] + + # This might help let's see + def pi_2_pi(self,angle): + return (angle + math.pi) % (2 * math.pi) - math.pi + +# Testing only +if __name__ == '__main__': + import matplotlib.pyplot as plt + + # two second simulation + T = 1 + u = [1.0,0.266] + u2 = [1.0,-0.266] + u3 = [1.0,-0.61] + u4 = [1.0,-0.61] + dt = 0.05 + + s = State() + num_primitives = 8 + traces = s.simulate_motion_primitives(T=T,num_primitives=num_primitives,throttle=1.0,dt=dt) + + for i in range(num_primitives): + tr1 = traces[i] + x = tr1[:,0] + y = tr1[:,1] + plt.plot(x, y,'.') + + plt.xlabel("x (m)") + plt.ylabel("y (m)") + plt.axis("equal") + plt.grid(True) + plt.show() + + # need to see if I set the speed to zero if something bad happens lol + s = State() + tr = s.simulate_forward(u,T,dt) + x = tr1[:,0] + y = tr1[:,1] + plt.plot(x, y,'.') + + # from the previous solution + s = State(tr1[-1]) + tr1 = s.simulate_forward(u,T,dt) + x = tr1[:,0] + y = tr1[:,1] + plt.plot(x, y,'.') + plt.xlabel("x (m)") + plt.ylabel("y (m)") + plt.axis("equal") + plt.grid(True) + plt.show() + + + + + + + diff --git a/kinodynamic_rrt/plotting.py b/kinodynamic_rrt/plotting.py new file mode 100644 index 0000000..5a8b051 --- /dev/null +++ b/kinodynamic_rrt/plotting.py @@ -0,0 +1,135 @@ +""" Plotting Tools for Sampling-based algorithms +@author: Patrick Musau + +Inspiried by Huming Zhou +""" +#import env +import matplotlib.pyplot as plt +import matplotlib.patches as patches +import numpy as np +import os +import sys + +class Plotting: + + """ + x_start: 2D Point + y_start: 2D Point + + free (0), + occupied (100), + and unknown (-1) + """ + + + def __init__(self, grid_file,x_start,x_goal): + + # create the figure + self.fig, self.ax = plt.subplots(figsize=(20, 15)) + + # the origin and res come from the map.yaml file + self.origin= (-100,-100) + self.res = 0.04 + self.grid=np.load(grid_file).astype(int) + item = (np.where(self.grid>0)) + item2 = (np.where(self.grid==0)) + # just plot the boundaries + self.x_obs = (item[0] * self.res) + self.origin[0] + self.y_obs = (item[1] * self.res) + self.origin[1] + self.x_free = (item2[0] * self.res) + self.origin[0] + self.y_free = (item2[1] * self.res) + self.origin[1] + self.count =0 + + + # get the start and goal + self.xI, self.xG = x_start, x_goal + # create object that defines the planning workspace + + def animation(self,nodelist,path,name, animation=False,block=False): + # plot the grid + #self.ax.clear() + self.plot_grid(name) + # plot visited nodes + self.plot_visited(nodelist, animation) + # plot the final path + self.plot_path(path,block=block) + + + + """ + I'm a visual learner so this method just shows me how points are sampled + """ + def random_sample(self): + x = np.random.choice(self.x_free) + y = np.random.choice(self.y_free) + x_index = int(round((x - self.origin[0])/(self.res))) + y_index = int(round((y - self.origin[1])/(self.res))) + print(x,y,x_index,y_index) + print("Sanity Check") + print((x_index*self.res)+self.origin[0],(y_index*self.res)+self.origin[1],) + print(x_index,y_index,self.grid[x_index][y_index]) + self.ax.plot([x], [y], "ks", linewidth=3,label="random_point") + plt.legend() + plt.show(block=True) + + + + # plot the 2d grid + def plot_grid(self, name,block=False): + + + + self.ax.plot(self.x_obs,self.y_obs,'k.',label="boundaries") + self.ax.plot(self.x_free,self.y_free,'y.',label="freespace") + + self.ax.plot(self.xI[0], self.xI[1], "bs", linewidth=3,label="init") + self.ax.plot(self.xG[0], self.xG[1], "ms", linewidth=3,label="goal") + + # set equal aspect ratio for figures + plt.xlabel("x(m)") + plt.ylabel("y(m)") + plt.title(name) + plt.axis("equal") + + plt.show(block=block) + if(self.count<1): + plt.legend() + self.count+=1 + + + # Go through the list of nodes and connect each node to it's parent + # Pausing if you want to animate + #@staticmethod + def plot_visited(self,nodelist, animation): + if animation: + count = 0 + for node in nodelist: + count += 1 + if node.parent: + self.ax.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: + [exit(0) if event.key == 'escape' else None]) + if count % 10 == 0: + plt.pause(0.001) + else: + for node in nodelist: + if node.parent: + plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") + + # plotting a path via list comprehensions + #@staticmethod + def plot_path(self,path,block=False): + if len(path) != 0: + self.ax.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2) + plt.pause(0.01) + plt.show(block=block) + + +if __name__ == "__main__": + x_start = (0,0) + x_goal = (1.422220,1.244794) + grid = 'porto_grid.npy' + pl = Plotting(grid,x_start,x_goal) + pl.plot_grid("Porto Occupancy Grid",block=False) + pl.random_sample() diff --git a/kinodynamic_rrt/utils.py b/kinodynamic_rrt/utils.py new file mode 100644 index 0000000..5e5241f --- /dev/null +++ b/kinodynamic_rrt/utils.py @@ -0,0 +1,24 @@ +""" +Utilities for collision checking in rrt_star +@author: patrick musau +Inspired by huiming zhou and Hongrui Zheng + +Overall forom this paper: https://arxiv.org/pdf/1105.1186.pdf +""" + + + +import math +import numpy as np +import os +import sys + +# I need this file to get the occupancy grid +import plotting + +# import node from rrt_star (x,y,parent) +from rrt_star import Node + + + +self.plotting = plotting.Plotting(grid,x_start,x_goal) \ No newline at end of file