API Reference (C++)

Core Library

struct Box

#include <geometry_wrappers.hpp>

Temporary wrapper struct to represent a box geometry.

Public Functions

inline Box(double x, double y, double z)

Construct a Box object wrapper.

Parameters:

x – The X dimension of the box.
y – The y dimension of the box.
z – The z dimension of the box.

Public Members

std::shared_ptr<hpp::fcl::Box> geom_ptr: The underlying Coal box geometry.

struct CartesianConfiguration

#include <types.hpp>

Represents a robot Cartesian configuration.

This comprises a transform, as well as the names of the frames in the robot model.

Public Members

std::string base_frame: The name of the base (or reference) frame.

std::string tip_frame: The name of the tip (or target) frame.

Eigen::Matrix4d tform = Eigen::Matrix4d::Identity(): The transformation matrix from the base to the tip frame. NOTE: I’d like this to be an Isometry3d but nanobind doesn’t have off the shelf bindings for this.

struct JointConfiguration

#include <types.hpp>

Represents a robot joint configuration.

Creating and validating these structures are handled by separate utility functions.

Public Members

std::vector<std::string> joint_names: The names of the joints.

Eigen::VectorXd positions: The joint positions, in the same order as the names.

Eigen::VectorXd velocities: The joint velocities, in the same order as the names.

Eigen::VectorXd accelerations: The joint accelerations, in the same order as the names.

struct JointGroupInfo

#include <types.hpp>

Contains information about a named group of joints.

Public Members

std::vector<std::string> joint_names: The joint names that make up the group.

std::vector<size_t> joint_indices: The joint indices in the group.

Eigen::VectorXi q_indices: The position vector indices in the group.

Eigen::VectorXi v_indices: The velocity vector indices in the group.

bool has_continuous_dofs = {false}: Whether the group has any continuous degrees of freedom.

size_t nq_collapsed

The number of collapsed degrees of freedom.

To get the full (expanded) value, this is q_indices.size().

Friends

friend std::ostream &operator<<(std::ostream &os, const JointGroupInfo &info): Prints basic information about the joint group.

struct JointInfo

#include <types.hpp>

Contains joint information relevant to motion planning and control.

Public Functions

JointInfo(JointType joint_type)

Constructor for joint info.

Parameters:: joint_type – The type of the joint. All other variables will be initialized according to this value.

Public Members

JointType type: The type of the joint.

size_t num_position_dofs: The number of positional degrees of freedom.

size_t num_velocity_dofs

The number of velocity degrees of freedom.

This also corresponds to higher derivatives like acceleration and jerk.

JointLimits limits: The joint limit information for each degree of freedom.

std::optional<JointMimicInfo> mimic_info: The joint mimic information.

struct JointLimits

#include <types.hpp>

Contains joint limit information.

Values are all vectorized to denote multi-DOF joints.

Public Members

Eigen::VectorXd min_position: The minimum positions of the joint.

Eigen::VectorXd max_position: The maximum positions of the joint.

Eigen::VectorXd max_velocity: The maximum (symmetric) velocities of the joint.

Eigen::VectorXd max_acceleration: The maximum (symmetric) accelerations of the joint.

Eigen::VectorXd max_jerk: The maximum (symmetric) jerks of the joint.

struct JointMimicInfo

#include <types.hpp>

Contains joint mimic information.

Public Members

std::string mimicked_joint_name: The name of the joint being mimicked.

double scaling = 1.0: The scaling factor for the mimic relationship.

double offset = 0.0: The offset for the mimic relationship.

struct JointPath

#include <types.hpp>

Contains a path of joint configurations.

Public Members

std::vector<std::string> joint_names: The list of joint names.

std::vector<Eigen::VectorXd> positions: The list of joint configuration positions.

Friends

friend std::ostream &operator<<(std::ostream &os, const JointPath &path): Prints basic information about the path.

struct JointTrajectory

#include <types.hpp>

Contains a trajectory of joint configurations.

Public Members

std::vector<std::string> joint_names: The list of joint names.

std::vector<double> times: The list of times.

std::vector<Eigen::VectorXd> positions: The list of joint positions.

std::vector<Eigen::VectorXd> velocities: The list of joint velocities.

std::vector<Eigen::VectorXd> accelerations: The list of joint accelerations.

Friends

friend std::ostream &operator<<(std::ostream &os, const JointTrajectory &traj): Prints basic information about the trajectory.

class PathShortcutter

#include <path_utils.hpp>

Shortcuts joint paths with random sampling and checking connections.

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

Public Functions

PathShortcutter(const std::shared_ptr<Scene> scene, const std::string &group_name)

Construct a new path shortcutter instance.

Parameters:

scene – The scene for checking connectability between joint positions.
group_name – The name of the group to use for path shortcutting.

JointPath shortcut(const JointPath &path, double max_step_size, unsigned int max_iters = 100, int seed = 0)

Attempts to shortcut a specified path.

Parameters:

path – The JointPath to try to shorten.
max_step_size – Maximum step size to use in collision checking, and the minimum separable distance between points in a shortcut.
max_iters – Maximum number of iterations of random sampling (default 100).
seed – Seed for the random generator, if < 0 then use a random seed (default -1).

Returns:

A shortcutted JointPath, if available.

tl::expected<Eigen::VectorXd, std::string> getPathLengths(const JointPath &path)

Computes configuration distances from the start to each pose in a path.

Parameters:: path – The JointPath to evaluate.
Returns:: A vector of incremental path distances, if there is sufficient data. Otherwise an error.

tl::expected<Eigen::VectorXd, std::string> getNormalizedPathScaling(const JointPath &path)

Computes length-normalized scaling values along a JointPath.

Parameters:: path – The path to length-normalize.
Returns:: A vector of scaling values between 0.0 and 1.0 at each point in the path if available, otherwise an error.

std::pair<Eigen::VectorXd, size_t> getConfigurationFromNormalizedPathScaling(const JointPath &path, const Eigen::VectorXd &path_scalings, double value)

Gets joint configurations from a path with normalized joint scalings.

Parameters:

path – A JointPath of joint poses.
path_scalings – The corresponding path scalings (between 0 and 1) to the provided path.
value – A value between 0.0 and 1.0 pointing to the intermediate point along the path.

Returns:

a pair containing the joint configuration at the scaled value along the path, as well as the index corresponding to the next point along the path.

Private Members

std::shared_ptr<Scene> scene_: A pointer to the scene.

JointGroupInfo joint_group_info_: The joint group info for the path shortcutter.

Eigen::VectorXd q_full_: The full joint position vector for the scene (to prevent multiple allocations).

class Scene

#include <scene.hpp>

Primary scene representation for planning and control.

Public Functions

Scene(const std::string &name, const std::filesystem::path &urdf_path, const std::filesystem::path &srdf_path, const std::vector<std::filesystem::path> &package_paths = std::vector<std::filesystem::path>(), const std::filesystem::path &yaml_config_path = std::filesystem::path())

Basic constructor.

Parameters:

name – The name of the scene.
urdf_path – Path to the URDF file.
srdf_path – Path to the SRDF file.
package_paths – A vector of package paths to look for packages.
yaml_config_path – Path to the YAML configuration file with additional information.

Scene(const std::string &name, const std::string &urdf, const std::string &srdf, const std::vector<std::filesystem::path> &package_paths = std::vector<std::filesystem::path>(), const std::filesystem::path &yaml_config_path = std::filesystem::path())

Basic constructor with pre-parsed URDF and SRDF options.

Parameters:

name – The name of the scene.
urdf – XML String of the URDF.
srdf – XML String of the SRDF.
package_paths – A vector of package paths to look for packages.
yaml_config_path – Path to the YAML configuration file with additional information.

inline const std::string &getName() const

Gets the scene’s name.

Returns:: The scene name.

inline const pinocchio::Model &getModel() const

Gets the scene’s internal Pinocchio model.

Returns:: The Pinocchio model.

inline const std::vector<std::string> &getJointNames() const

Gets the scene’s full joint names, including mimic joints.

Returns:: A vector of joint names.

inline const std::vector<std::string> &getActuatedJointNames() const

Gets the scene’s actuated (non-mimic) joint names.

Returns:: A vector of joint names.

tl::expected<JointInfo, std::string> getJointInfo(const std::string &joint_name) const

Gets the information for a specific joint.

Parameters:: joint_name – The name of the joint.
Returns:: The joint information struct if successful, else a string describing the error.

double configurationDistance(const Eigen::VectorXd &q_start, const Eigen::VectorXd &q_end) const

Gets the distance between two joint configurations.

Parameters:

q_start – The starting joint positions.
q_end – The ending joint positions.

Returns:

The configuration-space distance between the two positions.

void setRngSeed(unsigned int seed)

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

Parameters:: seed – The seed to set.

Eigen::VectorXd randomPositions()

Generates random positions for the robot model.

Returns:: The random positions.

std::optional<Eigen::VectorXd> randomCollisionFreePositions(size_t max_samples = 1000)

Generates random collision-free positions for the robot model.

Parameters:: max_tries – The maximum number of samples to attempt.
Returns:: The random positions, if successful, else std::nullopt.

bool hasCollisions(const Eigen::VectorXd &q, const bool debug = false) const

Checks collisions at specified joint positions.

Parameters:

q – The joint positions.
debug – If true, prints debug information and does not stop at first collision. This parameter is disabled by default.

Returns:

True if there are collisions, else false.

bool isValidPose(const Eigen::VectorXd &q) const

Checks if the specified joint positions are valid with respect to joint limits.

Parameters:: q – The joint positions.
Returns:: True if the positions respect joint limits, else false.

void applyMimics(Eigen::VectorXd &q) const

Applies mimic joint relationships to a position vector.

Parameters:: q – The joint positions.

Eigen::VectorXd toFullJointPositions(const std::string &group_name, const Eigen::VectorXd &q) const

Converts partial joint positions to full joint positions.

This includes adding new joints and applying mimic relationships.

Parameters:

group_name – The name of the joint group.
q – The original (partial) joint positions.

Returns:

The full joint positions.

Eigen::VectorXd interpolate(const Eigen::VectorXd &q_start, const Eigen::VectorXd &q_end, const double fraction) const

Interpolates between two joint configurations.

Parameters:

q_start – The starting joint configuration.
q_end – The ending joint configuration.
fraction – The interpolation coefficient, between 0 and 1.

Eigen::Matrix4d forwardKinematics(const Eigen::VectorXd &q, const std::string &frame_name) const

Calculates forward kinematics for a specific frame.

Parameters:

q – The joint configuration.
frame_name – The name of the frame for which to perform forward kinematics.

Returns:

The 4x4 matrix denoting the transform of the specified frame.

tl::expected<pinocchio::FrameIndex, std::string> getFrameId(const std::string &name) const

Get the Pinocchio model ID of a frame by its name.

Parameters:: name – The name of the frame to look up.
Returns:: The Pinocchio frame ID if successful, else a string describing the error.

tl::expected<JointGroupInfo, std::string> getJointGroupInfo(const std::string &name) const

Get the joint group information of a scene by its name.

Parameters:: name – The name of the joint group to look up.
Returns:: The joint group information if successful, else a string describing the error.

inline const Eigen::VectorXd &getCurrentJointPositions() const

Get the current joint positions for the full robot state.

Returns:: The current joint position vector.

inline void setJointPositions(const Eigen::VectorXd &positions)

Set the joint positions for the full robot state.

Returns:: The desired joint position vector.

Eigen::VectorXi getJointPositionIndices(const std::vector<std::string> &joint_names) const

Get the joint position indices for a set of joint names.

Parameters:: joint_names – The joint names for which to look up position indices.
Returns:: The corresponding joint position indices.

tl::expected<void, std::string> addBoxGeometry(const std::string &name, const std::string &parent_frame, const Box &box, const Eigen::Matrix4d &tform, const Eigen::Vector4d &color)

Adds a box geometry to the scene.

Parameters:

name – The name of the object to add.
parent_frame – The name of the parent frame to add the object to.
box – The box geometry instance to add.
tform – The transform between the parent frame and the geometry.
color – The color of the geometry, in RGBA vector format.

Returns:

Void if successful, else a string describing the error.

tl::expected<void, std::string> addSphereGeometry(const std::string &name, const std::string &parent_frame, const Sphere &sphere, const Eigen::Matrix4d &tform, const Eigen::Vector4d &color)

Adds a sphere geometry to the scene.

Parameters:

name – The name of the object to add.
parent_frame – The name of the parent frame to add the object to.
sphere – The sphere geometry instance to add.
tform – The transform between the parent frame and the geometry.
color – The color of the geometry, in RGBA vector format.

Returns:

Void if successful, else a string describing the error.

tl::expected<void, std::string> addGeometry(const pinocchio::GeometryObject &geom_obj)

Adds a Pinocchio geometry object to the scene.

This can be made the sole public entrypoint to add a geometry once Pinocchio and Coal have working nanobind bindings compatible with this library.

Parameters:: geom_obj – The geometry object instance to add.
Returns:: Void if successful, else a string describing the error.

tl::expected<void, std::string> updateGeometryPlacement(const std::string &name, const std::string &parent_frame, Eigen::Matrix4d &tform)

Updates the placement of an object geometry in the scene.

Parameters:

name – The name of the object to update.
parent_frame – The parent frame of the transformation.
tform – The transform between the parent frame and the geometry.

tl::expected<void, std::string> removeGeometry(const std::string &name)

Removes a geometry from the scene.

Parameters:: name – The name of the object to remove.

tl::expected<std::vector<pinocchio::GeomIndex>, std::string> getCollisionGeometryIds(const std::string &body)

Gets a list of collision geometry IDs corresponding to a specified body.

The body name can either be a model frame name or a collision model geometry name.

Parameters:: body – The name of the body.
Returns:: A std::vector of collision geometry indices for the body if successful, else a string describing the error.

tl::expected<void, std::string> setCollisions(const std::string &body1, const std::string &body2, const bool enable)

Sets the allowable collisions for a pair of bodies in the model.

The body names can either be model frame names or collision model geometry names.

Parameters:

body1 – The name of the first body.
body2 – The name of the second body.
enable – If true, enables the collision; if false, disables it.

Returns:

Void if successful, else a string describing the error.

Private Members

std::string name_: The name of the scene.

pinocchio::Model model_: The Pinocchio model representing the robot and its environment.

mutable pinocchio::Data model_data_

The default data structure for the underlying Pinocchio model.

This won’t be thread-safe unless each thread uses its own data.

pinocchio::GeometryModel collision_model_: The Pinocchio collision model representing the robot and its environment.

mutable pinocchio::GeometryData collision_model_data_

The default data structure for the underlying Pinocchio collision model.

This won’t be thread-safe unless each thread uses its own data.

std::vector<std::string> joint_names_: The full list of joint names in the model (including mimic joints).

std::vector<std::string> actuated_joint_names_: The list of actuated (non-mimic) joint names in the model.

std::unordered_map<std::string, JointInfo> joint_info_map_: Map from joint names to their corresponding information.

std::unordered_map<std::string, JointGroupInfo> joint_group_info_map_: Map from joint group names to their corresponding information.

std::mt19937 rng_gen_: A random number generator for the scene.

JointConfiguration cur_state_: The current state of the model (used to fill in partial states).

std::unordered_map<std::string, pinocchio::FrameIndex> frame_map_: Maps each frame name to its respective Pinocchio frame ID.

std::unordered_map<std::string, pinocchio::GeomIndex> collision_geometry_map_: Maps each added collision geometry to its respective Pinocchio geometry ID.

Friends

friend std::ostream &operator<<(std::ostream &os, const Scene &scene): Prints basic information about the scene.

struct Sphere

#include <geometry_wrappers.hpp>

Temporary wrapper struct to represent a sphere geometry.

Public Functions

inline Sphere(double radius)

Construct a Sphere object wrapper.

Parameters:: radius – The radius of the sphere.

Public Members

std::shared_ptr<hpp::fcl::Sphere> geom_ptr: The underlying Coal sphere geometry.

namespace roboplan

Enums

enum JointType

Enumeration that describes different types of joints.

Values:

enumerator UNKNOWN

enumerator PRISMATIC

enumerator REVOLUTE

enumerator CONTINUOUS

enumerator PLANAR

enumerator FLOATING

Functions

std::unordered_map<std::string, pinocchio::FrameIndex> createFrameMap(const pinocchio::Model &model)

Creates a map of the robot’s frame names to IDs.

Parameters:: model – The Pinocchio model.
Returns:: The map of robot frame names to IDs.

std::unordered_map<std::string, JointGroupInfo> createJointGroupInfo(const pinocchio::Model &model, const std::string &srdf)

Creates the joint group information for the scene;.

Parameters:

model – The Pinocchio model.
srdf_stream – The SRDF file contents.

Returns:

The map of robot joint group names to group info.

tl::expected<Eigen::VectorXd, std::string> collapseContinuousJointPositions(const Scene &scene, const std::string &group_name, const Eigen::VectorXd &q_orig)

Collapses a joint position vector’s continuous joints for downstream algorithms.

That is, positions that are expressed as [cos(theta), sin(theta)] will be collapsed to [theta], assuming being between +/- pi.

Parameters:

scene – The scene from which to look up joint information.
group_name – The name of the joint group corresponding to the position vector.
q_orig – The original position vectors.

Returns:

The collapsed position vectors if successful, else a string describing the error.

tl::expected<Eigen::VectorXd, std::string> expandContinuousJointPositions(const Scene &scene, const std::string &group_name, const Eigen::VectorXd &q_orig)

Expands a joint position vector’s continuous joints from downstream algorithms.

That is, positions that are expressed as [theta] will be expanded to [cos(theta), sin(theta)].

Parameters:

scene – The scene from which to look up joint information.
group_name – The name of the joint group corresponding to the position vector.
q_orig – The original position vectors.

Returns:

The expanded position vectors if successful, else a string describing the error.

std::vector<Eigen::Matrix4d> computeFramePath(const Scene &scene, const Eigen::VectorXd &q_start, const Eigen::VectorXd &q_end, const std::string &frame_name, const double max_step_size)

Computes the Cartesian path of a specified frame.

Parameters:

scene – The scene to use for interpolating positions.
q_start – The starting joint positions.
q_end – The ending joint positions.
frame_name – The name of the frame in which to compute the Cartesian path.
max_step_size – The maximum configuration distance step size for interpolation.

Returns:

A list of 4x4 matrices corresponding to the poses of the frame along the path.

bool hasCollisionsAlongPath(const Scene &scene, const Eigen::VectorXd &q_start, const Eigen::VectorXd &q_end, const double max_step_size)

Checks collisions along a specified configuration space path.

Parameters:

scene – The scene to use for interpolating positions and checking collisions.
q_start – The starting joint positions.
q_end – The ending joint positions.
max_step_size – The maximum configuration distance step size for interpolation.

Returns:

True if there are collisions, else false.

std::string readFile(const std::filesystem::path &path)

Returns the contents of a file as a string.

Parameters:: name – path The path to the file.

Variables

const std::map<std::string, roboplan::JointType> kPinocchioJointTypeMap = {{"JointModelPrismaticUnaligned", JointType::PRISMATIC}, {"JointModelPX", roboplan::JointType::PRISMATIC}, {"JointModelPY", roboplan::JointType::PRISMATIC}, {"JointModelPZ", roboplan::JointType::PRISMATIC}, {"JointModelRX", roboplan::JointType::REVOLUTE}, {"JointModelRY", roboplan::JointType::REVOLUTE}, {"JointModelRZ", roboplan::JointType::REVOLUTE}, {"JointModelRevoluteUnaligned", roboplan::JointType::REVOLUTE}, {"JointModelRUBX", roboplan::JointType::CONTINUOUS}, {"JointModelRUBY", roboplan::JointType::CONTINUOUS}, {"JointModelRUBZ", roboplan::JointType::CONTINUOUS}, {"JointModelRevoluteUnboundedUnaligned", roboplan::JointType::CONTINUOUS}, {"JointModelPlanar", roboplan::JointType::PLANAR}, {"JointModelFreeFlyer", roboplan::JointType::FLOATING}, {"JointModelMimic", roboplan::JointType::UNKNOWN},}: Map from Pinocchio joint model short names to RoboPlan joint type enums.

file geometry_wrappers.hpp: #include <hpp/fcl/shape/geometric_shapes.h>

file path_utils.hpp: #include <vector>

#include <Eigen/Dense>

#include <roboplan/core/scene.hpp>

#include <tl/expected.hpp>

file scene.hpp: #include <filesystem>

#include <iostream>

#include <map>

#include <optional>

#include <stdexcept>

#include <string>

#include <pinocchio/algorithm/frames.hpp>

#include <pinocchio/algorithm/geometry.hpp>

#include <pinocchio/algorithm/joint-configuration.hpp>

#include <pinocchio/fwd.hpp>

#include <pinocchio/multibody/data.hpp>

#include <pinocchio/multibody/geometry.hpp>

#include <pinocchio/multibody/model.hpp>

#include <tl/expected.hpp>

#include <roboplan/core/geometry_wrappers.hpp>

#include <roboplan/core/types.hpp>

file scene_utils.hpp: #include <map>

#include <string>

#include <pinocchio/multibody/model.hpp>

#include <roboplan/core/scene.hpp>

#include <roboplan/core/types.hpp>

file types.hpp: #include <iostream>

#include <optional>

#include <string>

#include <vector>

#include <Eigen/Dense>

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.1.0/roboplan/include/roboplan/core

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.1.0/roboplan/include

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.1.0/roboplan

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.1.0/roboplan/include/roboplan

Simple IK

class SimpleIk

#include <simple_ik.hpp>

Simple inverse kinematics (IK) solver based on the Jacobian pseudoinverse.

Public Functions

SimpleIk(const std::shared_ptr<Scene> scene, const SimpleIkOptions &options)

Constructor.

Parameters:

scene – A pointer to the scene to use for solving IK.
options – A struct containing IK solver options.

bool solveIk(const CartesianConfiguration &goal, const JointConfiguration &start, JointConfiguration &solution)

Solves inverse kinematics.

Parameters:

goal – The goal Cartesian configuration.
start – The starting joint configuration. (should be optional)
solution – The IK solution, as a joint configuration.

Returns:

Whether the IK solve succeeded.

Private Members

std::shared_ptr<Scene> scene_: A pointer to the scene.

SimpleIkOptions options_: The struct containing IK solver options.

pinocchio::Data data_: Pinocchio data for the IK solver.

JointGroupInfo joint_group_info_: The joint group info for the IK solver.

Eigen::MatrixXd full_jacobian_: The full model Jacobian (for allocating memory once).

Eigen::MatrixXd jacobian_: The joint group’s Jacobian (for allocating memory once).

Eigen::MatrixXd jjt_: The Jacobian times Jacobian transpose (for allocating memory once).

Eigen::VectorXd vel_: The full joint velocity vector for integrating (for allocating memory once).

struct SimpleIkOptions

#include <simple_ik.hpp>

Options struct for simple IK solver.

Public Members

std::string group_name = "": The joint group name to be used by the solver.

size_t max_iters = 100: Max iterations for one try of the solver.

double max_time = 0.01: Max total computation time, in seconds.

size_t max_restarts = 2: Maximum number of restarts until success.

double step_size = 0.01: The integration step for the solver.

double damping = 0.001: Damping value for the Jacobian pseudoinverse.

double max_error_norm = 0.001: The maximum error norm. TODO: Separate into linear and angular?

bool check_collisions = true: Whether to check collisions.

namespace roboplan

file simple_ik.hpp: #include <memory>

#include <string>

#include <roboplan/core/scene.hpp>

#include <roboplan/core/types.hpp>

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.1.0/roboplan_simple_ik/include

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.1.0/roboplan_simple_ik

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.1.0/roboplan_simple_ik/include/roboplan_simple_ik

RRT

struct Node

#include <graph.hpp>

Defines a graph node for search-based planners.

Public Functions

inline Node(const Eigen::VectorXd &config_, int parent_id_)

Constructor.

Parameters:

config_ – The configuration of the node.
parent_id_ – The parent id of the node.

Public Members

Eigen::VectorXd config: The configuration (e.g., joint positions) of this node.

int parent_id

The parent node ID.

-1 means this is the root node.

class RRT

#include <rrt.hpp>

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.

Private Functions

Eigen::VectorXd extend(const Eigen::VectorXd &q_start, const Eigen::VectorXd &q_goal, double max_connection_dist)

Runs the RRT extend operation from a start node to a goal node.

Parameters:

q_start – The start configuration.
q_goal – The goal configuration.
max_connection_dist – The maximum configuration distance to extend to.

Returns:

The extended configuration.

Private Members

std::shared_ptr<Scene> scene_: A pointer to the scene.

RRTOptions options_: The struct containing IK solver options.

JointGroupInfo joint_group_info_: The joint group info for the IK solver.

CombinedStateSpace state_space_: A state space for the k-d tree for nearest neighbor lookup.

std::mt19937 rng_gen_: A random number generator for the planner.

std::uniform_real_distribution<double> uniform_dist_ = {0.0, 1.0}: A uniform distribution for goal biasing sampling.

std::vector<Node> start_nodes_: The start tree nodes.

std::vector<Node> goal_nodes_: The goal tree nodes.

struct RRTOptions

#include <rrt.hpp>

Options struct for RRT planner.

Public Members

std::string group_name = "": The joint group name to be used by the planner.

size_t max_nodes = 1000: The maximum number of nodes to sample.

double max_connection_distance = 3.0: The maximum configuration distance between two nodes.

double collision_check_step_size = 0.05: The configuration-space step size for collision checking along edges.

double goal_biasing_probability = 0.15

The probability of sampling the goal node instead of a random node.

Must be between 0 and 1.

double max_planning_time = 0

The maximum amount of time to allow for planning, in seconds.

If <= 0 then planning will never timeout.

bool rrt_connect = false: If true, use the RRT-Connect algorithm to grow the search trees.

namespace roboplan

Typedefs

using CombinedStateSpace = dynotree::Combined<double>

using KdTree = dynotree::KDTree<int, -1, 32, double, CombinedStateSpace>

file graph.hpp: #include <Eigen/Dense>

file rrt.hpp: #include <memory>

#include <optional>

#include <random>

#include <string>

#include <vector>

#include <dynotree/KDTree.h>

#include <tl/expected.hpp>

#include <roboplan/core/scene.hpp>

#include <roboplan/core/types.hpp>

#include <roboplan_rrt/graph.hpp>

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.1.0/roboplan_rrt/include

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.1.0/roboplan_rrt

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.1.0/roboplan_rrt/include/roboplan_rrt

TOPP-RA

class PathParameterizerTOPPRA

#include <toppra.hpp>

Trajectory time parameterizer using the TOPP-RA algorithm.

This directly uses https://github.com/hungpham2511/toppra.

Public Functions

PathParameterizerTOPPRA(const std::shared_ptr<Scene> scene, const std::string &group_name = "")

Constructor.

Parameters:

scene – A pointer to the scene to use for path parameterization.
group_name – The name of the joint group to use.

tl::expected<JointTrajectory, std::string> generate(const JointPath &path, const double dt, const double velocity_scale = 1.0, const double acceleration_scale = 1.0)

Time-parameterizes a joint-space path using TOPP-RA.

Parameters:

path – The path to time parameterize.
dt – The sample time of the output trajectory, in seconds.
velocity_scale – A scaling factor (between 0 and 1) for velocity limits.
acceleration_scale – A scaling factor (between 0 and 1) for acceleration limits.

Returns:

A time-parameterized joint trajectory.

Private Members

std::shared_ptr<Scene> scene_: A pointer to the scene.

std::string group_name_: The name of the joint group.

JointGroupInfo joint_group_info_: The joint group info for the path parameterizer.

toppra::Vector vel_lower_limits_: The stored velocity lower limits.

toppra::Vector vel_upper_limits_: The stored velocity upper limits.

toppra::Vector acc_lower_limits_: The stored acceleration lower limits.

toppra::Vector acc_upper_limits_: The stored acceleration upper limits.

std::vector<size_t> continuous_joint_indices_

A list of indices of joints with continuous degrees of freedom.

This is used to figure out which joints need to be wrapped.

namespace roboplan

file toppra.hpp: #include <string>

#include <vector>

#include <roboplan/core/scene.hpp>

#include <roboplan/core/types.hpp>

#include <tl/expected.hpp>

#include <toppra/algorithm/toppra.hpp>

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.1.0/roboplan_toppra/include

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.1.0/roboplan_toppra

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.1.0/roboplan_toppra/include/roboplan_toppra

Example Models

namespace roboplan

namespace example_models

Functions

std::filesystem::path get_install_prefix(): Provides compile time access to the resources install directory.

std::filesystem::path get_package_share_dir(): Provides compile time access to the resources shared directory for accessing robot models or other resource files.

std::filesystem::path get_package_models_dir(): Provides compile time access to the directory under the resources shared directory which contains all the example robot models.

file resources.hpp: #include <filesystem>

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.1.0/roboplan_example_models/include

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.1.0/roboplan_example_models

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.1.0/roboplan_example_models/include/roboplan_example_models