trying to figure out closed-loop rrt

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
+# CMD source /opt/ros/melodic/setup.bash

kinodynamic_rrt/README.md

@@ -0,0 +1 @@
+### Kinodynamic RRT

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()
+
+
+
+
+
+
+

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()

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)