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<coal::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 CartesianPath

#include <types.hpp>

Contains a path of Cartesian configurations.

Public Functions

CartesianPath() = default: Default constructor.

inline CartesianPath(const std::vector<std::string> &base_frames, const std::vector<std::string> &tip_frames, const std::vector<std::vector<Eigen::Matrix4d>> &tforms): Constructor.

Public Members

std::vector<std::string> base_frames: The names of the base (or reference) frames.

std::vector<std::string> tip_frames: The names of the tip (or target) frames.

std::vector<std::vector<Eigen::Matrix4d>> tforms

The list of Cartesian transforms from each base frame to each tip frame.

The outer vector indexes the end-effector frame and the inner vector indexes path waypoints, so tforms[frame_idx][path_idx] is the transform for tip_frames[frame_idx] at that path waypoint. NOTE: I’d like this to be a std::vector<std::vector<Eigen::Isometry3d>> but nanobind doesn’t have off the shelf bindings for this.

Friends

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

struct CartesianTrajectory

#include <types.hpp>

Contains a trajectory of Cartesian configurations.

Public Functions

CartesianTrajectory() = default: Default constructor.

inline CartesianTrajectory(const std::vector<std::string> &base_frames, const std::vector<std::string> &tip_frames, const std::vector<double> &times, const std::vector<std::vector<Eigen::Matrix4d>> &tforms): Constructor.

Public Members

std::vector<std::string> base_frames: The names of the base (or reference) frames.

std::vector<std::string> tip_frames: The names of the tip (or target) frames.

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

std::vector<std::vector<Eigen::Matrix4d>> tforms

The list of Cartesian transforms from each base frame to each tip frame.

The outer vector indexes the end-effector frame and the inner vector indexes time, so tforms[frame_idx][time_idx] is the transform for tip_frames[frame_idx] at times[time_idx]. NOTE: I’d like this to be a std::vector<std::vector<Eigen::Isometry3d>> but nanobind doesn’t have off the shelf bindings for this.

Friends

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

class CollisionContext

#include <collision_context.hpp>

Per-consumer scratch space for collision queries (Data + GeometryData + broadphase tree).

A Scene’s collision-query path mutates shared scratch state (joint/frame placements, the geometry world transforms, and the broadphase AABB tree), so a single Scene cannot answer collision queries from multiple threads concurrently. A CollisionContext owns its own copy of that scratch over the Scene’s immutable Model and GeometryModel (shared by pointer), so an algorithm such as the RRT can run its collision queries without contending with anything else.

A context is a snapshot of the scene’s collision geometry at construction time. If the scene geometry changes, discard the context and build a new one; it does not auto-sync.

Public Functions

explicit CollisionContext(const Scene &scene): Snapshots the current collision geometry of scene.

CollisionContext(const CollisionContext&) = delete

CollisionContext &operator=(const CollisionContext&) = delete

CollisionContext(CollisionContext&&) = delete

CollisionContext &operator=(CollisionContext&&) = delete

bool hasCollisions(const Eigen::VectorXd &q) const: Checks collisions at q, stopping at the first collision. Uses this context’s own scratch, so it is safe to call concurrently with queries on other contexts or the Scene.

Private Types

using BroadPhaseManager = pinocchio::BroadPhaseManagerTpl<coal::DynamicAABBTreeCollisionManager>

Private Members

const pinocchio::Model &model_: Borrowed; immutable; owned by the Scene.

const pinocchio::GeometryModel &collision_model_: Borrowed; immutable; owned by the Scene.

mutable pinocchio::Data data_: Owned scratch for forward kinematics.

mutable pinocchio::GeometryData geom_data_: Owned scratch for geometry placements.

mutable std::optional<BroadPhaseManager> manager_: Owned; bound to this context’s geom_data_.

struct Cylinder

#include <geometry_wrappers.hpp>

Temporary wrapper struct to represent a cylinder geometry (oriented along the Z axis).

Public Functions

inline Cylinder(double radius, double length)

Construct a Cylinder object wrapper.

Parameters:

radius – The radius of the cylinder.
length – The total length of the cylinder along its Z axis.

Public Members

std::shared_ptr<coal::Cylinder> geom_ptr: The underlying Coal cylinder geometry.

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 Mesh

#include <geometry_wrappers.hpp>

Temporary wrapper struct to represent a triangle mesh geometry loaded from a file.

Public Functions

inline Mesh(const std::filesystem::path &filename, const Eigen::Vector3d &scale = Eigen::Vector3d::Ones())

Construct a Mesh object wrapper by loading from a mesh file (e.g. STL, OBJ, DAE).

Parameters:

filename – Path to the mesh file to load.
scale – Per-axis scale factors applied to the loaded mesh. Defaults to (1, 1, 1).

inline Mesh(const std::shared_ptr<coal::BVHModelBase> &mesh_geom): Construct a Mesh object wrapper from a pre-loaded Coal BVH model.

Public Members

std::shared_ptr<coal::BVHModelBase> geom_ptr: The underlying Coal BVH mesh geometry.

struct OcTree

Public Functions

inline OcTree(const std::vector<Eigen::Matrix<double, 6, 1>> &boxes, const double resolution)

inline OcTree(const std::shared_ptr<coal::OcTree> &octree_geom)

Public Members

std::shared_ptr<coal::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 pinocchio::GeometryModel &getCollisionModel() const

Gets the scene’s internal Pinocchio collision model.

Returns:: The Pinocchio collision (geometry) model.

inline const pinocchio::GeometryData &getCollisionData() const

Gets the scene’s internal Pinocchio collision (geometry) data.

The data is shared with the scene; computations such as hasCollisions() and computeCollisionDistances() write into this data.

Returns:: The Pinocchio collision (geometry) data.

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

Updates geometry placements and computes distance results for all active collision pairs at the specified joint configuration.

After this call, distances are available via getCollisionData().distanceResults.

Parameters:: q – The joint configuration at which to compute the distances.

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

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

Returns:: A vector of joint names.

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

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

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.

void randomizeJointPositions(const std::vector<std::string> &joint_names, Eigen::VectorXd &q)

Randomizes the positions of the specified joints in-place within a full configuration.

Only the degrees of freedom belonging to joint_names are overwritten; all other entries of q are left untouched. This avoids allocating a full configuration and sampling joints outside of a planning group on every call (e.g. in the RRT sampling loop).

Parameters:

joint_names – The names of the joints to randomize.
q – The full configuration vector to modify in-place. Must be sized to the model’s nq.

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 isValidConfiguration(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.

Eigen::VectorXd clampToValidConfiguration(const Eigen::VectorXd &q) const

Clamps the specified joint positions to valid joint limits.

Bounded joints are clamped to their position limits, while continuous and planar rotation representations are renormalized onto the unit circle.

Parameters:: q – The joint positions.
Returns:: A new vector of joint positions that respects joint limits.

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.

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 std::string &base_frame = "") 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.
base_frame – Optional base frame. If empty, returns the world-frame pose.

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

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

Computes the joint Jacobians for every joint at the given configuration.

Populates the internal Pinocchio data so that pinocchio::getJointJacobian can be called for any joint after this. Also runs forward kinematics.

Parameters:: q – The joint configuration.

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 Pinocchio configuration vector (model.nq).

This is the internal planning layout (e.g. continuous joints as [cos, sin]). Joint count may differ from getJointNames().size().

Returns:: The current joint position vector.

Eigen::VectorXd getCurrentJointPositionsWithMimics() const

Get current joint positions for all joints in getJointNamesWithMimics() order.

Actuated joints copy values from the Pinocchio configuration; mimic joints use scaling * mimicked_position + offset per degree of freedom.

Returns:: The joint position vector aligned with getJointNamesWithMimics().

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

Set the joint positions for the full robot state.

Parameters:: positions – The desired joint position vector (size model.nq).

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 bool collapsed = false) 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.
collapsed – If true, collapses limits for continuous rotation degrees of freedom into one value; else, leaves them expanded as two values for cos(theta) and sin(theta).

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> addCylinderGeometry(const std::string &name, const std::string &parent_frame, const Cylinder &cylinder, const Eigen::Matrix4d &tform, const Eigen::Vector4d &color)

Adds a cylinder 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.
cylinder – The cylinder 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> addMeshGeometry(const std::string &name, const std::string &parent_frame, const Mesh &mesh, const Eigen::Matrix4d &tform, const Eigen::Vector4d &color)

Adds a triangle mesh 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.
mesh – The mesh 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 Types

using BroadPhaseManager = pinocchio::BroadPhaseManagerTpl<coal::DynamicAABBTreeCollisionManager>: Broadphase collision manager type, using a dynamic AABB tree to cull non-overlapping geometry pairs before narrow-phase collision checking.

Private Functions

void rebuildBroadphaseManager()

(Re)builds broadphase_manager_ from the current collision model and data.

Must be called after collision_model_data_ is (re)assigned, since the manager caches pointers and geometry state derived from it.

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.

mutable std::optional<BroadPhaseManager> broadphase_manager_

Broadphase manager used to accelerate hasCollisions().

Caches AABB-tree state and holds pointers into collision_model_ and collision_model_data_, so it must be rebuilt (see rebuildBroadphaseManager) whenever the collision geometry or its data is changed. Mutable for the same reason as collision_model_data_ (updated in place during a const collision query), and not thread-safe across shared Scenes.

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_: Actuated (non-mimic) joint names in model order.

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.

class SE3LowPassFilter

#include <se3_low_pass_filter.hpp>

First-order low-pass filter for SE3 poses.

Public Functions

SE3LowPassFilter(double tau = 0.1)

Creates an SE3 low-pass filter.

Parameters:: tau – Time constant in seconds. Larger values produce slower, smoother tracking.

void reset(const pinocchio::SE3 &pose)

Resets the filter state to a specific pose.

Parameters:: pose – Pose used as the new filtered state.

pinocchio::SE3 update(const pinocchio::SE3 &target_pose, double dt)

Updates the filtered state toward a target pose.

Parameters:

target_pose – Target pose to filter toward.
dt – Time step in seconds.

Returns:

The updated filtered pose.

double tau() const

Returns the filter time constant in seconds.

Returns:: Filter time constant.

void setTau(double tau)

Sets the filter time constant.

Parameters:: tau – Time constant in seconds. Larger values produce slower, smoother tracking.

bool isInitialized() const

Checks whether the filter has an active filtered state.

Returns:: True if the filter has been initialized by reset() or update().

Private Members

double tau_: Filter time constant in seconds.

bool initialized_: Whether the filter currently has a valid filtered pose.

Eigen::Vector3d filtered_position_: Current filtered translation component.

Eigen::Quaterniond filtered_quaternion_: Current filtered rotation component.

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<coal::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::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 CollisionContext &collision_context, const Eigen::VectorXd &q_start, const Eigen::VectorXd &q_end, const double max_step_size, const bool bisection = false, const bool check_endpoints = true)

Checks collisions along a specified configuration space path.

All collision checks are answered by the caller-owned context, so the traversal does not contend on the Scene’s shared collision scratch. Interpolation and distance use scene, which only reads the immutable model and is therefore safe to share.

Parameters:

scene – The scene to use for interpolating positions and computing distances.
collision_context – The collision context whose scratch is used for all collision checks.
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, visits the interior grid points in a coarse-to-fine bisection order instead of a linear scan. This checks exactly the same minimal number of points as the linear scan, but can find collisions faster in collision-dense environments since points near the middle of the path are checked first.
check_endpoints – If True, checks the start and end endpoints for collisions. Callers that already know both endpoints are collision-free (e.g. they are existing nodes in a search tree) can set this to False to skip redundant, expensive collision checks.

Returns:

True if there are collisions, else false.

bool hasCollisionsAlongPath(const Scene &scene, const Eigen::VectorXd &q_start, const Eigen::VectorXd &q_end, const double max_step_size, const bool bisection = false, const bool check_endpoints = true)

Checks collisions along a specified configuration space path using the Scene’s own scratch.

This convenience overload answers every collision check via scene.hasCollisions, which uses the Scene’s internal (shared) collision scratch. It avoids constructing a per-call CollisionContext, but carries the same caveat as every other Scene collision query: it is not safe to call concurrently with other queries on the same Scene. Callers that need to parallelize should own a CollisionContext and use the overload above.

Parameters:

scene – The scene to use for interpolation, distances, and collision checks.
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, visits the interior grid points in a coarse-to-fine bisection order instead of a linear scan. This checks exactly the same minimal number of points as the linear scan, but can find collisions faster in collision-dense environments since points near the middle of the path are checked first.
check_endpoints – If True, checks the start and end endpoints for collisions. Callers that already know both endpoints are collision-free (e.g. they are existing nodes in a search tree) can set this to False to skip redundant, expensive collision checks.

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.

Eigen::VectorXd jointPositionsWithMimicsFromPinocchio(const Scene &scene, const Eigen::VectorXd &q)

Builds joint positions for all joints in getJointNamesWithMimics() order.

Non-mimic joints copy their Pinocchio q block; mimic joints use the mimic law.

Parameters:

scene – The scene from which to look up joint information.
q – The Pinocchio configuration vector (model.nq).

Returns:

Position vector aligned with getJointNamesWithMimics().

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

Returns the contents of a file as a string.

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

Variables

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

file collision_context.hpp: #include <optional>

#include <Eigen/Dense>

#include <pinocchio/collision/broadphase-manager.hpp>

#include <pinocchio/multibody/data.hpp>

#include <pinocchio/multibody/geometry.hpp>

#include <pinocchio/multibody/model.hpp>

#include <hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h>

file geometry_wrappers.hpp: #include <filesystem>

#include <hpp/fcl/BVH/BVH_model.h>

#include <hpp/fcl/mesh_loader/loader.h>

#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/collision/broadphase-manager.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>

#include <hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h>

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>

file se3_low_pass_filter.hpp: #include <Eigen/Geometry>

#include <pinocchio/spatial/se3.hpp>

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

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/latest/roboplan/include/roboplan/filters

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

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

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/latest/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 lower_position_limits_: Lower position limits for the joint group, aligned with q_indices.

Eigen::VectorXd upper_position_limits_: Upper position limits for the joint group, aligned with q_indices.

Eigen::VectorXd error_: The full error vector (for allocating memory once).

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

Eigen::Matrix<double, 6, 6> Jlog_: The derivative of log6 at a frame’s pose error (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.005: Max total computation time, in seconds.

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

double step_size = 0.25: 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.

bool fast_return = true

If true, returns when the first ik solution is found.

Otherwise the entire time budget will be consumed to attempt to find a solution that is closest to the starting configuration.

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/latest/roboplan_simple_ik/include

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

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/latest/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, roboplan::SelfCollisionBarrier

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

void formatQpInequalities(Eigen::Ref<Eigen::MatrixXd> G, Eigen::Ref<Eigen::VectorXd> b) const

Format the QP inequality constraints from already-computed barrier values/Jacobian.

Produces: G_b * delta_q <= b_b Where: G_b = -J_h / dt b_b = γ·(h - m) / (1 + |h - m|) (saturating class-K function)

Parameters:

G – Output constraint matrix (pre-sized view: num_barriers x num_variables)
b – Output constraint upper bound vector (pre-sized view: num_barriers)

Pre:

computeBarrier() and computeJacobian() must have been called first.

void formatQpObjective(const Scene &scene, Eigen::Ref<Eigen::MatrixXd> H, Eigen::Ref<Eigen::VectorXd> c) const

Format the QP objective contribution from already-computed barrier Jacobian.

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

Parameters:

scene – The scene (passed to computeSafeDisplacement)
H – Output Hessian matrix contribution (num_variables x num_variables)
c – Output gradient vector contribution (num_variables)

Pre:

computeBarrier() and computeJacobian() must have been called first.

tl::expected<void, std::string> computeQpInequalities(const Scene &scene, Eigen::Ref<Eigen::MatrixXd> G, Eigen::Ref<Eigen::VectorXd> b): Convenience: compute barrier + Jacobian, then format QP inequalities. Equivalent to calling computeBarrier(), computeJacobian(), formatQpInequalities().

tl::expected<void, std::string> computeQpObjective(const Scene &scene, Eigen::Ref<Eigen::MatrixXd> H, Eigen::Ref<Eigen::VectorXd> c): Convenience: compute barrier + Jacobian, then format QP objective. Equivalent to calling computeBarrier(), computeJacobian(), formatQpObjective().

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

int priority = 1: Priority level (default: 1). Tasks at higher priority numbers are projected into the nullspace of all lower priority numbers. Must be >= 1.

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.

int priority = 1: Priority level (default: 1). Tasks at higher priority numbers are projected into the nullspace of all lower priority numbers. Must be >= 1.

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

Eigen::MatrixXd jacobian_stack

Eigen::MatrixXd nullspace_projector

Eigen::MatrixXd projected_weighted_jacobian

Eigen::VectorXd weighted_error

std::vector<Task*> sorted_tasks

pinocchio::Data enforce_barriers_data

Private Functions

tl::expected<void, std::string> addTaskContribution(const Scene &scene, Task *task)

Compute task’s Jacobian and error, and add its contribution to the QP Hessian and gradient (projecting through the current nullspace_projector for hierarchical priorities).

Parameters:

scene – The scene containing robot model and state.
task – The task to add to the QP objective.

Returns:

void if successful, else an error message describing the failure.

void rebuildNullspaceProjector(double lambda_sq)

Rebuild nullspace_projector from the current jacobian_stack via a damped pseudoinverse, so subsequent priority levels are projected into the nullspace of everything stacked so far.

Parameters:: lambda_sq – Damping factor for the pseudoinverse.

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 SelfCollisionBarrier : public roboplan::Barrier 

#include <self_collision_barrier.hpp>

Self-collision avoidance barrier based on collision pair distances.

Constrains the closest n_collision_pairs collision pairs in the scene to remain at least d_min apart: h_i(q) = d(p_a, p_b)_i - d_min for i in 0 .. n_collision_pairs - 1 where the index i refers to the i-th closest pair at the current configuration.

The barrier Jacobian row is built from the witness points returned by collision checking and the joint Jacobians of the parent joints of each colliding body: J_i = n^T J^1_p + (r_1 × n)^T J^1_w

n^T J^2_p - (r_2 × n)^T J^2_w where n is the unit vector from witness point 1 to witness point 2 (in world coordinates), r_k is the vector from joint k origin to its witness point, and J^k_p / J^k_w are the linear / angular parts of the LOCAL_WORLD_ALIGNED joint Jacobian.

Uses the same saturating class-K function α(h) = γ·h/(1+|h|) as other barriers.

Note

Inspired by Pink’s pink.barriers.SelfCollisionBarrier (Apache-2.0). https://github.com/stephane-caron/pink/blob/main/pink/barriers/self_collision_barrier.py

Public Functions

SelfCollisionBarrier(const Oink &oink, const Scene &scene, int n_collision_pairs, double dt, double gain = 1.0, double safe_displacement_gain = 1.0, double d_min = 0.02, double safety_margin = 0.0)

Constructs a self-collision barrier.

Parameters:

oink – The Oink solver this barrier will be used with.
scene – The scene whose collision model defines the collision pairs to monitor.
n_collision_pairs – Number of closest collision pairs to consider. Must be > 0 and no greater than the number of collision pairs in the scene’s collision model. If fewer pairs than the total are requested, only the closest ones are constrained.
dt – Timestep matching the control loop period (must match actual control loop).
gain – Barrier gain (gamma), controls convergence to safe set. Default 1.0.
safe_displacement_gain – Gain for safe displacement regularization. Default 1.0.
d_min – Minimum allowed distance between any pair of bodies. Must be non-negative. Default 0.02.
safety_margin – Conservative margin for hard constraint guarantee. Default 0.0.

Throws:

std::invalid_argument – if d_min is negative, n_collision_pairs is non-positive, or n_collision_pairs exceeds the number of collision pairs in the scene.

virtual int getNumBarriers(const Scene &scene) const override

Get the number of active barrier constraints.

Returns:: The number of collision pairs being constrained.

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

Compute barrier function values h(q) for the closest n_collision_pairs pairs.

Triggers distance computation on the scene’s collision data, then fills barrier_values with the distances of the closest pairs, each shifted by -d_min. The selected pair indices are cached in closest_pair_indices for use by computeJacobian().

Parameters:: scene – The scene containing the robot collision model and current state.
Returns:: Expected void on success, error message on failure.

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

Compute the Jacobian rows for the closest n_collision_pairs collision pairs.

Assumes computeBarrier() has been called first (it caches the selected pair indices). Each row uses the witness points and the parent-joint Jacobians of the two bodies in the collision pair. If the witness points coincide, the row is zeroed (Jacobian is undefined there). Any NaN values are clamped to zero.

Parameters:: scene – The scene containing the robot collision model and current state.
Returns:: Expected 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 override

Evaluate the minimum barrier value at a candidate configuration.

Refreshes geometry placements on a private GeometryData copy and runs narrow-phase distance only on the pairs cached by the most recent computeBarrier() call (closest_pair_indices). For small displacements between the configuration used in computeBarrier() and q, those are the active constraints, and skipping narrow phase on the remaining pairs is the dominant per-solve speedup. computeBarrier() must have run before this method.

Parameters:

model – Pinocchio model (must be the same as the scene’s model).
data – Pinocchio data (will be modified by FK / distance computation).
q – Candidate joint configuration to evaluate.

Returns:

Expected containing the minimum barrier value (negative if any pair is in collision), or an error message on failure.

Public Members

const int n_collision_pairs: Number of closest collision pairs to constrain.

const double d_min: Minimum allowed distance between any pair of bodies.

Eigen::VectorXi v_indices: Velocity indices of the joint group (for Jacobian column selection).

std::vector<std::size_t> closest_pair_indices: Indices into the scene’s collision model collision pairs that were selected in the most recent computeBarrier() call (size n_collision_pairs).

Eigen::VectorXd all_distances: Workspace buffer holding min_distance for every collision pair in the model, repopulated on each computeBarrier() call from the scene’s GeometryData.

mutable Eigen::MatrixXd joint_jacobian1: Pre-allocated workspace for one parent-joint Jacobian (6 x model.nv).

mutable Eigen::MatrixXd joint_jacobian2: Pre-allocated workspace for the other parent-joint Jacobian (6 x model.nv).

mutable Eigen::RowVectorXd full_row: Pre-allocated workspace for a single barrier-Jacobian row across all model.nv velocity DOFs (before selecting the joint-group columns).

const pinocchio::GeometryModel *collision_model: Pointer to the scene’s collision model (non-owning). Captured at construction.

mutable pinocchio::GeometryData eval_geom_data: Private GeometryData used by evaluateAtConfiguration to avoid mutating scene state.

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:

Call initializeStorage() in their constructor with correct dimensions
Implement computeJacobian() to fill jacobian_container
Implement computeError() to fill error_container

Subclassed by roboplan::ConfigurationTask, roboplan::FrameTask

Public Functions

inline Task(int task_priority, 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

const int priority = 1

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 self_collision_barrier.hpp: #include <Eigen/Dense>

#include <vector>

#include <pinocchio/multibody/geometry.hpp>

#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/latest/roboplan_oink/include/roboplan_oink/barriers

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/latest/roboplan_oink/include/roboplan_oink/constraints

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/latest/roboplan_oink/include

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/latest/roboplan_oink

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/latest/roboplan_oink/include/roboplan_oink

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/latest/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, const CollisionContext &collision_context, bool greedy)

Attempt to add node(s) to the provided tree and node set, growing toward q_sample.

Parameters:

tree – The tree to grow.
nodes – The set of sampled nodes so far.
q_sample – The configuration to extend towards (or connect to).
collision_context – This plan’s private collision context, used for all collision checks.
greedy – If true (the RRT-Connect CONNECT step), repeatedly extend toward q_sample until it is reached or an obstacle is hit. If false (a single EXTEND step), add at most one node, one max_connection_distance step toward q_sample.

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, const CollisionContext &collision_context)

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.
collision_context – This plan’s private collision context, used for all collision checks.

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.

Eigen::VectorXd collapse(const Eigen::VectorXd &q_group) const

Collapses group joint positions to the k-d tree state space coordinates.

Thin wrapper around collapseContinuousJointPositions that throws on failure, so the tree operations can call it without repeating the error handling at each call site.

Parameters:: q_group – The group joint positions, in expanded (original) coordinates.
Returns:: The collapsed configuration used for nearest-neighbor lookups.

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/collision_context.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/latest/roboplan_rrt/include

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

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/latest/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.

Eigen::VectorXi q_indices_: Position indices of the joint group within the full model configuration.

std::vector<size_t> continuous_q_indices_

A list of position indices with continuous degrees of freedom.

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

bool has_planar_joints_ = {false}

Whether the joint group contains any planar joints.

When true, edges between path waypoints are densely resampled using the scene’s SE(2)-aware interpolation so the spline knots track the actual Lie-group motion of the planar joint(s).

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/latest/roboplan_toppra/include

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

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/latest/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/latest/roboplan_example_models/include

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

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