The main task is to implement the curvature-constraint randomized sampling-based algorithms in RRT and PRM.
Deadline | 24. November 2018, 23:59 PST |
Points | 5 |
Label in BRUTE | Task06 |
Files to submit | archive with samplingplanner directory |
Minimal content of the archive: samplingplanner/DubinsPRMPlanner.py , samplingplanner/DubinsRRTPlanner.py |
|
Resources | Task06 resource package |
Additional environments to test the planner |
Extend the previous work on Task05 - Randomized sampling-based algorithms to planning with a non-holonomic Dubins vehicle. Implement the Probabilistic Roadmap (PRM) and Rapidly Exploring Random Trees (RRT) randomized sampling-based path planning algorithms. The algorithms shall provide a collision free path through the environment represented by a geometrical map.
In file DubinsPRMPlanner.py
implement the PRM algorithm that respects motion constraints of the Dubins vehicle.
In file DubinsRRTPlanner.py
implement the RRT algorithm that respects motion constraints of the Dubins vehicle.
The implementation requirements are as follows:
DubinsPRMPlanner
and DubinsRRTPlanner
plan a path in 3-DOF only - $x,y$ position of the robot and $\phi$ heading
DubinsPRMPlanner
and DubinsRRTPlanner
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 3 state-space variables $(x,y,\phi)$, where $x,y$ represent the position of the robot in the environment and $\phi \in (0,2\pi)$ represent the orientation of the robot
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,\phi)$
x_lower = limits[0][0] x_upper = limits[0][1] y_lower = limits[1][0] y_upper = limits[1][1] max_translation = 1/250.0 * np.max([ x_upper-x_lower, y_upper-y_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 Dubins vehicle model connects two configurations of the robot by a shortest curve, with a constraint on the curvature of the path and with prescribed initial and terminal tangents to the path. Hence, each Dubins path consists of either turns with a predefined radius (right turn (R), left turn (L)) or straight-line segments (straight (S)). The optimal path type can be described using these primitives and it will always be at least one of the six types: RSR, RSL, LSR, LSL, RLR, LRL.
We will use the Python implementation of the Dubins path (pip3 install dubins
)
https://github.com/AndrewWalker/pydubins
For our purpose of the randomized sampling-based motion planning we need to sample the path into individual configurations. An example of such a sampling can be done as follows
#!/usr/bin/env python3 import dubins as dubins import math import matplotlib.pyplot as plt x0 = 2 y0 = 2 theta0 = math.pi x1 = 0 y1 = 0 theta1 = 0 #math.pi turning_radius = 1.0 step_size = 0.1 plt.ion() for i in range(0,100): theta1 = i*math.pi/50.0 q0 = (x0, y0, theta0) q1 = (x1, y1, theta1) #calcuating the shortest path between two configurations path = dubins.shortest_path(q0, q1, turning_radius) print("Path maneuver: " + str(path.path_type()) + ", Path length: " + str(path.path_length())) #sampling the path configurations, _ = path.sample_many(step_size) #quiver plot of the path l = 0.2 xx = [x[0] for x in configurations] yy = [x[1] for x in configurations] uu = [math.cos(x[2])*l for x in configurations] vv = [math.sin(x[2])*l for x in configurations] plt.clf() plt.quiver(xx,yy,uu,vv) plt.axis('equal') ax = plt.gca() ax.axis([-3,3,-3,3]) plt.draw() plt.pause(0.1)
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 DubinsPRMPlanner as dubinsprm import DubinsRRTPlanner as dubinsrrt 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,math.pi/2), [(-3,3), (-3,3), (0,0), (0,0), (0,0), (0,2*math.pi)]), ("environments/squares", (2,-3,0,0,0,0), (4,8,0,0,0,0), [(0,5), (-5,10), (0,0), (0,0), (0,0), (0,2*math.pi)]), ("environments/maze1", (2,2,0,0,0,math.pi/4), (35,35,0,0,0,math.pi/2), [(0,40), (0,40), (0,0), (0,0), (0,0), (0,2*math.pi)]), ("environments/maze2", (2,2,0,0,0,math.pi/4), (35,26,0,0,0,3*math.pi/2), [(0,40), (0,40), (0,0), (0,0), (0,0), (0,2*math.pi)]), ("environments/maze3", (2,2,0,0,0,0), (25,36,0,0,0,0), [(0,40), (0,40), (0,0), (0,0), (0,0), (0,2*math.pi)]), ("environments/maze4", (2,2,0,0,0,0), (27,36,0,0,0,math.pi/2), [(0,40), (0,40), (0,0), (0,0), (0,0), (0,2*math.pi)])] #enable dynamic drawing in matplotlib plt.ion() ######################################## ## EVALUATION OF THE DUBINS 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 = dubinsrrt.DubinsRRTPlanner(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.01) ######################################## ## EVALUATION OF THE DUBINS 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 = dubinsprm.DubinsPRMPlanner(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.01)