pyroboplan.planning package

Motion planning module.

This module contains motion planners and related utilities.

Submodules

pyroboplan.planning.cartesian_planner module

Utilities for Cartesian (or task space) motion planning.

class pyroboplan.planning.cartesian_planner.CartesianPlanner(model, target_frame, tforms, ik_solver, options=<pyroboplan.planning.cartesian_planner.CartesianPlannerOptions object>)

Bases: object

Cartesian (or task space) motion planner.

This tool will plan a Cartesian trajectory through a set of poses, and then use an inverse kinematics solver to incrementally solve for valid robot joint configurations to achieve this task-space trajectory.

Some good resources:
generate(q_init, dt)

Generates a set of joint trajectories from the Cartesian plan at a specified sample time.

Parameters:

  • q_init (array-like) – The starting joint configuration for the robot. This is used as the initial condition for solving inverse kinematics.

  • dt (float) – The sample time to use for trajectory generation, in seconds.

Returns:

The tuple of (success, t_vec, q_vec) corresponding to a success metric, the vector of times, and vector of joint positions, respectively.

Return type:

tuple(bool, array-like, array-like)

class pyroboplan.planning.cartesian_planner.CartesianPlannerOptions(use_trapezoidal_scaling=True, max_linear_velocity=1.0, max_linear_acceleration=1.0, max_angular_velocity=1.0, max_angular_acceleration=1.0)

Bases: object

Options for Cartesian planning.

pyroboplan.planning.graph module

Generic graph class for use in robot motion planning algorithms.

class pyroboplan.planning.graph.Edge(nodeA, nodeB, cost=0.0)

Bases: object

Describes an individual edge in a graph.

static parse(s)

Reconstruct an Edge object from its string representation.

class pyroboplan.planning.graph.Graph

Bases: object

Describes a graph of robot configuration states for motion planning.

add_edge(nodeA, nodeB)

Adds an edge to the graph.

This is an undirected graph, so if A -> B is an edge, so is B -> A.

Parameters:

  • nodeA (pyroboplan.planning.graph.Node) – The first node in the edge.

  • nodeB (pyroboplan.planning.graph.Node) – The second node in the edge.

Returns:

The edge that was added.

Return type:

pyroboplan.planning.graph.Edge

add_node(node)

Adds a node to the graph.

Parameters:

node (pyroboplan.planning.graph.Node) – The node to add to the graph.

get_nearest_neighbors(q, radius)

Gets a list of the nearest neighbors to the specified robot configuration.

Neighboring nodes will be returned as a sorted list of tuples of nodes along with the distance to the specified configuration. If the provided configuration is in the graph, it will not be returned (no distance 0 node will be returned).

Parameters:

  • q (array-like) – The robot configuration to use in this query.

  • radius (float) – The maximum radius in which to consider neighbors

Returns:

A list of tuples of all the nodes within the specified radius of the provided robot configuration, along with their corresponding distances.

Return type:

List of (pyroboplan.planning.graph.Node, float)

get_nearest_node(q)

Gets the nearest node to a specified robot configuration. If the configuration is in the graph the corresponding node will be returned. Namely, the node with distance 0.

Parameters:

q (array-like) – The robot configuration to use in this query.

Returns:

The nearest node to the input configuration, or None if the graph is empty.

Return type:

pyroboplan.planning.graph.Node or None

get_node(q)

Returns the node corresponding to the provided robot configuration, or None if not present.

Raises a ValueError if the specified configuration is not in the Graph.

Parameters:

q (array-like) – The robot configuration for the desired node.

Returns:

The node with the specified robot configuration.

Return type:

pyroboplan.planning.graph.Node

classmethod load_from_file(filename)

Loads a graph from a file.

Parameters:

filename (str) – The full filepath from which to read a Graph.

Returns:

The reconstructed Graph from file.

Return type:

pyroboplan.planning.graph.Graph

remove_edge(nodeA, nodeB)

Attempts to remove an edge from a graph, if it exists.

Parameters:

  • nodeA (pyroboplan.planning.graph.Node) – The first node in the edge.

  • nodeB (pyroboplan.planning.graph.Node) – The second node in the edge.

Returns:

True if the edge was successfully removed, else False.

Return type:

bool

remove_node(node)

Removes a node from the graph, along with all of its corresponding edges.

Parameters:

node (pyroboplan.planning.graph.Node) – The node to remove from the graph.

Returns:

True if the graph was modified, False otherwise.

Return type:

bool

save_to_file(filename)

Write the graph to file.

Only node and edge data is persisted. Neighbors can be rederived, but all parent information is lost.

Parameters:

filename (str) – The full filepath in which to save the Graph.

class pyroboplan.planning.graph.Node(q, parent=None, cost=0.0)

Bases: object

Describes an individual node in a graph.

static parse(s)

Reconstruct a Node object from its string representation.

pyroboplan.planning.path_shortcutting module

Capabilities to shorten paths produced by motion planners.

pyroboplan.planning.path_shortcutting.get_configuration_from_normalized_path_scaling(q_path, path_scalings, value)

Given a path and its corresponding length-normalized scalings between 0.0 and 1.0, get the joint configuration at a specific scale value.

For example:

  • 0.0 will return the start configuration of the path

  • 1.0 will return the end configuration of the path

  • 0.5 will return the configuration halfway along the length of the path

Parameters:

  • q_path (list[array-like]) – The list of joint configurations describing the path.

  • path_scalings (list[float]) – The list of scaling values, between 0.0 and 1.0, at each of the path waypoints.

  • value (float) – The value, between 0.0 and 1.0, at which to get the joint configuration along the path.

Returns:

A tuple containing the joint configuration at the specified scaling value along the path, as well as the index corresponding to the next point along the path.

Return type:

tuple (array-like, int)

pyroboplan.planning.path_shortcutting.get_normalized_path_scaling(q_path)

Returns a list of length-normalized scaling values along a joint configuration path, from 0.0 at the start to 1.0 at the end.

Parameters:

q_path (list[array-like]) – The list of joint configurations describing the path.

Returns:

The list of scaling values, between 0.0 and 1.0, at each of the path waypoints.

Return type:

list[float]

pyroboplan.planning.path_shortcutting.shortcut_path(model, collision_model, q_path, max_iters=100, max_step_size=0.05)

Performs path shortcutting by sampling two random points along the path and verifying whether those two points can be connected directly without collisions.

This implementation is based on section 3.5.3 of: https://motion.cs.illinois.edu/RoboticSystems/MotionPlanningHigherDimensions.html

Parameters:

  • model (pinocchio.Model) – The model of the robot.

  • collision_model (pinocchio.Model.) – The model to use for collision checking.

  • q_path (list[array-like]) – The list of joint configurations describing the path.

  • max_iters (int) – The maximum iterations for randomly sampling shortcut points.

  • max_step_size (float) – Maximum joint configuration step size for collision checking along path segments.

Returns:

The shortest length path over all the shortcutting attempts.

Return type:

list[array-like]

pyroboplan.planning.prm module

Utilities for manipulation-specific Probabilistic Roadmaps (PRMs).

class pyroboplan.planning.prm.PRMPlanner(model, collision_model, options=<pyroboplan.planning.prm.PRMPlannerOptions object>)

Bases: object

Probabilistic Roadmap (PRM) planner.

This is a sampling-based motion planner that constructs a graph in the free configuration space and then searches for a path using standard graph search functions.

Graphs can be persisted to disk for use in future applications.

Some helpful resources:
connect_node(new_node, radius, k)

Identifies all neighbors and makes connections to the added node.

Parameters:

  • parent_node (pyroboplan.planning.graph.Node) – The node to add.

  • radius (float) – Only consider connections within the provided radius.

  • k (int) – Only consider up to a maximum of k neighbors to make connections.

Returns:

True if the node was connected to the graph. False otherwise.

Return type:

bool

connect_planning_nodes(start_node, goal_node)

Ensures the start and goal configurations can be connected to the PRM.

Parameters:

  • start_node (pyroboplan.planning.graph.Node) – The start node to connect.

  • goal_node (pyroboplan.planning.graph.Node) – The goal node to connect.

Returns:

True if the nodes were able to be connected, False otherwise.

Return type:

bool

construct_roadmap(sample_generator=None)

Grows the graph by sampling nodes using the provided generator, then connecting them to the Graph. The caller can optionally override the default generator, if desired.

Parameters:

sample_generator (Generator[array-like, None, None]) – The sample function to use in construction of the roadmap. Defaults to randomly sampling the robot’s configuration space.

plan(q_start, q_goal)

Plans a path from a start to a goal configuration using the constructed graph.

Parameters:

  • q_start (array-like) – The starting robot configuration.

  • q_goal (array-like) – The goal robot configuration.

Returns:

A path from the start to the goal state, if one exists. Otherwise None.

Return type:

list[array-like]

reset()

Resets the PRM’s transient data between queries.

visualize(visualizer, frame_name, path_name='planned_path', graph_name='prm', show_path=True, show_graph=False)

Visualizes the PRM and the latest path found within it.

Parameters:

  • visualizer (pinocchio.visualize.meshcat_visualizer.MeshcatVisualizer, optional) – The visualizer to use for this solver.

  • frame_name (str) – The name of the frame to use when visualizing paths in Cartesian space.

  • path_name (str, optional) – The name of the MeshCat component for the path.

  • graph_name (str, optional) – The name of the MeshCat component for the tree.

  • show_path (bool, optional) – If true, shows the final path from start to goal.

  • show_graph (bool, optional) – If true, shows the entire PRM graph.

class pyroboplan.planning.prm.PRMPlannerOptions(max_step_size=0.05, max_neighbor_radius=0.5, max_neighbor_connections=15, max_construction_nodes=5000, construction_timeout=10.0, rng_seed=None, prm_star=False, prm_file=None)

Bases: object

Options for Probabilistic Roadmap (PRM) planning.

pyroboplan.planning.prm.random_model_state_generator(model)

pyroboplan.planning.rrt module

Utilities for manipulation-specific Rapidly-Exploring Random Trees (RRTs).

class pyroboplan.planning.rrt.RRTPlanner(model, collision_model, options=<pyroboplan.planning.rrt.RRTPlannerOptions object>)

Bases: object

Rapidly-expanding Random Tree (RRT) planner.

This is a sampling-based motion planner that finds collision-free paths from a start to a goal configuration.

Some good resources:
add_node_to_tree(tree, q_new, parent_node)

Add a new node to the tree. If the RRT* algorithm is enabled, will also rewire.

Parameters:

  • tree (pyroboplan.planning.graph.Graph) – The tree to which to add the new node.

  • q_new (array-like) – The robot configuration from which to create a new tree node.

  • parent_node (pyroboplan.planning.graph.Node) – The parent node to connect the new node to.

Returns:

The new node that was added to the tree.

Return type:

pyroboplan.planning.graph.Node

extend_or_connect(tree, parent_node, q_sample)

Extends a tree towards a sampled node with steps no larger than the maximum connection distance.

Parameters:

  • tree (pyroboplan.planning.graph.Graph) – The tree to use when performing this operation.

  • parent_node (pyroboplan.planning.graph.Node) – The node from which to start extending or connecting towards the sample.

  • q_sample (array-like) – The robot configuration sample to extend or connect towards.

Returns:

The latest node that was added to the tree, or None if no node was found.

Return type:

pyroboplan.planning.graph.Node, optional

extract_path_from_trees(start_tree_final_node, goal_tree_final_node)

Extracts the final path from the RRT trees

from the start tree root to the goal tree root passing through both final nodes.

Parameters:

  • start_tree_final_node (pyroboplan.planning.graph.Node) – The last node of the start tree.

  • goal_tree_final_node (pyroboplan.planning.graph.Node, optional) – The last node of the goal tree. If None, this means the goal tree is ignored.

Returns:

A list of robot configurations describing the path waypoints in order.

Return type:

list[array-like]

plan(q_start, q_goal)

Plans a path from a start to a goal configuration.

Parameters:

  • q_start (array-like) – The starting robot configuration.

  • q_goal (array-like) – The goal robot configuration.

reset()

Resets all the planning data structures.

visualize(visualizer, frame_name, path_name='planned_path', tree_name='rrt', show_path=True, show_tree=False)

Visualizes the RRT path.

Parameters:

  • visualizer (pinocchio.visualize.meshcat_visualizer.MeshcatVisualizer, optional) – The visualizer to use for this solver.

  • frame_name (str) – The name of the frame to use when visualizing paths in Cartesian space.

  • path_name (str, optional) – The name of the MeshCat component for the path.

  • tree_name (str, optional) – The name of the MeshCat component for the tree.

  • show_path (bool, optional) – If true, shows the final path from start to goal.

  • show_tree (bool, optional) – If true, shows the entire sampled tree.

class pyroboplan.planning.rrt.RRTPlannerOptions(max_step_size=0.05, max_connection_dist=inf, rrt_connect=False, bidirectional_rrt=False, rrt_star=False, max_rewire_dist=inf, max_planning_time=10.0, rng_seed=None, fast_return=True, goal_biasing_probability=0.0, collision_distance_padding=0.0)

Bases: object

Options for Rapidly-exploring Random Tree (RRT) planning.

pyroboplan.planning.utils module

Utilities for motion planning.

pyroboplan.planning.utils.discretize_joint_space_path(q_path, max_angle_distance)

Discretizes a joint space path given a maximum angle distance between samples.

This is used primarily for producing paths for collision checking.

Parameters:

  • q_path (list[array-like]) – A list of the joint configurations describing a path.

  • max_angle_distance (float) – The maximum angular displacement, in radians, between samples.

Returns:

A list of joint configuration arrays between the start and end points, inclusive.

Return type:

list[array-like]

pyroboplan.planning.utils.discretized_joint_space_generator(model, step_size, generate_random=True)

Discretizes the entire joint space of the model at step_size increments. Once the entire space has been returned, the generator can optionally continue returning random samples from the configuration space - in which case this generator will never terminate.

This is an extraordinarily expensive operation for high DOF manipulators and small step sizes!

Parameters:

  • model (pinocchio.Model) – The robot model containing lower and upper position limits.

  • step_size (float) – The step size for sampling.

  • generate_random (bool) – If True, continue randomly sampling the configuration space. Otherwise this generator will terminate.

Yields:

np.ndarray – The next point in the configuration space.

pyroboplan.planning.utils.extend_robot_state(q_parent, q_sample, max_connection_distance)

Determines an incremental robot configuration between the parent and sample states, if one exists.

Parameters:

  • q_parent (array-like) – The starting robot configuration.

  • q_sample (array-like) – The candidate sample configuration to extend towards.

  • max_connection_distance (float) – Maximum angular distance, in radians, for connecting nodes.

Returns:

The resulting robot configuration, or None if it is not feasible.

Return type:

array-like

pyroboplan.planning.utils.has_collision_free_path(q1, q2, max_step_size, model, collision_model, data=None, collision_data=None, distance_padding=0.0)

Determines if there is a collision free path between the provided nodes and models.

Parameters:

  • q1 (array-like) – The starting robot configuration.

  • q2 (array-like) – The destination robot configuration.

  • max_step_size (float) – Maximum joint configuration step size for collision checking along path segments.

  • model (pinocchio.Model) – The model for the robot configuration.

  • collision_model (pinocchio.Model) – The model to use for collision checking.

  • data (pinocchio.Data, optional) – The model data to use for this solver. If None, data is created automatically.

  • collision_data (pinocchio.GeometryData, optional) – The collision_model data to use for this solver. If None, data is created automatically.

  • distance_padding (float, optional) – The padding, in meters, to use for distance to nearest collision.

Returns:

True if the configurations can be connected, False otherwise.

Return type:

bool

pyroboplan.planning.utils.retrace_path(goal_node)

Retraces a path to the specified goal_node from a root node (a node with no parent).

The resulting path will be returned in order form the start at index 0 to the goal_node at the index -1.

Parameters:

goal_node (pyroboplan.planning.graph.Node) – The starting joint configuration.

Returns:

A list a nodes from the root to the specified goal_node.

Return type:

list[pyroboplan.planning.graph.Node]