Code Description

Driver Code

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

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