Motion Planners

The following are all of the available motion planners in sbp-env.

RRT*

class sbp_env.planners.rrtPlanner.RRTPlanner(args)[source]

Bases: Planner

The Rapidly-exploring random tree.

Rapidly-exploring Random Tree* (RRT*) is an anytime, asymptotic optimal sampling-based motion planner that continuously updates its solution within the given budget. The planner itself is implemented in RRTPlanner, while the default sampling policy is implemented in RandomPolicySampler.

Parameters: args (PlanningOptions) –

add_newnode(node)[source]

Add a new node to the tree

Parameters: node (Node) – node to be added

choose_least_cost_parent(newnode, nn=None, nodes=None, skip_optimality=False, use_rtree=False, poses=None)[source]

Given a new node, a node from root, return a node from root that has the least cost (toward the newly added node)

Parameters

newnode (Node) – the newly created node that we want to search for a new parent
nn (Optional[Node], optional) – the current closest node, optional.
Default: None
nodes (Optional[List[Node]], optional) – the list of node to search against
Default: None
skip_optimality (bool, optional) – skip searching for optimality (i.e. non-asymptomatic)
Default: False
use_rtree (bool, optional) – whether use rtree to store tree or not
Default: False
poses (Optional[List[ndarray]], optional) – list of configurations, with the same length as nodes
Default: None

Return type

Node

Returns

the node with the lowest cost

static find_nearest_neighbour_idx(pos, poses)[source]

Find the nearest neighbour from the list of nodes

Parameters

pos (ndarray) – the position to search against the array of positions
poses (ndarray) – the array of positions

init(**kwargs)[source]

The delayed initialisation function

Parameters

start_pt (numpy.ndarray) – the start configuration
goal_pt (numpy.ndarray) – the goal configuration

rewire(newnode, nodes, already_rewired=None, skip_optimality=False, use_rtree=True, poses=None)[source]

Rewire the given new node

Parameters

newnode (Node) – the newly created node that we want to rewires
nodes (List[Node]) – the list of node to search against
already_rewired (Optional[bool], optional) – if the node had already been rewired previously
Default: None
skip_optimality (bool, optional) – skip optimality to speed up (reduce to RRT as opposed to RRT*)
Default: False
use_rtree (bool, optional) – if we should use rtree as opposed to native numpy operations
Default: True
poses (Optional[ndarray], optional) – an array of positions
Default: None

run_once()[source]

Execute the main planning procedure once (mostly of the time this corresponds to adding one new node to the tree)

This where the main bulk of actions happens, e.g., creating nodes or edges.

The sampler is registered as follows

sampler_id = "random"

planner_registry.register_sampler(sampler_id, sampler_class=RandomPolicySampler)

and the planner is registered as follows

planner_registry.register_planner(
    "rrt",
    planner_class=RRTPlanner,
    visualise_pygame_paint=pygame_rrt_paint,
    visualise_klampt_paint=klampt_rrt_paint,
    sampler_id="random",
)

Bi-RRT*

class sbp_env.planners.birrtPlanner.BiRRTPlanner(args)[source]

Bases: RRTPlanner

The bidrectional RRT* planner, or sometimes it’s also referred to as the RRT-Connect*.

The class BiRRTPlanner uses an adopted version of random policy sampler that makes it suitable for using in both the start and goal trees, which is implemented in BiRRTSampler.

Parameters: args (PlanningOptions) –

init(**kwargs)[source]

The delayed initialisation function

Parameters

start_pt (numpy.ndarray) – the start configuration
goal_pt (numpy.ndarray) – the goal configuration

run_once()[source]

Execute the main planning procedure once (mostly of the time this corresponds to adding one new node to the tree)

This where the main bulk of actions happens, e.g., creating nodes or edges.

The sampler is registered as follows

planner_registry.register_sampler(
    "birrt_sampler",
    sampler_class=BiRRTSampler,
)

and the planner is registered as follows

planner_registry.register_planner(
    "birrt",
    planner_class=BiRRTPlanner,
    visualise_pygame_paint=pygame_birrt_planner_paint,
    visualise_klampt_paint=klampt_birrt_paint,
    sampler_id="birrt_sampler",
)

RRdT*

class sbp_env.planners.rrdtPlanner.RRdTPlanner(args)[source]

Bases: RRTPlanner

The Rapidly-exploring Random disjointed-Trees. The RRdT* planner is implemented based on Lai et. al.’s 1 work. The main idea is that the planner keeps a pool of disjointed trees

\[\mathbb{T}=\{\mathcal{T}_\text{root}, \mathcal{T}_1, \ldots, \mathcal{T}_k\} \]

where it consists of a rooted tree that connects to the \(q_\text{start}\) starting configuration, and \(k\) many disjointed trees that randomly explores C-Space. Each disjointed tree is modelled as an arm in the Multi-Armed Bandit problem, i.e. each \(\mathcal{T}_i\) has an arm \(a_i\), where the probability to draw each arm is dependent on its previous success as given by

\[\mathbb{P}(a_{i,t} \mid a_{i,t-1}, o_{t-1})\,\forall_{i\in\{1,...,k\}} \]

with \(o_{t-1}\) as the arm \(a_i\)’s previous observation.

1: Lai, Tin, Fabio Ramos, and Gilad Francis. “Balancing global exploration and local-connectivity exploitation with rapidly-exploring random disjointed-trees.” 2019 International Conference on Robotics and Automation ( ICRA). IEEE, 2019.

Parameters: args (PlanningOptions) –

add_pos_to_existing_tree(newnode, parent_tree)[source]

Try to add pos to existing tree. If success, return True.

Parameters

newnode (Node) – the node to be added
parent_tree (Optional[DTreeType]) – the tree to add the node

Return type

Optional[DTreeType]

connect_two_nodes(newnode, nn, parent_tree=None)[source]

Add node to disjoint tree OR root tree.

Parameters

newnode (Node) – the new node to connects
nn (Optional[Node]) – a node from the existing tree to be connected
parent_tree (Optional[DTreeType], optional) – if given, add newnode to this tree
Default: None

find_nearest_node_from_neighbour(node, parent_tree, radius)[source]

Given a tree, a node within that tree, and radius Return a list of cloest nodes (and its corresponding tree) within the radius (that’s from other neighbourhood trees)

Parameters

node (Node) – the node to be added
parent_tree (DTreeType) – the tree to add the given node
radius (float) – the maximum radius to add the given node

Return type

List[Tuple[Node, DTreeType]]

Returns

a list of potential nodes

join_tree_to_root(tree, middle_node, root_tree_node)[source]

It will join the given tree to the root

Parameters

tree (DTreeType) – the disjointed tree to be added to root tree
middle_node (Node) – the middle node that connects the disjointed tree and the root tree
root_tree_node (Node) – a node from the root tree

join_trees(tree1, tree2, tree1_node, tree2_node)[source]

Join the two given tree together (along with their nodes). It will delete the particle reference from the second tree. It will use RRT* method to add all nodes if one of the tree is the ROOT.

tree1_node & 2 represent the nodes that join the two tree together. It only matters currently to joining root tree to disjointed tree itself.

Parameters

tree1 (DTreeType) – disjointed tree 1 \(\mathcal{T}_1\)
tree2 (DTreeType) – disjointed tree 2 \(\mathcal{T}_2\)
tree1_node (Node) – a node from tree 1 \(v_1 \in \mathcal{T}_1\)
tree2_node (Node) – a node from tree 2 \(v_2 \in \mathcal{T}_2\)

rrt_star_add_node(newnode, nn=None)[source]

This function perform finding optimal parent, and rewiring.

Parameters

newnode (Node) – the node to add to the tree
nn (Optional[Node], optional) – an approximate of nearest node
Default: None

run_once()[source]

Execute the main planning procedure once (mostly of the time this corresponds to adding one new node to the tree)

This where the main bulk of actions happens, e.g., creating nodes or edges.

The sampler and planner is registered as follows

sampler_id = "rrdt_sampler"

planner_registry.register_sampler(
    sampler_id,
    sampler_class=RRdTSampler,
    visualise_pygame_paint=pygame_rrdt_sampler_paint,
    visualise_pygame_paint_init=pygame_rrdt_sampler_paint_init,
)

planner_registry.register_planner(
    "rrdt",
    planner_class=RRdTPlanner,
    visualise_pygame_paint=pygame_rrdt_planner_paint,
    visualise_klampt_paint=klampt_rrdt_planner_paint,
    sampler_id=sampler_id,
)

Informedrrt-RRT*

The Informed-RRT* motion planning behaves similar to sbp_env.planners.rrtPlanner .RRTPlanner before a first solution is found. After an initial solution is found, this planner uses an ellipses heuristic to speed up convergence rate of the resulting solution.

The bulk of the implementation occurs in InformedRRTSampler. The Informed-RRT* is implemented by directly deriving the base motion planner as RRTPlanner, and registering its sampler as the InformedRRTSampler.

The sampler is registered as follows

sampler_id = "informed_sampler"

planner_registry.register_sampler(
    sampler_id,
    sampler_class=InformedSampler,
    visualise_pygame_paint=pygame_informed_sampler_paint,
)

and the planner is registered as follows

planner_registry.register_planner(
    "informedrrt",
    planner_class=rrtPlanner.RRTPlanner,
    visualise_pygame_paint=rrtPlanner.pygame_rrt_paint,
    sampler_id=informedSampler.sampler_id,
)

PRM*

class sbp_env.planners.prmPlanner.PRMPlanner(args)[source]

Bases: RRTPlanner

Probabilistic Roadmap motion planner, the multi-query sampling-based planner.

Parameters: args (PlanningOptions) –

build_graph()[source]: Performs the graph building process where

\[G = (V, E). \]

clear_graph()[source]: Clear the current roadmap graph

get_nearest_free(node, neighbours)[source]

Internal method to get the closest existing node that is free to connects to the given node.

Parameters

node (Node) – the node of interest
neighbours (List[Node]) – the list of nodes to search against

get_solution()[source]: Build the solution path

init(*argv, **kwargs)[source]

The delayed initialisation function

Parameters

start_pt (numpy.ndarray) – the start configuration
goal_pt (numpy.ndarray) – the goal configuration

run_once()[source]

Execute the main planning procedure once (mostly of the time this corresponds to adding one new node to the tree)

This where the main bulk of actions happens, e.g., creating nodes or edges.

The sampler is registered as follows

sampler_id = "prm_sampler"

planner_registry.register_sampler(
    sampler_id,
    sampler_class=PRMSampler,
)

and the planner is registered as follows

planner_registry.register_planner(
    "prm",
    planner_class=PRMPlanner,
    visualise_pygame_paint=pygame_prm_planner_paint,
    visualise_pygame_paint_terminate=pygame_prm_planner_paint_when_terminate,
    visualise_klampt_paint=klampt_prm_paint,
    visualise_klampt_paint_terminate=klampt_prm_planner_paint_when_terminate,
    sampler_id=prmSampler.sampler_id,
)

Likelihood Planner

Refers to sbp_env.samplers.likelihoodPolicySampler.LikelihoodPolicySampler for details.

The bulk of the implementation occurs in LikelihoodPolicySampler. The Informed-RRT* is implemented by directly deriving the base motion planner as RRTPlanner, and registering its sampler as the LikelihoodPolicySampler.

The sampler is registered as follows

sampler_id = "likelihood_sampler"

planner_registry.register_sampler(
    sampler_id,
    sampler_class=LikelihoodPolicySampler,
    visualise_pygame_paint=pygame_likelihood_sampler_paint,
    visualise_pygame_paint_init=pygame_likelihood_sampler_paint_init,
)

and the planner is registered as follows

planner_registry.register_planner(
    "likelihood",
    planner_class=rrtPlanner.RRTPlanner,
    visualise_pygame_paint=rrtPlanner.pygame_rrt_paint,
    sampler_id=likelihoodPolicySampler.sampler_id,
)

Nearby Planner

Refers to sbp_env.samplers.nearbyPolicySampler.NearbyPolicySampler for details.

The sampler is registered as follows

sampler_id = "nearby_sampler"

planner_registry.register_sampler(
    sampler_id,
    sampler_class=likelihoodPolicySampler.LikelihoodPolicySampler,
    visualise_pygame_paint=likelihoodPolicySampler.pygame_likelihood_sampler_paint,
    visualise_pygame_paint_init=likelihoodPolicySampler.pygame_likelihood_sampler_paint_init,
)

and the planner is registered as follows

planner_registry.register_planner(
    "nearby",
    planner_class=rrtPlanner.RRTPlanner,
    visualise_pygame_paint=rrtPlanner.pygame_rrt_paint,
    sampler_id=nearbyPolicySampler.sampler_id,
)

Mouse Planner

Refers to sbp_env.samplers.mouseSampler.MouseSampler for details.

The sampler is registered as follows

sampler_id = "mouse"

planner_registry.register_sampler(
    sampler_id,
    sampler_class=MouseSampler,
)

and the planner is registered as follows

planner_registry.register_planner(
    "mouse",
    planner_class=rrtPlanner.RRTPlanner,
    visualise_pygame_paint=rrtPlanner.pygame_rrt_paint,
    sampler_id=mouseSampler.sampler_id,
)

Abstract Base Planner

There is also a special base planner that all motion planner should be derived from.

class sbp_env.planners.basePlanner.Planner(args)[source]

Bases: ABC

Abstract base planner that does nothing by itself.

Parameters: args (PlanningOptions) –

get_solution_path()[source]

Retrieve the current solution path via transversing each connected node’s parent, in reverse, starting from the goal node.

Returns: a list of node that represent the solution path