Class RRT

Class Documentation

class RRT

Motion planner based on the Rapidly-exploring Random Tree (RRT) algorithm.

Public Functions

RRT(const std::shared_ptr<Scene> scene, const RRTOptions &options)

Constructor.

Parameters:

  • scene – A pointer to the scene to use for motion planning.

  • options – A struct containing RRT options.

tl::expected<JointPath, std::string> plan(const JointConfiguration &start, const JointConfiguration &goal)

Plan a path from start to goal.

Parameters:

  • start – The starting joint configuration.

  • goal – The goal joint configuration.

Returns:

A joint-space path, if planning succeeds, otherwise an error message.

void setRngSeed(unsigned int seed)

Sets the seed for the random number generator (RNG).

For reproducibility, this also seeds the underlying scene. For now, this means it would break multi-threaded applications.

Parameters:

seed – The seed to set.

void initializeTree(KdTree &tree, std::vector<Node> &nodes, const Eigen::VectorXd &q_init, size_t max_size = 1000)

Initializes the search tree with the specified start pose.

Parameters:

  • tree – Reference to an empty tree.

  • nodes – Reference to the nodes vector.

  • q_init – The first node to add to the tree.

  • max_size – The maximum size of the tree.

bool growTree(KdTree &tree, std::vector<Node> &nodes, const Eigen::VectorXd &q_sample)

Attempt to add a sampled node to the provided tree and node set.

Parameters:

  • tree – The tree to grow.

  • nodes – The set of sampled nodes so far.

  • q_sample – Randomly sampled node to extend towards (or connect).

Returns:

True if node(s) were added to the tree, false otherwise.

std::optional<JointPath> joinTrees(const std::vector<Node> &nodes, const KdTree &target_tree, const std::vector<Node> &target_nodes, bool grow_start_tree)

Attempts to connect the target_tree to the latest added node in nodes.

The “latest added node” refers to nodes.back(). The function will identify the nearest node in the target_tree, and attempt to make a connection. If successful, will return a path from the start node to the goal node.

Parameters:

  • nodes – The list of source tree nodes, nodes.back() is the most recently added node.

  • target_tree – The tree to connect to the nodes list.

  • target_nodes – The nodes in the target tree.

  • grow_start_tree – If true, the target_tree is the goal tree.

Returns:

A completed path from the start to the goal node if it exists, otherwise none.

JointPath getPath(const std::vector<Node> &nodes, const Node &end_node)

Returns a path from the specified index to the first added node.

Parameters:

  • nodes – The list of nodes in the tree.

  • end_node – The index of the goal destination in the nodes list.

Returns:

A JointPath between end_node and nodes[0].

inline std::pair<std::vector<Node>, std::vector<Node>> getNodes()

Returns the start and goal trees’ node vectors, for visualization purposes.