The main task is to implement the basic randomized sampling-based algorithms - RRT and PRM.
Deadline | 17. November 2018, 23:59 PST |
Points | 6 |
Label in BRUTE | Task05 |
Files to submit | archive with samplingplanner directory |
Minimal content of the archive: samplingplanner/PRMPlanner.py , samplingplanner/RRTPlanner.py |
|
Resources | Task05 resource package |
The resource package has been updated on 5. November 2018. The original version of the package is accessible here | |
Additional environments to test the planner |
Implement the Probabilistic Roadmap (PRM) and Rapidly Exploring Random Trees (RRT) randomized sampling-based path planning algorithms according to the description and pseudocode presented in the Lecture 5. Randomized Sampling-based Motion Planning Methods. The algorithms shall provide a collision free path through the environment represented by a geometrical map.
In file PRMPlanner.py
implement the PRM algorithm.
In file RRTPlanner.py
implement the RRT algorithm.
The implementation requirements are as follows:
PRMPlanner
and RRTPlanner
plan a path in 6-DOF
PRMPlanner
and RRTPlanner
implements function plan
that takes following arguments on the input:
environment
- an instance of the Environment
class that provides the interface for collision checking between the robot and the obstacles
start
and goal
- the initial and goal
configurations of the robot. Each configuration is given as a tuple of 6 state-space variables $(x,y,z,\phi_x,\phi_y,\phi_z)$, where $x,y,z$ represent the position of the robot in the environment. $\phi_x, \phi_y, \phi_z \in (0,2\pi)$ represent the orientation of the robot as the rotation angles around the respective axis
plan
function is a list of robot poses given in $SE(3)$ which codes the full configuration of the robot into a single matrix
self.limits
variable as a list of lower-bound and upper-bound limit tuples, i.e. list( (lower_bound, upper_bound) )
, for each of the variables $(x,y,z,\phi_x,\phi_y,\phi_z)$
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] max_translation = 1/250.0 * np.max([ x_upper-x_lower, y_upper-y_lower, z_upper-z_lower ])Note, The configuration space sampling is not affected by this requirement. Individual random samples may be arbitrarily far away; however, their connection shall adhere to the given constraint on the maximum distance and rotation to ensure sufficient sampling of the configuration space and smooth motion of the robot
self.environment.check_robot_collision
function that takes on the input an $SE(3)$ pose matrix. The collision checking function returns True
if there is collision between the robot and the environment and False
if there is no collision.
The provided source files provides only the ability to check for the collision between the robot and the environment. The collision avoidance software used is RAPID1) collision checking library. Following instructions might be used to help solve the given assignment:
self.environment
and is necessary to produce the desired output of the plan
method
start
and goal
given the requirement on the maximum distance and the maximum rotation. To simplify further tasks, the function may return the distance between the start
and the goal
configuration and also a list of interlying configurations.
#random sample n_points in the configuration space n_points = 30 #random sampling from uniform distribution between 0 and 1 samples = np.random.rand(6,n_points) #change the sampling based on the limits in individual axes - scale and shift the samples i = 0 for limit in self.limits: #for each DOF in configuration scale = limit[1] - limit[0] #calculate the scale samples[i,:] = samples[i,:]*scale + limit[0] #scale and shift the random samples i += 1
Following marks will be considered in evaluation
The simplified evaluation script for testing of the implementation is following
#!/usr/bin/env python3 # -*- coding: utf-8 -*- import sys import math import time import numpy as np import matplotlib.pyplot as plt from collections import deque sys.path.append('environment') sys.path.append('samplingplanner') import Environment as env import PRMPlanner as prm import RRTPlanner as rrt if __name__ == "__main__": #define the planning scenarios #scenario name #start configuration #goal configuration #limits for individual DOFs scenarios = [("environments/simple_test", (2,2,0,0,0,0), (-2,-2,0,0,0,0), [(-3,3), (-3,3), (0,0), (0,0), (0,0), (0,0)]), ("environments/simple_test", (2,2,0,0,0,0), (-2,-2,0,0,0,math.pi/2), [(-3,3), (-3,3), (0,0), (0,0), (0,0), (0,2*math.pi)]), ("environments/alpha_puzzle", (0,5,0,0,0,0),(25,25,25,0,0,0), [(-40,70),(-40,70),(-40,70),(0,2*math.pi),(0,2*math.pi),(0,2*math.pi)])] #enable dynamic drawing in matplotlib plt.ion() ##################################### ## EVALUATION OF THE RRT PLANNER ##################################### for scenario in scenarios: name = scenario[0] start = np.asarray(scenario[1]) goal = np.asarray(scenario[2]) limits = scenario[3] print("processing scenario: " + name) #initiate environment and robot meshes environment = env.Environment() environment.load_environment(name) #instantiate the planner planner = rrt.RRTPlanner(limits) #plan the path through the environment path = planner.plan(environment, start, goal) #plot the path step by step ax = None for Pose in path: ax = environment.plot_environment(Pose, ax=ax, limits=limits) plt.pause(0.1) ##################################### ## EVALUATION OF THE PRM PLANNER ##################################### for scenario in scenarios: name = scenario[0] start = np.asarray(scenario[1]) goal = np.asarray(scenario[2]) limits = scenario[3] print("processing scenario: " + name) #initiate environment and robot meshes environment = env.Environment() environment.load_environment(name) #instantiate the planner planner = prm.PRMPlanner(limits) #plan the path through the environment path = planner.plan(environment, start, goal) #plot the path step by step ax = None for Pose in path: ax = environment.plot_environment(Pose, ax=ax, limits=limits) plt.pause(0.1)
On Linux (tested with Ubuntu 14.04, 16.04, 18.04)
make
the rapid library in environment/rapid
directory
On MacOS
environment/rapid/Makefile
change TARGET=librapid.so
to TARGET=librapid.dylib
environment/rapid/Makefile
change -soname
to -install_name