Warning

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
Implement the RRT and PRM approaches for robot path planning in the maze
Lab resources

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
x_upper = limits
y_lower = limits
y_upper = limits
z_lower = limits
z_upper = limits
#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 - limit
samples[i,:] = samples[i,:]*scale + limit
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
#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,state,state)
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 - sample2
dphi_y = sample1 - sample2
dphi_z = sample1 - sample2

#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

courses/b4m36uir/labs/lab05.txt · Last modified: 2018/11/16 15:58 by cizekpe6 