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.

struct OcTree

Public Functions

inline OcTree(const std::vector<Eigen::Matrix<double, 6, 1>> &boxes, const double resolution)
inline OcTree(const std::shared_ptr<hpp::fcl::OcTree> &octree_geom)

Public Members

std::shared_ptr<hpp::fcl::OcTree> geom_ptr
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::VectorXd integrate(const Eigen::VectorXd &q, const Eigen::VectorXd &v) const

Integrates a velocity vector from a configuration using Lie group operations.

Parameters:

  • q – The starting joint configuration (size model.nq).

  • v – The velocity / displacement vector to integrate (size model.nv).

Returns:

The resulting joint configuration after integration.

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<EigenVectorPair, std::string> getPositionLimitVectors(const std::string &group_name = "") const

Get the joint position limit vectors for a specified group.

Parameters:

group_name – The name of the group. Defaults to the complete robot model.

Returns:

A pair of vectors for the lower and upper joint position limits, if successful, or a string describing any errors.

tl::expected<EigenVectorPair, std::string> getVelocityLimitVectors(const std::string &group_name = "") const

Get the joint velocity limit vectors for a specified group.

Parameters:

group_name – The name of the group. Defaults to the complete robot model.

Returns:

A pair of vectors for the lower and upper joint velocity limits, if successful, or a string describing any errors.

tl::expected<EigenVectorPair, std::string> getAccelerationLimitVectors(const std::string &group_name = "") const

Get the joint acceleration limit vectors for a specified group.

Parameters:

group_name – The name of the group. Defaults to the complete robot model.

Returns:

A pair of vectors for the lower and upper joint acceleration limits, if successful, or a string describing any errors.

tl::expected<EigenVectorPair, std::string> getJerkLimitVectors(const std::string &group_name = "") const

Get the joint jerk limit vectors for a specified group.

Parameters:

group_name – The name of the group. Defaults to the complete robot model.

Returns:

A pair of vectors for the lower and upper joint jerk limits, if successful, or a string describing any errors.

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> addOcTreeGeometry(const std::string &name, const std::string &parent_frame, const OcTree &octree, const Eigen::Matrix4d &tform, const Eigen::Vector4d &color)

Adds a octree 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.

  • octree – The octree 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

Typedefs

using EigenVectorPair = std::pair<Eigen::VectorXd, Eigen::VectorXd>

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 by interpolating sparse positions.

Parameters:

  • scene – The scene to use.

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

std::vector<Eigen::Matrix4d> computeFramePath(const Scene &scene, const std::vector<Eigen::VectorXd> &q_vec, const std::string &frame_name)

Computes the Cartesian path of a specified frame using a vector of provided points.

Parameters:

  • scene – The scene to use.

  • q_vec – A vector of joint positions.

  • frame_name – The name of the frame in which to compute the Cartesian path.

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, const bool bisection = false)

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.

  • bisection – If True, uses bisection instead of linear search. Bisection could help find collisions faster in collision-dense environments, but is slower in the worst-case scenario since it requires a number of samples that is a power of 2 to guarantee maximum distance between points.

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/octree.h>
#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 <utility>
#include <vector>
#include <Eigen/Dense>
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.0/roboplan/include/roboplan/core
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.0/roboplan/include
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.0/roboplan
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.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.3.0/roboplan_simple_ik/include
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.0/roboplan_simple_ik
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.0/roboplan_simple_ik/include/roboplan_simple_ik

Optimal IK (OInK)

struct Barrier
#include <optimal_ik.hpp>

Abstract base class for Control Barrier Functions.

Barriers enforce safety constraints derived from the CBF condition:

Standard CBF: ḣ(q) + α(h(q)) ≥ 0 Discrete time: J_h · δq/dt + α(h(q)) ≥ 0 Rearranging: -J_h · δq ≤ dt · α(h(q)) QP form: G · δq ≤ b where G = -J_h/dt, b = α(h(q))

Uses a saturating class-K function: α(h) = γ·h / (1 + |h|) This provides bounded recovery force, preventing over-reaction when far from the boundary while giving smooth, proportional behavior near constraints.

Safe displacement regularization adds a QP objective term: (safe_displacement_gain / (2·‖J_h‖²)) · ‖δq - δq_safe‖²

This encourages the robot to move toward a known safe configuration when near constraint boundaries. The weighting by 1/‖J_h‖² normalizes the contribution based on how sensitive the barrier is to joint motion.

The safety_margin parameter provides a conservative buffer for hard constraints. When safety_margin > 0, the CBF constraint is tightened by this amount, meaning the barrier will begin to resist motion earlier (when h = safety_margin rather than h = 0). This compensates for linearization errors in the discrete-time formulation.

Subclassed by roboplan::PositionBarrier

Public Functions

explicit Barrier(double gain, double dt, double safe_displacement_gain = 1.0, double safety_margin = 0.0)

Constructor with barrier parameters.

Parameters:

  • gain – Barrier gain (gamma), controls aggressiveness

  • dt – Timestep for discrete-time formulation (must match control loop period)

  • safe_displacement_gain – Gain for safe displacement regularization

  • safety_margin – Conservative margin for hard constraint guarantee (default 0.0)

void initializeStorage(int num_barriers, int num_vars)

Initialize pre-allocated storage.

Parameters:

  • num_barriers – Number of barrier constraints this barrier produces

  • num_vars – Number of optimization variables (model.nv)

virtual int getNumBarriers(const Scene &scene) const = 0

Get the number of barrier constraints this barrier produces.

Parameters:

scene – The scene containing robot state and model

Returns:

Number of barrier constraint rows

virtual tl::expected<void, std::string> computeBarrier(const Scene &scene) = 0

Compute the barrier function values h(q)

Note

Barrier values h(q) >= 0 indicate safety; h(q) < 0 indicates violation

Parameters:

scene – The scene containing robot state and model

Returns:

void on success, error message on failure

virtual tl::expected<void, std::string> computeJacobian(const Scene &scene) = 0

Compute the barrier Jacobian J_h = dh/dq.

Parameters:

scene – The scene containing robot state and model

Returns:

void on success, error message on failure

virtual Eigen::VectorXd computeSafeDisplacement(const Scene &scene) const

Compute safe displacement for regularization.

Subclasses can override to provide a non-zero safe displacement that the robot will be encouraged to move toward when near constraint boundaries.

Parameters:

scene – The scene containing robot state and model

Returns:

Safe displacement vector (num_variables), default is zero

tl::expected<void, std::string> computeQpInequalities(const Scene &scene, Eigen::Ref<Eigen::MatrixXd> G, Eigen::Ref<Eigen::VectorXd> b)

Compute QP inequality constraints for this barrier.

Computes: G_b * delta_q <= b_b Where: G_b = -J_h / dt b_b = gain * h(q) (linear class-K function)

Parameters:

  • scene – The scene containing robot state and model

  • G – Output constraint matrix (pre-sized view: num_barriers x num_variables)

  • b – Output constraint upper bound vector (pre-sized view: num_barriers)

Returns:

void on success, error message on failure

tl::expected<void, std::string> computeQpObjective(const Scene &scene, Eigen::Ref<Eigen::MatrixXd> H, Eigen::Ref<Eigen::VectorXd> c)

Compute QP objective contribution for safe displacement regularization.

Computes: (safe_displacement_gain / (2·‖J_h‖²)) · ‖δq - δq_safe‖²

This encourages the robot to move toward a safe configuration when near constraint boundaries. The weighting by 1/‖J_h‖² normalizes the contribution based on how sensitive the barrier is to joint motion.

Parameters:

  • scene – The scene containing robot state and model

  • H – Output Hessian matrix contribution (num_variables x num_variables)

  • c – Output gradient vector contribution (num_variables)

Returns:

void on success, error message on failure

virtual tl::expected<double, std::string> evaluateAtConfiguration(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q) const

Evaluate the minimum barrier value at a candidate configuration using FK.

This method allows post-solve validation by computing the actual barrier value at a candidate configuration q, independent of the linearized constraint used in the QP. Used by Oink::enforceBarriers() to detect linearization errors.

Parameters:

  • model – Pinocchio model

  • data – Pinocchio data (will be modified by FK computation)

  • q – Candidate joint configuration to evaluate

Returns:

Expected containing minimum barrier value across all barrier constraints, or infinity if this barrier type does not support configuration evaluation. Returns error message if evaluation fails (e.g., frame not found).

virtual ~Barrier() = default

Public Members

const double gain

Barrier gain (gamma)

const double dt

Timestep.

const double safe_displacement_gain

Gain for safe displacement regularization.

const double safety_margin

Conservative margin for hard constraints.

int num_variables = 0
Eigen::VectorXd barrier_values

Pre-allocated containers.

h(q) values (num_barriers)

Eigen::MatrixXd jacobian_container

J_h matrix (num_barriers x num_variables)

struct ConfigurationTask : public roboplan::Task
#include <configuration.hpp>

Task for tracking a target joint configuration.

This task computes the error between a target configuration and the current configuration in the tangent space, enabling joint-space tracking with per-joint weights.

The task owns pre-allocated storage for its nv×nv Jacobian and nv error vector, allocated at construction time to avoid runtime allocations during IK solving.

Public Functions

ConfigurationTask(const Oink &oink, const Eigen::VectorXd &target_q, const Eigen::VectorXd &joint_weights, const ConfigurationTaskOptions &options = {})

Constructs a ConfigurationTask for tracking a target configuration.

Pre-allocates storage for the (nv_group × nv_group) Jacobian and nv_group error vector. The group’s velocity indices are taken from the Oink solver to correctly extract sub-group errors from the full-robot tangent space.

Parameters:

  • oink – The Oink solver this task will be used with (provides q_indices, v_indices).

  • target_q – The target joint configuration for the group (size oink.q_indices.size()).

  • joint_weights – Per-joint weights for the group joints. Size must equal oink.num_variables. All weights must be non-negative.

  • options – Optional task options.

Throws:

std::invalid_argument – if joint_weights size doesn’t match oink.num_variables or if any joint weight is negative.

virtual tl::expected<void, std::string> computeError(const Scene &scene) override

Computes the configuration space error. 

error = pin.difference(model, q_current, q_target)

Parameters:

scene – The scene containing the robot model and current state.

Returns:

Void if successful, else an error message string.

virtual tl::expected<void, std::string> computeJacobian(const Scene &scene) override

Computes the task Jacobian for the configuration task.

The task Jacobian J(q) ∈ ℝ^(nv × nv) is the negative identity matrix (-I). The negative sign ensures that the QP formulation (minimize ||J*dq + α*e||²) produces movement toward the target configuration.

Results are stored in jacobian_container.

Parameters:

scene – The scene containing the robot model and current state.

Returns:

Void if successful, else an error message string.

Public Members

Eigen::VectorXd target_q

Target joint configuration to reach.

Eigen::VectorXd joint_weights

Per-joint weights for cost function (one per group velocity DOF).

Eigen::VectorXi q_indices

Position indices of the joint group.

Eigen::VectorXi v_indices

Velocity indices of the joint group.

Public Static Functions

static Eigen::MatrixXd createWeightMatrix(const Eigen::VectorXd &joint_weights)

Creates a diagonal weight matrix from per-joint weights.

The weight matrix W ∈ ℝ^(nv × nv) is constructed as: W = diag(√joint_weights[i])

Parameters:

joint_weights – Per-joint weight vector.

Returns:

A nv×nv diagonal weight matrix.

struct ConfigurationTaskOptions
#include <configuration.hpp>

ConfigurationTask configuration.

Public Members

double task_gain = 1.0

Proportional gain for error feedback (default: 1.0).

double lm_damping = 0.0

Levenberg-Marquardt damping for regularization (default: 0.0).

struct ConstraintAxisSelection
#include <position_barrier.hpp>

Axis selection for position barrier constraints.

Public Members

bool x = true

Constrain X axis.

bool y = true

Constrain Y axis.

bool z = true

Constrain Z axis.

struct Constraints

Subclassed by roboplan::PositionLimit, roboplan::VelocityLimit

Public Functions

virtual ~Constraints() = default
virtual int getNumConstraints(const Scene &scene) const = 0

Get the number of constraint rows this constraint will produce.

Parameters:

scene – The scene containing robot state and model

Returns:

Number of constraint rows

virtual tl::expected<void, std::string> computeQpConstraints(const Scene &scene, Eigen::Ref<Eigen::MatrixXd> constraint_matrix, Eigen::Ref<Eigen::VectorXd> lower_bounds, Eigen::Ref<Eigen::VectorXd> upper_bounds) const = 0

Compute QP constraint matrices using pre-allocated workspace views.

The constraint_matrix, lower_bounds, and upper_bounds parameters are Eigen::Ref views into pre-allocated workspace memory. The views are already sized to match getNumConstraints() rows, so implementations should fill the entire view.

Parameters:

  • scene – The scene containing robot state and model

  • constraint_matrix – Output constraint matrix G (pre-sized view: num_constraints × num_variables)

  • lower_bounds – Output lower bounds vector (pre-sized view: num_constraints)

  • upper_bounds – Output upper bounds vector (pre-sized view: num_constraints)

Returns:

void on success, error message on failure

struct FrameTask : public roboplan::Task
#include <frame.hpp>

Task for tracking a target Cartesian pose with a specified frame.

This task computes the SE(3) error between a target pose and the current frame pose, enabling full 6-DOF (position + orientation) tracking.

The task owns pre-allocated storage for its 6×nv Jacobian and 6D error vector, allocated at construction time to avoid runtime allocations during IK solving.

Public Functions

FrameTask(const Oink &oink, const Scene &scene, const CartesianConfiguration &target_pose, const FrameTaskOptions &options = {})

Constructs a FrameTask for tracking a target pose.

The Oink solver provides the velocity indices (for Jacobian column selection). The scene is used at construction time to resolve the frame ID and allocate the full Jacobian buffer.

Parameters:

  • oink – The Oink solver instance this task will be used with.

  • scene – The scene used to resolve the frame ID and allocate storage.

  • target_pose – The target Cartesian configuration to reach.

  • options – Optional task options (default: all options set to defaults).

Throws:

std::runtime_error – if the frame name is not found in the scene.

virtual tl::expected<void, std::string> computeError(const Scene &scene) override

Computes the SE(3) error between target and current frame pose.

The error is computed as the logarithm of the relative transform: error = log_6(T_frame_to_world^{-1} * T_target_to_world)

Results are stored in error_container.

Parameters:

scene – The scene containing the robot model and current state.

Returns:

Void if successful, else an error message string.

virtual tl::expected<void, std::string> computeJacobian(const Scene &scene) override

Computes the task Jacobian for the frame tracking task.

The task Jacobian J(q) ∈ ℝ^(6 × n_v) is the derivative of the task error e(q) ∈ ℝ^6 with respect to the configuration q. The formula is: 

J(q) = -Jlog_6(T_frame_to_target) * J_frame(q)
Where:

  • T_frame_to_target: Transform from current frame to target

  • J_frame(q): Frame Jacobian (expressed in frame coordinates)

  • Jlog_6: Pinocchio’s logarithmic Jacobian

Results are stored in jacobian_container.

Parameters:

scene – The scene containing the robot model and current state.

Returns:

Void if successful, else an error message string.

inline void setTargetFrameTransform(const Eigen::Matrix4d &tform)

Sets the target transform for this frame task.

Parameters:

tform – The target transform.

Public Members

std::string frame_name

Name of the frame to track (e.g., end-effector link name).

pinocchio::Index frame_id

Index of the frame in the scene’s Pinocchio model.

Eigen::VectorXi v_indices

Velocity vector indices for the joint group (used to select Jacobian columns).

CartesianConfiguration target_pose

Target Cartesian configuration to reach.

double max_position_error

Maximum position error magnitude (meters). Infinite means no limit.

double max_rotation_error

Maximum rotation error magnitude (radians). Infinite means no limit.

mutable Eigen::MatrixXd full_jacobian
mutable Eigen::Matrix<double, 6, 6> Jlog = Eigen::Matrix<double, 6, 6>::Identity()

Public Static Functions

static Eigen::MatrixXd createWeightMatrix(double position_cost, double orientation_cost)

Creates a diagonal weight matrix from scalar cost weights.

The weight matrix W ∈ ℝ^(6 × 6) is constructed as: W = diag(√position_cost * I_3, √orientation_cost * I_3)

Parameters:

  • position_cost – Cost weight for position error (first 3 dimensions).

  • orientation_cost – Cost weight for orientation error (last 3 dimensions).

Returns:

A 6×6 diagonal weight matrix.

struct FrameTaskOptions
#include <frame.hpp>

Optional parameters for FrameTask configuration.

Public Members

double position_cost = 1.0

Cost weight for position error (default: 1.0).

double orientation_cost = 1.0

Cost weight for orientation error (default: 1.0).

double task_gain = 1.0

Proportional gain for error feedback (default: 1.0).

double lm_damping = 0.0

Levenberg-Marquardt damping for regularization (default: 0.0).

double max_position_error = std::numeric_limits<double>::infinity()

Maximum position error magnitude in meters (default: unlimited). Limits the position error norm to prevent large jumps that can invalidate CBF linearization. Recommended: 0.1-0.2m for systems with barriers.

double max_rotation_error = std::numeric_limits<double>::infinity()

Maximum rotation error magnitude in radians (default: unlimited). Limits the rotation error norm to prevent large jumps. Recommended: 0.5-1.0 rad for systems with barriers.

struct Oink
#include <optimal_ik.hpp>

Oink - Optimal Inverse Kinematics solver.

Public Functions

Oink(const Scene &scene, const std::string &group_name)

Constructs an Oink solver for a named joint group.

Resolves the group to its velocity indices and sizes all internal matrices to the group’s velocity DOF count, which can be much smaller than model.nv when planning for a subset of joints.

Parameters:

  • scene – The scene used to resolve the group at construction time.

  • group_name – Joint group name. Pass an empty string for the full robot.

Throws:

std::runtime_error – if group_name is not found in the scene.

Oink(const Scene &scene, const std::string &group_name, const OsqpEigen::Settings &custom_settings)

Constructs an Oink solver for a named joint group with custom OSQP settings.

Parameters:

  • scene – The scene used to resolve the group at construction time.

  • group_name – Joint group name. Pass an empty string for the full robot.

  • custom_settings – Custom OSQP solver settings.

Throws:

std::runtime_error – if group_name is not found in the scene.

inline explicit Oink(const Scene &scene)

Constructs an Oink solver for the full robot (all joints).

Equivalent to Oink(scene, “”).

inline Oink(const Scene &scene, const OsqpEigen::Settings &custom_settings)

Constructs an Oink solver for the full robot with custom OSQP settings.

Equivalent to Oink(scene, “”, custom_settings).

tl::expected<void, std::string> solveIk(const Scene &scene, const std::vector<std::shared_ptr<Task>> &tasks, Eigen::Ref<Eigen::VectorXd, 0, Eigen::InnerStride<Eigen::Dynamic>> delta_q, double regularization = 1e-12)

Solve inverse kinematics for tasks only.

Solves a QP optimization problem to compute the joint velocity that minimizes weighted task errors.

Parameters:

  • scene – The scene containing robot model and state

  • tasks – Vector of weighted tasks to optimize for

  • delta_q – Pre-allocated output buffer for configuration displacement

  • regularization – Tikhonov regularization weight (default: 1e-12)

Returns:

void on success, error message on failure

tl::expected<void, std::string> solveIk(const Scene &scene, const std::vector<std::shared_ptr<Task>> &tasks, const std::vector<std::shared_ptr<Constraints>> &constraints, Eigen::Ref<Eigen::VectorXd, 0, Eigen::InnerStride<Eigen::Dynamic>> delta_q, double regularization = 1e-12)

Solve inverse kinematics for tasks with constraints.

Solves a QP optimization problem to compute the joint velocity that minimizes weighted task errors while satisfying all constraints.

Parameters:

  • scene – The scene containing robot model and state

  • tasks – Vector of weighted tasks to optimize for

  • constraints – Vector of constraints to satisfy

  • delta_q – Pre-allocated output buffer for configuration displacement

  • regularization – Tikhonov regularization weight (default: 1e-12)

Returns:

void on success, error message on failure

tl::expected<void, std::string> solveIk(const Scene &scene, const std::vector<std::shared_ptr<Task>> &tasks, const std::vector<std::shared_ptr<Barrier>> &barriers, Eigen::Ref<Eigen::VectorXd, 0, Eigen::InnerStride<Eigen::Dynamic>> delta_q, double regularization = 1e-12)

Solve inverse kinematics for tasks with barriers.

Solves a QP optimization problem to compute the joint velocity that minimizes weighted task errors while satisfying all barrier functions.

Parameters:

  • scene – The scene containing robot model and state

  • tasks – Vector of weighted tasks to optimize for

  • barriers – Vector of barrier functions for safety constraints

  • delta_q – Pre-allocated output buffer for configuration displacement

  • regularization – Tikhonov regularization weight (default: 1e-12)

Returns:

void on success, error message on failure

tl::expected<void, std::string> solveIk(const Scene &scene, const std::vector<std::shared_ptr<Task>> &tasks, const std::vector<std::shared_ptr<Constraints>> &constraints, const std::vector<std::shared_ptr<Barrier>> &barriers, Eigen::Ref<Eigen::VectorXd, 0, Eigen::InnerStride<Eigen::Dynamic>> delta_q, double regularization = 1e-12)

Solve inverse kinematics for tasks with constraints and barriers.

Solves a QP optimization problem to compute the joint velocity that minimizes weighted task errors while satisfying all constraints and barrier functions. The result is written directly into the provided delta_q buffer.

Note

The delta_q parameter must be pre-allocated to the correct size before calling. Eigen::Ref cannot be resized, so passing an empty or incorrectly sized vector will result in a failure.

Parameters:

  • scene – The scene containing robot model and state

  • tasks – Vector of weighted tasks to optimize for

  • constraints – Vector of constraints to satisfy

  • barriers – Vector of barrier functions for safety constraints

  • delta_q – Pre-allocated output buffer for configuration displacement. Must be sized to num_variables (velocity space dimension). Using Eigen::Ref allows zero-copy access from Python numpy arrays.

  • regularization – Tikhonov regularization weight added to the Hessian diagonal. This provides numerical stability by ensuring the Hessian is strictly positive definite. Higher values increase regularization but may reduce task tracking accuracy. Default is 1e-12.

Returns:

void on success, error message on failure

tl::expected<void, std::string> enforceBarriers(const Scene &scene, const std::vector<std::shared_ptr<Barrier>> &barriers, Eigen::Ref<Eigen::VectorXd, 0, Eigen::InnerStride<Eigen::Dynamic>> delta_q, double tolerance = 0.0)

Validate delta_q against barriers using forward kinematics.

This method provides a post-solve safety check by evaluating the actual barrier values at the candidate configuration (q + delta_q). If any barrier would be violated, delta_q is set to zero to prevent unsafe motion.

This is a backup safety mechanism for cases where the linearized CBF constraint in the QP has significant error (e.g., large jumps, near-boundary configurations). The QP constraint uses a first-order approximation h(q + δq) ≈ h(q) + J_h · δq, which can have significant error O(||δq||²) for large displacements.

Note

Only barriers that implement evaluateAtConfiguration() are checked. Barriers returning infinity are assumed safe.

Parameters:

  • scene – The scene containing robot model and state

  • barriers – Vector of barrier functions to check

  • delta_q – Configuration displacement to validate. Modified in place: set to zero if barrier violation is detected.

  • tolerance – Tolerance for barrier violation detection. A barrier is considered violated if h(q + delta_q) < -tolerance. Default is 0.0.

Returns:

void on success, error message if barrier evaluation fails

Public Members

OsqpEigen::Solver solver
OsqpEigen::Settings settings
int num_variables
Eigen::VectorXi q_indices

Position indices of the joint group (used to scatter group q into model.nq space).

Eigen::VectorXi v_indices

Velocity indices of the joint group (used to scatter delta_q back into model.nv space).

Eigen::VectorXd task_c
Eigen::SparseMatrix<double> task_H
Eigen::SparseMatrix<double> H
Eigen::VectorXd c
Eigen::MatrixXd constraint_workspace_A
Eigen::VectorXd constraint_workspace_lower
Eigen::VectorXd constraint_workspace_upper
Eigen::SparseMatrix<double> A_sparse
std::vector<int> constraint_sizes
int last_constraint_rows = -1
Eigen::MatrixXd barrier_workspace_G
Eigen::VectorXd barrier_workspace_h
std::vector<int> barrier_sizes
int last_barrier_rows = 0
Eigen::MatrixXd barrier_H_contribution
Eigen::VectorXd barrier_c_contribution
struct PositionBarrier : public roboplan::Barrier
#include <position_barrier.hpp>

Position barrier constraint for end-effector box constraint.

Constrains a frame’s position to remain within an axis-aligned bounding box: p_min <= p(q) <= p_max

This creates up to 6 barrier constraints (2 per enabled axis).

The barrier functions are: h_lower_i = p_i(q) - p_min_i (for min bounds) h_upper_i = p_max_i - p_i(q) (for max bounds)

Uses a saturating class-K function α(h) = γ·h/(1+|h|) for smooth behavior.

Safe displacement regularization encourages moving toward the center of the safe region.

Public Functions

PositionBarrier(const Oink &oink, const Scene &scene, const std::string &frame_name, const Eigen::Vector3d &p_min, const Eigen::Vector3d &p_max, double dt, const ConstraintAxisSelection &axis_selection = ConstraintAxisSelection(), double gain = 1.0, double safe_displacement_gain = 1.0, double safety_margin = 0.0)

Constructs a position barrier for box constraint.

Note

The dt parameter significantly affects barrier behavior - ensure it matches your actual control/integration timestep.

Parameters:

  • oink – The Oink solver this barrier will be used with (provides num_variables and v_indices).

  • scene – The scene used to resolve the frame ID and allocate storage.

  • frame_name – Name of the frame to constrain.

  • p_min – Minimum position bounds [x, y, z] in world frame (use -inf for no constraint).

  • p_max – Maximum position bounds [x, y, z] in world frame (use +inf for no constraint).

  • dt – Timestep matching your control loop period (required; must match actual control loop).

  • axis_selection – Which axes to constrain (default: all three axes).

  • gain – Barrier gain (gamma), controls convergence to safe set. Default 1.0

  • safe_displacement_gain – Gain for safe displacement regularization. Default 1.0

  • safety_margin – Conservative margin for hard constraint guarantee. Default 0.0

Throws:

std::runtime_error – if frame_name is not found in the scene.

virtual int getNumBarriers(const Scene &scene) const override

Get the number of active barrier constraints.

Parameters:

scene – The scene containing robot model and state.

Returns:

Number of active barriers (up to 6: 2 per enabled axis).

virtual tl::expected<void, std::string> computeBarrier(const Scene &scene) override

Compute barrier function values h(q) for all active constraints.

Evaluates the barrier functions:

  • h_lower_i = p_i(q) - p_min_i (for min bounds on enabled axes)

  • h_upper_i = p_max_i - p_i(q) (for max bounds on enabled axes)

Also computes right-hand side bounds using the saturating class-K function: rhs_i = dt * gamma * h_i / (1 + |h_i|) - safety_margin

Results are stored in the inherited barrier_values and barrier_rhs vectors.

Parameters:

scene – The scene containing robot model and current state.

Returns:

Expected void on success, or error message if frame is not found.

virtual tl::expected<void, std::string> computeJacobian(const Scene &scene) override

Compute barrier constraint Jacobian matrix.

Computes the Jacobian -J_h used in the QP constraint: -J_h * delta_q <= rhs Each barrier uses one row of the frame’s position Jacobian (first 3 rows only). The sign is negated for upper bounds to convert p_max - p(q) >= 0 into the standard form.

Results are stored in the inherited barrier_jacobian matrix (num_barriers x nv).

Parameters:

scene – The scene containing robot model and current state.

Returns:

Expected void on success, or error message if frame is not found.

virtual tl::expected<double, std::string> evaluateAtConfiguration(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q) const override

Evaluate minimum barrier value at a candidate configuration.

Computes forward kinematics for the candidate configuration and returns the minimum barrier value across all position constraints (x, y, z min/max).

Parameters:

  • model – Pinocchio model

  • data – Pinocchio data (will be modified by FK computation)

  • q – Candidate joint configuration to evaluate

Returns:

Expected containing minimum barrier value (negative if any constraint is violated), or error message if frame is not found

Eigen::Vector3d getFramePosition(const Scene &scene) const

Get current frame position in world coordinates.

Parameters:

scene – The scene containing robot state.

Returns:

Frame position in world coordinates.

Public Members

const std::string frame_name

Name of the frame to constrain.

const ConstraintAxisSelection axis_selection

Axis selection for constraints (x, y, z).

const Eigen::Vector3d p_min

Minimum position bounds in world frame for each axis.

const Eigen::Vector3d p_max

Maximum position bounds in world frame for each axis.

Eigen::VectorXi v_indices

Velocity indices of the joint group (for Jacobian column selection).

pinocchio::FrameIndex frame_id

Frame index (resolved eagerly at construction).

mutable Eigen::MatrixXd full_jacobian

Pre-allocated full-robot Jacobian workspace (6 x model.nv).

struct PositionLimit : public roboplan::Constraints
#include <position_limit.hpp>

Position limit constraint for inverse kinematics.

Implements joint position constraints by restricting velocities to prevent exceeding joint limits. The constraint is formulated as: l <= G*dq <= u where G is an identity matrix, and the bounds are computed based on the distance to limits.

Public Functions

explicit PositionLimit(const Oink &oink, double gain = 1.0)

Constructor with pre-allocation for optimal performance.

Parameters:

  • oink – The Oink solver this constraint will be used with (provides num_variables and v_indices for selecting the correct joint limits from the full model).

  • gain – Scaling factor for how aggressively to steer away from limits (0 < gain <= 1)

virtual int getNumConstraints(const Scene &scene) const override

Get the number of constraint rows (number_variables)

Parameters:

scene – The scene containing robot state and model

Returns:

Number of constraint rows

virtual tl::expected<void, std::string> computeQpConstraints(const Scene &scene, Eigen::Ref<Eigen::MatrixXd> constraint_matrix, Eigen::Ref<Eigen::VectorXd> lower_bounds, Eigen::Ref<Eigen::VectorXd> upper_bounds) const override

Compute QP constraint matrices for position limits.

Parameters:

  • scene – The scene containing robot state and model

  • constraint_matrix – Output constraint matrix G (number_variables × number_variables)

  • lower_bounds – Output lower bounds vector (number_variables)

  • upper_bounds – Output upper bounds vector (number_variables)

Returns:

void on success, error message on failure

Public Members

double config_limit_gain
int num_variables

Gain parameter for steering away from limits.

Eigen::VectorXi v_indices

Number of group velocity DOFs.

mutable Eigen::VectorXd q_max

Velocity indices of the joint group.

mutable Eigen::VectorXd q_min

Pre-allocated maximum joint position limits.

mutable Eigen::VectorXd delta_q_max

Pre-allocated minimum joint position limits.

mutable Eigen::VectorXd delta_q_min

Pre-allocated workspace for max joint deltas.

struct Task
#include <optimal_ik.hpp>

Abstract base class for IK tasks.

Each task owns pre-allocated storage for Jacobian, error, and H_dense matrices. Subclasses must:

  1. Call initializeStorage() in their constructor with correct dimensions

  2. Implement computeJacobian() to fill jacobian_container

  3. Implement computeError() to fill error_container

Subclassed by roboplan::ConfigurationTask, roboplan::FrameTask

Public Functions

inline Task(Eigen::MatrixXd weight_matrix, double task_gain = 1.0, double lm_damp = 0.0)
virtual ~Task() = default
inline void initializeStorage(int task_rows, int num_vars)

Initialize pre-allocated storage with correct dimensions.

Parameters:

  • task_rows – Number of rows for the task (e.g., 6 for SE(3), nv for configuration)

  • num_vars – Number of optimization variables (model.nv)

virtual tl::expected<void, std::string> computeJacobian(const Scene &scene) = 0

Compute the task Jacobian and store in jacobian_container.

Parameters:

scene – The scene containing robot model and state.

Returns:

void on success, error message on failure.

virtual tl::expected<void, std::string> computeError(const Scene &scene) = 0

Compute the task error and store in error_container.

Parameters:

scene – The scene containing robot model and state.

Returns:

void on success, error message on failure.

tl::expected<void, std::string> computeQpObjective(const Scene &scene, Eigen::SparseMatrix<double> &H, Eigen::VectorXd &c)

Compute QP objective matrices (H, c) for this task.

Computes the contribution of this task to the quadratic program objective: minimize ½ ‖J Δq + α e‖²_W

This is equivalent to: minimize ½ Δq^T H Δq + c^T Δq

Where:

  • J: Task Jacobian matrix

  • Δq: Configuration displacement

  • α: Task gain for low-pass filtering

  • e: Task error vector

  • W: Weight matrix for cost normalization

The method returns:

  • H = J_w^T J_w + μ I (num_variables x num_variables Hessian matrix, sparse)

  • c = -J_w^T e_w (num_variables x 1 linear term)

Where J_w = W*J, e_w = -α*W*e, and μ is the Levenberg-Marquardt damping.

Parameters:

  • scene – The scene containing robot model and state.

  • H – Output Hessian matrix (sparse)

  • c – Output linear cost term

Returns:

void on success, error message on failure.

Public Members

const double gain = 1.0
const Eigen::MatrixXd weight
const double lm_damping = 0.0
int num_variables = 0
Eigen::MatrixXd jacobian_container

Pre-allocated Jacobian container (task_rows × num_variables).

Eigen::VectorXd error_container

Pre-allocated error container (task_rows).

Eigen::MatrixXd H_dense

Pre-allocated dense Hessian matrix (num_variables × num_variables).

struct VelocityLimit : public roboplan::Constraints
#include <velocity_limit.hpp>

Velocity limit constraint for inverse kinematics.

Implements joint velocity constraints to ensure velocities stay within robot limits. The constraint is formulated as: l <= G*dq <= u where G is an identity matrix, l = -dt*v_max, and u = dt*v_max.

Public Functions

VelocityLimit(const Oink &oink, double dt, const Eigen::VectorXd &v_max)

Constructor with dimension validation.

Parameters:

  • oink – The Oink solver this constraint will be used with (provides num_variables).

  • dt – Time step for the velocity integration (seconds)

  • v_max – Maximum velocity vector for each group joint (rad/s or m/s). Size must equal oink.num_variables.

virtual int getNumConstraints(const Scene &scene) const override

Get the number of constraint rows (number_variables)

Parameters:

scene – The scene containing robot state and model

Returns:

Number of constraint rows

virtual tl::expected<void, std::string> computeQpConstraints(const Scene &scene, Eigen::Ref<Eigen::MatrixXd> constraint_matrix, Eigen::Ref<Eigen::VectorXd> lower_bounds, Eigen::Ref<Eigen::VectorXd> upper_bounds) const override

Compute QP constraint matrices for velocity limits.

Parameters:

  • scene – The scene containing robot state and model

  • constraint_matrix – Output constraint matrix G (number_variables × number_variables)

  • lower_bounds – Output lower bounds vector (number_variables)

  • upper_bounds – Output upper bounds vector (number_variables)

Returns:

void on success, error message on failure

Public Members

double dt
Eigen::VectorXd v_max
int num_variables
namespace roboplan

Variables

constexpr int kSpatialDimension = 6

SE(3) spatial dimension (3 position + 3 orientation).

file position_barrier.hpp
#include <Eigen/Dense>
#include <string>
#include <roboplan_oink/optimal_ik.hpp>
file position_limit.hpp
#include <Eigen/Dense>
#include <roboplan_oink/optimal_ik.hpp>
file velocity_limit.hpp
#include <Eigen/Dense>
#include <roboplan_oink/optimal_ik.hpp>
file optimal_ik.hpp
#include <memory>
#include <string>
#include “OsqpEigen/OsqpEigen.h”
#include <tl/expected.hpp>
#include <roboplan/core/scene.hpp>
#include <roboplan/core/types.hpp>
file configuration.hpp
#include <Eigen/Dense>
#include <roboplan/core/scene.hpp>
#include <roboplan_oink/optimal_ik.hpp>
file frame.hpp
#include <limits>
#include <memory>
#include <string>
#include <Eigen/Dense>
#include <roboplan/core/scene.hpp>
#include <roboplan/core/types.hpp>
#include <roboplan_oink/optimal_ik.hpp>
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.0/roboplan_oink/include/roboplan_oink/barriers
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.0/roboplan_oink/include/roboplan_oink/constraints
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.0/roboplan_oink/include
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.0/roboplan_oink
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.0/roboplan_oink/include/roboplan_oink
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.0/roboplan_oink/include/roboplan_oink/tasks

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.

bool collision_check_use_bisection = true

If true, uses bisection instead of linear search 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.3.0/roboplan_rrt/include
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.0/roboplan_rrt
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.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 SplineFittingMode mode = SplineFittingMode::Hermite, const double velocity_scale = 1.0, const double acceleration_scale = 1.0, const int max_adaptive_iterations = 10, const double max_adaptive_step_size = 0.05)

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

  • SplineFittingMode::Hermite: Fits a cubic Hermite spline with zero velocity at all waypoints. This can cause slow execution, but guarantees perfect adherence to the path.

  • SplineFittingMode::Cubic: Fits a cubic spline with zero velocity only at the endpoints. This is smoother, but can cause deviations from the desired path that could lead to collision.

  • SplineFittingMode::Adaptive: Uses the cubic mode but iteratively collision checks and adds intermediate points if it finds collisions, up to a maximum number of iterations. If the path is not collision-free after the maximum iterations, falls back to Hermite mode. Refer to Section 3.5 of https://groups.csail.mit.edu/rrg/papers/Richter_ISRR13.pdf for more details on this approach.

Parameters:

  • velocity_scale – A scaling factor (between 0 and 1) for velocity limits.

  • acceleration_scale – A scaling factor (between 0 and 1) for acceleration limits.

  • max_adaptive_iterations – Maximum number of adaptive iterations, if adaptive mode is enabled.

  • max_adaptive_step_size – If adaptive mode is enabled, this is the maximum joint configuration step size to sample generated splines for collision checking.

  • path – The path to time parameterize.

  • dt – The sample time of the output trajectory, in seconds.

  • mode – The mode to use for spline fitting the path. Options include:

Returns:

A time-parameterized joint trajectory.

Private Functions

tl::expected<toppra::Vectors, std::string> getPathPositionVectors(const JointPath &path)

Helper function to convert the raw joint path to TOPP-RA compatible position vectors.

Parameters:

path – The joint path to convert.

Returns:

The TOPP-RA compatible vectors of collapsed joint paths if successful, else a string describing the error.

std::shared_ptr<toppra::PiecewisePolyPath> generateCubicSpline(const toppra::Vectors &path_pos_vecs)

Helper function to extract a natural cubic spline from a joint path.

This defines zero velocity and acceleration at the endpoints only, meaning that intermediate waypoints are passed through smoothly, but could deviate from the original path and therefore lead to collisions.

Parameters:

path_pos_vecs – The joint path position vectors.

Returns:

The resulting cubic spline.

std::shared_ptr<toppra::PiecewisePolyPath> generateCubicHermiteSpline(const toppra::Vectors &path_pos_vecs)

Helper function to extract a cubic Hermite spline from a joint path.

This enforces zero velocity and acceleration at all waypoints, meaning the desired path is exactly adhered to. If the path was checked for collisions, this spline is also safe.

Parameters:

path_pos_vecs – The joint path position vectors.

Returns:

The resulting cubic Hermite spline.

Private Members

std::shared_ptr<Scene> scene_

A pointer to the scene.

std::string group_name_

The name of the joint group.

std::vector<std::string> joint_names_

The names of the joints in the group.

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

Enums

enum class SplineFittingMode

Enumeration for TOPP-RA spline fitting mode.

Refer to the PathParameterizerTOPPRA options for more information.

Values:

enumerator Hermite
enumerator Cubic
enumerator Adaptive
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>
#include <toppra/geometric_path/piecewise_poly_path.hpp>
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.0/roboplan_toppra/include
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.0/roboplan_toppra
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.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.3.0/roboplan_example_models/include
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.0/roboplan_example_models
dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.3.0/roboplan_example_models/include/roboplan_example_models