Class PathShortcutter

Class Documentation

class PathShortcutter

Shortcuts joint paths with random sampling and checking connections.

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

Public Functions

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

Construct a new path shortcutter instance.

Parameters:

  • scene – The scene for checking connectability between joint positions.

  • group_name – The name of the group to use for path shortcutting.

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

Attempts to shortcut a specified path.

Parameters:

  • path – The JointPath to try to shorten.

  • max_step_size – Maximum step size to use in collision checking, and the minimum separable distance between points in a shortcut.

  • max_iters – Maximum number of iterations of random sampling (default 100).

  • seed – Seed for the random generator, if < 0 then use a random seed (default -1).

Returns:

A shortcutted JointPath, if available.

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

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

Parameters:

path – The JointPath to evaluate.

Returns:

A vector of incremental path distances, if there is sufficient data. Otherwise an error.

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

Computes length-normalized scaling values along a JointPath.

Parameters:

path – The path to length-normalize.

Returns:

A vector of scaling values between 0.0 and 1.0 at each point in the path if available, otherwise an error.

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

Gets joint configurations from a path with normalized joint scalings.

Parameters:

  • path – A JointPath of joint poses.

  • path_scalings – The corresponding path scalings (between 0 and 1) to the provided path.

  • value – A value between 0.0 and 1.0 pointing to the intermediate point along the path.

Returns:

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