====== Lab05 - Randomized Sampling-based Algorithms ====== ^ Motivations and Goals ^ | Become familiar with sampling-based motion planning | | Understand the probabilistic roadmap approach and rapidly growing random tree approach | ^ Tasks ([[courses:b4m36uir:internal:instructions:lab06|teacher]]) ^ | Implement the RRT and PRM approaches for robot path planning in the maze | ^ Lab resources ^ | {{ :courses:b4m36uir:hw:task05.zip |Task05 resource package}} | ==== Lab code for task05 PRM algorithm ==== #!/usr/bin/env python3 # -*- coding: utf-8 -*- import sys import math import numpy as np import collections import heapq import matplotlib.pyplot as plt import Environment as env class PRMPlanner: def __init__(self, limits): """ Parameters ---------- limits: list((float, float)) translation limits in individual axes """ self.limits = limits x_lower = limits[0][0] x_upper = limits[0][1] y_lower = limits[1][0] y_upper = limits[1][1] z_lower = limits[2][0] z_upper = limits[2][1] #calculate the maximum step length self.max_translation = 1/250.0 * np.max([ x_upper-x_lower, y_upper-y_lower, z_upper-z_lower ]) self.max_rotation = math.pi/6 def plan(self, environment, start, goal): """ Method to plan the path Parameters ---------- environment: Environment Map of the environment that provides collision checking start: numpy array (4x4) start pose of the robot given in SE(3) goal: numpy array (4x4) goal pose of the robot given in SE(3) Returns ------- list(numpy array (4x4)) the path between the start and the goal Pose in SE(3) coordinates """ path = [] #random sample x points in the configuration space #for RRT iterate for the specific number of iterations n_points = 300 #sample the points in the full configuration space - we are sampling in the full space samples = np.random.rand(6,n_points) #apply the limits in individual axes i = 0 for limit in self.limits: scale = limit[1] - limit[0] samples[i,:] = samples[i,:]*scale + limit[0] i += 1 #add start and goal sonfigurations to the samples samples = np.append(samples, start.reshape(6,1), axis=1) samples = np.append(samples, goal.reshape(6,1), axis=1) #debug plot samples #environment.plot_environment(P_start) #plt.plot(samples[0,:],samples[1,:],samples[2,:],'r.') #check the transition between individual configurations and filter the ones that are not feasible #it is recommended to construct the navigation graph in this step, for that, you will need a container to save the information about edges and their cost (preferably an oriented graph that will come handy in the 6th task) tx = samples.shape[1] #in this implementation make the full transition graph for i in range(0,tx-1): for j in range(i+1,tx): p1 = samples[:,i] p2 = samples[:,j] #check, that there is a feasible path between the two configurations dist, edge = self.check_path_for_collision(environment, p1, p2) #if there is the path, add it to the list of feasible edges if dist > 0: #add it to the oriented graph #debug plot the path #xx = [x[0,3] for x in edge] #yy = [x[1,3] for x in edge] #zz = [x[2,3] for x in edge] #plt.plot(xx,yy,zz,'b', linewidth=0.5) #plan the path on the resulting graph -> result is a sparse path #reconstruct the dense path to adhere to the distance limit between the samples, i.e. recalculate the proper path pose matrices return(path) def construct_pose(self, state): R = self.rotation_matrix(state[5],state[4],state[3]) T = state[0:3] #construct the SE(3) matrix P = np.hstack((R,T.reshape((3,1)))) P = np.vstack((P,[0,0,0,1])) return P def check_path_for_collision(self, environment, sample1, sample2): dist = 0 edge = [] #Note, decouple the translation and rotation parts ... #get the translation direction towards the target T_start = sample1[0:3] T_goal = sample2[0:3] T_dir = (T_goal-T_start) #get how many samples there is necessary to sample along the line to adhere to the maximum translation limit dist = np.linalg.norm(T_dir) samples_translation = int(dist/self.max_translation) #get the difference in the rotation angles dphi_x = sample1[3] - sample2[3] dphi_y = sample1[4] - sample2[4] dphi_z = sample1[5] - sample2[5] #get how many samples there is necessary to sample the rotation to adhere to the maximum rotation limit samples_rotation = int(np.max([np.abs(dphi_x/(self.max_rotation)), np.abs(dphi_y/(self.max_rotation)), np.abs(dphi_z/(self.max_rotation))])) #select the maximum of sampling n_points = np.max([samples_translation, samples_rotation]) # iterate and check for collision for i in range(0,n_points): #construct the translation part of the pose T = T_start + T_dir*i/n_points #construct the rotation part of the pose yaw = ... pitch = ... roll = ... #construct the SE(3) pose matrix P = ... #check for collision with the environment ret = environment.check_robot_collision(P) #if in collision return... if ret: return 0, [] else: edge.append(P) #don't forget to calculate the distance #return return dist,edge def rotation_matrix(self, yaw, pitch, roll): """ Constructs rotation matrix given the euler angles yaw = rotation around z axis pitch = rotation around y axis roll = rotation around x axis Parameters ---------- yaw: float pitch: float roll: float respective euler angles """ R_x = np.array([[1, 0, 0], [0, math.cos(roll), math.sin(roll)], [0, -math.sin(roll), math.cos(roll)]]) R_y = np.array([[math.cos(pitch), 0, -math.sin(pitch)], [0, 1, 0], [math.sin(pitch), 0, math.cos(pitch)]]) R_z = np.array([[math.cos(yaw), math.sin(yaw), 0], [-math.sin(yaw), math.cos(yaw), 0], [0, 0, 1]]) R = R_x.dot(R_y).dot(R_z) return R