Code Description

Driver Code

RRTStarReedsShepp.add_graph(from_vertex, destination_data, steps_ag, cost, graph, tree_map, tree_lines, geom_skip_factor)

Helper method to add new edge or update them. This method also call update_tree.

Parameters:
  • from_vertex – Car object
  • destination_data – Car object
  • cost – cost of destination vertex
  • steps_ag – np.array of xyo
  • geom_skip_factor – Defines the precision of curves while drawing. Higher values results in course resolution.
Returns:

None
RRTStarReedsShepp.rand_state(env)

The limits of this random function are determined by env.resolution and angular range.

Returns:nd.array(x, y, orientation)
RRTStarReedsShepp.update_tree(destination_data, steps_ut, tree_to_append, geom_skip_factor)

Helper class to keep track of lines to be drawn (tree_exploration/final_path lines). If the path is ‘s’ or ‘b’ or ‘f’, then only the starting and end points of the line segment is tracked, otherwise for drawing the curve, the points are discretized based on geom_skip_factor value. This is to ensure memory optimization at the cost of final graph plot legibility.

Parameters:
  • destination_data – ReedsShepp car object.
  • steps_ut – nd.array Steps of xy coordinates
  • tree_to_append – The tree list reference onto which the path needs to updated
  • geom_skip_factor – Defines the precision of curves while drawing. Higher values results in course resolution.
Returns:

None.
RRTStarReedsShepp.vertices_within_box(reference_vertex, graph, tolerance, gamma, D)

To get only the vertices within the reference vertex. Radius is defined by balanced box decomposition. For more information, refer http://www.cs.berkeley.edu/~pabbeel/cs287-fa15/optreadings/rrtstar.pdf. :param reference_vertex: Vertex object :return: List of nearest vertices within the computed radius.

DrawGraph.plot_from_file()

This method plot the environment and the path from the output file generated by the RRTStarReedsShepp method.

ReedsShepp Car

class ReedsSheppCar.ReedsShepp(xy, orientation, cost, variable_time_integration=True, env_check=None, action=None)
__init__(xy, orientation, cost, variable_time_integration=True, env_check=None, action=None)

Car class to define ReedsShepp car object. Any mentioning of configuration mean [x, y, orientation] data structure.

Parameters:
  • xy – xy coordinate tuple of the rear axle center
  • orientation – orientation angle of the car w.r.t to x-axis
  • cost – cost to reach the current configuration from initial configuration
  • variable_time_integration – True if the integration needs to be performed over a variable time, and is used for RRT*. False if the integration step size is fixed, and is used for RRT.
  • env_check – Function to check if the given configuration is intersecting an obstacle of not. Function should accept an argument of tuple/list/nd.array which states and center point of rear axle and orientation. (x, y, orientation) is the format.
  • action – A String, which states on what action (“straight”, “left”, “right”, “reverse”, “left_reverse”, “right_reverse”) the current object is derived from the parent and the time for which the control was given.
Returns:

None
_fn(t, data, us_fn, uphi_fn)

Function that defines the differential constraint for ReedsShepp car.Differential constraints for ReedsShepp Car is as mentioned in http://planning.cs.uiuc.edu/ch13.pdf pg. 6 of the pdf.

Parameters:
  • t – time. Here this doesn’t matter as the differential equation doesn’t have a variable t.
  • data – (x, y, orientation) tuple or list or nd.array.
  • us_fn – Linear velocity in m/s or cm/s.
  • uphi_fn – Turning angle of the car
Returns:

Next state after integrating or time t.
_generate_steer_possibilities_fixed()

Function to generate all possible 6 steer methods whose integration time is limited to data[“epsilon”] for the car at this current configuration. This function updates self.steer_possibilities_fixed dictionary.

Returns:None
_generate_steer_possibilities_vr()

Function to generate all possible 6 steer methods whose integration time is limited to data[“time_to_cover_entire_perimeter”] for the car at this current configuration. This function updates self.steer_possibilities_vr dictionary.

Returns:None
_roll_over(angle)

Converting an angle which is not bounded by any limit to an angle in -math.pi to math.pi range. eg: math.radians(182) = math.radians(-172)

Parameters:angle – in radians
Returns:Angle in -math.pi to math.pi
_runge_kutta_fixed(us_rk, uphi_rk)

4th order runge-kutta integration method with fixed step size given in data[“epsilon”]. Note that the integration breaks once the car hits an obstacle. Return next state after integrating over the step size.

Parameters:
  • us_rk – Linear velocity control input. A number.
  • uphi_rk – Turning angle control input. A number.
Returns:

None if no possible steps, else a numpy array of numpy array which gives the information of on x, y, orientation upon each integrating steps (h).
_runge_kutta_helper(ur_h, uphi_h, epsilon)

Helper function for runge-kutta 4th order integration.

Parameters:
  • ur_h – Linear velocity control input. A number.
  • uphi_h – Turning angle control input. A number.
  • epsilon – Step size of integration.
Returns:

None if no possible steps, else a numpy array of numpy array which gives the information of on x, y, orientation upon each integrating steps (h).
_runge_kutta_variable_step(Us_rk, Uphi_rk)

4th order runge-kutta integration method step size limited to data[“time_to_cover_entire_perimeter”]. Note that the integration breaks once the car hits an obstacle. Return next state after integrating over the step size.

Parameters:
  • us_rk – Linear velocity control input. A number.
  • uphi_rk – Turning angle control input. A number.
Returns:

None if no possible steps, else a numpy array of numpy array which gives the information of on x, y, orientation upon each integrating steps (h).
get_next_best_state(nearest_conf, distance_function)
Obtains the best possible steer method to reach nearest_conf configuration as close as possible from the current configuration. Note that this method considers fixed integration. Raises exception if self.variable_time_integration is True.
Parameters:
  • nearest_conf – The destination configuration.
  • distance_function – Distance function for determining the closest neighbour.
Returns:

None if no possible motions else, a tuple with control name at first index (0) and np.array of next possible configurations at second index (1).
steer(z_end, distance_fn, tolerance_steer, within_tolerance=False)

Obtains the best possible steer method to reach z_end configuration from the current configuration. This method takes the best of best configuration of all possible steer types (“straight”, “left”, etc). Note that this method considers variable integration. Raises exception if self.variable_time_integration is False.

Parameters:
  • z_end – The destination configuration.
  • distance_fn – Distance function for determining the closest neighbour.
  • within_tolerance – boolean. True if the destination needs to be reached within the tolerant limit, and False if the car needs to reach z_end as close as possible.
Returns:

None if no possible motions else, a tuple of following format. (min_steer_name, (min_t, min_value, min_index, steps)), where min_steer_name is the name of the action, min_t is the integration time, min_value is the value from distance fuction between z_end and config at min_t and steps is the nd.array for all the intermediate configurations.
class ReedsSheppCar.DataNotDefinedException

static variable “data” a dictionary needs to be defined before using this class. “data” should have the following attributes.

  • data[‘l’] : distance between front_axle and rear axle of the car
  • data[‘Us’] : linear_velocity of the car
  • data[‘Uphi’] : maximum turning angle of the car (same of all turns)
  • data[“ro_min”] : minimum turning radius, given by abs(data[“l”]/math.tan(data[“Uphi”]))
  • data[“delta_t”] : h value for 4th order runge kutta integration
  • data[“epsilon”] : step size for integration. Required for RRT and not for RRTStar
  • data[“time_to_cover_entire_perimeter”] : time to cover entire perimeter for the car, given by 2*math.pi*data[“ro_min”]/data[“Us”]. i.e, distance/velocity.
  • data[“collision_check_skip_factor”] : how often the collision check should be skipped while integrating.
  • data[“tolerance”] : maximum tolerance between any match in configuration
  • data[“theta_norm”] : [x, y, theta] normalization vector for the distance function.

Environment Configuration

class Config.Environment(input_file)
__init__(input_file)

Environment helper class for collision detection and matplotlib plotting.

Parameters:input_file – File name on input json.
Returns:None
_change_axis(theta, xy)

This is a rotation technique. The car object is defined in environment as if its center (center of rear axle) is super imposed in the origin of euclidean plane. To get the new position, i.e, the coordinates of its vertices the position of the car is at xy and rotated to theta radians, first we need to rotate the car theta radians from origin position, then add the translated xy coordinates to all the vertices.

  • rot(theta) = [[cos(theta), -sin(theta)], [sin(theta), cos(theta)]]
  • point = [[vertex_point[0]], [vertex_point[1]]]
  • coordinate after rotation, CR = rot(theta) x point
  • after translation, AT = CR + xy
Parameters:
  • theta – Orientation of car w.r.t x-axis.
  • xy – Center of rear axle after translation.
Returns:

nd.array of rotated and translated vertices.
draw_env(lines, path)

Method to draw an arrow in the environment. Format for lines and path is [[[x1, y1], [x2, y2]], [[x3, y3], [x4, y4]]], where [[x1, y1], [x2, y2]] is one line segment.

Parameters:
  • lines – List of blue lines.
  • path – List of black lines.
Returns:
get_free_area()

To get the total free area.

Returns:Total area - obstacle area.
is_car_inside(xyo)

Check if the car is intersecting any obstacles.

Parameters:xyo – Position of the center of rear axle.
Returns:Boolean. True if car intersects any obstacle and False if car is not the boundary.
is_car_within_boundary(xyo)

Check if a car is inside boundary.

Parameters:xyo – Position of the center of rear axle.
Returns:Boolean. True if car is within boundary and False if car is outside the boundary.
is_point_inside(xy)
Parameters:xy – tuple with x coordinate as first element and y coordinate as second element
Returns:True if the point is inside the obstacles and False if it isn’t
is_within_boundary(xy)

Check if a point is inside boundary.

Parameters:xy – Position of the point
Returns:Boolean. True if point is within boundary and False if point is outside the boundary.
read_env_from_file(input_file)

Read json input and sets the object variables.

Parameters:input_file – Input json file name.
Returns:None

Graph Data Structure

class Graph.Vertex(name, data)
__init__(name, data)

Vertex class for graph data structure.

Parameters:
  • name – Name of the vertex.
  • data – Additional associated data.
Returns:

None
remove_edge(destination)

Removes an edge. raise key error if the object is not found. This O(1) operation since the self.adj_vertices is a dictionary.

Parameters:destination – vertex object
Returns:None.
class Graph.Edge(source, destination, weight)
__init__(source, destination, weight)

Edge class to store edge information.

Parameters:
  • source – Source vertex object.
  • destination – Destination vertex object.
  • weight – Edge weight which is a number.
Returns:

None
class Graph.Graph
__init__()

Graph data structure. self.vertex_map holds all the vertex mapped by its name. Hence the name needs to hashable.

add_edge(source, destination, weight, source_data=None, destination_data=None, directed=False, ignore_previous_destination_data_validation=True, set_destination_parent=False)

Method to add a new edge.

Parameters:
  • source – Source object.
  • destination – Destination object.
  • weight – Edge weight.
  • source_data – Optional source_data if adding the source vertex for the first time.
  • destination_data – Optional destination_data if adding the destination vertex for the first time.
  • directed – True for undirected and False for directed.
  • ignore_previous_destination_data_validation – If set to True, raise ObjectAlreadyPresentException if destination is present already. If set to False, the method ignore this check.
  • set_destination_parent – If directed it set to True and set_destination_parent set to True, then each child will have it parent stored in variable pi in vertex object.
Returns:

None
add_vertex(node_name, data=None)

Add a new vertex.

Parameters:
  • node_name – Name of the vertex.
  • data – Associated data object.
has_key(k)

To check is a vertex k exists of not.

Parameters:k – Vertex name.
Returns:boolean. True if present, False if not.
min_path(source, goal, return_as_list=False)

Dijkstra algorithm.

Parameters:
  • source – Source name
  • goal – Destination name
  • return_as_list – Boolean variable which determines the return output.
Returns:

If return_as_list is set to True, return a list of vertex names as a list else, None.
traverse_to(source, destination)

To traverse from child to parent until the parent’s pi variable is None.

Parameters:
  • source – Source object name.
  • destination – Destination object name.
Returns:

A list of traversal path.
class MinHeap.MinHeap(data, key)

This class is for Object and key for which the priority has to built should be passed while initializing. Build heap method will be called once the object is instantiated. updateData will also call the build heap method with the new data.

build_heap()

Builds heap from self.data.

Returns:None
decrease_key(node, newValue, setKeyFunction)

Decrease the key by bubble up operation.

Parameters:
  • node – The object whose priority has to decreased
  • newValue – The new value of the Key in the object
  • setKeyFunction – Key setter function in object
Returns:

Boolean
extract_min()

To extract min element from the heap.

Returns:Min element object
min_heapify(i)

Performs min heapify at index i.

Parameters:i – index
Returns:None
updateData(data, key)

This method populates data and updates key if a heap object has to be reused.

Parameters:
  • data – iteratable data
  • key – function to point the comparable variable.
Returns:

None