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.

void computeDistances(const Eigen::VectorXd &q, std::optional<double> broadphase_margin = std::nullopt) const

Refreshes geometry placements at q and computes the distance for every active collision pair into this context’s own GeometryData.

Runs on this context’s private scratch, so it is safe to call concurrently with queries on other contexts or the Scene. Results are available via getCollisionData().

Parameters:: broadphase_margin – Broadphase cull distance. Pairs whose world axis-aligned bounding boxes are farther apart than this are skipped: their (cheap) AABB-gap lower bound is stored as the distance and their witness points are collapsed to the origin, so any Jacobian built from them is a zero row. Such pairs are, by construction, farther than the margin and cannot be the binding constraint of a barrier whose minimum distance is well inside it. The exact narrow-phase distance and witness points are still computed for every pair within the margin. Pass std::nullopt (the default) to disable culling and compute the exact distance for every pair (equivalent to pinocchio::computeDistances).

void computeJointJacobians(const Eigen::VectorXd &q) const: Computes the joint Jacobians at q into this context’s own Data, so consumers that build task/barrier Jacobians (e.g. self-collision) do not have to touch the Scene’s scratch. Results are available via getData().

inline const pinocchio::Model &getModel() const: The immutable model shared with the originating Scene.

inline pinocchio::Data &getData() const: This context’s private Data scratch (forward kinematics, joint Jacobians).

inline const pinocchio::GeometryModel &getCollisionModel() const: The immutable collision geometry model shared with the originating Scene.

inline pinocchio::GeometryData &getCollisionData() const: This context’s private GeometryData scratch (geometry placements, distance results).

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

mutable std::vector<coal::CollisionObject> aabb_objects_: One coal collision object per geometry object (parallel to collision_model_’s geometryObjects), reused by computeDistances() to refresh world AABBs for broadphase culling.

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 = Eigen::VectorXd(): The joint positions, in the same order as the names.

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

Eigen::VectorXd accelerations = Eigen::VectorXd(): 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.

std::vector<std::string> link_names

The link (body) names that make up the group.

This includes the links driven by the group’s joints (e.g., the links along a chain, or the child links of the listed joints) as well as any links manually specified via <link> elements inside the group in the SRDF. Useful for selecting the subset of visual or collision geometry that belongs to a 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 PathShortcuttingOptions &options)

Construct a new path shortcutter instance.

Parameters:

scene – The scene for checking connectability between joint positions.
options – A struct containing path shortcutting options.

JointPath shortcut(const JointPath &path)

Attempts to shortcut a specified path.

Each iteration samples two configurations along the path and, if they connect collision-free, splices in the straight connection. Because successful corner-cutting shortcuts introduce new interpolated vertices, a deterministic redundant-vertex removal pass is interleaved periodically and run once more at the end to collapse vertices whose neighbors became directly connectable, preventing accumulation of unhelpful micro-segments.

Parameters:: path – The JointPath to try to shorten.
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 Functions

size_t removeRedundantVertices(std::vector<Eigen::VectorXd> &path_configs, const CollisionContext &collision_context)

Removes interior vertices whose neighbors are directly connectable.

Sweeps the path and deletes any interior vertex whose preceding and following vertices can be connected by a collision-free straight segment, repeating until a full sweep removes nothing. The endpoints are never removed. This collapses the redundant, nearly-collinear vertices left behind by corner-cutting shortcuts. Endpoint collision checks are skipped because every vertex is already an existing (collision-free) path node.

Parameters:

path_configs – The path configurations to prune in place.
collision_context – The collision context whose scratch backs the connection checks.

Returns:

The number of vertices removed.

Private Members

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

PathShortcuttingOptions options_: The path shortcutting options.

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

struct PathShortcuttingOptions

#include <path_utils.hpp>

Options struct for path shortcutting.

Public Members

std::string group_name = "": The joint group name to be used for path shortcutting.

double max_step_size = 0.05: Maximum step size used in collision checking, and the minimum separable distance between points in a shortcut.

unsigned int max_iters = 100: Maximum number of iterations of random sampling.

int seed = 0: Seed for the random generator. If < 0, a random seed is used.

unsigned int max_convergence_iters = 20: Stop early once this many consecutive iterations fail to apply a shortcut (i.e., the path has converged), instead of always running the full max_iters. A value of 0 disables early stopping.

unsigned int redundant_removal_iters = 20: Cadence (in iterations) at which to interleave the redundant-vertex removal pass that cleans up the micro-segments introduced by shortcutting.

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 expressed in world frame.

Note

Requires that forward kinematics and frame placements are up-to-date, or that this is the first kinematics call for the given q (the underlying Pinocchio call runs FK).

Parameters:

q – The joint configuration.
frame_id – The Pinocchio frame ID of the frame.
reference_frame – The reference frame for the Jacobian output (LOCAL, WORLD, or LOCAL_WORLD_ALIGNED).
jacobian – Output matrix to store the Jacobian (must be pre-allocated to 6 x nv).

void computeRelativeFrameJacobian(const Eigen::VectorXd &q, pinocchio::FrameIndex frame_id, const std::string &base_frame, pinocchio::ReferenceFrame reference_frame, Eigen::Ref<Eigen::MatrixXd> jacobian) const

Computes the Jacobian of a frame’s velocity relative to a (possibly moving) base frame.

Computes the Jacobian of the EE frame velocity relative to the base frame, expressed in the reference frame of the relative transform T_rel = T_base^{-1} * T_ee.

Note

Requires that forward kinematics and frame placements are up-to-date, or that this is the first kinematics call for the given q (the underlying Pinocchio call runs FK).

Parameters:

q – The joint configuration.
frame_id – The Pinocchio frame ID of the end-effector frame.
base_frame – The name of the base frame (its ID is looked up internally).
reference_frame – The reference frame for the Jacobian output. LOCAL is expressed in the body frame of T_rel; LOCAL_WORLD_ALIGNED is at the T_rel origin with world orientation.
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.

struct UrdfExtendedJointLimits

#include <scene_utils.hpp>

Holds extended joint limits (acceleration, jerk) parsed from a URDF <limit> tag.

This is a temporary holdover until Pinocchio properly supports URDF 1.2 extended limits in its own parsers. See https://github.com/stack-of-tasks/pinocchio/issues/2893

Public Members

std::optional<double> acceleration

std::optional<double> jerk

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.

std::vector<Eigen::VectorXd> resampleUniform(const std::vector<Eigen::VectorXd> &positions, size_t count, const Scene &scene, const Eigen::VectorXi &q_indices)

Resamples a dense sequence of group joint positions to count waypoints spaced uniformly in configuration-space arc length (endpoints preserved).

Arc length and interpolation are computed with Scene::configurationDistance and Scene::interpolate so that continuous / free-rotating joints are measured and blended on their true manifold rather than as raw coordinates: differencing those coordinates as a flat Euclidean vector mishandles their tangent space (e.g. the wrap from +pi to -pi reads as a large jump, and the SO(2) cos/sin pair of a continuous joint does not subtract linearly).

This is useful when downstream consumers need evenly spaced knots: e.g. TOPP-RA parameterizes its spline by waypoint index, so unevenly spaced waypoints (clustered where a tracker throttled at corners) leave large gaps that the spline overshoots, deviating from the path.

Parameters:

positions – Dense group joint positions, each of size q_indices.size().
count – Target number of (uniformly spaced) waypoints.
scene – Scene providing the manifold-aware distance/interpolation over the full model.
q_indices – The full-configuration indices occupied by the group’s coordinates.

Returns:

The resampled group joint positions.

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.

tl::expected<double, std::string> computePathLength(const Scene &scene, const std::string &group_name, const JointPath &path)

Computes the total configuration-space length of a joint path.

Sums the Scene’s configuration distance between consecutive waypoints. The path’s positions are group positions, so each is expanded to full joint positions before measuring.

Parameters:

scene – The scene used to measure configuration distances.
group_name – The joint group the path was planned for.
path – The joint path to measure. Must contain at least two waypoints.

Returns:

The total path length, or an error if the path has fewer than two points.

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::unordered_map<std::string, UrdfExtendedJointLimits> parseUrdfExtendedJointLimits(const std::string &urdf)

Parses extended joint limits (acceleration, jerk) from URDF <limit> tags.

Reads acceleration and jerk attributes if present, regardless of URDF version. Returns an empty map only if parsing fails. This is a temporary holdover until Pinocchio properly supports URDF 1.2 extended limits. See https://github.com/stack-of-tasks/pinocchio/issues/2893

Parameters:: urdf – The URDF XML string.
Returns:: A map from joint name to its extended limits.

void overrideJointLimitsFromYaml(const pinocchio::Model &model, const YAML::Node &yaml_config, const std::unordered_map<std::string, UrdfExtendedJointLimits> &urdf_extended_limits, const std::string &joint_name, JointInfo &info)

Overrides a joint’s limits in-place from a YAML configuration.

Position, velocity, acceleration, and jerk limits may each be overridden via a joint_limits/<joint_name> entry, where every limit is a sequence sized to the joint’s number of velocity DOFs. When no override is present, velocity limits fall back to the URDF values from the model and acceleration/jerk limits fall back to the extended URDF limits. Position limits for free-rotating DOFs (continuous joints and the orientation DOFs of planar/floating joints) are meaningless and are discarded with a warning unless given as ‘.inf’ / ‘-.inf’.

Parameters:

model – The Pinocchio model, used for URDF-derived velocity limits.
yaml_config – The parsed YAML configuration node (may be empty/null).
urdf_extended_limits – Extended (acceleration, jerk) limits parsed from the URDF.
joint_name – The name of the joint to override.
info – The joint info to modify in-place.

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

Returns the contents of a file as a string.

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

std::pair<double, double> poseError(const Eigen::Matrix4d &a, const Eigen::Matrix4d &b)

Computes the position (meters) and orientation (radians) error between two SE(3) transforms expressed in the same frame.

Parameters:

a – The first transform.
b – The second transform.

Returns:

A pair of {position error, orientation error}.

Eigen::Matrix4d interpolatePose(const Eigen::Matrix4d &start, const Eigen::Matrix4d &end, double fraction)

Interpolates between two SE(3) transforms: linear in position, SLERP in orientation.

Parameters:

start – The transform at fraction 0.
end – The transform at fraction 1.
fraction – The interpolation coefficient, between 0 and 1.

Returns:

The interpolated transform.

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 <vector>

#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 pose_utils.hpp: #include <utility>

#include <Eigen/Dense>

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 <optional>

#include <string>

#include <unordered_map>

#include <pinocchio/multibody/model.hpp>

#include <yaml-cpp/yaml.h>

#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/0.5.0/roboplan/include/roboplan/core

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

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

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

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

Example Models

namespace roboplan

namespace example_models

Functions

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

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

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

file resources.hpp: #include <filesystem>

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

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

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

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

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

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

Optimal IK (OInK)

struct AccelerationLimit : public roboplan::Constraints 

#include <acceleration_limit.hpp>

Acceleration limit constraint for inverse kinematics.

Bounds the change in joint velocity between successive differential-IK steps so the executed motion does not “jump” in velocity (i.e. unbounded acceleration). Inspired by pink.limits.AccelerationLimit.

The limit combines two inequalities, both expressed as box bounds on the configuration displacement Δq (the QP variable). With dt the control timestep, a_max the per-joint acceleration limit, and Δq_prev the displacement applied on the previous step:

Finite-difference acceleration bound: -a_max <= ((Δq/dt) - (Δq_prev/dt)) / dt <= a_max i.e. Δq_prev - a_max*dt² <= Δq <= Δq_prev + a_max*dt²
”Braking distance” to the joint position limits, so the velocity can always be brought to zero before hitting a limit (see [Flacco2015], [DelPrete2018]): -dt*sqrt(2*a_max*(q - q_min)) <= Δq <= dt*sqrt(2*a_max*(q_max - q))

The tighter of the two is taken per joint, yielding box bounds l <= G*Δq <= u with G = identity (one constraint row per group velocity DOF).

Public Functions

AccelerationLimit(const Oink &oink, double dt, const Eigen::VectorXd &a_max)

Constructor with dimension validation.

Parameters:

oink – The Oink solver this constraint will be used with (provides num_variables and v_indices for selecting the correct joints from the full model).
dt – Control timestep used to convert acceleration to displacement (seconds).
a_max – Maximum acceleration vector for each group joint (rad/s² or m/s²). Size must equal oink.num_variables. Entries must be non-negative; use infinity to leave a joint unconstrained.

Throws:

std::invalid_argument – if dt <= 0, a_max size mismatches, or any entry is negative.

void setLastVelocity(const Eigen::VectorXd &v_prev)

Record the velocity applied on the previous IK step.

The acceleration bound is centered on the previous velocity, so this must be called once per control step (before solving) with the velocity that was just integrated. The constraint timestep dt is reused to form the previous displacement: Δq_prev = v_prev * dt.

Parameters:: v_prev – Latest integrated group velocity (size oink.num_variables).
Throws:: std::invalid_argument – if v_prev size mismatches.

void reset(): Reset the previous-step displacement to zero (e.g. when the robot is at rest).

virtual int getNumConstraints(const Scene &scene) const override: Get the number of constraint rows (num_variables).

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 acceleration limits.

Parameters:

scene – The scene containing robot state and model.
constraint_matrix – Output constraint matrix G (num_variables × num_variables).
lower_bounds – Output lower bounds vector (num_variables).
upper_bounds – Output upper bounds vector (num_variables).

Returns:

void on success, error message on failure.

Public Members

double dt

Eigen::VectorXd a_max: Control timestep (seconds).

Eigen::VectorXd Delta_q_prev: Maximum acceleration per group joint.

int num_variables: Displacement applied on the previous step.

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 distance to upper limit.

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.

void setTargetConfiguration(const Eigen::VectorXd &target)

Sets the target joint configuration for this task.

Allows updating the tracked configuration at runtime (e.g., to follow a sequence of joint targets) without reconstructing the task. Mirrors FrameTask::setTargetFrameTransform for joint-space tracking.

Parameters:: target – The new target joint configuration (size must equal q_indices.size()).
Throws:: std::invalid_argument – if target size doesn’t match the group’s q_indices size.

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

std::optional<pinocchio::FrameIndex> base_frame_id: Optional index of the base frame (from CartesianConfiguration::base_frame). When set, computeError converts the target to world frame using the base frame’s current FK pose, and computeJacobian returns the relative Jacobian of the EE w.r.t. the base.

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). It 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 error O(||δq||²) for large displacements.

Enforcement is per-barrier rather than global. For each barrier that would be violated at the candidate configuration, only the joints that affect that barrier (its nonzero Jacobian columns) are zeroed, so an unrelated kinematic chain, e.g, the other arm in a dual-arm setup, is not frozen just because one frame left its bound. A step that is still violated but strictly reduces the violation is allowed/ For example, a frame that started outside its bound can recover instead of deadlocking.

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: the joints of each violated, non-recovering barrier are set to zero.
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

inline const CollisionContext &getCollisionContext() const

The solver’s shared collision scratch (Data + GeometryData + broadphase).

Tasks, constraints, and/or barriers that require collision queries should use this context instead of building their own, so a single snapshot of the scene’s collision geometry is reused across the whole solve. It is snapshotted from the scene at construction; if the scene’s collision geometry changes, rebuild the solver (context does not auto-sync).

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

std::unique_ptr<CollisionContext> collision_context_

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, double dt, const SelfCollisionBarrierOptions &options = {})

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.
dt – Timestep matching the control loop period. Must be positive.
options – Optional tuning parameters (default: all options set to defaults).

Throws:

std::invalid_argument – if d_min is negative or n_collision_pairs is non-positive.

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 this barrier’s own CollisionContext scratch 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 – Unused; kept for the Barrier interface. Distances are evaluated on this barrier’s CollisionContext, which owns the model shared with the scene.
data – Unused; kept for the Barrier interface.
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

int n_collision_pairs: Number of closest collision pairs constrained.

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

const std::optional<double> d_max: Maximum distance (meters) at which a collision pair is tracked; pairs whose bounding boxes are farther apart than this skip exact narrow-phase distance. Visibility / performance bound, not a separation limit. std::nullopt disables culling. See SelfCollisionBarrierOptions::d_max.

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 CollisionContext’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 CollisionContext *collision_context: Non-owning pointer to the Oink solver’s shared collision scratch (Data + GeometryData), captured from oink.getCollisionContext() at construction. All distance / joint-Jacobian queries run on this context instead of the scene’s shared collision data, so the barrier never mutates scene state. The referenced Oink must outlive this barrier.

struct SelfCollisionBarrierOptions

#include <self_collision_barrier.hpp>

Parameters for SelfCollisionBarrier configuration.

Public Members

int n_collision_pairs = 1: Maximum number of closest collision pairs to constrain. Must be > 0. Only the closest n_collision_pairs pairs at the current configuration are constrained. Values larger than the number of collision pairs in the scene are clipped to that count.

double gain = 1.0: Barrier gain (gamma), controls convergence to safe set (default: 1.0).

double safe_displacement_gain = 1.0: Gain for safe displacement regularization (default: 1.0).

double d_min = 0.02: Minimum allowed distance between any pair of bodies. Must be non-negative (default: 0.02).

double safety_margin = 0.0: Conservative margin for hard constraint guarantee (default: 0.0).

std::optional<double> d_max = 0.5

Maximum distance (meters) at which a collision pair is tracked.

Pairs whose bounding boxes are farther apart than this skip exact narrow-phase distance computation (the dominant per-solve cost on dense / mesh-heavy models) and therefore exert no influence on the barrier.

This is a visibility / performance bound, NOT a separation limit: it does not constrain how far apart bodies may be. When set comfortably larger than the distances at which the barrier actively pushes (a few times d_min), it does not change the solution at all — only a too-small value silently drops mid-range pairs. Default 0.5. Set to std::nullopt to disable culling. Paired with d_min, it defines the band [d_min, d_max] of separations in the barrier.

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 <optional>

#include <vector>

#include <pinocchio/multibody/geometry.hpp>

#include <roboplan/core/collision_context.hpp>

#include <roboplan_oink/optimal_ik.hpp>

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

#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/collision_context.hpp>

#include <roboplan/core/scene.hpp>

#include <roboplan/core/types.hpp>

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

#include <roboplan/core/scene.hpp>

#include <roboplan_oink/optimal_ik.hpp>

file frame.hpp: #include <limits>

#include <memory>

#include <string>

#include <Eigen/Dense>

#include <roboplan/core/scene.hpp>

#include <roboplan/core/types.hpp>

#include <roboplan_oink/optimal_ik.hpp>

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

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

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

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

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

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

RRT

struct Node

#include <graph.hpp>

Defines a graph node for search-based planners.

Public Functions

inline Node(const Eigen::VectorXd &config_, int parent_id_, double cost_ = 0.0)

Constructor.

Parameters:

config_ – The configuration of the node.
parent_id_ – The parent id of the node.
cost_ – The cost-to-come from the tree root to this 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.

double cost

The cost-to-come from the tree root to this node.

Only maintained by the RRT* variant; left at zero by the other planners.

std::vector<int> children

The IDs of this node’s children in the tree.

Maintained by the RRT* variant so cost changes can be propagated through a node’s subtree after a rewire. Left empty by the other planners.

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<std::pair<JointPath, double>> 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:

If a path is found, a pair of the completed start-to-goal path and its total cost-to-come (the two connected nodes’ costs plus the connecting edge length); otherwise none. The cost is only meaningful when the planner tracks node costs (RRT*, or any mode with fast_return disabled); callers returning the first path can ignore it.

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.

std::vector<int> findNearNodes(const KdTree &tree, const std::vector<Node> &nodes, const Eigen::VectorXd &q) const

Finds the IDs of all nodes in a tree within rewire_distance (in configuration distance) of a configuration.

Used by the RRT* variant to gather candidate parents and rewiring targets. Only nodes already inserted into tree are returned, so calling this before inserting the query node excludes it from the result.

Parameters:

tree – The tree to search.
nodes – The nodes backing tree, used to measure configuration distance to candidates.
q – The query configuration, as full (model-sized) joint positions.

Returns:

The IDs of the neighboring nodes.

int rewire(KdTree &kd_tree, std::vector<Node> &nodes, const Eigen::VectorXd &q_new, int default_parent_id, const CollisionContext &collision_context)

Inserts a node into an RRT* tree, choosing the best parent and rewiring its neighbors.

The RRT* insertion step: among the new node’s neighbors (within rewire_distance) it picks the collision-free parent that minimizes the new node’s cost-to-come, inserts the node, then reconnects any neighbor through the new node when that lowers the neighbor’s cost-to-come (propagating the change through that neighbor’s subtree).

Parameters:

kd_tree – The tree to insert into.
nodes – The nodes backing kd_tree.
q_new – The configuration to insert. Must already be validated as collision-free.
default_parent_id – The node q_new was extended from, used as the fallback parent.
collision_context – This plan’s private collision context, used for all collision checks.

Returns:

The ID of the newly inserted node.

void propagateCost(std::vector<Node> &nodes, int root_id)

Propagates an RRT* cost change down a node’s subtree.

Call after a node’s parent and cost have been updated by a rewire. Each descendant’s cost-to-come is recomputed from its (already updated) parent’s cost plus the edge length.

Parameters:

nodes – The list of nodes in the tree.
root_id – The node whose cost just changed.

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.

int state_dim_ = 0

The runtime dimension of the (collapsed) k-d tree state space.

Cached at construction because CombinedStateSpace::get_runtime_dim() is not const-qualified in the vendored dynotree, so it cannot be called from the const findNearNodes where the dimension is needed.

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.

bool rrt_star = false

If true, use the RRT* algorithm to grow asymptotically optimal trees.

As new nodes are added, RRT* picks the lowest-cost parent among nearby nodes and rewires nearby nodes through the new node when that lowers their cost. Unlike plain RRT, it does not stop at the first solution: it keeps sampling and rewiring until the node or time budget is exhausted, then returns the lowest-cost path found. Compatible with rrt_connect, in which case both trees are rewired.

double rewire_distance = 5.0

The configuration-space radius used to find neighbors for RRT* rewiring.

Only used when rrt_star is true. Expressed in the same units as max_connection_distance, and should generally be at least that large so that neighbors a single connection step away are considered. Larger values consider more neighbors when choosing parents and rewiring, improving path quality at the cost of more collision checks.

bool fast_return = true

If true, return as soon as the first path is found; if false, keep planning until the node or time budget is exhausted and return the lowest-cost path found.

Applies to every mode. With RRT* (rrt_star), set this to false to obtain the asymptotically optimal behavior; with plain RRT or RRT-Connect, setting it to false simply keeps the cheapest path discovered across the whole budget.

namespace roboplan

Typedefs

using CombinedStateSpace = dynotree::Combined<double>

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

file graph.hpp: #include <vector>

#include <Eigen/Dense>

file rrt.hpp: #include <memory>

#include <optional>

#include <random>

#include <string>

#include <utility>

#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/0.5.0/roboplan_rrt/include

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

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

TOPP-RA

class LinearBlendPath : public toppra::GeometricPath

#include <linear_blend_path.hpp>

A geometric path made of straight-line segments joined by circular corner blends, parameterized by arc length and exposed through the TOPP-RA GeometricPath interface.

This is the path representation used by time-optimal trajectory generation (Kunz & Stilman 2012, https://www.roboticsproceedings.org/rss08/p27.pdf). Unlike an interpolating cubic spline, straight segments have exactly zero curvature, so that densely sampled, slightly noisy waypoints no longer inflate the spline’s centripetal acceleration term (q’’ * s_dot^2) and force TOPP-RA to crawl. Curvature exists only at the corner blends and is bounded by the max_deviation tolerance (the maximum distance the blend may stray from the sharp corner).

Each interior waypoint is rounded with a circular arc built from the midpoints of its two adjacent segments; using midpoints guarantees a blend consumes at most half of each segment, so adjacent blends never overlap. The path is C1 (continuous position and unit tangent) with piecewise-constant curvature.

Note

The waypoints are expected to be monotone along the path (no sharp reversals). A near-180-degree reversal cannot be blended and becomes a tangent-discontinuous cusp, which a time parameterization cannot traverse without stopping; this matches the assumptions of TOTG and is satisfied by Cartesian-path-following traces.

Public Functions

LinearBlendPath(const toppra::Vectors &waypoints, double max_deviation)

Constructs the line+blend path through the given waypoints.

Parameters:

waypoints – The (collapsed) joint-space waypoints; must contain at least 2 points all of the same dimension.
max_deviation – Maximum distance (in the waypoint’s units) a corner blend may deviate from the sharp corner. Values <= 0 disable blending (pure polyline).

Throws:

std::invalid_argument – if fewer than 2 waypoints are provided.

toppra::Vector eval_single(toppra::value_type s, int order = 0) const override

Evaluates the path at arc-length position s (clamped to the path interval).

Parameters:

s – Arc-length parameter.
order – Derivative order: 0 = position, 1 = unit tangent, 2 = curvature vector.

toppra::Bound pathInterval() const override: Returns the path interval [0, total arc length].

toppra::Vector segmentBoundaries() const

Returns the arc-length positions of every segment boundary, including the path endpoints (0 and the total length).

Curvature is discontinuous at line<->arc junctions, so TOPP-RA must place gridpoints on these boundaries to enforce the acceleration limit correctly. Seed proposeGridpoints with these (as initialGridpoints) before handing the grid to the solver via setGridpoints.

Private Functions

const Segment &segmentAt(double s) const: Returns the segment containing arc-length s (clamped to the path interval).

Private Members

std::vector<Segment> segments_: The ordered path segments.

double total_length_ = 0.0: Total arc length of the path.

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 TOPPRAOptions &options = {})

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

Parameters:

path – The path to time parameterize.
options – Options controlling the time parameterization. Refer to TOPPRAOptions.

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

struct Segment

One path piece: either a straight line or a circular arc, in arc-length terms.

Public Members

double start_s = 0.0: Cumulative arc length at the start of this segment.

double length = 0.0: Arc length of this segment.

bool is_arc = false: True for a circular arc, false for a straight line.

Eigen::VectorXd point: Linear: segment start. Arc: unused.

Eigen::VectorXd direction: Linear: unit direction. Arc: unused.

Eigen::VectorXd center: Arc: circle center. Linear: unused.

Eigen::VectorXd x: Arc: unit vector from center to arc start.

Eigen::VectorXd y: Arc: unit tangent direction at arc start.

double radius = 0.0: Arc: blend radius.

struct TOPPRAOptions

#include <toppra.hpp>

Options controlling TOPP-RA time parameterization.

Public Members

double dt = 0.01: The sample time of the output trajectory, in seconds. Must be strictly positive.

SplineFittingMode mode = SplineFittingMode::Hermite 

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

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.
SplineFittingMode::LinearBlend: Represents the path as straight-line segments joined by circular corner blends (the geometry used by time-optimal trajectory generation, Kunz & Stilman 2012, https://www.roboticsproceedings.org/rss08/p27.pdf). Unlike the spline modes, straight segments have exactly zero curvature, so densely sampled or slightly noisy waypoints do not inflate the acceleration constraint and slow the trajectory. Corners are rounded within max_blend_deviation, and the blended path is collision checked, falling back to Hermite mode if a collision is found.

double velocity_scale = 1.0: A scaling factor (between 0 and 1) for velocity limits.

double acceleration_scale = 1.0: A scaling factor (between 0 and 1) for acceleration limits.

int max_adaptive_iterations = 10: Maximum number of adaptive iterations, if adaptive mode is enabled.

double max_adaptive_step_size = 0.05: If adaptive mode is enabled, this is the maximum joint configuration step size to sample generated splines for collision checking.

double max_blend_deviation = 0.01: Maximum distance a corner blend may deviate from the sharp corner, in the joint configuration’s units. Only used by SplineFittingMode::LinearBlend. Larger values round corners more (faster, but the path strays further from the waypoints); values <= 0 disable blending (a pure polyline that stops at each corner).

namespace roboplan

Enums

enum class SplineFittingMode

Enumeration for TOPP-RA spline fitting mode.

Refer to the TOPPRAOptions::mode documentation for more information.

Values:

enumerator Hermite

enumerator Cubic

enumerator Adaptive

enumerator LinearBlend

namespace toppra

file linear_blend_path.hpp: #include <vector>

#include <Eigen/Dense>

#include <toppra/geometric_path.hpp>

#include <toppra/toppra.hpp>

file toppra.hpp: #include <string>

#include <vector>

#include <roboplan/core/scene.hpp>

#include <roboplan/core/types.hpp>

#include <tl/expected.hpp>

#include <toppra/algorithm/toppra.hpp>

#include <toppra/geometric_path/piecewise_poly_path.hpp>

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

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

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

Cartesian Planning

class CartesianPathPlanner

#include <cartesian_path_planner.hpp>

Offline Cartesian path planner that traces a CartesianPath in joint space.

Uses the Oink optimal IK solver as a differential-IK tracker.

Public Functions

CartesianPathPlanner(const std::shared_ptr<Scene> scene, const CartesianPlannerOptions &options)

Constructor that builds the default differential-IK setup internally.

Constructs its own OInK solver and, on each plan() call, one FrameTask per end-effector in the path plus a nullspace ConfigurationTask, bounded by VelocityLimit and PositionLimit constraints, configured from options.

Parameters:

scene – A pointer to the scene to use for planning.
options – A struct containing planner options.

Throws:

std::runtime_error – if the joint group cannot be resolved.

CartesianPathPlanner(const std::shared_ptr<Scene> scene, const CartesianPlannerOptions &options, const CartesianPlannerComponents &components)

Constructor that uses a caller-supplied OInK solver and IK objectives.

The planner traces the path by updating each components.tracking_tasks target every control step and solving with the provided solver, tasks, constraints, and barriers. The Oink-related fields of options (costs, gains, limits, etc.) are ignored in this mode since the caller owns the objectives; timing/tolerance fields (dt, speeds, max errors, speed_mode, scales) still apply.

Parameters:

scene – A pointer to the scene to use for planning.
options – A struct containing planner options.
components – The caller-supplied Oink solver and IK objectives.

Throws:

std::runtime_error – if the joint group cannot be resolved, or if components.oink is null, or components.tracking_tasks is empty or contains a null entry.

tl::expected<JointTrajectory, std::string> plan(const CartesianPath &path, const JointConfiguration &q_start)

Plans a joint trajectory that traces the provided Cartesian path.

Supports one or more end-effector frames (each entry in the path’s base_frames/tip_frames/tforms is traced simultaneously by its own FrameTask).

Parameters:

path – The Cartesian waypoint path to trace.
q_start – The seed/start configuration, as a full model configuration (size model.nq). The robot should already be at (or near) the first waypoint.

Returns:

The time-parameterized joint trajectory on success, else a string describing the error. Quality metrics (peak limit ratios, achieved path length) are not bundled in; compute them on demand from the returned trajectory with computePeakLimitRatios() / computeAchievedPathLength().

std::pair<double, double> computePeakLimitRatios(const JointTrajectory &trajectory) const

Computes the peak |velocity|/limit and |acceleration|/limit ratios across the trajectory, so callers can see how close the result is to the joint limits.

Parameters:: trajectory – The joint trajectory to evaluate (e.g. the output of plan()).
Returns:: A pair of {peak velocity ratio, peak acceleration ratio}. Values <= 1.0 mean the respective joint limits are respected.

double computeAchievedPathLength(const JointTrajectory &trajectory, const CartesianPath &path) const

Computes the achieved Cartesian path length (meters) traced by the path’s tip frames.

Re-runs forward kinematics over the trajectory and sums the world-frame translation travelled by every tip frame in path. Joints outside the planning group are held at the scene’s current state; because that contributes only a constant rigid offset, it cancels in the per-step differences and does not affect the result.

Parameters:

trajectory – The joint trajectory to evaluate (e.g. the output of plan()).
path – The Cartesian path whose tip frames were traced.

Returns:

The summed Cartesian path length (meters) across all tip frames.

Private Functions

void buildStaticSolverComponents(const std::optional<CartesianPlannerComponents> &components)

Builds the parts of the OInK problem that do not depend on the path or seed, once, at construction time.

Populates the reused solver-input buffers (constraints_, barriers_) and the velocity-limit verification vector. When components is provided this also assembles the full task list (tracking tasks followed by extra tasks) and caches the tracking tasks (tracking_tasks_) for per-plan() wiring, since the caller’s objectives are fixed; otherwise the per-end-effector tasks_ are (re)built per plan() because they depend on the path’s frames and the seed configuration.

Parameters:: components – The caller-supplied objectives, or std::nullopt for the default setup.
Throws:: std::runtime_error – if the joint velocity limits cannot be resolved (default setup).

tl::expected<std::vector<FrameTrack>, std::string> buildFrameTracks(const CartesianPath &path, const Eigen::VectorXd &q_start_full, double linear_speed, double angular_speed)

Builds the per-end-effector tracking state (arc-length-timed waypoints, world<-base transform, tip frame, and tracking task) for every frame in the path, and wires the tracking tasks into the reused solver task list (tasks_).

In the custom-components mode the tasks are the caller’s (validated to match the path’s tip-frame order); otherwise one priority-1 FrameTask per end-effector plus a priority-2 nullspace ConfigurationTask are (re)built from the seed configuration.

FeedrateProfile buildFeedrateProfile(const std::vector<FrameTrack> &tracks, double total_time, double linear_speed, double angular_speed, double linear_acceleration, double angular_acceleration) const

Precomputes the trapezoidal feedrate profile for the given references.

Bounds the feedrate acceleration so the Cartesian acceleration stays within the commanded maxima, caps the feedrate at each corner (see cornerFeedrateCap), and runs a backward pass so the feedrate can always decelerate to each corner cap and stop at the end. A non-positive acceleration returns a disabled profile (constant full feedrate).

tl::expected<void, std::string> solveStep(const std::vector<FrameTrack> &tracks, const Eigen::VectorXd &q, double s, Eigen::VectorXd &q_candidate, Eigen::VectorXd &delta_q, double &position_error, double &orientation_error): Runs one differential-IK step that retargets every frame to its pose at reference time s from the committed configuration q. Writes the candidate configuration, the group step delta_q, and the worst-case (max over frames) FK pose error. Does not commit.

tl::expected<void, std::string> convergeToStart(const std::vector<FrameTrack> &tracks, Eigen::VectorXd &q): Drives every frame onto its first waypoint within tolerance, updating q in place.

tl::expected<JointTrajectory, std::string> runServoLoop(const std::vector<FrameTrack> &tracks, const FeedrateProfile &profile, double total_time, Eigen::VectorXd &q)

Runs the timed servo loop from the converged start q, advancing the reference under the feedrate profile and throttling further to stay within the path tolerance and joint limits.

Returns:: A trajectory with positions, velocities, and times populated. The accelerations are left for the plan*() caller to fill.

tl::expected<JointTrajectory, std::string> trackReference(const CartesianPath &path, const Eigen::VectorXd &q_start_full, double linear_speed, double angular_speed, double linear_acceleration, double angular_acceleration)

Resolves the Cartesian path into a joint-space trace with the Oink tracker.

Orchestrates buildFrameTracks -> buildFeedrateProfile -> convergeToStart -> runServoLoop. The reference advances at the commanded Cartesian speeds, ramped under a trapezoidal feedrate profile bounded by the accelerations, and throttled to stay within the path tolerance. Joint velocity and position limits are enforced per step.

Parameters:: linear_acceleration, angular_acceleration – Cartesian acceleration maxima for the trapezoidal feedrate profile. A non-positive value disables the profile (constant feedrate).
Returns:: The trajectory from runServoLoop (positions, velocities, times; see there).

void fillAccelerations(JointTrajectory &trajectory) const: Fills a trajectory’s accelerations by backward finite difference of the velocities, zeroing the (at-rest) boundary accelerations.

tl::expected<JointTrajectory, std::string> planBounded(const CartesianPath &path, const Eigen::VectorXd &q_start_full)

Generates a trajectory that traces the path under a trapezoidal Cartesian feedrate profile: the tool speed ramps up/down within the commanded Cartesian acceleration maxima and is capped at the commanded speeds.

If the resulting motion still exceeds the (scaled) joint velocity or acceleration limits, the whole trace is re-timed slower (commanded speeds/accelerations scaled down) and retried, so the commanded values act as maxima that are relaxed only as needed.

tl::expected<JointTrajectory, std::string> planTimeOptimal(const CartesianPath &path, const Eigen::VectorXd &q_start_full): Resolves the path geometrically, then time-parameterizes it with TOPP-RA so the result respects joint velocity and acceleration limits (tool speed varies).

Private Members

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

CartesianPlannerOptions options_: The planner options.

JointGroupInfo joint_group_info_: The resolved joint group info.

std::shared_ptr<Oink> oink_

The differential-IK solver used to resolve the Cartesian path into a joint trace.

Constructed once for the planner’s joint group (or supplied by the caller) and reused across plan() calls.

std::vector<std::shared_ptr<FrameTask>> tracking_tasks_

Caller-supplied tracking tasks, non-empty only when the components constructor is used.

Cached at construction so buildFrameTracks() can wire each FrameTrack to its task on every plan() call; a non-empty value also marks the custom-components mode (in which the default FrameTask/ConfigurationTask/VelocityLimit/PositionLimit setup is bypassed). The other caller-supplied objectives are consumed into oink_/tasks_/constraints_/barriers_ at construction, so they do not need to be retained.

std::vector<std::shared_ptr<Task>> tasks_

Reused solver task list passed to Oink::solveIk each control step.

Assembled once at construction in the custom-components mode; rebuilt in place each plan() in the default mode (the per-end-effector FrameTasks depend on the path and seed).

std::vector<std::shared_ptr<Constraints>> constraints_: Constraints passed to the solver each step. Built once at construction.

std::vector<std::shared_ptr<Barrier>> barriers_: Barriers passed to the solver each step. Built once at construction.

Eigen::VectorXd verify_v_max_

Joint velocity limits used to verify each committed step. Built once at construction.

Default mode uses the scene limits scaled by options_.velocity_scale; custom mode uses the unscaled scene limits as a hard-limit sanity net.

bool has_velocity_check_ = false: Whether the per-step velocity verification runs (false if no limits are available).

PathParameterizerTOPPRA toppra_

The TOPP-RA time parameterizer, used by the TimeOptimal speed mode.

Constructed once for the planner’s joint group and reused across plan() calls.

struct CartesianPlannerComponents

#include <cartesian_path_planner.hpp>

User-supplied OInK solver and objectives for the Cartesian path planner.

Lets callers fully customize the differential-IK problem the planner solves at each control step instead of relying on the planner’s built-in setup (one FrameTask per end-effector plus a nullspace ConfigurationTask, bounded by VelocityLimit and PositionLimit constraints). Pass an instance to the corresponding CartesianPathPlanner constructor to inject your own solver, tasks, constraints, and barriers.

The planner drives the motion by repeatedly updating each tracking FrameTask’s target pose, so one tracking task must be provided per end-effector in the CartesianPath. All other tasks/constraints/barriers are passed to the solver unchanged on every step. The same objects are reused across all plan() calls; the planner never rebuilds or mutates them (other than the tracking tasks’ targets), so any q_start-dependent setup (e.g. seeding a ConfigurationTask) is the caller’s responsibility.

Public Members

std::shared_ptr<Oink> oink

The OInK solver to use.

Must be constructed for the same scene and joint group as the planner. Must not be null.

std::vector<std::shared_ptr<FrameTask>> tracking_tasks

The FrameTasks whose target poses uses to trace the path, one per end-effector.

Entry i tracks the frame named by path.tip_frames[i] of the CartesianPath, so the count and order must match the path’s specified tip frames. Each task must be constructed against oink and must track the matching tip frame. The tracking tasks are prepended to the solver’s task list automatically. Must be non-empty with no null entries.

std::vector<std::shared_ptr<Task>> extra_tasks: Additional tasks solved alongside the tracking tasks (e.g., a nullspace ConfigurationTask). May be empty.

std::vector<std::shared_ptr<Constraints>> constraints: Constraints applied at every control step (e.g. VelocityLimit, PositionLimit). May be empty.

std::vector<std::shared_ptr<Barrier>> barriers: Control barrier functions applied at every control step. May be empty.

struct CartesianPlannerOptions

#include <cartesian_path_planner.hpp>

Options struct for the Cartesian path planner.

Public Functions

tl::expected<void, std::string> validate() const

Validates the mode-independent option values (dt, tolerances, scales).

Mode-specific options (e.g. the Bounded mode’s commanded speeds/accelerations) are validated where they are used.

Returns:: Nothing on success, else a string describing the first invalid option.

Public Members

std::string group_name = "": The joint group name to plan for. Empty means the full robot.

double dt = 0.01

The output trajectory sample period (control period), in seconds.

This is also the sample time used by the OInK solver.

CartesianSpeedMode speed_mode = CartesianSpeedMode::Bounded : Which timing/speed strategy to use.

double max_linear_speed = 0.1

Maximum linear tool speed along the path, in meters/second.

Only used in Bounded speed mode.

double max_angular_speed = 0.5

Maximum angular tool speed along the path, in radians/second.

Only used in Bounded speed mode.

double max_linear_acceleration = 0.5

Maximum linear tool acceleration along the path, in meters/second^2.

Only used in Bounded speed mode, where the tool speed is ramped up and down so the Cartesian linear acceleration stays within this bound.

double max_angular_acceleration = 2.5

Maximum angular tool acceleration along the path, in radians/second^2.

Only used in Bounded speed mode, where the tool speed is ramped up and down so the Cartesian angular acceleration stays within this bound.

double max_position_error = 0.005: Maximum allowed position deviation from the path, in meters.

double max_orientation_error = 0.01: Maximum allowed orientation deviation from the path, in radians.

double position_cost = 1.0: Oink FrameTask position cost weight.

double orientation_cost = 1.0: Oink FrameTask orientation cost weight.

double task_gain = 1.0: Oink FrameTask proportional gain.

double lm_damping = 0.01: Oink FrameTask Levenberg-Marquardt damping.

double regularization = 1e-6: Tikhonov regularization weight for the Oink QP Hessian.

double config_task_weight = 0.05: Weight of the priority-2 ConfigurationTask that regularizes redundant joints toward the seed configuration (uses only nullspace freedom).

double velocity_scale = 1.0: Scaling factor (0, 1] applied to the joint velocity limits used to bound each differential-IK step.

double acceleration_scale = 1.0

Scaling factor (0, 1] applied to the joint acceleration limits.

Used both by the TimeOptimal re-timing and by the Bounded mode’s joint-acceleration throttle (which slows the feedrate wherever a step would exceed the scaled limits).

double limit_ratio_tolerance = 1.05

Acceptance tolerance (>= 1.0) for the Bounded mode’s slow-down retry.

A trace is accepted once its peak joint velocity and acceleration ratios land within this factor of the (scaled) limits; otherwise the whole motion is re-timed slower and retried. Because the Bounded mode has no hard joint-acceleration constraint and estimates acceleration by finite difference, a value > 1.0 absorbs single-sample spikes at corners/tolerance events and avoids needless extra slow-down passes. Set to 1.0 to require the peaks to land within the (scaled) limits; for extra margin below the limits use velocity_scale/acceleration_scale instead. Only used in Bounded speed mode.

double toppra_blend_deviation = 0.05: Corner-rounding tolerance (joint-space radians) for the TimeOptimal speed mode, which times the path with TOPP-RA over a straight-segment + circular-blend geometry. Each corner is rounded by a circular arc that deviates from the sharp corner by at most this much. Larger values round corners more aggressively (faster motion, but the joint path strays further from the resolved waypoints); a value <= 0 disables blending (the trajectory stops at every waypoint).

double position_limit_gain = 1.0: Gain (0, 1] for the position-limit constraint that steers each step away from the joint position limits.

int max_attempts_per_step = 16

Maximum number of feedrate-throttling attempts per control step before stalling.

A stall is declared when the robot cannot stay within tolerance even when nearly stationary.

struct FeedrateProfile

Trapezoidal feedrate profile: a ceiling on the feedrate (ds/dt) over a uniform reference-time grid that bounds the Cartesian acceleration and forces a stop at the path end.

Public Functions

double sample(double s) const: Samples the ceiling at reference time s (linear interpolation; 1.0 if disabled).

Public Members

bool enabled = false: When false, sample() returns 1.0 (constant full feedrate).

std::vector<double> ceiling: Feedrate ceiling at each grid knot.

double ds = 0.0: Reference-time spacing between knots (s).

double s_ddot_max = 0.0: Maximum feedrate acceleration (1/s).

size_t num_corner_knots = 0: Number of corner caps applied (used to size the loop cap).

struct FrameTrack

Per-end-effector tracking state for one frame of the path: the arc-length-timed waypoint reference plus the world transform, tip frame, and task used to trace it. Held together so the servo loop indexes a single vector instead of several parallel ones.

Public Functions

Eigen::Matrix4d eval(double s) const: Base-relative reference pose at reference time s in [0, total_time].

inline Eigen::Matrix4d target(double s) const: World-frame target pose of this frame at reference time s.

Public Members

std::vector<Eigen::Matrix4d> waypoints: Base-relative SE(3) waypoints.

std::vector<double> cumulative_times: Cumulative reference time at each waypoint (s).

double total_time = 0.0: Total reference duration (s).

Eigen::Matrix4d world_T_base = Eigen::Matrix4d::Identity(): Fixed world<-base transform.

std::shared_ptr<FrameTask> task: Tracking task whose target is retargeted each step.

std::string tip_frame: Name of the tip frame traced (path.tip_frames[f]).

namespace roboplan

Enums

enum class CartesianSpeedMode

Selects how the planner assigns speed/timing along the Cartesian path.

Values:

enumerator Bounded

Trace the path under bounded Cartesian velocity and acceleration.

Builds a trapezoidal feedrate profile that ramps the Cartesian tool speed up to the commanded linear/angular maxima and back down to a stop at the path end, bounding the Cartesian linear/angular acceleration by the commanded maxima. The profile is traced with the differential-IK tracker, and the feedrate is throttled further wherever the robot would otherwise exceed its joint velocity or acceleration limits, or fall outside the path tolerance (e.g., near a singularity). The commanded speeds and accelerations therefore act as maxima, not fixed values.

enumerator TimeOptimal

Time-optimal re-timing respecting joint velocity/acceleration limits.

Resolves the waypoints to a joint path and hands it to a PathParameterizerTOPPRA instance using linear segments with circular blends. Tool speed will vary along the path in this mode.

file cartesian_path_planner.hpp: #include <memory>

#include <optional>

#include <string>

#include <utility>

#include <vector>

#include <Eigen/Dense>

#include <tl/expected.hpp>

#include <roboplan/core/scene.hpp>

#include <roboplan/core/types.hpp>

#include <roboplan_oink/optimal_ik.hpp>

#include <roboplan_toppra/toppra.hpp>

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.5.0/roboplan_cartesian_planning/include

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.5.0/roboplan_cartesian_planning

dir /home/docs/checkouts/readthedocs.org/user_builds/roboplan/checkouts/0.5.0/roboplan_cartesian_planning/include/roboplan_cartesian_planning