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 pinocchio::Data &getData() const

Gets the scene’s internal Pinocchio data (read-only).

Returns:

The Pinocchio data.

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.

void computeFrameJacobian(const Eigen::VectorXd &q, pinocchio::FrameIndex frame_id, pinocchio::ReferenceFrame reference_frame, Eigen::Ref<Eigen::MatrixXd> jacobian) const

Computes the frame Jacobian for a specific frame.

Parameters:

  • q – The joint configuration.

  • frame_id – The Pinocchio frame ID.

  • reference_frame – The reference frame for the Jacobian (LOCAL or WORLD).

  • jacobian – Output matrix to store the Jacobian (must be pre-allocated to 6 x nv).

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::string readFile(const std::filesystem::path &path)

Returns the contents of a file as a string.

Parameters:

name – path The path to the file.

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::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.

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.2.0/roboplan/include/roboplan/core
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.2.0/roboplan/include
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.2.0/roboplan
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.2.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.

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

Solves inverse kinematics (single goal).

This just calls the multiple goal version internally.

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.

bool solveIk(const std::vector<CartesianConfiguration> &goals, const JointConfiguration &start, JointConfiguration &solution)

Solves inverse kinematics (multiple goal).

Parameters:

  • goals – The goal Cartesian configurations.

  • 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::VectorXd error_

The full error vector (for allocating memory once).

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_linear_error_norm = 0.001

The maximum linear error norm, in meters.

double max_angular_error_norm = 0.001

The maximum angular error norm, in radians.

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.2.0/roboplan_simple_ik/include
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.2.0/roboplan_simple_ik
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.2.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.2.0/roboplan_rrt/include
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.2.0/roboplan_rrt
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.2.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.2.0/roboplan_toppra/include
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.2.0/roboplan_toppra
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.2.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.2.0/roboplan_example_models/include
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.2.0/roboplan_example_models
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.2.0/roboplan_example_models/include/roboplan_example_models