C++ API#

Logging and Error Handling#

#include <cumotion/cumotion.h>

Shared interface functions for the cuMotion library.

std::string cumotion::VersionString()#

Get cuMotion library version as a std::string.

Also see the CUMOTION_VERSION_STRING macro, which serves a similar purpose. The two should always agree. If they don’t, it indicates a mismatch between the installed cuMotion headers and installed cuMotion libraries.

enum class cumotion::LogLevel#

Logging levels, ordered from least to most verbose.

Values:

enumerator FATAL#: Logging level for nonrecoverable errors (minimum level, so always enabled).

enumerator ERROR#: Logging level for recoverable errors.

enumerator WARNING#: Logging level for warnings, indicating possible cause for concern.

enumerator INFO#: Logging level for informational messages.

enumerator VERBOSE#: Logging level for highly verbose informational messages.

class Logger#

Base class for user-defined logger that allows custom handling of log messages, warnings, and errors.

Such a custom logger may be installed via SetLogger().

Public Functions

virtual std::ostream &log(LogLevel log_level) = 0#: Return an ostream suitable for logging messages at the given log_level.

inline virtual void finalizeLogMessage( [[maybe_unused]] LogLevel log_level, )#: Perform any required post-processing of individual log messages. This function is called after the message has been logged to the std::ostream returned by log() but before handleFatalError() is called (if applicable).

virtual void handleFatalError() = 0#: Handle fatal errors. This callback will be called after any associated error message has been written to the ostream returned by log(FATAL). Because fatal errors are non-recoverable, this function must ensure that any active cuMotion objects are discarded.

class FatalException : public std::exception#

Exception thrown by the default logger for fatal errors.

This is the only exception thrown by cuMotion, so clients may avoid exceptions completely by installing a custom logger.

void cumotion::SetLogLevel(LogLevel log_level)#

Suppress output for all log messages with associated verbosity higher than log_level.

Messages suppressed in this way incur very low overhead, so it’s best to take advantage of this facility even if a custom Logger is provided.

Until SetLogLevel() is called, the default log level is WARNING. The lowest supported log_level is FATAL, since it is not possible to supress fatal errors.

void cumotion::SetLogger(std::shared_ptr<Logger> logger = nullptr)#

Install a custom logger, derived from the above Logger class.

If logger is a null pointer, then the default logger is reenabled. The default logger directs all output to stdout and throws a FatalException in the event of a fatal error.

void cumotion::SetDefaultLoggerTextStyle( LogLevel log_level, const std::string &style, )#

Set color/style used by the default logger for messages of a given log_level. The style string may contain one or more ANSI control sequences (e.g., enabling fatal error messages to be rendered in bold with a red foreground color).

For convenience, a selection of common control sequences is provided in include/cumotion/text_style.h

void cumotion::SetDefaultLoggerPrefix(const std::string &prefix)#

Set prefix used by the default logger for all messages. The prefix string is logged after the style string set for each log_level by SetDefaultLoggerTextStyle() and prior to the content of the logged message.

The default prefix is an empty string.

Rotations and Poses#

Rotations#

#include <cumotion/rotation3.h>

Public interface for representing a rotation in 3d.

class Rotation3#

Class representing a 3d rotation.

Public Functions

explicit Rotation3( const Eigen::Quaterniond &quaternion, bool skip_normalization = false, )#

Construct rotation from a quaternion.

By default, the quaternion will be normalized, but the argument skip_normalization can be set to true to skip this normalization. This should only be done if the quaternion is known to be normalized; otherwise, Rotation3 operations will not work as expected.

inline Eigen::Quaterniond quaternion() const#: Return quaternion representation of the rotation. Returned quaternion is always normalized.

inline double w() const#: Return w component of the quaternion representation of the rotation.

inline double x() const#: Return x component of the quaternion representation of the rotation.

inline double y() const#: Return y component of the quaternion representation of the rotation.

inline double z() const#: Return z component of the quaternion representation of the rotation.

Eigen::Matrix3d matrix() const#: Return matrix representation of the rotation.

Eigen::Vector3d scaledAxis() const#: Return scaled axis representation of the rotation where magnitude of the returned vector represents the rotation angle in radians.

Rotation3 inverse() const#: Returns the inverse rotation.

Public Static Functions

static Rotation3 FromAxisAngle( const Eigen::Vector3d &axis, double angle, bool skip_normalization = false, )#

Return a rotation that rotates by an angle (in radians) around a given axis.

By default, the axis will be normalized (and if the axis norm is near zero, an identity rotation will be constructed). The argument skip_normalization can set to true to skip this normalization. This should only be done if the axis is known to be normalized; otherwise, Rotation3 may represent an invalid rotation.

static Rotation3 FromScaledAxis(const Eigen::Vector3d &scaled_axis)#

Return a rotation converted from a scaled axis.

The magnitude of scaled_axis specifies the rotation angle in radians and the direction of scaled_axis specifies the axis of rotation.

static Rotation3 FromMatrix( const Eigen::Matrix3d &rotation_matrix, bool skip_normalization = false, )#

Return a rotation converted from a 3 x 3 rotation matrix.

Internally, the rotation_matrix will be converted to a Eigen::Quaterniond. By default, this quaternion will be normalized, but the argument skip_normalization can be set to true to skip this normalization. This should only be done if the rotation_matrix is known to be SO3; otherwise, Rotation3 operations will not work as expected.

static Rotation3 Identity()#: Create identity rotation.

static double Distance( const Rotation3 &rotation0, const Rotation3 &rotation1, )#: Compute the minimum angular distance (in radians) between two rotations.

static Rotation3 Slerp( const Rotation3 &rotation0, const Rotation3 &rotation1, double t, )#

Smoothly interpolate between two rotations using spherical linear interpolation (“slerp”).

Intermediate rotations will follow a geodesic between rotation0 and rotation1 when represented as quaternions on a unit sphere.

The parameter t must be in range [0, 1], with 0 corresponding to rotation0 and 1 corresponding to rotation1.

Friends

friend Rotation3 operator*( const Rotation3 &lhs, const Rotation3 &rhs, )#: Compose rotations via * operator.

friend Eigen::Vector3d operator*( const Rotation3 &rotation, const Eigen::Vector3d &vector, )#: Rotates a vector by the given rotation.

Poses#

#include <cumotion/pose3.h>

Public interface for representing a pose in 3d.

struct Pose3#

Class representing a 3d pose.

Public Functions

Pose3()#: Default constructor (set rotation identity and translation to zero).

Pose3(const Rotation3 &rotation, const Eigen::Vector3d &translation)#: Construct pose from a rotation and translation.

explicit Pose3(const Eigen::Matrix<double, 4, 4> &matrix)#: Construct pose from a 4 x 4 homogeneous transform matrix.

Note

The bottom row of the input matrix is assumed to be [0, 0, 0, 1], but is not explicitly checked.

Eigen::Matrix4d matrix() const#: Return matrix representation of the pose.

Pose3 inverse() const#: Returns the inverse pose.

std::vector<Eigen::Vector3d> transformVectors( const std::vector<Eigen::Vector3d> &vectors, ) const#: Transforms a collection of 3d vectors by this pose.

Eigen::Matrix<double, 3, Eigen::Dynamic> transformVectors( const Eigen::Matrix<double, 3, Eigen::Dynamic> &vectors, ) const#

Transforms a collection of 3d vectors by this pose.

Each column of vectors represents a 3d vector to be transformed.

Public Members

Rotation3 rotation#: Rotation component of pose.

Eigen::Vector3d translation#: Translation component of pose.

Public Static Functions

static Pose3 FromRotation(const Rotation3 &rotation)#: Create a pure rotational pose.

static Pose3 FromTranslation(const Eigen::Vector3d &translation)#: Create a pure translational pose.

static Pose3 Identity()#: Create identity pose.

Friends

friend Pose3 operator*(const Pose3 &lhs, const Pose3 &rhs)#: Compose poses via * operator.

friend Eigen::Vector3d operator*( const Pose3 &pose, const Eigen::Vector3d &vector, )#: Transforms a vector by the given pose.

Robot Specification#

#include <cumotion/robot_description.h>

Public interface for loading and accessing a robot description.

class RobotDescription#

Class encapsulating all semantic and kinematic properties of a robot.

Public Functions

virtual int numCSpaceCoords() const = 0#: Return the number of actuated joints for the robot.

virtual std::string cSpaceCoordName(int coord_index) const = 0#: Return the name of a given joint of the robot.

virtual Eigen::VectorXd defaultCSpaceConfiguration() const = 0#

Return default joint positions for the robot.

Returned vector will have length equal to numCSpaceCoords().

virtual std::unique_ptr<Kinematics> kinematics() const = 0#: Return a copy of robot kinematics.

virtual std::vector<std::string> toolFrameNames() const = 0#: Return the names of all tool frames (if any) specified in the robot description.

std::unique_ptr<RobotDescription> cumotion::LoadRobotFromFile( const std::filesystem::path &robot_xrdf, const std::filesystem::path &robot_urdf, )#

Load a robot description from an XRDF (robot_xrdf) and a URDF (robot_urdf).

It is recommended that robot_xrdf and robot_urdf be specified as absolute filepaths. Relative paths will be resolved using the same rules as the std::filesystem::path type.

The “Extended Robot Description Format” (XRDF) is documented at: https://nvidia-isaac-ros.github.io/v/release-3.2/concepts/manipulation/xrdf.html

A fatal error will be logged if:

robot_xrdf is not a valid file path,
robot_urdf is not a valid file path,
robot_xrdf cannot be successfully parsed, OR
robot_urdf cannot be successfully parsed.

std::unique_ptr<RobotDescription> cumotion::LoadRobotFromMemory( const std::string &robot_xrdf, const std::string &robot_urdf, )#

Load a robot description from the contents of an XRDF (“robot_xrdf”) and the contents of a URDF (“robot_urdf”).

The “Extended Robot Description Format” (XRDF) is documented at: https://nvidia-isaac-ros.github.io/v/release-3.2/concepts/manipulation/xrdf.html

World Specification#

Obstacle Definition#

#include <cumotion/obstacle.h>

class Obstacle#

Obstacles represent 3d geometries that can be added to cumotion::World.

See cumotion/world.h for usage.

Public Types

enum class Type#

Indicates what geometric primitive the obstacle represents.

Each Obstacle::Type has one or more attributes that can be set via setAttribute(), detailed in the documentation for the corresponding enum value.

Values:

enumerator CAPSULE#

A cylinder with rounded ends.

The capsule is oriented along the z-axis.

Origin: Geometric center, with the radius in the x-y plane, and spheres placed equidistant
        along the z-axis.

Attributes:
- RADIUS: Radius of the capsule.
          Default: 0.5
- HEIGHT: Height of the capsule, defined to be the distance between the centers of the two
          spheres defining the capsule.
          Default: 1.0

enumerator CUBOID#

An axis-aligned cuboid (i.e., rectangular prism).

The cuboid is constructed to be aligned with the axes of its local coordinate frame.

Origin: Geometric center.

Attributes:
- SIDE_LENGTHS: Dimensions along each axis (in the local obstacle frame). Each value
                represents the full extent (i.e., "length") of the cuboid, not the
                half-length.
                Default: {1.0, 1.0, 1.0}

enumerator SPHERE#

A sphere.

Origin: The center of the sphere.

Attributes:
- RADIUS: The radius of the sphere.
          Default: 0.5

enumerator SDF#

An axis-aligned uniform grid of signed distances.

Origin: Center of the grid.

Attributes:
- GRID: Defines the dimensions and resolution of the SDF.
        A grid precision of `Grid::Precision::DOUBLE` is unsupported and will result in a
        fatal error if specified.

enum class Attribute#

Attributes are used to modify obstacles from their default geometry.

Each Attribute may be applicable to one or more Obstacle::Types. Details about which Attribute can be used with which Obstacle::Type are included in documentation for Obstacle::Type.

Each Attribute has a required type for the AttributeValue used in conjunction with setAttribute(), detailed in the documentation for each enum value below.

Values:

enumerator HEIGHT#

The height of an obstacle (typically aligned with the z-axis).

Data Type: `double`

enumerator RADIUS#

The radius of an obstacle.

Data Type: `double`

enumerator SIDE_LENGTHS#

The dimensions of the obstacle along the cardinal axes.

Data Type: `Eigen::Vector3d`

enumerator GRID#

An axis-aligned regular grid of points.

Data Type: `Obstacle::Grid`

Public Functions

virtual void setAttribute( Attribute attribute, const AttributeValue &value, ) = 0#

Set attribute for obstacle.

Available attributes are based on the Obstacle::Type and detailed in documentation for Obstacle::Type.

Example usage: my_sphere.setAttribute(Obstacle::Attribute::RADIUS, 3.0); my_cuboid.setAttribute(Obstacle::Attribute::SIDE_LENGTHS, Eigen::Vector3d(2.0, 3.0, 1.0));

virtual Type type() const = 0#: Return the Obstacle::Type of this obstacle.

struct AttributeValue#

Used to specify the value for a given Attribute.

The required AttributeValue constructor for each Attribute is detailed in the documentation for Attribute.

Public Functions

AttributeValue(double value)#: Create AttributeValue from double.

AttributeValue(const Eigen::Vector3d &value)#: Create AttributeValue from Eigen::Vector3d.

AttributeValue(const Grid &value)#: Create AttributeValue from Grid.

struct Grid#

Used to specify the defining attributes for a grid of voxels that covers an axis-aligned rectangular region of the workspace.

Grid fully describes how the grid values passed to World::setGridValuesFromHost() and World::setGridValuesFromDevice() will be interpreted. An Attribute::GRID specifies a grid of voxels that each contain a scalar value. Each voxel has a fixed workspace position associated with its value that is implicitly defined by the parameters in Grid.

Public Types

enum class Precision#

Floating-point precision of grid data.

Underlying integer values correspond to the size of each floating-point type in bytes.

Values:

enumerator HALF#

enumerator FLOAT#

enumerator DOUBLE#

Public Functions

inline Grid( int num_voxels_x, int num_voxels_y, int num_voxels_z, double voxel_size, Precision host_precision, Precision device_precision, )#

A Grid covers a rectangular region in the workspace whose minimal coordinate is the origin and whose voxel positions are determined by num_voxels_x, num_voxels_y, num_voxels_z, and voxel_size.

The values associated with a Grid correspond to the center of each voxel. The minimal corner of the minimal voxel in a Grid rests at the origin. I.e., the origin is not at a voxel center; it is at a voxel corner.

Voxels in a Grid have length voxel_size along each axis.

Grid contains num_voxels_x voxels along the X dimension, num_voxels_y voxels along the Y dimension, and num_voxels_z voxels along the Z dimension. This implies that the maximal position in the region of the workspace that contains a Grid is voxel_size * [num_voxels_x, num_voxels_y, num_voxels_z].

When grid values are set for an obstacle, up to two copies of the data will be maintained at separately specified levels of precision. For example, it is possible to pass a set of grid values of type double and to specify that a set of grid values of type double should be kept in host (CPU) memory for use in distance queries made on the host, along with a set of grid values of type float kept resident in device (GPU) memory for use in distance queries made on the device.

Public Members

int num_voxels_x#: The number of voxels along the X dimension of Grid.

int num_voxels_y#: The number of voxels along the Y dimension of Grid.

int num_voxels_z#: The number of voxels along the Z dimension of Grid.

double voxel_size#: The size of the voxels along each dimension.

Precision host_precision#: The type of data for storing a set of grid values on CPU.

Precision device_precision#: The type of data for storing a set of grid values on GPU.

std::unique_ptr<Obstacle> cumotion::CreateObstacle( Obstacle::Type type, )#

Create an obstacle with the given type.

Available attributes and default attribute values for the given type are included in the documentation for Obstacle::Type.

World and World View#

#include <cumotion/world.h>

class World#

World represents a collection of obstacles.

Public Functions

virtual ObstacleHandle addObstacle( const Obstacle &obstacle, std::optional<Pose3> pose = std::nullopt, ) = 0#

Add obstacle to the world.

All attributes of obstacle are copied to world and subsequent changes to obstacle will not be reflected in the world.

If a pose is not provided for the obstacle, Pose3::Identity() will be used.

Obstacles are automatically enabled when added.

virtual void removeObstacle(const ObstacleHandle &obstacle) = 0#: Permanently remove obstacle, invalidating its handle.

virtual void enableObstacle(const ObstacleHandle &obstacle) = 0#: Enable an obstacle for the purpose of collision checks and distance evaluations.

virtual void disableObstacle(const ObstacleHandle &obstacle) = 0#: Disable an obstacle for the purpose of collision checks and distance evaluations.

virtual void setPose( const ObstacleHandle &obstacle, const Pose3 &pose, ) = 0#: Set the pose of the given obstacle.

virtual void setSdfGridValuesFromHost( const ObstacleHandle &obstacle, const void *values, Obstacle::Grid::Precision grid_precision, ) = 0#

Set the grid values for an obstacle of type SDF using a host-resident values buffer.

It is assumed that values is stored with the z index varying fastest and has dimensions given by the Obstacle::Grid associated with obstacle. For example, for an obstacle with Grid parameters num_voxels_x, num_voxels_y, and num_voxels_z, the length of values should be num_voxels_x * num_voxels_y * num_voxels_z, and in the provided coordinates, adjacent elements in memory should correspond to voxels with adjacent Z coordinates, and voxels with adjacent X coordinates should be separated by num_voxels_y * num_voxels_z elements.

precision specifies the floating-point type of values. Obstacle::Grid::Precision::HALF corresponds to the __half data type defined in the cuda_fp16.h header.

If the type of obstacle is not Obstacle::Type::SDF, a fatal error will be logged.

virtual void setSdfGridValuesFromDevice( const ObstacleHandle &obstacle, const void *values, Obstacle::Grid::Precision grid_precision, ) = 0#

Set the grid values for an obstacle of type SDF using a device-resident buffer values.

It is assumed that values is stored with the z index varying fastest and has dimensions given by the Obstacle::Grid associated with obstacle. For example, for an obstacle with Grid parameters num_voxels_x, num_voxels_y, and num_voxels_z, the length of values should be num_voxels_x * num_voxels_y * num_voxels_z, and in the provided coordinates, adjacent elements in memory should correspond to voxels with adjacent Z coordinates, and voxels with adjacent X coordinates should be separated by num_voxels_y * num_voxels_z elements.

precision specifies the floating-point type of values. Obstacle::Grid::Precision::HALF corresponds to the __half data type defined in the cuda_fp16.h header.

If the type of obstacle is not Obstacle::Type::SDF, a fatal error will be logged.

virtual WorldViewHandle addWorldView() = 0#

Create a view into the world that can be used for collision checks and distance evaluations.

Each world view will maintain a static view of the world until it is updated. When a world view is updated, it will reflect any changes to the world since its last update.

struct ObstacleHandle#: Opaque handle to an obstacle.

std::shared_ptr<World> cumotion::CreateWorld()#: Create an empty world with no obstacles.

struct WorldViewHandle#

A handle to a view of a cumotion::World.

This view can be independently updated to track updates made to a cumotion::World object. A WorldViewHandle may be copied, with all copies sharing the same underlying view.

To query spatial relationships in a world view, use cumotion::WorldInspector.

Public Functions

void update()#: Update world view such that any changes to the underlying world are reflected in this view.

World Inspector#

#include <cumotion/world_inspector.h>

Public interface for querying spatial relationships in a world.

class WorldInspector#

Interface for querying properties and spatial relationships within a world.

Public Functions

virtual bool inCollision( const Eigen::Vector3d &center, double radius, ) const = 0#: Test whether a sphere defined by center and radius is in collision with ANY enabled obstacle in the world.

virtual bool inCollision( const World::ObstacleHandle &obstacle, const Eigen::Vector3d &center, double radius, ) const = 0#: Test whether a sphere defined by center and radius is in collision with the obstacle specified by obstacle.

virtual double distanceTo( const World::ObstacleHandle &obstacle, const Eigen::Vector3d &point, Eigen::Vector3d *gradient = nullptr, ) const = 0#

Compute the distance from point to the obstacle specified by obstacle.

Returns distance between the point and obstacle. If the point intersects obstacle, a negative distance is returned. The distance gradient is written to gradient if provided.

virtual int distancesTo( const Eigen::Vector3d &point, std::vector<double> *distances, std::vector<Eigen::Vector3d> *distance_gradients = nullptr, ) const = 0#

Compute distances from point to all enabled obstacles.

Distances between the point and each enabled obstacle are written to distances. If the point intersects an obstacle, the resulting distance will be negative. The distance gradients are written to distance_gradients if provided.

The number of distances and/or distance_gradients that are written (i.e., the number of enabled obstacles) is returned.

If the length of distances or distance_gradients is less than the number of enabled obstacles, the vectors will be resized to the number of enabled obstacles. If the vector lengths are larger than the number of enabled obstacles, the vectors will NOT be resized. Only the first N elements will be written to, where N is the number of enabled obstacles. The values of extra elements at the end of the input vectors will NOT be changed.

virtual double minDistance( const Eigen::Vector3d &point, Eigen::Vector3d *gradient = nullptr, ) const = 0#

Compute the minimum distance from point to ANY enabled obstacle in the current view of the world.

Returns distance between the point and the nearest obstacle. If the point is inside an obstacle, a negative distance is returned. The distance gradient is written to gradient if provided.

virtual int numEnabledObstacles() const = 0#: Return the number of enabled obstacles in the current view of the world.

std::unique_ptr<WorldInspector> cumotion::CreateWorldInspector( const WorldViewHandle &world_view, )#: Create a WorldInspector for a given world_view.

Robot-World Inspector#

#include <cumotion/robot_world_inspector.h>

Public interface for querying spatial relationships between a robot and a world.

class RobotWorldInspector#

Interface for querying spatial relationships between a robot and a world.

Public Functions

virtual void clearWorldView() = 0#

Clear the existing world view from RobotWorldInspector.

All ObstacleHandles will be invalid from the perspective of RobotWorldInspector.

inCollisionWithObstacle() will always return false.

virtual void setWorldView(const WorldViewHandle &world_view) = 0#: Set the internal world view used by RobotWorldInspector.

virtual int numWorldCollisionSpheres() const = 0#: Return the number of world-collision spheres in the robot representation.

virtual int numSelfCollisionSpheres() const = 0#: Return the number of self-collision spheres in the robot representation.

virtual std::string worldCollisionSphereFrameName( int world_collision_sphere_index, ) const = 0#

Return the name of the frame associated with the world-collision sphere corresponding to world_collision_sphere_index.

A fatal error is logged if:

world_collision_sphere_index is greater than or equal to the number of world-collision spheres attached to the robot (i.e., numWorldCollisionSpheres()), OR
world_collision_sphere_index is negative.

virtual std::string selfCollisionSphereFrameName( int self_collision_sphere_index, ) const = 0#

Return the name of the frame associated with the self-collision sphere corresponding to self_collision_sphere_index.

A fatal error is logged if:

self_collision_sphere_index is greater than or equal to the number of self-collision spheres attached to the robot (i.e., numSelfCollisionSpheres()), OR
self_collision_sphere_index is negative.

virtual void worldCollisionSphereRadii( std::vector<double> &sphere_radii, ) const = 0#

Return the radii of all world-collision spheres on the robot.

Each radius is the “effective radius”, representing the sum of the “base radius” of the sphere and any specified world-collision buffer distance.

The order of sphere_radii corresponds to the order of sphere_positions returned by worldCollisionSpherePositions().

The length of sphere_radii will be set to the number of world-collision spheres specified in the RobotDescription.

virtual void selfCollisionSphereRadii( std::vector<double> &sphere_radii, ) const = 0#

Return the radii of all self-collision spheres on the robot.

Each radius is the “effective radius”, representing the sum of the “base radius” of the sphere and any specified self-collision buffer distance.

The order of sphere_radii corresponds to the order of sphere_positions returned by selfCollisionSpherePositions().

The length of sphere_radii will be set to the number of self-collision spheres specified in the RobotDescription.

virtual void worldCollisionSpherePositions( const Eigen::VectorXd &cspace_position, std::vector<Eigen::Vector3d> &sphere_positions, ) const = 0#

Compute positions of all world-collision spheres on the robot at the specified cspace_position.

The order of sphere_positions corresponds to order of sphere_radii returned by worldCollisionSphereRadii().

If the length of sphere_positions is less than the number of world-collision spheres specified in the RobotDescription, sphere_positions will be resized to the number of world-collision spheres. If the length of sphere_positions is larger than the number of world-collision spheres, the vector will NOT be resized. Only the first n elements will be written to, where n is the number of world-collision spheres.

virtual void selfCollisionSpherePositions( const Eigen::VectorXd &cspace_position, std::vector<Eigen::Vector3d> &sphere_positions, ) const = 0#

Compute positions of all self-collision spheres on the robot at the specified cspace_position.

The order of sphere_positions corresponds to order of sphere_radii returned by selfCollisionSphereRadii().

If the length of sphere_positions is less than the number of self-collision spheres specified in the RobotDescription, sphere_positions will be resized to the number of self-collision spheres. If the length of sphere_positions is larger than the number of self-collision spheres, the vector will NOT be resized. Only the first n elements will be written to, where n is the number of self-collision spheres.

virtual bool inCollisionWithObstacle( const Eigen::VectorXd &cspace_position, ) const = 0#

Determine whether a specified cspace_position puts the robot into collision with an obstacle.

If no world view is specified, false is returned for any cspace_position.

virtual double minDistanceToObstacle( const Eigen::VectorXd &cspace_position, ) const = 0#

Compute the minimum pair-wise signed distance between the set of robot spheres and the set of obstacles for the provided position in configuration space. A positive distance implies that the obstacle and the robot are NOT in collision.

A fatal error is logged if the RobotWorldInspector does not have a world view set.

NOTE: A distance of zero implies that the closest robot sphere to any obstacle intersects the obstacle at a point.

virtual double distanceToObstacle( const World::ObstacleHandle &obstacle, int world_collision_sphere_index, const Eigen::VectorXd &cspace_position, ) const = 0#

Compute the signed distance between a given obstacle and collision sphere for the provided position in configuration space. A positive distance implies that the obstacle and the sphere are NOT in collision.

A fatal error is logged if:

world_collision_sphere_index is greater than or equal to the number of world-collision spheres attached to the robot (i.e., numWorldCollisionSpheres()), or it is negative.
The RobotWorldInspector does not have a world view set.

NOTE: If the world-collision sphere intersects the obstacle a distance of zero is returned.

virtual bool inSelfCollision( const Eigen::VectorXd &cspace_position, ) const = 0#

Indicate whether the robot is in self-collision at the given cspace_position.

The robot is considered to be in self-collision if any collision sphere on any frame intersects a collision sphere from another frame for which collisions have not been masked.

virtual std::vector<std::pair<std::string, std::string>> framesInSelfCollision( const Eigen::VectorXd &cspace_position, ) const = 0#

Return pairs of frames for which self-collision occurs at the given cspace_position.

Each pair of self-collision frames will be returned once in arbitrary order (e.g., the pair {“linkA”, “linkB”} may be returned OR the pair {“linkB”, “linkA”} may be returned, but not both).

std::unique_ptr<RobotWorldInspector> cumotion::CreateRobotWorldInspector( const RobotDescription &robot_description, std::optional<WorldViewHandle> world_view, )#

Create a RobotWorldInspector for a given robot_description and world_view.

The world_view is optional. If not provided, queries related to obstacle collisions (e.g., distanceToObstacle()) will not be functional unless a world view is set by setWorldView() after construction.

Kinematics#

#include <cumotion/kinematics.h>

Public interface for querying a kinematic structure.

class Kinematics#

Class representing the mapping from configuration space to coordinate frames that are rigidly attached to the kinematic structure.

Public Functions

virtual int numCSpaceCoords() const = 0#: Return the number of configuration space coordinates for the kinematic structure.

virtual std::string cSpaceCoordName(int coord_index) const = 0#

Return the name of a given configuration space coordinate of the kinematic structure.

A fatal error will be logged if:

coord_index < 0, OR
coord_index >= numCSpaceCoords().

virtual Limits cSpaceCoordLimits(int coord_index) const = 0#

Return the limits of a given configuration space coordinate of the kinematic structure.

A fatal error will be logged if:

coord_index < 0, OR
coord_index >= numCSpaceCoords().

virtual bool withinCSpaceLimits( const Eigen::VectorXd &cspace_position, bool log_warnings, ) const = 0#

Determine whether a specified configuration space position is within limits for each coordinate.

If log_warnings is set to true and cspace_position is outside limits, a warning will be logged indicating which coordinates are outside limits.

virtual double cSpaceCoordVelocityLimit(int coord_index) const = 0#

Return the velocity limit of a given configuration space coordinate of the kinematic structure.

A fatal error will be logged if:

coord_index < 0, OR
coord_index >= numCSpaceCoords().

virtual double cSpaceCoordAccelerationLimit(int coord_index) const = 0#

Return the acceleration limit of a given configuration space coordinate of the kinematic structure.

A fatal error will be logged if:

coord_index < 0, OR
coord_index >= numCSpaceCoords().

virtual double cSpaceCoordJerkLimit(int coord_index) const = 0#

Return the jerk limit of a given configuration space coordinate of the kinematic structure.

A fatal error will be logged if:

coord_index < 0, OR
coord_index >= numCSpaceCoords().

virtual const std::vector<std::string> &frameNames() const = 0#: Return all the frame names in the kinematic structure.

virtual const std::string &frameName( const FrameHandle &frame, ) const = 0#: Return the name of the given frame.

virtual FrameHandle frame(const std::string &frame_name) const = 0#

Return a handle representing the frame with the given frame_name.

A fatal error will be logged if frame_name is not a valid frame name.

virtual FrameHandle baseFrame() const = 0#: Return a handle representing the base frame of the kinematic structure.

virtual Pose3 pose( const Eigen::VectorXd &cspace_position, const FrameHandle &frame, const FrameHandle &reference_frame, ) const = 0#: Return the pose of the given frame with respect to reference_frame at the given cspace_position.

virtual Pose3 pose( const Eigen::VectorXd &cspace_position, const FrameHandle &frame, ) const = 0#: Return the pose of the given frame with respect to the base frame (i.e., baseFrame()) at the given cspace_position.

virtual Eigen::Vector3d position( const Eigen::VectorXd &cspace_position, const FrameHandle &frame, const FrameHandle &reference_frame, ) const = 0#: Return the position of the origin of the given frame with respect to reference_frame at the given cspace_position.

virtual Eigen::Vector3d position( const Eigen::VectorXd &cspace_position, const FrameHandle &frame, ) const = 0#: Return the position of the origin of the given frame with respect to the base frame (i.e., baseFrame()) at the given cspace_position.

virtual Rotation3 orientation( const Eigen::VectorXd &cspace_position, const FrameHandle &frame, const FrameHandle &reference_frame, ) const = 0#: Return the orientation of the given frame with respect to reference_frame at the given cspace_position.

virtual Rotation3 orientation( const Eigen::VectorXd &cspace_position, const FrameHandle &frame, ) const = 0#: Return the orientation of the given frame with respect to the base frame (i.e., baseFrame()) at the given cspace_position.

virtual Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian( const Eigen::VectorXd &cspace_position, const FrameHandle &frame, ) const = 0#

Return the Jacobian of the origin of the given frame with respect to the base frame (i.e., baseFrame()) at the given cspace_position.

The returned Jacobian is a 6 x N matrix where N is the numCSpaceCoords. Column i of the returned Jacobian represents the perturbation contribution to origin of frame from the ith c-space element in the coordinates of the base frame. For each column, the first three elements represent the translational portion and the last three elements represent the rotational portion.

virtual Eigen::Matrix<double, 3, Eigen::Dynamic> positionJacobian( const Eigen::VectorXd &cspace_position, const FrameHandle &frame, ) const = 0#

Return the Jacobian of the position of the origin of the given frame with respect to the base frame (i.e., baseFrame()) at the given cspace_position.

The returned Jacobian is a 3 x N matrix where N is the numCSpaceCoords. Column i of the returned Jacobian represents the perturbation contribution to position of the origin of frame from the ith c-space element in the coordinates of the base frame.

virtual Eigen::Matrix<double, 3, Eigen::Dynamic> orientationJacobian( const Eigen::VectorXd &cspace_position, const FrameHandle &frame, ) const = 0#

Return the Jacobian of the orientation of the given frame with respect to the base frame (i.e., baseFrame()) at the given cspace_position.

The returned Jacobian is a 3 x N matrix where N is the numCSpaceCoords. Column i of the returned Jacobian represents the perturbation contribution to orientation of frame from the ith c-space element in the coordinates of the base frame.

virtual Pose3 getPrecedingFixedTransform( const FrameHandle &frame, ) const = 0#

Return the fixed transform between frame and its parent frame.

Internally, the Kinematics structure is represented as a directed rooted tree. Each frame has a single parent frame (other than the root which by definition has no parent). The edge from parent frame to child frame represents both a fixed transform (i.e., a rigid linkage) AND a “joint” transform (revolute, prismatic, or fixed). The root frame, while not having a parent, maintains a fixed transform that defines its pose relative to global coordinates.

This function returns the fixed transform that precedes the “joint” transform. The returned Pose3 is expressed relative to the parent frame. For example, if the wrist_frame is a child of the elbow_frame, the returned pose would be the pose of the wrist joint expressed in the local coordinates of the elbow_frame.

NOTE: In general, the returned pose will NOT be the pose of the child frame expressed in the local coordinates of the parent frame, as the “joint” transform is NOT included in the returned transform, and the “joint” transform, in general, will not be identity. In order to return the full transform from parent frame to child frame for a given c-space configuration, the pose() function should be used with the child frame input for frame and the parent frame input for reference_frame.

virtual void setPrecedingFixedTransform( const FrameHandle &frame, const Pose3 &transform, ) = 0#

Set the fixed transform between frame and its parent frame to transform.

Internally, the Kinematics structure is represented as a directed rooted tree. Each frame has a single parent frame (other than the root which by definition has no parent). The edge from parent frame to child frame represents both a fixed transform (i.e., a rigid linkage) AND a “joint” transform (revolute, prismatic, or fixed). The root frame, while not having a parent, maintains a fixed transform that defines its pose relative to global coordinates; it is valid to use this function to set that tranform to pose the base of the robot relative to the global frame.

This function sets the fixed transform that precedes the “joint” transform. The input transform is expressed relative to the parent frame. For example, if the wrist_frame is a child of the elbow_frame, the input transform would define the pose of the wrist joint expressed in the local coordinates of the elbow_frame.

struct FrameHandle#: Opaque handle to a frame.

struct Limits#

Lower and upper limits for a configuration space coordinate.

Public Members

double lower#: lower limit

double upper#: upper limit

Inverse Kinematics (Collision-Unaware)#

#include <cumotion/ik_solver.h>

Public interface for inverse kinematics solvers.

struct IkConfig#

Configuration for solving inverse kinematics.

Public Types

enum class CSpaceLimitBiasing#

The error function for BFGS descent optionally includes a cost-term for avoiding c-space coordinate values near position limits.

If set to ENABLE, this cost term will always be turned on. If set to DISABLE, this cost term will always be turned off. Setting to AUTO will turn this cost term on for systems with 7 or more degrees-of-freedom (DoF), while leaving it off for systems with 6 or fewer DoF. NOTE: DoF is defined as the numer of c-space coordinates upstream from the target frame.

Setting to AUTO is likely to provide desirable results for most common robots. Redundant mechanisms with less than 7-DoF are a potential exception. E.g., a 4-Dof robot restricted to planar motion may benefit from enabling a cost term to avoid position limits.

Values:

enumerator AUTO#

enumerator ENABLE#

enumerator DISABLE#

Public Functions

IkConfig() = default#: Construct configuration with default values likely to be appropriate for most 6- and 7-dof robotic arms.

Public Members

int max_num_descents = 100#

The maximum number of c-space positions that will be used as seeds (i.e., initial positions) for cyclic coordinate descent.

The inverse kinematic solver will terminate when a valid c-space configuration is found OR the number of attempted descents reaches max_num_descents.

Must be positive.

Default: 100

std::vector<Eigen::VectorXd> cspace_seeds = {}#

Optional parameter to set the initial c-space configurations for each descent.

These cspace_seeds will be used to attempt cyclic coordinate descent in the order provided, terminating before all cspace_seeds are tried if a valid cspace_position is found.

If the number of attempted descents is greater than the number of provided cspace_seeds, random starting configurations will be generated (see irwin_hall_sampling_order for details).

It is valid to provide no cspace_seeds. In this case all initial configurations will be randomly sampled.

int irwin_hall_sampling_order = 2#

If the number of attempted descents exceeds the provided number of cspace_seeds (including the case where no cspace_seeds are provided), then random c-space configurations will be sampled for each descent.

Each c-space coordinate value will be sampled from an Irwin-Hall distribution spanning the joint limits. The distribution mean will always be set to the midpoint between upper and lower joint limits.

Setting irwin_hall_sampling_order to 1 corresponds to a uniform distribution, setting to 2 corresponds to a triangular distribution, and higher values approximate a normal distribution.

See https://en.wikipedia.org/wiki/Irwin%E2%80%93Hall_distribution for reference, noting that the internal distribution is scaled s.t. all sampled values will be within joint limits.

Must be positive.

Default: 2

int sampling_seed = 123456#

Seed for random number generator used for Irwin-Hall sampling for initial c-space positions.

Must be non-negative.

Default: 123456

double position_tolerance = 1e-3#

Maximum position error (L2-norm) of the task space pose for a successful IK solution.

Must be positive.

Default: 1e-3

double orientation_tolerance = 0.01#

Maximum orientation error of the task space pose for a successful IK solution.

For each axis in task space, error is defined as the L2-norm of the difference between the axis direction and the target pose axis direction.

Must be positive.

Default: 0.01

int ccd_max_iterations = 10#

The number of iterations used for cyclic coordinate descent from each initial c-space position (i.e., seed).

Smaller values will decrease the amount of computation time per descent, but may require more descents to converge to valid c-space configuration.

CCD will be disabled for values less than 1.

Default: 10

double ccd_descent_termination_delta = 1e-1#

Each descent can terminate early if the L2-norm of the change to c-space coordinates is less than the descent_termination_delta.

Must be positive.

Default: 1e-1

double ccd_position_weight = 1.0#

Weight for the relative importance of position error term when performing cyclic coordinate descent.

Must be non-negative.

Default: 1.0

double ccd_orientation_weight = 0.05#

Weight for the relative importance of orientation error term when performing cyclic coordinate descent.

Must be non-negative.

Default: 0.05

int ccd_bracket_search_num_uniform_samples = 10#

Number of samples used to identify valid three-point bracket for numerical optimization of c-space updates.

NOTE: This parameter is ONLY used when more than one active upstream joint is controlled by a single c-space coordinate.

For the “many-to-one joint-to-cspace” case, quadratic fit search (QFS) is used to approximate an optimal c-space update during each iteration of cyclic coordinate descent. In order to find a valid three-point bracket for QFS, N points are sampled to attempt to find a region with minimum error. This sampling is incorporated into the numerical solver since, in general, the error function will not be unimodal within the domain bounded by the c-space position limits.

Must be greater than one.

Default: 10

int bfgs_max_iterations = 60#

The number of iterations used for Broyden-Fletcher-Goldfarb-Shanno (BFGS) descent from each initial c-space position (i.e., seed).

Smaller values will decrease the amount of computation time per descent, but may require more descents to converge to valid c-space configuration.

Must be positive.

Default: 60

double bfgs_gradient_norm_termination = 1e-6#

The BFGS solver will terminate if the L2-norm of the error function gradient at the current best c-space position is less than bfgs_gradient_norm_termination.

Low values for bfgs_gradient_norm_termination will increase solver accuracy, while high values will decrease solve times for a given problem.

Must be positive.

Default: 1e-6

double bfgs_gradient_norm_termination_coarse_scale_factor = 1e7#

The BFGS solver is implemented as a two-step process, with a coarse solve used to identify whether convergence to a valid c-space position is likely.

For the coarse solve, the bfgs_gradient_norm_termination is multiplied by the bfgs_gradient_norm_termination_coarse_scale_factor such that the optimization will end early.

Must be greater than or equal to 1.

Default: 1e7

CSpaceLimitBiasing bfgs_cspace_limit_biasing = CSpaceLimitBiasing::AUTO #

See documentation for CSpaceLimitBiasing.

Default: AUTO

double bfgs_cspace_limit_penalty_region = 0.01#

Define the region near c-space limits which will induce penalties when c-space limit biasing is active.

The region is defined as a fraction of the c-space span for each c-space coordinate. For example, imagine a 2d system for which the limits of the first coordinate are [-5, 5] and the limits of the second coordinate are [0, 2]. If the cspace_limit_penalty_region is set to 0.1, then the size of each penalty region for first coordinate will be: 0.1 * (5 - (-5)) = 1 Similarly, the size of each penalty region for the second coordinate will be: 0.1 * (2 - 0) = 0.2 This means that for the first coordinate, c-space limit errors will be active from [-5, -4] and [4, 5]. For the second coordinate, c-space limit errors will be active from [0, 0.2] and [1.8, 2]

Must be in the range [0, 0.5].

Default: 0.01

double bfgs_cspace_limit_biasing_weight = 1.0#

Relative weight applied to c-space limit error (if active).

Must be non-negative.

Default: 1.0

double bfgs_position_weight = 1.0#

Weight for the relative importance of position error term when performing BFGS descent.

Must be non-negative.

Default: 1.0

double bfgs_orientation_weight = 100.0#

Weight for the relative importance of orientation error term when performing BFGS descent.

Must be non-negative.

Default: 100.0

struct IkResults#

Results from solving inverse kinematics.

Public Members

bool success#: Indicate whether a valid cspace_position within the tolerances specified in the IkConfig has been found.

Eigen::VectorXd cspace_position#: If success is set to true, cspace_position will contain a valid joint configuration that corresponds to the target_pose passed to the IK solver within the tolerances specified in the IkConfig.

double position_error#

Position error (L2-norm) of the task space pose corresponding to the returned cspace_position

If success is set to true, the position_error will be less than or equal to the position_tolerance set in the IkConfig.

double x_axis_orientation_error#

X-axis orientation error (L2-norm) of the task space pose corresponding to the returned cspace_position

If success is set to true, the x_axis_orientation_error will be less than or equal to the orientation_tolerance set in the IkConfig.

double y_axis_orientation_error#

Y-axis orientation error (L2-norm) of the task space pose corresponding to the returned cspace_position

If success is set to true, the y_axis_orientation_error will be less than or equal to the orientation_tolerance set in the IkConfig.

double z_axis_orientation_error#

Z-axis orientation error (L2-norm) of the task space pose corresponding to the returned cspace_position

If success is set to true, the z_axis_orientation_error will be less than or equal to the orientation_tolerance set in the IkConfig.

int num_descents#

The number of unique c-space positions that were used for attempting cyclic coordinate descent prior to the IK solver terminating.

If success is set to true, this will be the number of descents that were attempted in order to get a cspace_position within the provided tolerance. If success is set to false, the number of descents will be set to the max_num_descents set in the IkConfig.

IkResults cumotion::SolveIk( const Kinematics &kinematics, const Pose3 &target_pose, const Kinematics::FrameHandle &target_frame, const IkConfig &config, )#

Attempt to solve inverse kinematics.

A target_pose is provided for the task space target_frame and the solver attempts to find a corresponding set of c-space coordinates. A valid c-space configuration must not exceed joint limits set in the kinematic structure and must be the target pose tolerances specified within config.

If the target_frame is selected such that some of the c-space coordinates do not affect the pose of the target frame, these “downstream” coordinates will be ignored by the IK solver. For the case where the successful descent was started from a seed provided in config.cspace_seeds, the “downstream” coordinates will not be altered from the provided seed. For the case where a random seed is used, the “downstream” coordinates will be returned with values between their respective c-space position limits. This behavior is intended to support cases such as branched kinematic structures (i.e., to solve for palm pose of a robotic system with an arm and multiple fingers) or a redundant serial linkage for which it is desired to set a pose of a frame not rigidly fixed to the end effector.

This implementation of cyclic coordinate descent (CCD) is based on “A Combined Optimization

Method for Solving the Inverse Kinematics Problem of Mechanical Manipulators” (1991) by Wang et al. Ref:

http://web.cse.ohio-state.edu/~parent.1/classes/788/Sp06/ReferenceMaterial/IK/WC91.pdf

As described by Wang et al., this “combined” method uses cyclic coordinate descent (CCD) as a quick method to find a feasible c-space position that is in the neighborhood of an exact solution to the inverse kinematics problem. These nearby c-space positions are then used to “warm start” the Broyden-Fletcher-Goldfarb-Shanno (BFGS) algorithm. Given a point sufficiently near an exact solution, the BFGS algorithm is able to leverage an estimate of curvature to rapidly converge to a very accurate approximation of the exact solution.

Concise explanations of the CCD algorithm with interactive demonstrations are available at: [1] https://zalo.github.io/blog/inverse-kinematics/ [2] http://rodolphe-vaillant.fr/?e=114

Collision-Aware Inverse Kinematics#

#include <cumotion/collision_free_ik_solver.h>

class CollisionFreeIkSolverConfig#

Configuration parameters for a CollisionFreeIkSolver.

Public Functions

virtual bool setParam( const std::string &param_name, ParamValue value, ) = 0#

Set the value of the parameter.

setParam() returns true if the parameter has been successfully updated and false if an error has occurred (e.g., invalid parameter).

The following parameters can be set for CollisionFreeIkSolver:

task_space_position_tolerance [double]

Maximum allowable violation of the user-specified constraint on the position of the tool frame.
It determines when a solution is considered to have satisfied position targets. Smaller values require more precise solutions but may be harder to achieve.
Units are in meters.
A default value of 1e-3 (1mm) is recommended for most use-cases.
Must be positive.

task_space_orientation_tolerance [double]

Maximum allowable violation of the user-specified constraint on the orientation of the tool frame.
It determines when a solution is considered to have satisfied orientation targets. Smaller values require more precise solutions but may be harder to achieve.
Units are in radians.
A default value of 5e-3 (approximately 0.29 degrees) is recommended for most use-cases.
Must be positive.

num_seeds [int]

Number of seeds used to solve the inverse kinematics (IK) problem.
The IK solver generates multiple pseudo-random c-space configurations and optimizes them to find diverse collision-free configurations for the desired tool pose. Higher values increase the likelihood of finding valid solutions but increase computational cost.
A default value of 25 is recommended for most use-cases.
Must be positive.

max_reattempts [int]

Maximum number of times to restart the IK problem with different random seeds, in case of failure, before giving up.
Higher values increase the likelihood of finding valid IK solutions but increase memory usage and the maximum possible time to find a solution. A value of 0 means no retries (i.e. only perform the initial attempt).
A default value of 10 is recommended for most use-cases.
Must be non-negative.

struct ParamValue#

Specify the value for a given parameter.

The required ParamValue constructor for each param is detailed in the documentation for setParam().

Public Functions

ParamValue(int value)#: Create ParamValue from int.

ParamValue(double value)#: Create ParamValue from double.

class CollisionFreeIkSolver#

Interface for using numerical optimization to generate collision-free c-space positions for task-space targets.

Public Functions

virtual std::unique_ptr<Results> solve( const TaskSpaceTarget &task_space_target, const std::vector<Eigen::VectorXd> &cspace_seeds = {}, ) const = 0#

Attempt to find c-space solutions that satisfy the constraints specified in task_space_target.

The cspace_seeds are optional inputs that can be used to “warm start” the IK optimization. NOTE: It is not required that the cspace_seeds represent valid c-space positions (i.e., be within position limits, be collision-free, etc.).

A fatal error will be logged if:

Any c-space position in cspace_seeds does not have the same number of c-space coordinates as the RobotDescription used to configure the CollisionFreeIkSolverConfig.

virtual std::unique_ptr<Results> solveGoalset( const TaskSpaceTargetGoalset &task_space_target_goalset, const std::vector<Eigen::VectorXd> &cspace_seeds = {}, ) const = 0#

Attempt to find c-space solutions that satisfy the constraints specified in task_space_target_goalset.

To be considered a valid solution, a c-space position must satisfy the constraints of any one of the targets specified in task_space_target_goalset.

The cspace_seeds are optional inputs that can be used to “warm start” the IK optimization. NOTE: It is not required that the cspace_seeds represent valid c-space positions (i.e., be within position limits, be collision-free, etc.).

A fatal error will be logged if:

Any c-space position in cspace_seeds does not have the same number of c-space coordinates as the RobotDescription used to configure the CollisionFreeIkSolverConfig.

class OrientationConstraint#

Orientation constraints restrict the orientation of a tool frame.

Each constraint may fully or partially constrain the orientation.

Public Static Functions

static OrientationConstraint None()#: Create an OrientationConstraint such that no tool frame orientation constraints are active.

static OrientationConstraint Target( const Rotation3 &orientation_target, std::optional<double> deviation_limit = std::nullopt, )#

Create an OrientationConstraint such that a tool frame orientation_target is fully specified.

The optional parameter deviation_limit can be used to allow deviation from the orientation_target. If input, deviation_limit is expressed in radians. This limit specifies the maximum allowable deviation from the desired orientation.

If deviation_limit is not input, then the deviation limit is assumed to be zero (i.e., it is desired that orientation be exactly orientation_target).

In general, it is suggested that the deviation_limit be set to a value less than pi. A limit that is near or greater than pi essentially disables the constraint (without culling the computation). Non-fatal warnings will be logged if:

deviation_limit is near or greater than pi.

A fatal error will be logged if:

deviation_limit is negative.

NOTE: The orientation_target is specified in world frame coordinates (i.e., relative to the base of the robot).

static OrientationConstraint Axis( const Eigen::Vector3d &tool_frame_axis, const Eigen::Vector3d &world_target_axis, std::optional<double> axis_deviation_limit = std::nullopt, )#

Create an OrientationConstraint such that the tool frame orientation is constrained to rotate about a “free axis”.

The “free axis” for rotation is defined by a tool_frame_axis (specified in the tool frame coordinates) and a corresponding world_target_axis (specified in world frame coordinates).

The optional axis_deviation_limit can be used to allow deviation from the desired axis alignment. If input, axis_deviation_limit is expressed in radians and the limit specifies the maximum allowable deviation of the rotation axis. If axis_deviation_limit is not input, then the deviation limit is assumed to be zero (i.e., it is desired that the tool frame axis be exactly aligned with world_target_axis).

In general, it is suggested that the axis_deviation_limit be set to a value less than pi. A limit that is near or greater than pi essentially disables the constraint (without culling the computation). Non-fatal warnings will be logged if:

axis_deviation_limit is near or greater than pi.

A fatal error will be logged if:

axis_deviation_limit is negative,
tool_frame_axis is (nearly) zero, OR
world_target_axis is (nearly) zero.

NOTE: tool_frame_axis and world_target_axis inputs will be normalized.

class OrientationConstraintGoalset#

Variant of OrientationConstraint for “goalset” planning.

For goalset planning, a set of OrientationConstraints are considered concurrently. Each OrientationConstraint in the goalset must have the same mode (e.g., “full target”) but may have different data for each OrientationConstraint.

Public Static Functions

static OrientationConstraintGoalset None()#: Create an OrientationConstraintGoalset such that no tool frame orientation constraints are active.

static OrientationConstraintGoalset Target( const std::vector<Rotation3> &orientation_targets, std::optional<double> deviation_limit = std::nullopt, )#

Create an OrientationConstraintGoalset such that tool frame orientation_targets are fully specified.

See OrientationConstraint::Target() for details.

A fatal error will be logged if:

Any condition of OrientationConstraint::Target() is not met, OR
orientation_targets is empty.

static OrientationConstraintGoalset Axis( const std::vector<Eigen::Vector3d> &tool_frame_axes, const std::vector<Eigen::Vector3d> &world_target_axes, std::optional<double> axis_deviation_limit = std::nullopt, )#

Create an OrientationConstraintGoalset such that the tool frame orientation is constrained to rotate about a “free axis”.

See OrientationConstraint::Axis() for details.

A fatal error will be logged if:

Any condition of OrientationConstraint::Axis() is not met,
tool_frame_axes and world_target_axes do not have the same number of elements, OR
tool_frame_axes and world_target_axes are empty.

class Results#

Results from an inverse kinematics solve.

Public Types

enum class Status#

Indicate the success or failure of the inverse kinematics solve.

Values:

enumerator SUCCESS#: One or more valid c-space positions found.

enumerator INVERSE_KINEMATICS_FAILURE#: The inverse kinematics solver failed to find a valid solution.

Public Functions

virtual Status status() const = 0#: Return the Status from the inverse kinematics solve.

virtual std::vector<Eigen::VectorXd> cSpacePositions() const = 0#

If status() returns SUCCESS, then cSpacePositions() returns unique IK solutions.

A SUCCESS status indicates that at least one IK solution was found. Returned solutions are ordered from the lowest cost to the highest cost.

If status() returns INVERSE_KINEMATICS_FAILURE, then an empty vector will be returned.

virtual std::vector<int> targetIndices() const = 0#

Return the indices of the targets selected for each valid c-space position.

For goalset planning (e.g., solveGoalset()), returned values represent indices into the constraint vectors used to create the goalset (e.g., TaskSpaceTargetGoalset).

For single target planning (e.g., solve()), zero will be returned for each valid c-space position in cSpacePositions().

In all cases, the length of returned targetIndices() will be equal to the length of returned cSpacePositions().

struct TaskSpaceTarget#

Task-space targets restrict the position and (optionally) orientation of the tool frame.

Public Functions

explicit TaskSpaceTarget( const TranslationConstraint &translation_constraint, const OrientationConstraint &orientation_constraint = OrientationConstraint::None(), )#: Create a task-space target.

struct TaskSpaceTargetGoalset#

Variant of TaskSpaceTarget for “goalset” planning.

For goalset planning, a set of pose constraints are considered concurrently. Each pose constraint in the goalset must have the same mode (e.g., “position target and no orientation

constraints”) but may have different data for each constraint.

Public Functions

explicit TaskSpaceTargetGoalset( const TranslationConstraintGoalset &translation_constraints, const OrientationConstraintGoalset &orientation_constraints = OrientationConstraintGoalset::None(), )#

Create a task-space target goalset.

A fatal error will be logged if:

The number of translation_constraints does not match the number of orientation_constraints.

class TranslationConstraint#

Translation constraints restrict the position of the origin of a tool frame.

Public Static Functions

static TranslationConstraint Target( const Eigen::Vector3d &translation_target, std::optional<double> deviation_limit = std::nullopt, )#

Create a TranslationConstraint such that the desired position is fully specified as translation_target.

The optional parameter deviation_limit can be used to allow deviation from the translation_target. This limit specifies the maximum allowable deviation from the desired position.

If deviation_limit is not input, then the deviation limit is assumed to be zero (i.e., it is desired that the tool frame position be exactly translation_target).

A fatal error will be logged if:

deviation_limit is negative.

NOTE: The translation_target is specified in world frame coordinates (i.e., relative to the base of the robot).

class TranslationConstraintGoalset#

Variant of TranslationConstraint for “goalset” planning.

For goalset planning, a set of TranslationConstraints are considered concurrently. Each TranslationConstraint in the goalset must have the same mode (e.g., “target”) but may have different data for each TranslationConstraint.

Public Static Functions

static TranslationConstraintGoalset Target( const std::vector<Eigen::Vector3d> &translation_targets, std::optional<double> deviation_limit = std::nullopt, )#

Create a TranslationConstraintGoalset such that translation_targets are fully specified.

See TranslationConstraint::Target() for details.

A fatal error will be logged if:

Any condition of TranslationConstraint::Target() is not met, OR
translation_targets is empty.

std::unique_ptr<CollisionFreeIkSolver> cumotion::CreateCollisionFreeIkSolver( const CollisionFreeIkSolverConfig &config, )#: Create a CollisionFreeIkSolver with the given config.

Trajectory Representation#

#include <cumotion/trajectory.h>

class Trajectory#

Represent a path through configuration space (i.e., “c-space”) parameterized by time.

Public Functions

virtual int numCSpaceCoords() const = 0#: Return the number of configuration space coordinates for the kinematic structure.

virtual Domain domain() const = 0#: Return the domain for the trajectory.

virtual void eval( double time, Eigen::VectorXd *cspace_position, Eigen::VectorXd *cspace_velocity = nullptr, Eigen::VectorXd *cspace_acceleration = nullptr, Eigen::VectorXd *cspace_jerk = nullptr, ) const = 0#: Evaluate the trajectory at the given time.

virtual Eigen::VectorXd eval( double time, int derivative_order = 0, ) const#

Evaluate specified trajectory derivative at the given time and return value.

The default derivative_order is the “zeroth derivative” (which is simply the c-space position of the trajectory). Setting derivative_order to 1 will output the c-space velocity, with higher derivative_orders corresponding to higher derivatives.

Trajectories are expected to support at least the third derivative (i.e., “jerk”).

The default implementation internally calls the above eval() function and will log a fatal error if the derivative_order is not in range [0, 3].

virtual Eigen::VectorXd minPosition() const = 0#

Return the minimum position for each c-space coordinate within the defined domain().

NOTE: These minimum position values are not, in general, synchronous. Instead, they represent the minimum position independently achieved by each coordinate.

virtual Eigen::VectorXd maxPosition() const = 0#

Return the maximum position for each c-space coordinate within the defined domain().

NOTE: These maximum position values are not, in general, synchronous. Instead, they represent the maximum position independently achieved by each coordinate.

virtual Eigen::VectorXd maxVelocityMagnitude() const = 0#

Return the maximum magnitude of velocity for each c-space coordinate within the defined domain().

NOTE: These maximum magnitude values are not, in general, synchronous. Instead, they represent the maximum magnitudes independently achieved by each coordinate.

virtual Eigen::VectorXd maxAccelerationMagnitude() const = 0#

Return the maximum magnitude of acceleration for each c-space coordinate within the defined domain().

NOTE: These maximum magnitude values are not, in general, synchronous. Instead, they represent the maximum magnitudes independently achieved by each coordinate.

virtual Eigen::VectorXd maxJerkMagnitude() const = 0#

Return the maximum magnitude of jerk for each c-space coordinate within the defined domain().

NOTE: These maximum magnitude values are not, in general, synchronous. Instead, they represent the maximum magnitudes independently achieved by each coordinate.

struct Domain#

Indicates the continuous range of time values, t, for which the trajectory and its derivatives are defined.

Public Functions

inline double span() const#: Convenience function to return the span of time values included in domain.

Public Members

double lower#: Minimum value of time defining domain.

double upper#: Maximum value of time defining domain.

Trajectory Optimization#

#include <cumotion/trajectory_optimizer.h>

class TrajectoryOptimizerConfig#

Configuration parameters for a TrajectoryOptimizer.

Public Functions

virtual bool setParam( const std::string &param_name, ParamValue value, ) = 0#

Set the value of the parameter.

setParam() returns true if the parameter has been successfully updated and false if an error has occurred (e.g., invalid parameter).

The following parameters can be set for TrajectoryOptimizer:

enable_self_collision [bool]

Enable or disable self-collision avoidance between robot spheres during optimization.
When true, both inverse kinematics (IK) and trajectory optimization will include collision costs to prevent robot spheres from colliding with each other.
When false, self-collision costs are disabled, which may improve solve time but allows self-intersecting trajectories.
Default value is true (self-collision avoidance enabled).

enable_world_collision [bool]

Enable or disable collision avoidance with world obstacles during optimization.
When true, both IK and trajectory optimization will include collision costs to avoid obstacles in the environment.
When false, world collision costs are disabled, which may improve performance but allows trajectories that intersect with obstacles.
Default value is true (world collision avoidance enabled).

task_space_terminal_position_tolerance [double]

Maximum allowable violation of the user-specified constraint on the position of the tool frame.
Used both for IK and trajectory optimization problems. It determines when a solution is considered to have satisfied position targets. Smaller values require more precise solutions but may be harder to achieve.
Units are in meters.
A default value of 1e-3 (1mm) is recommended for most use-cases.
Must be positive.

task_space_terminal_orientation_tolerance [double]

Maximum allowable violation of the user-specified constraint on the orientation of the tool frame.
Used both for IK and trajectory optimization problems. It determines when a solution is considered to have satisfied orientation targets. Smaller values require more precise solutions but may be harder to achieve.
Units are in radians.
A default value of 5e-3 (approximately 0.29 degrees) is recommended for most use-cases.
Must be positive.

ik/num_seeds [int]

Number of seeds used to solve the inverse kinematics (IK) problem.
The trajectory optimizer generates multiple pseudo-random c-space configurations and optimizes them to find diverse collision-free configurations for the desired tool pose. Higher values increase the likelihood of finding valid solutions but increase computational cost.
A default value of 25 is recommended for most use-cases.
Must be positive.

ik/max_reattempts [int]

Maximum number of times to restart the IK problem with different random seeds, in case of failure, before giving up.
Higher values increase the likelihood of finding valid IK solutions but increase memory usage and the maximum possible time to find a solution. A value of 0 means no retries (i.e. only perform the initial attempt).
A default value of 10 is recommended for most use-cases.
Must be non-negative.

ik/desired_cspace_position_weight [double]

Weight used on the distance to a desired c-space configuration when ranking IK solutions.
When this weight is zero, the valid IK solutions are ranked purely by their tool constraint violations.
A default value of 1.0 provides balanced behavior for most use-cases.
Must be positive.

ik/pbo/cost/tool_frame_position_error_penalty/weight [double]

Penalty weight applied in task-space position error violations at tool-frame constraints during PBO IK optimization.
Higher values more strongly enforce exact position matching at goal targets.
A default value of 100.0 is recommended for most use-cases.
Must be non-negative.

ik/pbo/cost/tool_frame_position_error_penalty/activation_distance [double]

Distance from the deviation limit at which task-space position error penalties activate. This value is subtracted from the deviation limit specified when creating the position constraint (e.g., TranslationConstraint::Target()) during PBO IK optimization.
If the activation distance is greater than the deviation limit (which is often zero), all non-zero position errors will be penalized, encouraging solutions that have as little error as possible.
If the activation distance is less than the deviation limit, any solution with an error less than deviation_limit - activation_distance will have equal cost. Most solutions will have a final error of deviation_limit - activation_distance, instead of an error near zero.
Units are in meters.
A default value of 0.01 is recommended for most use-cases.
Must be non-negative.

ik/pbo/cost/tool_frame_orientation_error_penalty/weight [double]

Penalty weight applied in task-space orientation error violations at tool-frame constraints during PBO IK optimization.
Higher values more strongly enforce exact orientation matching at goal targets.
A default value of 10.0 is recommended for most use-cases.
Must be non-negative.

ik/pbo/cost/tool_frame_orientation_error_penalty/activation_distance [double]

Angular distance from the deviation limit at which task-space orientation error penalties activate. This value is subtracted from the deviation limit specified when creating the orientation constraint (e.g., OrientationConstraint::TerminalTarget()) during PBO IK optimization.
If the activation distance is greater than the deviation limit (which is often zero), all non-zero orientation errors will be penalized, encouraging solutions that have as little angular error as possible.
If the activation distance is less than the deviation limit, any solution with an error less than deviation_limit - activation_distance will have equal cost. Most solutions will have a final angular error of deviation_limit - activation_distance, instead of an error near zero.
Units are in radians.
A default value of 0.01 is recommended for most use-cases.
Must be non-negative.

ik/pbo/cost/cspace_position_limit_penalty/weight [double]

Penalty weight applied when IK solutions violate c-space position limits, minus the activation distance, during Particle-Based Optimizer (PBO) IK optimization.
Higher values more strongly discourage solutions near joint limits.
A default value of 10.0 is recommended for most use-cases.
Must be non-negative.

ik/pbo/cost/cspace_position_limit_penalty/activation_distance [double]

Distance threshold from c-space position limits at which the position limit penalty activates, during PBO IK optimization.
Units correspond to rad for revolute joints, m for prismatic joints.
A default value equivalent to 5 degrees is recommended for most use-cases.
Must be non-negative.

ik/pbo/cost/enable_cspace_position_limit [bool]

Enable or disable c-space position limit penalties during PBO IK optimization.
When true, PBO IK will penalize solutions that violate or approach c-space position limits.
When false, position limit costs are ignored (not recommended).
Default value is true (position limit penalties enabled).

ik/pbo/cost/self_collision_penalty/weight [double]

Penalty weight applied in self-colliding robot configurations during PBO IK optimization.
Higher values more strongly discourage self-colliding configurations.
This parameter is only used when enable_self_collision is true.
A default value of 1000.0 is recommended for most use-cases.
Must be non-negative.

ik/pbo/cost/self_collision_penalty/activation_distance [double]

Distance threshold at which the self-collision penalty activates during PBO IK optimization.
The penalty becomes non-zero when robot collision geometries are within this distance of each other.
Units are in meters.
A default value of 0.01 m (1 cm) is recommended for most use-cases.
Must be non-negative.

ik/pbo/cost/world_collision_penalty/weight [double]

Penalty weight applied in world-colliding robot configurations during PBO IK optimization.
Higher values more strongly discourage configurations that collide with obstacles.
This parameter is only used when enable_world_collision is true.
A default value of 1000.0 is recommended for most use-cases.
Must be non-negative.

ik/pbo/cost/world_collision_penalty/activation_distance [double]

Distance threshold at which the world collision penalty activates during PBO IK optimization.
The penalty becomes non-zero when robot collision geometries are within this distance of obstacles.
Units are in meters.
A default value of 0.01 m (1 cm) is recommended for most use-cases.
Must be non-negative.

ik/lbfgs/cost/tool_frame_position_error_penalty/weight [double]

Penalty weight applied in task-space position error violations at tool-frame constraints during L-BFGS IK optimization.
Higher values more strongly enforce exact position matching at goal targets.
A default value of 100.0 is recommended for most use-cases.
Must be non-negative.

ik/lbfgs/cost/tool_frame_position_error_penalty/activation_distance [double]

Distance from the deviation limit at which task-space position error penalties activate. This value is subtracted from the deviation limit specified when creating the position constraint (e.g., TranslationConstraint::Target()) during L-BFGS IK optimization.
If the activation distance is greater than the deviation limit (which is often zero), all non-zero position errors will be penalized, encouraging solutions that have as little error as possible.
If the activation distance is less than the deviation limit, any solution with an error less than deviation_limit - activation_distance will have equal cost. Most solutions will have a final error of deviation_limit - activation_distance, instead of an error near zero.
Units are in meters.
A default value of 0.01 is recommended for most use-cases.
Must be non-negative.

ik/lbfgs/cost/tool_frame_orientation_error_penalty/weight [double]

Penalty weight applied in task-space orientation error violations at tool-frame constraints during L-BFGS IK optimization.
Higher values more strongly enforce exact orientation matching at goal targets.
A default value of 10.0 is recommended for most use-cases.
Must be non-negative.

ik/lbfgs/cost/tool_frame_orientation_error_penalty/activation_distance [double]

Angular distance from the deviation limit at which task-space orientation error penalties activate. This value is subtracted from the deviation limit specified when creating the orientation constraint (e.g., `OrientationConstraint::TerminalTarget()) during L-BFGS IK optimization.
If the activation distance is greater than the deviation limit (which is often zero), all non-zero orientation errors will be penalized, encouraging solutions that have as little angular error as possible.
If the activation distance is less than the deviation limit, any solution with an error less than deviation_limit - activation_distance will have equal cost. Most solutions will have a final angular error of deviation_limit - activation_distance, instead of an error near zero.
Units are in radians.
A default value of 0.01 is recommended for most use-cases.
Must be non-negative.

ik/lbfgs/cost/cspace_position_limit_penalty/weight [double]

Penalty weight applied when IK solutions violate c-space position limits, minus the activation distance, during L-BFGS IK optimization.
Higher values more strongly discourage solutions near joint limits.
A default value of 10.0 is recommended for most use-cases.
Must be non-negative.

ik/lbfgs/cost/cspace_position_limit_penalty/activation_distance [double]

Distance threshold from c-space position limits at which the position limit penalty activates, during L-BFGS IK optimization.
Units correspond to rad for revolute joints, m for prismatic joints.
A default value equivalent to 5 degrees is recommended for most use-cases.
Must be non-negative.

ik/lbfgs/cost/enable_cspace_position_limit [bool]

Enable or disable c-space position limit penalties during L-BFGS IK optimization.
When true, L-BFGS IK will penalize solutions that violate or approach c-space position limits.
When false, position limit costs are ignored (not recommended).
Default value is true (position limit penalties enabled).

ik/lbfgs/cost/self_collision_penalty/weight [double]

Penalty weight applied in self-colliding robot configurations during L-BFGS IK optimization.
Higher values more strongly discourage self-colliding configurations.
This parameter is only used when enable_self_collision is true.
A default value of 1000.0 is recommended for most use-cases.
Must be non-negative.

ik/lbfgs/cost/self_collision_penalty/activation_distance [double]

Distance threshold at which the self-collision penalty activates during L-BFGS IK optimization.
The penalty becomes non-zero when robot collision geometries are within this distance of each other.
Units are in meters.
A default value of 0.01 m (1 cm) is recommended for most use-cases.
Must be non-negative.

ik/lbfgs/cost/world_collision_penalty/weight [double]

Penalty weight applied in world-colliding robot configurations during L-BFGS IK optimization.
Higher values more strongly discourage configurations that collide with obstacles.
This parameter is only used when enable_world_collision is true.
A default value of 1000.0 is recommended for most use-cases.
Must be non-negative.

ik/lbfgs/cost/world_collision_penalty/activation_distance [double]

Distance threshold at which the world collision penalty activates during L-BFGS IK optimization.
The penalty becomes non-zero when robot collision geometries are within this distance of obstacles.
Units are in meters.
A default value of 0.01 m (1 cm) is recommended for most use-cases.
Must be non-negative.

trajopt/num_seeds [int]

Number of seeds used to solve the trajectory optimization problem.
The trajectory optimizer generates interpolated trajectories between the initial configuration and each of the IK solutions, and then optimizes each trajectory. Higher values provide more trajectory candidates but increase computational cost.
A default value of 4 is recommended for most use-cases.
Must be positive.

trajopt/num_knots_per_trajectory [int]

Number of knot points per trajectory used for trajectory optimization.
This parameter controls the temporal resolution of the optimized trajectory, where higher values provide finer temporal resolution but increase computational cost.
A default value of 32 is recommended for most use-cases.
Must be positive.

trajopt/pbo/enabled [bool]

Enable or disable Particle-Based Optimizer (PBO) for the trajectory optimization problem.
PBO is an algorithm that uses a set of “particles” to explore a non-linear design space. If enabled, PBO is used to find collision-free trajectories before starting the gradient-based L-BFGS optimization algorithm. PBO is helpful for complex scenarios where a linearly-interpolated path between the initial configuration and the IK solutions is not sufficient to find a collision-free trajectory.
When set to false, PBO is disabled, and L-BFGS is initialized with a linearly-interpolated path.
When set to true, PBO is enabled and the trajopt/pbo parameters control its behavior.
Default value is true (PBO enabled).

trajopt/pbo/num_particles_per_problem [int]

Number of particles used by PBO in the trajectory optimization problem, when enabled.
This parameter controls the population size of particles in the PBO algorithm. Higher values increase the likelihood of finding better solutions but increase solve time and memory usage.
This parameter is only used when trajopt/pbo/enabled is set to true.
A default value of 25 is recommended for most use-cases.
Must be positive.

trajopt/pbo/num_iterations [int]

Number of iterations to run PBO in the trajectory optimization problem, when enabled.
This parameter controls how many optimization iterations PBO performs. Higher values allow for more thorough optimization but increase computational cost.
This parameter is only used when trajopt/pbo/enabled is set to true.
A default value of 5 is recommended for most use-cases.
Must be positive.

trajopt/pbo/cost/path_position_error_penalty/weight [double]

Penalty weight applied in task-space position error violations at tool-frame path constraints during PBO trajectory optimization for non-terminal knot points.
Higher values more strongly enforce exact position matching at goal targets.
A default value of 100.0 is recommended for most use-cases.
Must be non-negative.

trajopt/pbo/cost/path_position_error_penalty/activation_distance [double]

Distance from the deviation limit at which task-space path position error penalties activate. This value is subtracted from the deviation limit specified when creating the path position constraint (e.g., TranslationConstraint::LinearPathConstraint()) during PBO trajectory optimization.
If the activation distance is greater than the deviation limit (which is often zero), all non-zero position errors will be penalized, encouraging solutions that have as little error as possible.
If the activation distance is less than the deviation limit, any solution with an error less than deviation_limit - activation_distance will have equal cost. Most solutions will have a final error of deviation_limit - activation_distance, instead of an error near zero.
Units are in meters.
A default value of 0.01 is recommended for most use-cases.
Must be non-negative.

trajopt/pbo/cost/path_orientation_error_penalty/weight [double]

Penalty weight applied in task-space path orientation error violations at tool-frame path constraints during PBO trajectory optimization.
Higher values more strongly enforce exact orientation matching at goal targets.
A default value of 100.0 is recommended for most use-cases.
Must be non-negative.

trajopt/pbo/cost/path_orientation_error_penalty/activation_distance [double]

Angular distance from the deviation limit at which task-space path orientation error penalties activate. This value is subtracted from the deviation limit specified when creating the path orientation constraint (e.g., OrientationConstraint::TerminalAndPathTarget()) during PBO trajectory optimization.
If the activation distance is greater than the deviation limit (which is often zero), all non-zero orientation errors will be penalized, encouraging solutions that have as little angular error as possible.
If the activation distance is less than the deviation limit, any solution with an error less than deviation_limit - activation_distance will have equal cost. Most solutions will have a final angular error of deviation_limit - activation_distance, instead of an error near zero.
Units are in radians.
A default value of 0.01 is recommended for most use-cases.
Must be non-negative.

trajopt/pbo/cost/cspace_position_limit_penalty/weight [double]

Penalty weight applied in c-space position limit violations during PBO trajectory optimization.
It discourages trajectories that violate joint limits.
A default value of 10.0 is recommended for most use-cases.
Must be non-negative.

trajopt/pbo/cost/cspace_position_limit_penalty/activation_distance [double]

Distance threshold from c-space position limits for PBO trajectory optimization penalties.
Units correspond to rad/s for revolute joints, m/s for prismatic joints.
A default value of 0.0 (no activation distance) is used.
Must be non-negative.

trajopt/pbo/cost/cspace_velocity_limit_penalty/weight [double]

Penalty weight applied in c-space velocity limit violations during PBO trajectory optimization.
It discourages trajectories with velocities that exceed joint limits.
A default value of 0.0 (disabled) is used.
Must be non-negative.

trajopt/pbo/cost/cspace_velocity_limit_penalty/activation_distance [double]

Distance threshold from velocity limits for PBO trajectory optimization penalties.
Units correspond to rad/s for revolute joints, m/s for prismatic joints.
A default value of 0.0 is used.
Must be non-negative.

trajopt/pbo/cost/cspace_acceleration_limit_penalty/weight [double]

Penalty weight applied in c-space acceleration limit violations during PBO trajectory optimization.
It discourages trajectories with accelerations that exceed joint limits.
A default value of 0.0 (disabled) is used.
Must be non-negative.

trajopt/pbo/cost/cspace_acceleration_limit_penalty/activation_distance [double]

Distance threshold from acceleration limits for PBO trajectory optimization penalties.
Units correspond to rad/s^2 for revolute joints, m/s^2 for prismatic joints.
A default value of 0.0 is used.
Must be non-negative.

trajopt/pbo/cost/cspace_jerk_limit_penalty/weight [double]

Penalty weight applied in c-space jerk limit violations during PBO trajectory optimization.
It discourages trajectories with jerks that exceed joint limits.
A default value of 0.0 (disabled) is used.
Must be non-negative.

trajopt/pbo/cost/cspace_jerk_limit_penalty/activation_distance [double]

Distance threshold from jerk limits for PBO trajectory optimization penalties.
Units correspond to rad/s^3 for revolute joints, m/s^3 for prismatic joints.
A default value of 0.0 is used.
Must be non-negative.

trajopt/pbo/cost/cspace_acceleration_smoothing_cost [double]

Weight used to encourage smooth trajectories during PBO trajectory optimization by penalizing c-space acceleration.
Higher values encourage smoother trajectories by penalizing non-zero accelerations.
A default value of 10.0 is recommended for most use-cases.
Must be non-negative.

trajopt/pbo/cost/cspace_jerk_smoothing_cost [double]

Weight applied to minimize c-space jerk (smoothness regularization) during PBO trajectory optimization.
Higher values encourage smoother trajectories by penalizing non-zero jerk values.
A default value of 0.0 (disabled) is used.
Must be non-negative.

trajopt/pbo/cost/self_collision_penalty/weight [double]

Penalty weight applied in self-colliding robot configurations during PBO trajectory optimization.
This parameter is only used when enable_self_collision is true.
A default value of 1000.0 is recommended for most use-cases.
Must be non-negative.

trajopt/pbo/cost/self_collision_penalty/activation_distance [double]

Distance threshold at which the self-collision penalty activates during PBO trajectory optimization.
The penalty becomes non-zero when robot collision geometries are within this distance of each other.
Units are in meters.
A default value of 0.01 m (1 cm) is recommended for most use-cases.
Must be non-negative.

trajopt/pbo/cost/world_collision_penalty/weight [double]

Penalty weight applied in world-colliding robot configurations during PBO trajectory optimization.
This parameter is only used when enable_world_collision is true.
A default value of 1000.0 is recommended for most use-cases.
Must be non-negative.

trajopt/pbo/cost/world_collision_penalty/activation_distance [double]

Distance threshold at which the world collision penalty activates during PBO trajectory optimization.
The penalty becomes non-zero when robot collision geometries are within this distance of obstacles.
Units are in meters.
A default value of 0.01m (1cm) is recommended for most use-cases.
Must be non-negative.

trajopt/pbo/cost/world_collision_max_sweep_steps [int]

Maximum number of discrete sampling steps when “sweeping” collision spheres between trajectory knot points during PBO optimization. “Sweeping” samples sphere positions at intervals along the path between knot points to detect collisions that might be missed if only checking at the knot points themselves.
Higher values provide more accurate collision detection but increase computational cost.
This parameter is only used when enable_world_collision is true.
A default value of 5 is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/path_position_error_penalty/weight [double]

Penalty weight applied in task-space position error violations at tool-frame path constraints during L-BFGS trajectory optimization for non-terminal knot points.
Higher values more strongly enforce exact position matching at goal targets.
A default value of 100.0 is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/path_position_error_penalty/activation_distance [double]

Distance from the deviation limit at which task-space path position error penalties activate. This value is subtracted from the deviation limit specified when creating the path position constraint (e.g., TranslationConstraint::LinearPathConstraint()) during L-BFGS trajectory optimization.
If the activation distance is greater than the deviation limit (which is often zero), all non-zero position errors will be penalized, encouraging solutions that have as little error as possible.
If the activation distance is less than the deviation limit, any solution with an error less than deviation_limit - activation_distance will have equal cost. Most solutions will have a final error of deviation_limit - activation_distance, instead of an error near zero.
Units are in meters.
A default value of 0.01 is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/path_orientation_error_penalty/weight [double]

Penalty weight applied in task-space path orientation error violations at tool-frame path constraints during L-BFGS trajectory optimization.
Higher values more strongly enforce exact orientation matching at goal targets.
A default value of 100.0 is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/path_orientation_error_penalty/activation_distance [double]

Angular distance from the deviation limit at which task-space path orientation error penalties activate. This value is subtracted from the deviation limit specified when creating the path orientation constraint (e.g., OrientationConstraint::TerminalAndPathTarget()) during L-BFGS trajectory optimization.
If the activation distance is greater than the deviation limit (which is often zero), all non-zero orientation errors will be penalized, encouraging solutions that have as little angular error as possible.
If the activation distance is less than the deviation limit, any solution with an error less than deviation_limit - activation_distance will have equal cost. Most solutions will have a final angular error of deviation_limit - activation_distance, instead of an error near zero.
Units are in radians.
A default value of 0.01 is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/cspace_position_limit_penalty/weight [double]

Penalty weight applied in c-space position limit violations during L-BFGS trajectory optimization.
Higher values more strongly discourage trajectories that violate joint limits.
A default value of 10.0 is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/cspace_position_limit_penalty/activation_distance [double]

Distance threshold from c-space position limits for L-BFGS trajectory optimization penalties.
Units correspond to rad/s for revolute joints, m/s for prismatic joints.
A default value equivalent to 5 degrees is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/cspace_velocity_limit_penalty/weight [double]

Penalty weight applied in c-space velocity limit violations during L-BFGS trajectory optimization.
Higher values more strongly discourage trajectories with excessive joint velocities.
A default value of 10.0 is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/cspace_velocity_limit_penalty/activation_distance [double]

Distance threshold from velocity limits for L-BFGS trajectory optimization penalties.
Units correspond to rad/s for revolute joints and m/s for prismatic joints.
A default value equivalent to 5 degrees/s is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/cspace_acceleration_limit_penalty/weight [double]

Penalty weight applied in c-space acceleration limit violations during L-BFGS trajectory optimization.
Higher values more strongly discourage trajectories with excessive joint accelerations.
A default value of 10.0 is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/cspace_acceleration_limit_penalty/activation_distance [double]

Distance threshold from acceleration limits for L-BFGS trajectory optimization penalties.
Units correspond to rad/s^2 for revolute joints and m/s^2 for prismatic joints.
A default value equivalent to 5 degrees/s^2 is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/cspace_jerk_limit_penalty/weight [double]

Penalty weight applied in c-space jerk limit violations during L-BFGS trajectory optimization.
Higher values more strongly discourage trajectories with excessive joint jerk.
A default value of 10.0 is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/cspace_jerk_limit_penalty/activation_distance [double]

Distance threshold from jerk limits for L-BFGS trajectory optimization penalties.
Units correspond to rad/s^3 for revolute joints and m/s^3 for prismatic joints.
A default value equivalent to 5 degrees/s^3 is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/cspace_acceleration_smoothing_cost [double]

Weight applied to minimize c-space acceleration (smoothness regularization) during L-BFGS trajectory optimization.
Higher values encourage smoother trajectories by penalizing large accelerations.
A default value of 0.1 is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/cspace_jerk_smoothing_cost [double]

Weight applied to minimize c-space jerk (smoothness regularization) during L-BFGS trajectory optimization.
Higher values encourage smoother trajectories by penalizing large jerk values.
A default value of 0.01 is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/self_collision_penalty/weight [double]

Penalty weight applied in self-colliding robot configurations during L-BFGS trajectory optimization.
This parameter is only used when enable_self_collision is true.
A default value of 1000.0 is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/self_collision_penalty/activation_distance [double]

Distance threshold at which the self-collision penalty activates during L-BFGS trajectory optimization.
The penalty becomes non-zero when robot collision geometries are within this distance of each other.
Units are in meters.
A default value of 0.01 m (1 cm) is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/world_collision_penalty/weight [double]

Penalty weight applied in world-colliding robot configurations during L-BFGS trajectory optimization.
This parameter is only used when enable_world_collision is true.
A default value of 1000.0 is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/world_collision_penalty/activation_distance [double]

Distance threshold at which the world-collision penalty activates during L-BFGS trajectory optimization.
The penalty becomes non-zero when robot collision geometries are within this distance of obstacles.
Units are in meters.
A default value of 0.01 m (1 cm) is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/cost/world_collision_max_sweep_steps [int]

Maximum number of discrete sampling steps when “sweeping” collision spheres between trajectory knot points during L-BFGS optimization. “Sweeping” samples sphere positions at intervals along the path between knot points to detect collisions that might be missed if only checking at the knot points themselves.
Higher values provide more accurate collision detection but increase computational cost.
This parameter is only used when enable_world_collision is true.
A default value of 5 is recommended for most use-cases.
Must be non-negative.

trajopt/lbfgs/initial_iterations [int]

Number of L-BFGS iterations used to solve the initial trajectory optimization problem.
A smaller number of iterations will result in a more coarse solution used when retiming the trajectory.
The default value is a reasonable starting point, but performance improvements may be found by tuning this parameter.
A value of 0 will result in the initial retiming attempt using the output of the particle-based optimizer.
Default value: 300
Must be non-negative.

trajopt/lbfgs/retiming_initial_iterations [int]

Number of L-BFGS iterations used to solve the 2nd trajectory optimization problem, after the initial solutions are retimed using trajopt/timestep_scaling_factor to minimize the trajectory time by saturating the kinematic limits.
A smaller number of iterations will result in a more coarse solution used when retiming solution used when retiming the trajectory.
The default value is a reasonable starting point, but performance improvements may be found by tuning this parameter.
Default value: 400
Must be positive.

trajopt/lbfgs/retiming_iterations [int]

Number of L-BFGS iterations used to solve the trajectory optimization problem after the initial solve and first retiming attempt, after the previous solution is scaled by trajopt/timestep_scaling_factor to minimize trajectory time by saturating the kinematic limits.
A smaller number of iterations will result in a more coarse final solution, including the possibility of failing to find a valid solution where one would have been found with a greater number of iterations.
A larger number of iterations will require more compute time, but may provide a higher-quality final trajectory (typically meaning smoother kinematic derivatives).
The default value is a reasonable starting point, but performance improvements may be found by tuning this parameter.
Default value: 100
Must be positive.

trajopt/num_geometric_planning_attempts [int]

Number of attempts that will be made to use a geometric planner to generate a feasible path to the target as a fallback when standard trajectory optimization fails. Geometric planning is, in general, computationally expensive relative to trajectory optimization, but it can be necessary in complex environments.
A default value of 4 is used.
Must be non-negative.

trajopt/geometric_planning/max_iterations [int]

Maximum number of iterations for the geometric planner per attempt.
Higher values increase the likelihood of finding a solution in complex environments but increase the maximum runtime of each geometric planning attempt.
Default value: 10000
Must be positive.

trajopt/initial_timestep_scaling_factor [double]

Scales the saturating timestep calculated on the initial trajectory seeds.
Smaller values result in shorter initial trajectories where the derivative limits are violated for a greater percentage of the trajectory.
A value of 1.0 or more will produce an initial trajectory that does not violate any kinematic derivatives along the trajectory.
The default value is a reasonable starting point, but performance improvements may be found by tuning this parameter.
Should typically be less than 1.0, since values greater than 1.0 will undersaturate the derivative limits, leading to longer trajectory times.
Default value: 0.85
Must be positive.

trajopt/timestep_scaling_factor [double]

Scales the saturating timestep calculated on the retimed trajectory seeds (after the initial solve).
Smaller values result in shorter retimed trajectories where the derivative limits are violated for a greater percentage of the trajectory.
A value of 1.0 or more will produce a retimed trajectory that does not violate any kinematic derivatives along the trajectory.
The default value is a reasonable starting point, but performance improvements may be found by tuning this parameter.
Should typically be less than 1.0, since values greater than 1.0 will undersaturate the derivative limits, leading to longer trajectory times.
Default value: 0.85
Must be positive.

trajopt/num_retiming_iterations [int]

Number of times the trajectory is retimed after the initial solve.
A smaller number of iterations may result in a shorter planning time and a longer final trajectory time. A higher number of iterations may result in a shorter final trajectory time.
The optimizer may exit early if no trajectories are valid after retiming.
The default value is a reasonable starting point, but performance improvements may be found by tuning this parameter.
A value of 0 will disable retiming and the trajectory will use the timestep calculated by saturating the kinematic limits of the initial solution.
Default value: 5
Must be non-negative.

struct ParamValue#

Specify the value for a given parameter.

The required ParamValue constructor for each param is detailed in the documentation for setParam().

Public Functions

ParamValue(int value)#: Create ParamValue from int.

ParamValue(double value)#: Create ParamValue from double.

ParamValue(bool value)#: Create ParamValue from bool.

ParamValue(const std::vector<double> &value)#: Create ParamValue from std::vector<double>.

std::unique_ptr<TrajectoryOptimizerConfig> cumotion::CreateTrajectoryOptimizerConfigFromFile( const std::filesystem::path &trajectory_optimizer_config_file, const RobotDescription &robot_description, const std::string &tool_frame_name, const WorldViewHandle &world_view, )#

Load a set of TrajectoryOptimizer configuration parameters from file.

These parameters are combined with robot_description, tool_frame_name, and world_view to create a configuration for trajectory optimization.

Default values will be used for any parameters not specified in trajectory_optimizer_config_file.

A configuration will NOT be created if:

trajectory_optimizer_config_file path is invalid or empty,
trajectory_optimizer_config_file points to an invalid YAML file,
trajectory_optimizer_config_file contains invalid contents (e.g., parameters, version),
robot_description is invalid, OR
tool_frame_name is not a valid frame in robot_description.

In the case of failure, a non-fatal error will be logged and a nullptr will be returned.

std::unique_ptr<TrajectoryOptimizerConfig> cumotion::CreateDefaultTrajectoryOptimizerConfig( const RobotDescription &robot_description, const std::string &tool_frame_name, const WorldViewHandle &world_view, )#

Use default parameters to create a configuration for trajectory optimization.

These default parameters are combined with robot_description, tool_frame_name, and world_view.

A configuration will NOT be created if:

robot_description is invalid, OR
tool_frame_name is not a valid frame in robot_description.

In the case of failure, a non-fatal error will be logged and a nullptr will be returned.

class TrajectoryOptimizer#

Interface for using numerical optimization to generate collision-free trajectories for a robot.

See documentation for corresponding Python class.

Public Functions

virtual std::unique_ptr<Results> planToTaskSpaceTarget( const Eigen::VectorXd &initial_cspace_position, const TaskSpaceTarget &task_space_target, ) const = 0#: Attempt to find a trajectory from initial_cspace_position to task_space_target.

virtual std::unique_ptr<Results> planToTaskSpaceGoalset( const Eigen::VectorXd &initial_cspace_position, const TaskSpaceTargetGoalset &task_space_target_goalset, ) const = 0#: Attempt to find a trajectory from initial_cspace_position to task_space_target_goalset.

virtual std::unique_ptr<Results> planToCSpaceTarget( const Eigen::VectorXd &initial_cspace_position, const CSpaceTarget &cspace_target, ) const = 0#: Attempt to find a trajectory from initial_cspace_position to cspace_target.

class CSpaceTarget#

C-space targets fully restrict the c-space configuration at the termination of the trajectory while allowing (optional) task-space constraints along the path.

Public Functions

explicit CSpaceTarget( const Eigen::VectorXd &cspace_position_terminal_target, const TranslationPathConstraint &translation_path_constraint = TranslationPathConstraint::None(), const OrientationPathConstraint &orientation_path_constraint = OrientationPathConstraint::None(), )#: Create a c-space target.

class OrientationPathConstraint#

Orientation path constraints restrict the orientation of a tool frame along the path.

Public Static Functions

static OrientationPathConstraint None()#: Create a OrientationPathConstraint such that the orientation of the tool frame is not restricted along the path.

static OrientationPathConstraint Constant( const double *path_deviation_limit = nullptr, bool use_terminal_orientation = true, )#

Create an OrientationPathConstraint such that the tool frame orientation along the path is constant.

If use_terminal_orientation is set to true, then the tool frame orientation corresponding to the terminal c-space target will be used to define the orientation target along the path. Otherwise, the tool frame orientation corresponding to the initial c-space position will be used.

The optional parameter path_deviation_limit can be used to allow deviations from this constant orientation along the path. If input, the units are radians. These limits specify the maximum allowable deviation from the desired orientation.

If the path_deviation_limit is NOT input, its value is set to the most restrictive feasible value. This feasible limit is determined by the angular distance between the tool frame orientations at the initial and terminal c-space positions. If the path_deviation_limit is input, it must be greater than or equal to this angular distance.

In general, it is suggested that angular deviation limits are set to values less than pi. A limit that is near or greater than pi essentially disables the constraint (without culling the computation). Non-fatal warnings will be logged if:

path_deviation_limit is near or greater than pi.

A fatal error will be logged if:

path_deviation_limit is negative, OR
path_deviation_limit is infeasible for the given initial c-space position and terminal c-space target.

NOTE: Condition [3] will only be validated when the resulting OrientationPathConstraint is used for trajectory optimization (i.e., it is used as input to planToCSpaceTarget()).

static OrientationPathConstraint Axis( const Eigen::Vector3d &tool_frame_axis, const Eigen::Vector3d &world_target_axis, const double *path_axis_deviation_limit = nullptr, )#

Create an OrientationPathConstraint such that the tool frame orientation is constrained to rotate about a “free axis” along the path.

The “free axis” for rotation is defined by a tool_frame_axis (specified in the tool frame coordinates) and a corresponding world_target_axis (specified in world frame coordinates).

The optional parameter path_axis_deviation_limit can be used to allow deviation from the desired axis alignment along the path. If input, the units are radians.

If the path_axis_deviation_limit is NOT input, its value is set to the most restrictive feasible value. This feasibility is determined by the tool frame orientations at the initial and terminal c-space positions. For each orientation, the angular distance (if any) from satisfying the constraint will be computed. The path_axis_deviation_limit will be set to the maximum of these angular distances. If the path_axis_deviation_limit is input, it must be greater than or equal to this minimum feasible deviation limit.

In general, it is suggested that angular deviation limits are set to values less than pi. A limit that is near or greater than pi essentially disables the constraint (without culling the computation). Non-fatal warnings will be logged if:

path_axis_deviation_limit near or greater than pi.

A fatal error will be logged if:

path_axis_deviation_limit is negative,
path_axis_deviation_limit is infeasible for the given initial c-space position and terminal c-space target,
tool_frame_axis is (nearly) zero, OR
world_target_axis is (nearly) zero.

NOTE: Condition [3] will only be validated when the resulting OrientationPathConstraint is used for trajectory optimization (i.e., it is used as input to planToCSpaceTarget()).

NOTE: tool_frame_axis and world_target_axis inputs will be normalized.

class TranslationPathConstraint#

Translation path constraints restrict the position of the origin of a tool frame along the path.

Public Static Functions

static TranslationPathConstraint None()#: Create a TranslationPathConstraint such that the position of the tool frame is not restricted along the path.

static TranslationPathConstraint Linear( const double *path_deviation_limit = nullptr, )#

Create a TranslationPathConstraint such that a linear translation constraint is active along the path.

The linear path constraint is defined by the line segment between the origin of the tool frame at the initial c-space position and the origin of the tool frame at the terminal c-space target. This path constraint is satisfied if the origin of the tool frame is on this line segment between the initial and final tool positions at all points in the trajectory. It is considered a constraint violation if the origin of the tool frame “overshoots” either the initial or final tool positions.

If the distance between the initial and final tool frames are nearly the same, the path constraint becomes a constraint on the distance from the initial and final positions in any direction.

The optional parameter path_deviation_limit can be used to allow deviation from the nominal task-space line along the path. This limit specifies the maximum allowable deviation from the desired linear path.

If path_deviation_limit is not input, then the deviation limit is assumed to be zero (i.e., it is desired that translation is always exactly coincident with the specified line).

A fatal error will be logged if:

path_deviation_limit is negative.

class OrientationConstraint#

Orientation constraints restrict the orientation of a tool frame.

These constraints may be active at the terminal point of the trajectory and/or along the path. Each constraint may fully or partially constrain the orientation.

Public Static Functions

static OrientationConstraint None()#: Create an OrientationConstraint such that no tool frame orientation constraints are active along the path OR at termination.

static OrientationConstraint Constant( const double *path_deviation_limit = nullptr, const double *terminal_deviation_limit = nullptr, )#

Create an OrientationConstraint such that the tool frame orientation along the path AND at termination are constrained to the tool frame orientation at the initial c-space position.

The optional parameters path_deviation_limit and terminal_deviation_limit can be used to allow deviations from this constant orientation along the path and at termination respectively. If input, the limits are expressed in radians. These limits specify the maximum allowable deviation from the desired orientation.

If both limits are input, the path_deviation_limit must be greater than or equal to (i.e., no more restrictive than) the terminal_deviation_limit.

If neither is input, then both deviation limits are assumed to be zero, indicating that orientation should be exactly constant.

It is valid to input a path_deviation_limit without a terminal_deviation_limit (implicitly setting the latter to zero) but not to input a terminal_deviation_limit without a path_deviation_limit. This ensures that the terminal constraint is more restrictive than the corresponding path constraint.

In general, it is suggested that angular deviation limits are set to values less than pi. A limit that is near or greater than pi essentially disables the constraint (without culling the computation). Non-fatal warnings will be logged if:

path_deviation_limit is near or greater than pi, OR
terminal_deviation_limit is near or greater than pi.

A fatal error will be logged if:

path_deviation_limit is negative,
terminal_deviation_limit is negative,
both path_deviation_limit and terminal_deviation_limit are defined, but path_deviation_limit < terminal_deviation_limit, OR
terminal_deviation_limit is defined without defining a path_deviation_limit.

static OrientationConstraint TerminalTarget( const Rotation3 &orientation_target, const double *terminal_deviation_limit = nullptr, )#

Create an OrientationConstraint such that a tool frame orientation_target is specified at termination, but no orientation constraints are active along the path.

The optional parameter terminal_deviation_limit can be used to allow deviation from the orientation_target at termination. If input, terminal_deviation_limit is expressed in radians. This limit specifies the maximum allowable deviation from the desired orientation.

If terminal_deviation_limit is not input, then the deviation limit is assumed to be zero (i.e., it is desired that terminal orientation be exactly orientation_target).

In general, it is suggested that angular deviation limits are set to values less than pi. A limit that is near or greater than pi essentially disables the constraint (without culling the computation). Non-fatal warnings will be logged if:

terminal_deviation_limit is near or greater than pi.

A fatal error will be logged if:

terminal_deviation_limit is negative.

NOTE: The orientation_target is specified in world frame coordinates (i.e., relative to the base of the robot).

static OrientationConstraint TerminalAndPathTarget( const Rotation3 &orientation_target, const double *path_deviation_limit = nullptr, const double *terminal_deviation_limit = nullptr, )#

Create an OrientationConstraint such that a tool frame orientation_target is specified along the path AND at termination.

The optional parameters path_deviation_limit and terminal_deviation_limit can be used to allow deviations from the orientation_target along the path and at termination respectively. If input, the limits are expressed in radians. These limits specify the maximum allowable deviation from the desired orientation.

If both limits are input, the path_deviation_limit must be greater than or equal to (i.e., no more restrictive than) the terminal_deviation_limit.

If the path_deviation_limit is NOT input, its value is set to the most restrictive feasible value. This feasibility is determined by the tool frame orientation at the initial c-space position. If the initial orientation exactly matches orientation_target, then the effective path_deviation_limit will be zero. Otherwise, the path_deviation_limit will be set to the angular distance between the initial orientation and orientation_target. If the path_deviation_limit is input, it must be greater than or equal to this angular distance.

If the terminal_deviation_limit is NOT input, its value is set to zero (i.e., the most restrictive feasible value). If terminal_deviation_limit AND path_deviation_limit are input, then terminal_deviation_limit must be less than or equal to path_deviation_limit. If terminal_deviation_limit is specified but path_deviation_limit is auto-computed, then terminal_deviation_limit will be clamped to the minimum of the input value and the auto-computed value of path_deviation_limit.

In general, it is suggested that angular deviation limits are set to values less than pi. A limit that is near or greater than pi essentially disables the constraint (without culling the computation). Non-fatal warnings will be logged if:

path_deviation_limit is near or greater than pi, OR
terminal_deviation_limit is near or greater than pi.

A fatal error will be logged if:

path_deviation_limit is negative,
path_deviation_limit results in an infeasible initial c-space position,
terminal_deviation_limit is negative, OR
terminal_deviation_limit is greater than path_deviation_limit.

NOTE: Condition [3] will only be validated when the resulting OrientationConstraint is used for trajectory optimization (i.e., it is used as input to planToTaskSpaceTarget()).

NOTE: The orientation_target is specified in world frame coordinates (i.e., relative to the base of the robot).

static OrientationConstraint TerminalAxis( const Eigen::Vector3d &tool_frame_axis, const Eigen::Vector3d &world_target_axis, const double *terminal_axis_deviation_limit = nullptr, )#

Create an OrientationConstraint such that the terminal tool frame orientation is constrained to rotate about a “free axis”, but no orientation constraints are active along the path.

The “free axis” for rotation is defined by a tool_frame_axis (specified in the tool frame coordinates) and a corresponding world_target_axis (specified in world frame coordinates).

The optional terminal_axis_deviation_limit can be used to allow deviation from the desired axis alignment at termination. If input, terminal_axis_deviation_limit is expressed in radians and the limit specifies the maximum allowable deviation of the rotation axis at termination. If axis_deviation_limit is not input, then the deviation limit is assumed to be zero (i.e., it is desired that the tool frame axis be exactly aligned with world_target_axis).

In general, it is suggested that angular deviation limits are set to values less than pi. A limit that is near or greater than pi essentially disables the constraint (without culling the computation). Non-fatal warnings will be logged if:

terminal_axis_deviation_limit is near or greater than pi.

A fatal error will be logged if:

terminal_axis_deviation_limit is negative,
tool_frame_axis is (nearly) zero, OR
world_target_axis is (nearly) zero.

NOTE: tool_frame_axis and world_target_axis inputs will be normalized.

static OrientationConstraint TerminalAndPathAxis( const Eigen::Vector3d &tool_frame_axis, const Eigen::Vector3d &world_target_axis, const double *path_axis_deviation_limit = nullptr, const double *terminal_axis_deviation_limit = nullptr, )#

Create an OrientationConstraint such that the tool frame orientation is constrained to rotate about a “free axis” along the path AND at termination.

The “free axis” for rotation is defined by a tool_frame_axis (specified in the tool frame coordinates) and a corresponding world_target_axis (specified in world frame coordinates).

The optional parameters path_axis_deviation_limit and terminal_axis_deviation_limit can be used to allow deviation from the desired axis alignment along the path and at termination respectively. If input, the limits are expressed in radians.

If both limits are input, the path_axis_deviation_limit must be greater than or equal to (i.e., no more restrictive than) the terminal_axis_deviation_limit.

If the path_axis_deviation_limit is NOT input, its value is set to the most restrictive feasible value. This feasibility is determined by the tool frame orientation at the initial c-space position. If the initial orientation exactly satisfies the axis constraint, then the effective path_axis_deviation_limit will be zero. Otherwise, the path_axis_deviation_limit will be set to the angular distance between the initial tool frame rotation axis and the desired tool frame rotation axis. If the path_axis_deviation_limit is input, it must be greater than or equal to this angular distance.

If the terminal_deviation_limit is NOT input, its value is set to zero (i.e., the most restrictive feasible value). If terminal_deviation_limit AND path_deviation_limit are input, then terminal_deviation_limit must be less than or equal to path_deviation_limit. If terminal_deviation_limit is specified but path_deviation_limit is auto-computed, then terminal_deviation_limit will be clamped to the minimum of the input value and the auto-computed value of path_deviation_limit.

In general, it is suggested that angular deviation limits are set to values less than pi. A limit that is near or greater than pi essentially disables the constraint (without culling the computation). Non-fatal warnings will be logged if:

path_axis_deviation_limit is near or greater than pi, OR
terminal_axis_deviation_limit is near or greater than pi.

A fatal error will be logged if:

path_axis_deviation_limit is negative,
path_axis_deviation_limit results in an infeasible initial c-space position,
terminal_axis_deviation_limit is negative,
terminal_axis_deviation_limit is > to path_deviation_limit,
tool_frame_axis is (nearly) zero, OR
world_target_axis is (nearly) zero.

NOTE: Condition [3] will only be validated when the resulting OrientationConstraint is used for trajectory optimization (i.e., it is used as input to planToTaskSpaceTarget()).

NOTE: tool_frame_axis and world_target_axis inputs will be normalized.

static OrientationConstraint TerminalTargetAndPathAxis( const Rotation3 &terminal_orientation_target, const Eigen::Vector3d &tool_frame_axis, const Eigen::Vector3d &world_target_axis, const double *terminal_deviation_limit = nullptr, const double *path_axis_deviation_limit = nullptr, )#

Create an OrientationConstraint such that a tool frame terminal_orientation_target is specified at termination, AND the tool frame orientation is constrained to rotate about a “free axis” along the path.

The optional parameter terminal_deviation_limit can be used to allow deviation from the orientation_target at termination. If input, terminal_deviation_limit is expressed in radians. This limit specifies the maximum allowable deviation from the desired orientation.

If terminal_deviation_limit is not input, then the deviation limit is assumed to be zero (i.e., it is desired that terminal orientation be exactly terminal_orientation_target).

The “free axis” for rotation along the path is defined by a tool_frame_axis (specified in the tool frame coordinates) and a corresponding world_target_axis (specified in world frame coordinates).

The optional path_axis_deviation_limit can be used to allow deviation from the desired axis alignment along the path. If input, path_axis_deviation_limit is expressed in radians. This limit specifies the maximum allowable deviation of the rotation axis along the path.

If the path_axis_deviation_limit is NOT input, its value is set to the most restrictive feasible value. This feasibility is determined by the tool frame orientation at the initial c-space position. If the initial orientation exactly satisfies the axis constraint, then the effective path_axis_deviation_limit will be zero. Otherwise, the path_axis_deviation_limit will be set to the angular distance between the initial tool frame rotation axis and the desired tool frame rotation axis. If the path_axis_deviation_limit is input, it must be greater than or equal to this angular distance.

In general, it is suggested that angular deviation limits are set to values less than pi. A limit that is near or greater than pi essentially disables the constraint (without culling the computation). Non-fatal warnings will be logged if:

path_axis_deviation_limit is near or greater than pi, OR
terminal_deviation_limit is near or greater than pi.

A fatal error will be logged if:

terminal_deviation_limit is negative,
path_axis_deviation_limit is negative,
path_axis_deviation_limit results in an infeasible initial c-space position,
tool_frame_axis is (nearly) zero, OR
world_target_axis is (nearly) zero.

NOTE: Condition [5] will only be validated when the resulting OrientationConstraint is used for trajectory optimization (i.e., it is used as input to planToTaskSpaceTarget()).

NOTE: tool_frame_axis and world_target_axis inputs will be normalized.

NOTE: The terminal_orientation_target is specified in world frame coordinates (i.e., relative to the base of the robot).

class OrientationConstraintGoalset#

Variant of OrientationConstraint for “goalset” planning.

For goalset planning, a set of OrientationConstraints are considered concurrently. Each OrientationConstraint

in the goalset must have the same mode (e.g., “full terminal target

with free axis path constraint”), but may have different data for each

OrientationConstraint.

Public Static Functions

static OrientationConstraintGoalset None()#: Create an OrientationConstraintGoalset such that no tool frame orientation constraints are active along the path OR at termination.

static OrientationConstraintGoalset Constant( const double *path_deviation_limit = nullptr, const double *terminal_deviation_limit = nullptr, )#

Create an OrientationConstraintGoalset such that the tool frame orientation along the path AND at termination are constrained to the tool frame orientation at the initial c-space position.

See OrientationConstraint::Constant() for details.

A fatal error will be logged if:

Any condition of OrientationConstraint::Constant() is not met.

static OrientationConstraintGoalset TerminalTarget( const std::vector<Rotation3> &orientation_targets, const double *terminal_deviation_limit = nullptr, )#

Create an OrientationConstraintGoalset such that tool frame orientation_targets are specified at termination, but no orientation constraints are active along the path.

See OrientationConstraint::TerminalTarget() for details.

A fatal error will be logged if:

Any condition of OrientationConstraint::TerminalTarget() is not met, OR
orientation_targets is empty.

static OrientationConstraintGoalset TerminalAndPathTarget( const std::vector<Rotation3> &orientation_targets, const double *path_deviation_limit = nullptr, const double *terminal_deviation_limit = nullptr, )#

Create an OrientationConstraintGoalset such that tool frame orientation_targets are specified along the path AND at termination.

See OrientationConstraint::TerminalAndPathTarget() for details.

A fatal error will be logged if:

Any condition of OrientationConstraint::TerminalAndPathTarget() is not met, OR
orientation_targets is empty.

static OrientationConstraintGoalset TerminalAxis( const std::vector<Eigen::Vector3d> &tool_frame_axes, const std::vector<Eigen::Vector3d> &world_target_axes, const double *terminal_axis_deviation_limit = nullptr, )#

Create an OrientationConstraintGoalset such that the terminal tool frame orientation is constrained to rotate about a “free axis”, but no orientation constraints are active along the path.

See OrientationConstraint::TerminalAxis() for details.

A fatal error will be logged if:

Any condition of OrientationConstraint::TerminalAxis() is not met,
tool_frame_axes and world_target_axes do not have the same number of elements, OR
tool_frame_axes and world_target_axes are empty.

static OrientationConstraintGoalset TerminalAndPathAxis( const std::vector<Eigen::Vector3d> &tool_frame_axes, const std::vector<Eigen::Vector3d> &world_target_axes, const double *path_axis_deviation_limit = nullptr, const double *terminal_axis_deviation_limit = nullptr, )#

Create an OrientationConstraintGoalset such that the tool frame orientation is constrained to rotate about a “free axis” along the path AND at termination.

See OrientationConstraint::TerminalAndPathAxis() for details.

A fatal error will be logged if:

Any condition of OrientationConstraint::TerminalAndPathAxis() is not met,
tool_frame_axes and world_target_axes do not have the same number of elements, OR
tool_frame_axes and world_target_axes are empty.

static OrientationConstraintGoalset TerminalTargetAndPathAxis( const std::vector<Rotation3> &terminal_orientation_targets, const std::vector<Eigen::Vector3d> &tool_frame_axes, const std::vector<Eigen::Vector3d> &world_target_axes, const double *terminal_deviation_limit = nullptr, const double *path_axis_deviation_limit = nullptr, )#

Create an OrientationConstraintGoalset such that tool frame terminal_orientation_targets are specified at termination, AND the tool frame orientation is constrained to rotate about a “free axis” along the path.

See OrientationConstraint::TerminalTargetAndPathAxis() for details.

A fatal error will be logged if:

Any condition of OrientationConstraint::TerminalTargetAndPathAxis() is not met,
terminal_orientation_targets, tool_frame_axes, and world_target_axes do not have the same number of elements, OR
terminal_orientation_targets, tool_frame_axes, and world_target_axes are empty.

class Results#

Results from a trajectory optimization.

Public Types

enum class Status#

Indicate the success or failure of the trajectory optimization.

Values:

enumerator SUCCESS#: Valid trajectory found.

enumerator INVALID_INITIAL_CSPACE_POSITION#

Invalid initial c-space position.

  NOTE: The `RobotWorldInspector` can be used to determine if this invalid state is
        due to world-collision, self-collision, c-space position limit violations, etc.

enumerator INVALID_TARGET_SPECIFICATION#: The c-space or task-space target specification is invalid.

enumerator INVERSE_KINEMATICS_FAILURE#: The inverse kinematics solver failed to find a valid solution.

enumerator GEOMETRIC_PLANNING_FAILURE#: Initial trajectory optimization attempts failed and geometric planning was attempted as a fallback, but this geometric planning routine also failed.

enumerator TRAJECTORY_OPTIMIZATION_FAILURE#: Initial trajectory optimization attempts failed and geometric planning was attempted as a fallback (and was successful), but secondary trajectory optimization using the geometrically planned path as a warm start failed to produce a valid trajectory.

Public Functions

virtual Status status() const = 0#: Return the Status from the trajectory optimization.

virtual std::unique_ptr<Trajectory> trajectory() const = 0#

If status() returns SUCCESS, then trajectory() returns a valid Trajectory.

If status() returns INVALID_INITIAL_CSPACE_POSITION, INVALID_TARGET_SPECIFICATION, or INVERSE_KINEMATICS_FAILURE, then nullptr will be returned.

If status() returns GEOMETRIC_PLANNING_FAILURE or TRAJECTORY_OPTIMIZATION_FAILURE, then the lowest-cost (but invalid) Trajectory will be returned.

virtual int targetIndex() const = 0#

Return the index of the target selected for the terminal knot point.

For goalset planning (e.g., planToTaskSpaceGoalset()) this represents an index into the constraint vectors used to create the goalset (e.g., TaskSpaceTargetGoalset).

For single target planning (e.g., planToTaskSpaceTarget() and planToCSpaceTarget()), zero will be returned for successful trajectory optimization.

In all cases, -1 will be returned if trajectory optimization is unsuccessful.

struct TaskSpaceTarget#

Task-space targets restrict the position and (optionally) orientation of the tool frame at the termination of a trajectory and (optionally) along the path.

Public Functions

explicit TaskSpaceTarget( const TranslationConstraint &translation_constraint, const OrientationConstraint &orientation_constraint = OrientationConstraint::None(), )#: Create a task-space target.

struct TaskSpaceTargetGoalset#

Variant of TaskSpaceTarget for “goalset” planning.

For goalset planning, a set of pose constraints are considered concurrently. Each pose constraint in the goalset must have the same mode (e.g., “terminal target

with linear path constraint and no orientation constraints”) but may have different data for each constraint.

Public Functions

explicit TaskSpaceTargetGoalset( const TranslationConstraintGoalset &translation_constraints, const OrientationConstraintGoalset &orientation_constraints = OrientationConstraintGoalset::None(), )#

Create a task-space target goalset.

A fatal error will be logged if:

The number of translation_constraints does not match the number of orientation_constraints.

class TranslationConstraint#

Translation constraints restrict the position of the origin of a tool frame.

These constraints are always active at the terminal point of the trajectory; partial constraints may be active along the path.

Public Static Functions

static TranslationConstraint Target( const Eigen::Vector3d &translation_target, const double *terminal_deviation_limit = nullptr, )#

Create a TranslationConstraint such that a translation_target is specified at termination, but no translation constraints are active along the path.

The optional parameter terminal_deviation_limit can be used to allow deviation from the translation_target at termination. This limit specifies the maximum allowable deviation from the desired position.

If terminal_deviation_limit is not input, then the deviation limit is assumed to be zero (i.e., it is desired that terminal translation be exactly translation_target).

A fatal error will be logged if:

terminal_deviation_limit is negative.

NOTE: The translation_target is specified in world frame coordinates (i.e., relative to the base of the robot).

static TranslationConstraint LinearPathConstraint( const Eigen::Vector3d &translation_target, const double *path_deviation_limit = nullptr, const double *terminal_deviation_limit = nullptr, )#

Create a TranslationConstraint such that a translation_target is specified at termination AND a linear translation constraint is active along the path.

The linear path constraint is defined by the line segment between translation_target and the origin of the tool frame at the initial c-space position. This path constraint is satisfied if the origin of the tool frame is on this line segment between the initial and final tool positions at all points in the trajectory. It is considered a constraint violation if the origin of the tool frame “overshoots” either the initial or final positions.

If the distance between the initial and final tool frames are nearly the same, the path constraint becomes a constraint on the distance from the initial and final positions in any direction.

The optional parameters path_deviation_limit and terminal_deviation_limit can be used to allow deviations from the specified line along the path and at the end of the path, respectively.

If both limits are input, the path_deviation_limit must be greater than or equal to (i.e., no more restrictive than) the terminal_deviation_limit.

If neither is input, then both deviation limits are assumed to be zero, indicating that translation should always be exactly coincident with the specified line.

It is valid to input a path_deviation_limit without a terminal_deviation_limit (implicitly setting the latter to zero) but not to input a terminal_deviation_limit without a path_deviation_limit. This ensures that the terminal constraint is more restrictive than the corresponding path constraint.

A fatal error will be logged if:

path_deviation_limit is negative,
terminal_deviation_limit is negative,
both path_deviation_limit and terminal_deviation_limit are defined, but path_deviation_limit < terminal_deviation_limit, OR
terminal_deviation_limit is defined without defining a path_deviation_limit.

NOTE: The translation_target is specified in world frame coordinates (i.e., relative to the base of the robot).

class TranslationConstraintGoalset#

Variant of TranslationConstraint for “goalset” planning.

For goalset planning, a set of TranslationConstraints are considered concurrently. Each TranslationConstraint

in the goalset must have the same mode (e.g., “terminal target

with linear path constraint”) but may have different data for each

TranslationConstraint.

Public Static Functions

static TranslationConstraintGoalset Target( const std::vector<Eigen::Vector3d> &translation_targets, const double *terminal_deviation_limit = nullptr, )#

Create a TranslationConstraintGoalset such that translation_targets are specified at termination, but no translation constraints are active along the path.

See TranslationConstraint::Target() for details.

A fatal error will be logged if:

Any condition of TranslationConstraint::Target() is not met, OR
translation_targets is empty.

static TranslationConstraintGoalset LinearPathConstraint( const std::vector<Eigen::Vector3d> &translation_targets, const double *path_deviation_limit = nullptr, const double *terminal_deviation_limit = nullptr, )#

Create a TranslationConstraintGoalset such that translation_targets are specified at termination AND linear translation constraints are active along the path.

See TranslationConstraint::LinearPathConstraint() for details.

A fatal error will be logged if:

Any condition of TranslationConstraint::LinearPathConstraint() is not met, OR
translation_targets is empty.

std::unique_ptr<TrajectoryOptimizer> cumotion::CreateTrajectoryOptimizer( const TrajectoryOptimizerConfig &config, )#: Create a TrajectoryOptimizer with the given config.

Path Specification#

C-Space Path Specification#

#include <cumotion/cspace_path_spec.h>

class CSpacePathSpec#

The CSpacePathSpec is used to procedurally specify a CSpacePath from a series of configuration space points.

Public Functions

virtual int numCSpaceCoords() const = 0#: Return the number of configuration space coordinates for the path specification.

virtual bool addCSpaceWaypoint(const Eigen::VectorXd &waypoint) = 0#

Add a path to connect the previous configuration to the waypoint.

The waypoint must have dimension equal to numCSpaceCoords() or it will be discarded.

If path segment is successfully added, true is returned. Else, false is returned and an error is logged.

std::unique_ptr<CSpacePathSpec> cumotion::CreateCSpacePathSpec( const Eigen::VectorXd &initial_cspace_position, )#: Create a CSpacePathSpec with the specified initial_cspace_position.

Task-Space Path Specification#

#include <cumotion/task_space_path_spec.h>

class TaskSpacePathSpec#

The TaskSpacePathSpec is used to procedurally specify a TaskSpacePath from a series of continuous task space segments.

Each segment can have position that is: [1] constant, [2] linearly interpolated, [3] or defined by a circular arc. For the case where position follows an arc, the arc can be defined either by a series of three points, or by two endpoints and a constraint that the arc be tangent to the previous segment.

Each segment may have orientation that is: [1] constant (w.r.t. to the fixed global frame), [2] smoothly blended via spherical linear interpolation (i.e., “slerp”), or [3] constant w.r.t. the tangent direction of the position path (i.e., “tangent orientation”). The “tangent orientation” specification is only relevant for path segments for which position is defined by an arc (since for linear position paths, the “tangent orientation” specification would reduce to a constant orientation w.r.t. the fixed global frame).

Public Functions

virtual bool addLinearPath( const Pose3 &target_pose, double blend_radius = 0.0, ) = 0#

Add a path to linearly connect the previous pose to the target_pose.

Position will use linear interpolation and orientation will use “slerp”.

An optional blend_radius can be used to add arc segments between successive segments. This arc segment will be added only if the successive segments both represent a path for which position is linearly interpolated. Moreover, the blend_radius will be capped such that no more than half of either linear segment is replaced by the intermediate arc segment. If blend_radius is <= 0, then no blending will be performed.

If path segment is successfully added, true is returned. Else, false is returned and an error is logged.

virtual bool addTranslation( const Eigen::Vector3d &target_position, double blend_radius = 0.0, ) = 0#

Add a translation-only path to linearly connect the previous pose to the target_position.

Position will use linear interpolation, and orientation will be constant.

An optional blend_radius can be used to add arc segments between successive segments. This arc segment will be added only if the successive segments both represent a path for which position is linearly interpolated. Moreover, the blend_radius will be capped such that no more than half of either linear segment is replaced by the intermediate arc segment. If blend_radius is <= 0, then no blending will be performed.

If path segment is successfully added, true is returned. Else, false is returned and an error is logged.

virtual bool addRotation(const Rotation3 &target_rotation) = 0#

Add a rotation-only path connecting the previous pose to the target_rotation.

Orientation will use “slerp” for interpolation, and position will be constant.

If path segment is successfully added, true is returned. Else, false is returned and an error is logged.

virtual bool addThreePointArc( const Eigen::Vector3d &target_position, const Eigen::Vector3d &intermediate_position, bool constant_orientation = true, ) = 0#

Add a path to connect the previous pose to the target_position along a circular arc that passes through intermediate_position.

Position will follow a “three-point arc” where the previous position and target_position are endpoints and intermediate_position is an intermediate point on the arc.

If constant_orientation is set to true, the orientation will be constant throughout the arc.

If constant_orientation is set to false, the orientation will remain fixed relative to the tangent direction of the arc (i.e., “tangent orientation”; this tangent orientation is defined such that if the angular distance of the arc is N radians, then the change in orientation throughout the arc will be N radians about the normal axis of the arc).

If path segment is successfully added, true is returned. Else, false is returned and an error is logged.

virtual bool addThreePointArcWithOrientationTarget( const Pose3 &target_pose, const Eigen::Vector3d &intermediate_position, ) = 0#

Add a path to connect the previous pose to the target_pose along a circular arc that passes through intermediate_position.

Position will follow a “three-point arc” where the previous position and target_pose.translation are endpoints and intermediate_position is an intermediate point on the arc.

Orientation will use “slerp” for interpolation between the previous orientation and target_pose.rotation.

If path segment is successfully added, true is returned. Else, false is returned and an error is logged.

virtual bool addTangentArc( const Eigen::Vector3d &target_position, bool constant_orientation = true, ) = 0#

Add a path to connect the previous pose to the target_position along a circular arc that is tangent to the previous segment

Position will follow a “tangent arc” where the previous position and target_position are endpoints and the arc is tangent to the previous segment.

It is required that at least one previous segment on the path defines a change in position for a “tangent arc” to be able to be added. If no segments have been added or only rotation segments have been added, then an error will be logged, no “tangent arc” segment will be added, and false will be returned.

If constant_orientation is set to true, the orientation will be constant throughout the arc.

If constant_orientation is set to false, the orientation will remain fixed relative to the tangent direction of the arc (i.e., “tangent orientation”; this tangent orientation is defined such that if the angular distance of the arc is N radians, then the change in orientation throughout the arc will be N radians about the normal axis of the arc).

If path segment is successfully added, true is returned. Else, false is returned and an error is logged.

virtual bool addTangentArcWithOrientationTarget( const Pose3 &target_pose, ) = 0#

Add a path to connect the previous pose to the target_pose along a circular arc that is tangent to the previous segment.

Position will follow a “tangent arc” where the previous position and target_pose.translation are endpoints and the arc is tangent to the previous segment.

It is required that at least one previous segment on the path defines a change in position for a “tangent arc” to be able to be added. If no segments have been added or only rotation segments have been added, then an error will be logged, no “tangent arc” segment will be added, and false will be returned.

Orientation will use “slerp” for interpolation between the previous orientation and target_pose.rotation.

If path segment is successfully added, true is returned. Else, false is returned and an error is logged.

virtual std::unique_ptr<TaskSpacePath> generatePath() const = 0#

Generate a continuous path between all of the procedurally added task space path segments.

The lower bound of the domain of the generated path will be zero and the upper bound will be determined by task space distance.

std::unique_ptr<TaskSpacePathSpec> cumotion::CreateTaskSpacePathSpec( const Pose3 &initial_pose, )#: Create a TaskSpacePathSpec with the specified initial_pose.

Composite Path Specification#

#include <cumotion/composite_path_spec.h>

class CompositePathSpec#

The CompositePathSpec is used to procedurally composite CSpacePathSpec and TaskSpacePathSpec segments into a single path specification.

Public Types

enum class PathSpecType#

Indicate whether a path specification is a TaskSpacePathSpec (i.e., TASK_SPACE) or a CSpacePathSpec (i.e., CSPACE).

Values:

enumerator TASK_SPACE#

enumerator CSPACE#

enum class TransitionMode#

Specify the transition preceding a TaskSpacePathSpec or CSpacePathSpec.

Values:

enumerator SKIP#

Skip either the initial task space pose of the TaskSpacePathSpec or the initial c-space configuration of the CSpacePathSpec.

For a TaskSpacePathSpec, the first task space path segment in the appended TaskSpacePathSpec will instead originate from the current task space pose of the CompositePathSpec.

For a CSpacePathSpec, The first c-space waypoint of the added CSpacePathSpec will be added directly after the current c-space configuration of the CompositePathSpec.

enumerator FREE#: Add a path from the current pose of the CompositePathSpec to the initial task space pose of the TaskSpacePathSpec or the initial c-space configuration of the CSpacePathSpec, with no restrictions on the form of the path.

enumerator LINEAR_TASK_SPACE#

Add a path that is linear in task space from the current task space pose of the CompositePathSpec to the initial task space pose of the TaskSpacePathSpec.

WARNING This mode is ONLY available for adding a TaskSpacePathSpec (via addTaskSpacePathSpec()). Usage with addCSpacePathSpec() will result in an error and the CSpacePathSpec will NOT be added.

Public Functions

virtual int numCSpaceCoords() const = 0#: Return the number of configuration space coordinates for the path specification.

virtual int numPathSpecs() const = 0#: Return the number of path specifications contained in the CompositePathSpec.

virtual PathSpecType pathSpecType(int path_spec_index) const = 0#

Given a path_spec_index in range [0, numPathSpecs()), return the type of the corresponding path specification.

A fatal error will be logged if path_spec_index is not in range [0, numPathSpecs()).

The path_spec_index corresponds to the order in which path specifications are added to the CompositePathSpec.

virtual std::unique_ptr<TaskSpacePathSpec> taskSpacePathSpec( int path_spec_index, ) const = 0#

Return a TaskSpacePathSpec at the given path_spec_index.

If the path_spec_index is invalid (i.e., not in range [0,numPathSpecs())) *OR* the path_spec_indexdoes not correspond to aTaskSpacePathSpec, thennullptr` will be returned and an error will be logged.

virtual std::unique_ptr<CSpacePathSpec> cSpacePathSpec( int path_spec_index, ) const = 0#

Return a CSpacePathSpec at the given path_spec_index.

If the path_spec_index is invalid (i.e., not in range [0,numPathSpecs())) *OR* the path_spec_indexdoes not correspond to aCSpacePathSpec, thennullptr` will be returned and an error will be logged.

virtual TransitionMode transitionMode(int path_spec_index) const = 0#

Given a path_spec_index in range [0, numPathSpecs()), return the corresponding transition mode.

A fatal error will be logged if path_spec_index is not in range [0, numPathSpecs()).

The path_spec_index corresponds to the order in which path specifications are added to the CompositePathSpec.

virtual bool addTaskSpacePathSpec( const TaskSpacePathSpec &path_spec, TransitionMode transition_mode, ) = 0#

Add a task space path_spec to the CompositePathSpec with the specified transition_mode.

Returns true if path specification is successfully added. Else, returns false.

virtual bool addCSpacePathSpec( const CSpacePathSpec &path_spec, TransitionMode transition_mode, ) = 0#

Add a c-space path_spec to the CompositePathSpec with the specified transition_mode.

path_spec will be discarded (with logged error) if it does not have the same number of c-space coordinates as the CompositePathSpec (i.e., numCSpaceCoords()).

path_spec will be discarded (with logged error) if the transition_mode is invalid (i.e., LINEAR_TASK_SPACE).

Returns true if path specification is successfully added. Else, returns false.

std::unique_ptr<CompositePathSpec> cumotion::CreateCompositePathSpec( const Eigen::VectorXd &initial_cspace_position, )#: Create a CompositePathSpec with the specified initial_cspace_position.

Path Specification I/O#

#include <cumotion/path_spec_yaml.h>

std::unique_ptr<CSpacePathSpec> cumotion::LoadCSpacePathSpecFromFile( const std::filesystem::path &cspace_path_spec_file, )#

Load a CSpacePathSpec from file with absolute path cspace_path_spec_file.

The cspace_spec_file is expected to correspond to a YAML file specifying a c-space path. This path specification must include: [1] an initial c-space position, and [2] a series of c-space waypoints following this initial c-space position.

The initial c-space position uses the key “initial c-space position” and is expected to have the following format:

initial c-space position: [#, #, … , #]

where each “#” represents a floating point value. The number of elements in this vector sets the expected number of c-space coordinates for the CSpacePathSpec.

The c-space waypoints are specified as a sequence under the key “waypoints”, with the following format:

waypoints:

[#, #, … , #]
[#, #, … , #]
[#, #, … , #]

where the number of waypoints is variable. Each waypoint must have the same number of c-space coordinates as the “initial c-space position”, or it will be discarded and a warning will be logged.

If “initial c-space position” or “waypoints” are unable to be parsed, nullptr will be returned. If any “waypoints” fail to be parsed, they will be discarded and a warning will be logged, but any other valid “waypoints” will continue to be added to the returned CSpacePathSpec.

std::unique_ptr<CSpacePathSpec> cumotion::LoadCSpacePathSpecFromMemory( const std::string &cspace_path_spec_yaml, )#

Load a CSpacePathSpec from the contents of a YAML file (cspace_path_spec_yaml).

See LoadCSpacePathFromFile() documentation for detailed description of required YAML format.

std::string cumotion::ExportCSpacePathSpecToMemory( const CSpacePathSpec &cspace_path_spec, )#

Export cspace_path_spec as a string.

The returned string will be in YAML format as documented in LoadCSpacePathSpecFromFile().

std::unique_ptr<TaskSpacePathSpec> cumotion::LoadTaskSpacePathSpecFromFile( const std::filesystem::path &task_space_path_spec_file, )#

Load a TaskSpacePathSpec from file with absolute path task_space_path_spec_file.

The task_space_spec_file is expected to correspond to a YAML file specifying a task space path. This path specification must include: [1] an initial task space pose, and [2] a series of path segments emanating from this initial pose.

The initial pose uses the key “initial pose” and is expected to have the following format:

initial pose: position: [#, #, #] orientation: {w: #, xyz: [#, #, #]}

where each “#” represents a floating point value. Position must be a 3d-vector and orientation is represented by a quaternion.

NOTE: Standard YAML formatting is allowed, wherein:

mapped keys can be included in any order (e.g., “position” and “orientation” can be included in either order),
mapped keys can be included on a single comma-separated line with curly braces, or expanded to multiple lines. E.g., “orientation” can be formatted as: orientation: {w: #, xyz: [#, #, #]} or equivalently as: orientation: w: # xyz: [#, #, #]
sequences can be included in a single comma-separated line with square brackets, or expanded into multiple lines with preceding dashes. E.g., “position” can be formatted as: position: [#, #, #] or equivalently as: position:
- #
- #
- #

The path segments are specified as a sequence under the key “path specs”. Each path specification must specify both a “position mode” and a “orientation mode”.

The “position mode” must be: “linear”, “constant”, “three_point_arc”, or “tangent_arc”. The “orientation mode” must be: “slerp”, “constant”, or “tangent”.

WARNING: Not all combinations of “position mode” and “orientation mode” are compatible. Specifically, a “linear” position mode is NOT compatible with a “tangent” orientation mode. A “constant” position mode is ONLY compatible with a “slerp” orientation mode. The two arc position modes (“three_point_arc” and “tangent_arc”) are compatible with all orientation modes.

Each (valid) combination of position and orientation mode requires specific input data:

[1] Linear path:

”linear” position mode with “slerp” orientation mode.
Requires “target pose” and optional “blend radius”.
See TaskSpacePathSpec::addLinearPath() in task_space_path_spec.h for more documentation.
Format:
- position mode: linear orientation mode: slerp target pose: position: [#, #, #] orientation: {w: #, xyz: [#, #, #]} blend radius: # [2] Translation path:
”linear” position mode with “constant” orientation mode.
Requires “target position” and optional “blend radius”.
See TaskSpacePathSpec::addTranslation() in task_space_path_spec.h for more documentation.
Format:
- position mode: linear orientation mode: constant target position: [#, #, #] blend radius: # [3] Rotation path:
”constant” position mode with “slerp” orientation mode.
Requires “target orientation” .
See TaskSpacePathSpec::addRotation() in task_space_path_spec.h for more documentation.
Format:
- position mode: constant orientation mode: slerp target orientation: {w: #, xyz: [#, #, #]} [4] Three-point arc with constant orientation:
”three_point_arc” position mode with “constant” orientation mode.
Requires “target position” and “intermediate position”.
See TaskSpacePathSpec::addThreePointArc() in task_space_path_spec.h for more documentation.
Format:
- position mode: three_point_arc orientation mode: constant target position: [#, #, #] intermediate position: [#, #, #] [5] Three-point arc with tangent orientation:
”three_point_arc” position mode with “tangent” orientation mode.
Requires “target position” and “intermediate position”.
See TaskSpacePathSpec::addThreePointArc() in task_space_path_spec.h for more documentation.
Format:
- position mode: three_point_arc orientation mode: tangent target position: [#, #, #] intermediate position: [#, #, #] [6] Three-point arc with orientation target:
”three_point_arc” position mode with “slerp” orientation mode.
Requires “target pose” and “intermediate position”.
See TaskSpacePathSpec::addThreePointArcWithOrientationTarget() in task_space_path_spec.h for more documentation.
Format:
- position mode: three_point_arc orientation mode: slerp target pose: position: [#, #, #] orientation: {w: #, xyz: [#, #, #]} intermediate position: [#, #, #] [7] Tangent arc with constant orientation:
”tangent_arc” position mode with “constant” orientation mode.
Requires “target position”.
See TaskSpacePathSpec::addTangentArc() in task_space_path_spec.h for more documentation.
Format:
- position mode: tangent_arc orientation mode: constant target position: [#, #, #] [8] Tangent arc with tangent orientation:
”tangent_arc” position mode with “tangent” orientation mode.
Requires “target position”.
See TaskSpacePathSpec::addTangentArc() in task_space_path_spec.h for more documentation.
Format:
- position mode: tangent_arc orientation mode: tangent target position: [#, #, #] [9] Tangent arc with orientation target:
”tangent_arc” position mode with “slerp” orientation mode.
Requires “target pose”.
See TaskSpacePathSpec::addTangentArcWithOrientationTarget() in task_space_path_spec.h for more documentation.
Format:
- position mode: tangent_arc orientation mode: slerp target pose: position: [#, #, #] orientation: {w: #, xyz: [#, #, #]}

If “initial pose” or “path specs” are unable to be parsed, nullptr will be returned. If any “path specs” fail to be parsed, they will be discarded and a warning will be logged, but any other valid “path specs” will continue to be added to the returned TaskSpacePathSpec.

std::unique_ptr<TaskSpacePathSpec> cumotion::LoadTaskSpacePathSpecFromMemory( const std::string &task_space_path_spec_yaml, )#

Load a TaskSpacePathSpec from the contents of a YAML file (task_space_path_spec_yaml).

See LoadTaskSpacePathFromFile() documentation for detailed description of required YAML format.

std::string cumotion::ExportTaskSpacePathSpecToMemory( const TaskSpacePathSpec &task_space_path_spec, )#

Export task_space_path_spec as a string.

The returned string will be in YAML format as documented in LoadTaskSpacePathSpecFromFile().

std::unique_ptr<CompositePathSpec> cumotion::LoadCompositePathSpecFromFile( const std::filesystem::path &composite_path_spec_file, )#

Load a CompositePathSpec from file with absolute path composite_path_spec_file.

The composite_path_spec_file is expected to correspond to a YAML file specifying a path composed of task space and/or c-space segments. This path specification must include: [1] an initial c-space position, and [2] a series of task-space and/or c-space path specifications following this initial c-space position.

The initial c-space position uses the key “initial c-space position” and is expected to have the following format:

initial c-space position: [#, #, … , #]

where each “#” represents a floating point value. The number of elements in this vector sets the expected number of c-space coordinates for the CSpacePathSpec.

The series of path specifications uses the key “path specs” and each spec is required to be labelled as either “c-space” or “task-space”. In addition to including the full specification for the task space or c-space path (see LoadTaskSpacePathSpecFromFile() and LoadCSpacePathSpecFromFile() for details), each path specification must specify a “transition mode” which must be “skip”, “free”, or “linear_task_space”. These modes correspond to the CompositePathSpec::TransitionMode defined in composite_path_spec.h.

An example specification may look like:

initial c-space position: [#, #, … , #] path specs:

c-space: transition mode: skip initial c-space position: [#, #, … , #] waypoints:
- [#, #, … , #]
- [#, #, … , #]
- …
- [#, #, … , #]
task-space: transition mode: linear_task_space initial pose: position: [#, #, #] orientation: { w: #, xyz: [#, #, #] } path specs:
- position mode: linear orientation mode: constant target position: [#, #, #] blend radius: 0.0
- …
…

where any combination of task space and c-space path specifications may be included. The order of the path specifications in the YAML file dictates the order in which they are added to the CompositePathSpec.

If “initial c-space position” or “path specs” are unable to be parsed, nullptr will be returned. If any “path specs” fail to be parsed, they will be discarded and a warning will be logged, but any other valid “path specs” will continue to be added to the returned CompositePathSpec.

std::unique_ptr<CompositePathSpec> cumotion::LoadCompositePathSpecFromMemory( const std::string &composite_path_spec_yaml, )#

Load a CompositePathSpec from the contents of a YAML file (composite_path_spec_yaml).

See LoadCompositePathFromFile() documentation for detailed description of required YAML format.

std::string cumotion::ExportCompositePathSpecToMemory( const CompositePathSpec &composite_path_spec, )#

Export composite_path_spec as a string.

The returned string will be in YAML format as documented in LoadCompositePathSpecFromFile().

Path Generation (Collision-Unaware)#

C-Space Path#

#include <cumotion/cspace_path.h>

class CSpacePath#

Represent a path through configuration space (i.e., c-space or “joint space”).

This path is parameterized by independent variable s and is generally expected to be continuous, but not necessarily smooth.

Subclassed by cumotion::LinearCSpacePath

Public Functions

virtual int numCSpaceCoords() const = 0#: Return the number of configuration space coordinates for the path.

virtual Domain domain() const = 0#: Return the domain for the path.

virtual Eigen::VectorXd eval(double s) const = 0#

Evaluate the path at the given s.

A fatal error is logged if s is outside of the path domain().

virtual double pathLength() const = 0#

Return the total distance accumulated along the path, where distance is computed using the L2-norm.

This length is not, in general, equal to the distance between the configurations at the lower and upper bounds of the domain.

virtual Eigen::VectorXd minPosition() const = 0#

Return the minimum position of the path within the defined domain().

NOTE: These minimum position values are not, in general, synchronous. Instead, they represent the minimum position independently achieved by each coordinate.

virtual Eigen::VectorXd maxPosition() const = 0#

Return the maximum position of the path within the defined domain().

NOTE: These maximum position values are not, in general, synchronous. Instead, they represent the maximum position independently achieved by each coordinate.

struct Domain#

Indicates continuous range of independent values, s, for which the path is defined.

Public Functions

inline double span() const#: Convenience function to return the span of s values included in domain.

Public Members

double lower#: Minimum value of s defining domain.

double upper#: Maximum value of s defining domain.

Linear C-Space Path#

#include <cumotion/linear_cspace_path.h>

class LinearCSpacePath : public cumotion::CSpacePath #

Represent a path linearly interpolated through configuration space (i.e., c-space) waypoints.

This path is parameterized by independent variable s and will be continuous but not (in general) smooth.

Public Functions

virtual int numCSpaceCoords() const override = 0#: Return the number of configuration space coordinates for the path.

virtual Domain domain() const override = 0#: Return the domain for the path.

virtual Eigen::VectorXd eval(double s) const override = 0#

Evaluate the path at the given s.

A fatal error is logged if s is outside of the path domain().

virtual double pathLength() const override = 0#

Return the total distance accumulated along the path, where distance is computed using the L2-norm.

This length is not, in general, equal to the distance between the configurations at the lower and upper bounds of the domain.

virtual Eigen::VectorXd minPosition() const override = 0#

Return the minimum position of the path within the defined domain().

NOTE: These minimum position values are not, in general, synchronous. Instead, they represent the minimum position independently achieved by each coordinate.

virtual Eigen::VectorXd maxPosition() const override = 0#

Return the maximum position of the path within the defined domain().

NOTE: These maximum position values are not, in general, synchronous. Instead, they represent the maximum position independently achieved by each coordinate.

virtual std::vector<Eigen::VectorXd> waypoints() const = 0#: Return all of the waypoints through which the path is linearly interpolated (including the initial and final c-space configurations).

std::unique_ptr<LinearCSpacePath> cumotion::CreateLinearCSpacePath( const CSpacePathSpec &cspace_path_spec, )#: Create a LinearCSpacePath from a c-space path specification.

Task-Space Path#

#include <cumotion/task_space_path.h>

class TaskSpacePath#

Represent a path through task space (i.e., SE(3) group representing 6-dof poses).

This path is parameterized by independent variable s and is generally expected to be continuous, but not necessarily smooth.

Public Functions

virtual Domain domain() const = 0#: Return the domain for the path.

virtual Pose3 eval(double s) const = 0#

Evaluate the path at the given s.

A fatal error is logged if s is outside of the path domain().

virtual double pathLength() const = 0#

Return the total translation distance accumulated along the path.

This length is not, in general, equal to the translation distance between the poses at the lower and upper bounds of the domain.

E.g., if a path moved linearly from (0,0,0) to (0,1,0) to (1,1,0) to (1,0,0), then the path length would be 3 while the net translation between the positions at the lower (0,0,0) and upper (1,0,0) bounds of the domain would be 1.

virtual double accumulatedRotation() const = 0#

Return the total angular distance (in radians) accumulated throughout the path.

Similar to pathLength(), this value is not, in general, equal to the minimum angle between the rotations at the lower and upper bounds of the domain.

E.g., if a path rotates 2 pi around a single axis followed by pi around another axis, then the accumulated rotation would be 3 pi, while the net rotation between the rotations at the lower and upper bounds of the domain would be pi.

virtual Eigen::Vector3d minPosition() const = 0#

Return the minimum position of the path within the defined domain().

NOTE: These minimum position values are not, in general, synchronous. Instead, they represent the minimum position independently achieved by each coordinate.

virtual Eigen::Vector3d maxPosition() const = 0#

Return the maximum position of the path within the defined domain().

NOTE: These maximum position values are not, in general, synchronous. Instead, they represent the maximum position independently achieved by each coordinate.

struct Domain#

Indicates continuous range of independent values, s, for which the path is defined.

Public Functions

inline double span() const#: Convenience function to return the span of s values included in domain.

Public Members

double lower#: Minimum value of s defining domain.

double upper#: Maximum value of s defining domain.

Path Conversion#

#include <cumotion/path_conversion.h>

struct TaskSpacePathConversionConfig#

Configuration parameters for converting a TaskSpacePathSpec into a series of c-space configurations.

Public Functions

TaskSpacePathConversionConfig() = default#

Create default set of configuration parameters for converting a TaskSpacePathSpec into a series of c-space configurations.

Default parameters are expected to work well for converting most task space paths to c-space paths.

If a tighter tolerance for adherence to the task space path is desired, then the min_position_deviation and/or max_position_deviation can be decreased (at the expense of more computational cost and more c-space waypoints which will likely lead to trajectories with longer time-spans). Conversely, tolerances can be loosened for faster path conversion and/or (likely) faster trajectories.

Beyond (optionally) adjusting position deviations, it is unlikely that other parameters will need to be modified from the provided default values.

Public Members

double min_position_deviation = 0.001#

For each c-space waypoint that is generated, the position deviation between the desired task space path and a task space mapping of a straight-line interpolation in c-space is approximated. The minimum position deviation places a lower bound of deviation required to add a c-space waypoint.

While it is somewhat unintuitive that the deviation could be too small, this minimum deviation is used to control the minimum spacing between c-space configurations along the task space path domain. A (relatively) sparse series of c-space waypoints is desirable for trajectory generation; if the minimum deviation is arbitrarily small then the c-space points will be (in general) too close together to generate a time-optimal trajectory. Generation of excessive c-space waypoints will also be computationally expensive and is, in general, best avoided.

min_position_deviation must be positive and less than max_position_deviation.

Default value is 0.001.

double max_position_deviation = 0.003#

For each c-space waypoint that is generated, the position deviation between the desired task space path and a task space mapping of a straight-line interpolation in c-space is approximated. The maximum position deviation places an upper bound of deviation allowed to add a c-space waypoint.

max_position_deviation must be positive and greater than min_position_deviation.

Default value is 0.003.

double initial_s_step_size = 0.05#

Initial step size in the domain value ‘s’ used to sample poses from the task space path to be converted to c-space waypoints.

The ‘s’ step size will be adaptively updated throughout the path conversion and the default value for initialization is generally recommended.

initial_s_step_size must be positive.

Default value is 0.05.

double initial_s_step_size_delta = 0.005#

Initial step size “delta” that is used to adaptively adjust the ‘s’ step size; ‘s’ is the domain value ‘s’ used to sample poses from the task space path to be converted to c-space waypoints.

The ‘s’ step size “delta” will be adaptively updated throughout the path conversion and the default value for initialization is generally recommended.

initial_s_step_size_delta must be positive.

Default value is 0.005.

double min_s_step_size = 1e-5#

Minimum allowable interval in domain value ‘s’ that can separate poses from the task space path to be converted to c-space waypoints.

The minimum ‘s’ step size serves to limit the number of c-space configurations that can be returned in the converted path. Specifically, the upper bound for the number of returned c-space configurations is (“span of the task space path domain” / min_s_step_size) + 1.

min_s_step_size must be positive.

Default value is 1e-5.

double min_s_step_size_delta = 1e-5#

Minimum allowable ‘s’ step size “delta” used to adaptively update the ‘s’ step size.

The min_s_step_size_delta serves to limit wasted iterations when (minimal) progress is being made towards path conversion. If min_s_step_size_delta is reached during the search for any c-space waypoint, then path conversion will fail.

The default value is generally recommended.

min_s_step_size_delta must be positive.

Default value is 1e-5.

int max_iterations = 50#

Maximum number of iterations to search for each c-space waypoint.

If max_iterations is reached for any c-space waypoint, then path conversion will fail.

max_iterations must be positive.

Default value is 50.

double alpha = 1.4#

“alpha” is used to exponentially scale the ‘s’ step size “delta” to speed convergence when the ‘s’ step size is being successively increased or successively decreased. When an increase is followed by a decrease, or vice versa, “alpha” is used to decrease the ‘s’ step size “delta” to reduce overshoot.

The default value is generally recommended.

alpha must be greater than 1.

Default value is 1.4.

std::unique_ptr<LinearCSpacePath> cumotion::ConvertCompositePathSpecToCSpace( const CompositePathSpec &composite_path_spec, const Kinematics &kinematics, const Kinematics::FrameHandle &control_frame, const TaskSpacePathConversionConfig &task_space_path_conversion_config = TaskSpacePathConversionConfig(), const IkConfig &ik_config = IkConfig(), )#

Convert a composite_path_spec into a linear c-space path.

The mapping from c-space to task space is defined by kinematics for the given control_frame.

If non-default configuration parameters for the path conversion process are desired, then task_space_conversion_config can (optionally) be specified.

If non-default configuration parameters for the inverse kinematics (IK) solver are desired, then ik_config can optionally be specified.

std::unique_ptr<LinearCSpacePath> cumotion::ConvertTaskSpacePathSpecToCSpace( const TaskSpacePathSpec &task_space_path_spec, const Kinematics &kinematics, const Kinematics::FrameHandle &control_frame, const TaskSpacePathConversionConfig &task_space_path_conversion_config = TaskSpacePathConversionConfig(), const IkConfig &ik_config = IkConfig(), )#

Convert a task_space_path_spec into a linear c-space path.

Inverse kinematics will be used to convert the initial task space pose of task_space_path_spec to a c-space position. If a particular c-space solution is desired, this can be set in ik_config.cspace_seeds. If the specified c-space does not correspond to the initial task space pose, it will simply be used as a warm start and the IK solver will proceed to search for an appropriate c-space position.

The mapping from c-space to task space is defined by kinematics for the given control_frame.

If non-default configuration parameters for the path conversion process are desired, then task_space_conversion_config can (optionally) be specified.

If non-default configuration parameters for the inverse kinematics (IK) solver are desired, then ik_config can optionally be specified.

Trajectory Generation (Collision-Unaware)#

#include <cumotion/trajectory_generator.h>

class CSpaceTrajectoryGenerator#

Configure a trajectory generator that can compute smooth* trajectories. A trajectory generation problem is specified by:

Bound constraints (position, velocity, and acceleration),
Intermediate position waypoints, and
Limits on some combination of position, velocity, acceleration and jerk.

Times for domain bounds and intermediate waypoints are not specified. It is assumed that a time-optimal trajectory that respects the constraints (with the definition of time-optimality being implementation-specific) will be generated.

The definition of “smooth” is implementation-specific. Currently, CSpaceTrajectoryGenerator uses a single implementation that interpolates through waypoints using a series of cubic splines. For this implementation, “smooth” is defined as smooth velocity, continuous acceleration, and bounded jerk.

Public Types

enum class InterpolationMode#

Interpolation modes used by interpolateTrajectory().

Values:

enumerator LINEAR#

Linear interpolation between c-space positions.

The resulting trajectory will have continuous (but not smooth) position and discontinuous
segments of constant velocity. Acceleration and jerk will be returned as zero (including at
velocity discontinuities where values are technically undefined).

enumerator CUBIC_SPLINE#

Piecewise solution with a cubic spline between adjacent c-space positions.

The resulting trajectory will have smooth position and velocity, continuous (but not smooth)
acceleration, and bounded (but discontinuous) jerk.

Public Functions

virtual int numCSpaceCoords() const = 0#: Return the number of configuration space coordinates for the trajectory generator.

virtual void setPositionLimits( const Eigen::VectorXd &min_position, const Eigen::VectorXd &max_position, ) = 0#

Set position limits.

A fatal error is logged if either min_position or max_position does not have a length matching the expected number of c-space coordinates, or if any coordinate of max_position is not greater than the corresponding coordinate of min_position.

virtual void setVelocityLimits(const Eigen::VectorXd &max_velocity) = 0#

Set velocity magnitude limits.

A fatal error is logged if max_velocity does not have a length matching the expected number of c-space coordinates, or if any coordinate of max_velocity is negative.

WARNING: The current implementation using a series of cubic splines requires velocity limits to be specified. This restriction may be lifted in future versions of the CSpaceTrajectoryGenerator.

virtual void setAccelerationLimits( const Eigen::VectorXd &max_acceleration, ) = 0#

Set acceleration magnitude limits.

A fatal error is logged if max_acceleration does not have a length matching the expected number of c-space coordinates, or if any coordinate of max_acceleration is negative.

virtual void setJerkLimits(const Eigen::VectorXd &max_jerk) = 0#

Set jerk magnitude limits.

A fatal error is logged if max_jerk does not have a length matching the expected number of c-space coordinates, or if any coordinate of max_jerk is negative.

virtual std::unique_ptr<Trajectory> generateTrajectory( const std::vector<Eigen::VectorXd> &waypoints, ) const = 0#

Attempt to generate a time-optimal trajectory passing through the specified waypoints with the specified constraints.

If a trajectory cannot be generated, nullptr is returned.

virtual std::unique_ptr<Trajectory> generateTimeStampedTrajectory( const std::vector<Eigen::VectorXd> &waypoints, const std::vector<double> &times, InterpolationMode interpolation_mode = InterpolationMode::CUBIC_SPLINE, ) const = 0#

Attempt to interpolate a trajectory passing through the specified waypoints at the specified times.

Interpolation will fail if:

The number of specified positions does not match the number of specified times;
There is not at least one position specified for LINEAR interpolation mode or at least two positions specified for CUBIC_SPLINE interpolation mode;
Not all positions have the same number of c-space coordinates;
Not all times are strictly increasing; OR
The interpolated trajectory does not satisfy specified limits on c-space position, velocity, acceleration, or jerk.

If a trajectory cannot be generated due to invalid input (items 1-4 above), a fatal error is logged. If failure is due to c-space limits (item 5 above), then nullptr is returned.

virtual bool setSolverParam( const std::string &param_name, SolverParamValue value, ) = 0#

Set the value of the solver parameter.

Currently, CSpaceTrajectoryGenerator uses a single solver implementation based on computing a series of cubic splines. The following parameters can be set for this piecewise cubic spline solver:

max_segment_iterations [int]

The first step towards finding a time-optimal trajectory using a series of cubic splines is to iteratively compute an optimal span for each spline segment.
Setting a relatively high max_segment_iterations will, in general, result in shorter trajectory time spans, but will tend to take longer to converge.
This step can be skipped completely by setting max_segment_iterations to zero.
max_segment_iterations must be non-negative. Additionally at least one of max_segment_iterations and max_aggregate_iterations must be non-zero.
Default value is 5.

max_aggregate_iterations [int]

The second step towards finding a time-optimal trajectory using a series of cubic splines is to iteratively compute an optimal span for the entire trajectory.
Relying more heavily on this second step (i.e., setting max_segment_iterations to a a relatively low value) will, in general, require less iterations (and thus less time) to converge, but the generated trajectories will tend to have longer time spans.
This step can be skipped completely by setting max_aggregate_iterations to zero.
max_aggregate_iterations must be non-negative. Additionally at least one of max_segment_iterations and max_aggregate_iterations must be non-zero.
Default value is 50.

convergence_dt [double]

The search for optimal time values will terminate if the maximum change to any time value during a given iteration is less than the convergence_dt.
convergence_dt must be positive.
Default value is 0.001.

max_dilation_iterations [int]

After the segment-wise and/or aggregate time-optimal search has converged or reached maximum iterations, the resulting set of splines will be tested to see if any derivative limits are exceeded.
If any derivative limits are exceeded, the splines will be iteratively scaled in time to reduce the maximum achieved derivative. This process will repeat until no derivative limits are exceeded (success) or max_dilation_iterations are reached (failure).
max_dilation_iterations must be non-negative.
Default value is 100.

dilation_dt [double]

For the iterative dilation step described in max_dilation_iterations documentation, the dilation_dt is the “epsilon” value added to the span of the trajectory that exceeds derivative limits.
dilation_dt must be positive.
Default value is 0.001.

min_time_span [double]

Specify the minimum allowable time span between adjacent waypoints/endpoints.
min_time_span must be positive.
Default value is 0.01.

time_split_method [string]

Specify the TimeSplitMethod for the initial distribution of time values that will be used to iteratively search for time-optimal values.
Valid settings are uniform, chord_length and centripetal.

struct SolverParamValue#

Specify the value for a given parameter.

The required SolverParamValue constructor for each parameter is detailed in the documentation for setSolverParam().

Public Functions

SolverParamValue(int value)#: Create SolverParamValue from int.

SolverParamValue(double value)#: Create SolverValue from double.

SolverParamValue(const char *value)#: Create SolverParamValue from const char*.

SolverParamValue(const std::string &value)#: Create SolverParamValue from std::string.

std::unique_ptr<CSpaceTrajectoryGenerator> cumotion::CreateCSpaceTrajectoryGenerator( const Kinematics &kinematics, )#

Create a CSpaceTrajectoryGenerator with the specified kinematics.

The kinematics will be used to specify the number of configuration space coordinates, position limits, and any available derivative limits.

Position and derivative limits may be added or overwritten after construction. Bound constraints and intermediate waypoints can be added after construction.

std::unique_ptr<CSpaceTrajectoryGenerator> cumotion::CreateCSpaceTrajectoryGenerator( int num_cspace_coords, )#

Create a CSpaceTrajectoryGenerator with the specified number of configuration space coordinates.

Position and derivative limits, bound constraints, and intermediate waypoints can be added after construction.

Graph-Based Path Planning#

#include <cumotion/motion_planner.h>

Public interface to cuMotion’s global motion planning implementation.

class MotionPlannerConfig#

Configuration parameters for a MotionPlanner.

Public Functions

virtual bool setParam( const std::string &param_name, ParamValue value, ) = 0#

Set the value of the parameter.

setParam returns true if the param has been successfully updated and false if an error has occurred (e.g., invalid parameter).

The following parameters can be set for MotionPlanner:

seed [int]

Used to initialize random sampling.
seed must be positive.
Default: 1234

step_size [double]

Step size for tree extension and (optionally) for discretization of expanded_path in Results
It is assumed that a straight path connecting two valid c-space configurations with separation distance <= step_size is a valid edge, where separation distance is defined as the L2-norm of the difference between the two configurations.
step_size must be positive.
Default: 0.05

max_iterations [int]

Maximum number of iterations of tree extensions that will be attempted.
If max_iterations is reached without finding a valid path, the Results will indicate path_found is false and path will be an empty vector.
max_iterations must be positive.
Default: 10,000

max_sampling [int]

Maximum number of configurations used for sampling in the environment setup.
max_sampling must be positive.
Default: 10’000

distance_metric_weights [std::vector<double>]

When selecting a node for tree extension, the closest node is defined using a weighted, squared L2-norm: distance = (q0 - q1)^T * W * (q0 - q1) where q0 and q1 represent two configurations and W is a diagonal matrix formed from distance_metric_weights.
The length of the distance_metric_weights must be equal to the number of c-space coordinates for the robot and each weight must be positive.
Default: Vectors of ones with length set by the number of c-space coordinates in robot_description.

tool_frame_name [std::string]

Indicate the name (from URDF) of the frame to be used for task space planning.
With current implementation, setting a tool_frame_name that is not found in the kinematics will throw an exception rather than failing gracefully.

task_space_limits [std::vector<Limit>]

Task space limits define a bounding box used for sampling task space when planning a path to a task space target.
The specified task_space_limits vector should be length 3, corresponding to the xyz dimensions of the bounding box.
Each upper limit must be >= the corresponding lower limit.
Default: Lower limits default to -1 and upper limits to 1.

enable_cuda_tree [bool]

Set to true to enable use of CUDA for storing explored configurations and performing nearest neighbor look-up, or false to disable usage of CUDA.
If set to true from the false state, default values for cuda_tree_params will be used.
If set to false from the true state, current values for cuda_tree_params will be discarded (i.e., if returned to true, default rather than previous values will be used).
NOTE: this parameter does NOT need to be set in the YAML configuration file. The presence of a cuda_tree_params with corresponding parameters in the configuration file indicates that enable_cuda_tree should be set to true.
Default: true

cuda_tree_params/max_num_nodes [int]

Maximum number of valid explored configurations that can be stored in a CUDA tree.
A default value of 10000 is recommended for most use-cases.
cuda_tree_params/max_num_nodes must be positive AND greater than num_nodes_cpu_gpu_crossover.
Default: 10’000

cuda_tree_params/max_buffer_size [int]

Maximum number of valid configurations that are buffered on CPU prior to transferring to GPU.
A default value of 30 is recommended for most use-cases.
cuda_tree_params/max_buffer_size must be positive.
Default: 30

cuda_tree_params/num_nodes_cpu_gpu_crossover [int]

Number of valid explored configurations added to tree prior to copying all configurations to GPU and using GPU for nearest neighbor lookup.
A default value of 3000 is recommended for most use-cases.
cuda_tree_params/num_nodes_cpu_gpu_crossover must be positive AND less than max_num_nodes.
Default: 3000

NOTE: For all of the cuda_tree_params listed above, the best values for optimal performance will depend on the number of c-space coordinates, the system hardware (e.g., CPU, GPU, and memory configuration), and software versions (e.g., CUDA, NVIDIA GPU driver, and cuMotion versions). The provided default recommendations are chosen to be appropriate for most expected use-cases.

cspace_planning_params/exploration_fraction [double]

The c-space planner uses RRT-Connect to try to find a path to a c-space target.
RRT-Connect attempts to iteratively extend two trees (one from the initial configuration and one from the target configuration) until the two trees can be connected. The configuration to which a tree is extended can be either a random sample (i.e., exploration) or a node on the tree to which connection is desired (i.e., exploitation). The exploration_fraction controls the fraction of steps that are exploration steps. It is generally recommended to set exploration_fraction in range [0.5, 1), where 1 corresponds to a single initial exploitation step followed by only exploration steps. Values of between [0, 0.5) correspond to more exploitation than exploration and are not recommended. If a value outside range [0, 1] is provided, a warning is logged and the value is clamped to range [0, 1].
A default value of 0.5 is recommended as a starting value for initial testing with a given system.
Default: 0.5

task_space_planning_params/translation_target_zone_tolerance [double]

A configuration has reached the task space translation target when task space position has an L2 Norm within translation_target_zone_tolerance of the target.
It is assumed that a valid configuration within the target tolerance can be moved directly to the target configuration using an inverse kinematics solver and linearly stepping towards the solution.
In general, it is recommended that the size of the translation target zone be on the same order of magnitude as the translational distance in task-space corresponding to moving the robot in configuration space by one step with an L2 norm of step_size.
Must be > 0.
Default: 0.05

task_space_planning_params/orientation_target_zone_tolerance [double]

A configuration has reached the task space pose target when task space orientation is within orientation_target_zone_tolerance radians and an L2 norm translation within translation_target_zone_tolerance of the target.
It is assumed that a valid configuration within the target tolerances can be moved directly to the target configuration using an inverse kinematics solver and linearly stepping towards the solution.
In general, it is recommended that the size of the orientation target zone be on the same order of magnitude as the rotational distance in task-space corresponding to moving the robot in configuration space by one step with an L2 norm of step_size.
Must be > 0.
Default: 0.09

task_space_planning_params/translation_target_final_tolerance [double]

Once a path is found that terminates within translation_target_zone_tolerance, an IK solver is used to find a configuration space solution corresponding to the task space target. This solver terminates when the L2-norm of the corresponding task space position is within translation_target_final_tolerance of the target.
Note: This solver assumes that if a c-space configuration within translation_target_zone_tolerance is found then this c-space configuration can be moved linearly in c-space to the IK solution. If this assumption is NOT met, the returned path will not reach the task space target within the translation_target_final_tolerance and an error is logged.
The recommended default value is 1e-4, but in general this value should be set to a positive value that is considered “good enough” precision for the specific system.
Default: 1e-4

task_space_planning_params/orientation_target_final_tolerance [double]

For pose targets, once a path is found that terminates within orientation_target_zone_tolerance and translation_target_zone_tolerance of the target, an IK solver is used to find a configuration space solution corresponding to the task space target. This solver terminates when the L2-norm of the corresponding task space position is within orientation_target_final_tolerance and translation_target_final_tolerance of the target.
Note: This solver assumes that if a c-space configuration within the target zone tolerances is found then this c-space configuration can be moved linearly in c-space to the IK solution. If this assumption is NOT met, the returned path will not reach the task space target within the final target tolerances and an error is logged.
The recommended default value is 1e-4, but in general this value should be set to a positive value that is considered “good enough” precision for the specific system.
Default: 0.005

task_space_planning_params/translation_gradient_weight [double]

For pose targets, computed translation and orientation gradients are linearly weighted by translation_gradient_weight and orientation_gradient_weight to compute a combined gradient step when using the Jacobian Transpose method to guide tree expansion towards a task space target.
The default value is recommended as a starting value for initial testing with a given system.
Must be > 0.
Default: 1.0

task_space_planning_params/orientation_gradient_weight [double]

For pose targets, computed translation and orientation gradients are linearly weighted by translation_gradient_weight and orientation_gradient_weight to compute a combined gradient step when using the Jacobian Transpose method to guide tree expansion towards a task space target.
The default value is recommended as a starting value for initial testing with a given system.
Must be > 0.
Default: 0.125

task_space_planning_params/nn_translation_distance_weight [double]

For pose targets, nearest neighbor distances are computed by linearly weighting translation and orientation distance by nn_translation_distance_weight and nn_orientation_distance_weight.
Nearest neighbor search is used to select the node from which the tree of valid configurations will be expanded.
The default value is recommended as a starting value for initial testing with a given system.
Must be > 0.
Default: 1.0

task_space_planning_params/nn_orientation_distance_weight [double]

For pose targets, nearest neighbor distances are computed by linearly weighting translation and orientation distance by nn_translation_distance_weight and nn_orientation_distance_weight.
Nearest neighbor search is used to select the node from which the tree of valid configurations will be expanded.
The default value is recommended as a starting value for initial testing with a given system.
Must be > 0.
Default: 0.125

task_space_planning_params/task_space_exploitation_fraction [double]

Fraction of iterations for which tree is extended towards target position in task space.
Must be in range [0, 1]. Additionally, the sum of task_space_exploitation_fraction and task_space_exploration_fraction must be <= 1.
The default value is recommended as a starting value for initial testing with a given system.
Default: 0.4

task_space_planning_params/task_space_exploration_fraction [double]

Fraction of iterations for which tree is extended towards random position in task space.
Must be in range [0, 1]. Additionally, the sum of task_space_exploitation_fraction and task_space_exploration_fraction must be <= 1.
The default value is recommended as a starting value for initial testing with a given system.
Default: 0.1

NOTE: The remaining fraction beyond task_space_exploitation_fraction and task_space_exploration_fraction is a cspace_exploration_fraction that is implicitly defined as: 1 - (task_space_exploitation_fraction + task_space_exploration_fraction In general, easier path searches will take less time with higher exploitation fraction while more difficult searches will waste time if the exploitation fraction is too high and benefit from greater combined exploration fraction.

task_space_planning_params/max_extension_substeps_away_from_target [int]

Maximum number of Jacobian transpose gradient descent substeps that may be taken while the end effector is away from the task-space target.
The threshold for nearness is determined by the extension_substep_target_region_scale_factor parameter.
The default value is recommended as a starting value for initial testing with a given system.
Must be >= 0.
Default: 6

task_space_planning_params/max_extension_substeps_near_target [int]

Maximum number of Jacobian transpose gradient descent substeps that may be taken while the end effector is near the task-space target.
The threshold for nearness is determined by the extension_substep_target_region_scale_factor parameter.
The default value is recommended as a starting value for initial testing with a given system.
Must be >= 0.
Default: 50

task_space_planning_params/extension_substep_target_region_scale_factor : [double]

A scale factor used to determine whether the end effector is close enough to the target to change the amount of gradient descent substeps allowed when adding a node in RRT.
The max_extension_substeps_near_target parameter is used when the distance (i.e., L2 norm) between the end effector and target position is less than extension_substep_target_region_scale_factor * x_zone_target_tolerance.
Must be greater than or equal to 1.0; a value of 1.0 effectively disables the max_extension_substeps_near_target parameter.
The default value is recommended as a starting value for initial testing with a given system.
Default: 2.0

task_space_planning_params/unexploited_nodes_culling_scalar [double]

Scalar controlling culling of unexploited nodes during task-space planning.
Must be >= 0.0.
Default: 1.0

task_space_planning_params/gradient_substep_size [double]

Size of each gradient-descent substep when following the Jacobian Transpose direction.
Must be > 0.0.
Default: 0.025

struct Limit#: Indicate lower and upper limits for a coordinate.

struct ParamValue#

Specify the value for a given parameter.

The required ParamValue constructor for each param is detailed in the documentation for setParam().

Public Functions

ParamValue(int value)#: Create ParamValue from int.

ParamValue(double value)#: Create ParamValue from double.

ParamValue(const Eigen::Vector3d &value)#: Create ParamValue from Eigen::Vector3d.

ParamValue(const std::vector<double> &value)#: Create ParamValue from std::vector<double>.

ParamValue(bool value)#: Create ParamValue from bool.

ParamValue(const char *value)#: Create ParamValue from const char*.

ParamValue(const std::string &value)#: Create ParamValue from std::string.

ParamValue(const std::vector<Limit> &value)#: Create ParamValue from std::vector<Limit>.

std::unique_ptr<MotionPlannerConfig> cumotion::CreateMotionPlannerConfigFromFile( const std::filesystem::path &motion_planner_config_file, const RobotDescription &robot_description, const std::string &tool_frame_name, const WorldViewHandle &world_view, )#

Load a set of MotionPlanner configuration parameters from file.

These parameters are combined with robot_description, tool_frame_name, and world_view to create a configuration for motion planning.

A fatal error will be logged if motion_planner_config_file is missing any required parameters. If any provided parameter values are invalid:

When recoverable, they will be clipped to their expected value range with a warning on calling CreateMotionPlanner().
When not recoverable, a fatal error will be logged.

A configuration will NOT be created if:

robot_description is invalid, OR
tool_frame_name is not a valid frame in robot_description. In the case of failure, a non-fatal error will be logged and a nullptr will be returned.

std::unique_ptr<MotionPlannerConfig> cumotion::CreateDefaultMotionPlannerConfig( const RobotDescription &robot_description, const std::string &tool_frame_name, const WorldViewHandle &world_view, )#

Use default parameters to create a configuration for motion planning.

See MotionPlannerConfig::setParam() for the default parameter values.

A configuration will NOT be created if:

robot_description is invalid, OR
tool_frame_name is not a valid frame in robot_description.

In the case of failure, a non-fatal error will be logged and a nullptr will be returned.

class MotionPlanner#

Interface class for planning collision-free paths for robotic manipulators. Supports both configuration space (i.e., joint position) targets and task space (e.g., end effector position) targets.

Public Functions

virtual Results planToCSpaceTarget( const Eigen::VectorXd &q_initial, const Eigen::VectorXd &q_target, bool generate_interpolated_path = false, ) = 0#: Attempt to find a path from initial configuration space position (q_initial) to a configuration space target (q_target). generate_interpolated_path determines whether interpolated_path will be populated in Results.

virtual Results planToTranslationTarget( const Eigen::VectorXd &q_initial, const Eigen::Vector3d &translation_target, bool generate_interpolated_path = false, ) = 0#: Attempt to find a path from initial configuration space position (q_initial) to a task space translation target (translation_target). generate_interpolated_path determines whether interpolated_path will be populated in Results.

virtual Results planToPoseTarget( const Eigen::VectorXd &q_initial, const Pose3 &pose_target, bool generate_interpolated_path = false, ) = 0#: Attempt to find a path from initial configuration space position (q_initial) to a task space pose target (pose_target). generate_interpolated_path determines whether interpolated_path will be populated in Results.

struct Results#

Results from a path search.

Public Members

bool path_found#: Indicate whether a collision-free path was found. If false, path and expanded_path will be empty.

std::vector<Eigen::VectorXd> path#: Minimal representation of collision-free path. Each vector represents a knot in configuration space, where successive knots can be linearly interpolated in configuration space to generate a collision-free path.

std::vector<Eigen::VectorXd> interpolated_path#: Interpolation of path such that successive knots are (in general) one step_size apart from each other (where distance is defined as L2-norm in c-space). NOTE: Each straight segment from path above are interpolated individually, where the distance between the last two returned knots for each segment will be less than one step_size from each other. The interpolated_path will only be populated if generate_interpolated_path is set to true when generating a path plan.

std::unique_ptr<MotionPlanner> cumotion::CreateMotionPlanner( const MotionPlannerConfig &config, )#: Create a MotionPlanner with the given config.

RMPflow#

#include <cumotion/rmpflow.h>

Public interface to cuMotion’s RMPflow implementation.

class RmpFlowConfig#

Interface class for loading and manipulating RMPflow parameters. WARNING: This interface may change in a future release.

Public Functions

virtual double getParam(const std::string &param_name) const = 0#: Get the value of a parameter, given a “param_name” string of the form “<rmp_name>/<parameter_name>”

virtual void setParam(const std::string &param_name, double value) = 0#: Set the value of the parameter.

virtual void getAllParams( std::vector<std::string> &names, std::vector<double> &values, ) const = 0#: Get the names and values of all parameters. The two vectors will be overwritten if not empty.

virtual void setAllParams( const std::vector<std::string> &names, const std::vector<double> &values, ) = 0#: Set all parameters at once. The vectors “names” and “values” must have the same size. The parameter corresponding to names[i] will be set to the value given by values[i].

virtual void setWorldView(const WorldViewHandle &world_view) = 0#: Set the world view that will be used for obstacle avoidance. All enabled obstacles in world_view will be avoided by the RMPflow policy.

std::unique_ptr<RmpFlowConfig> cumotion::CreateRmpFlowConfigFromFile( const std::filesystem::path &rmpflow_config_file, const RobotDescription &robot_description, const std::string &end_effector_frame, const WorldViewHandle &world_view, )#: Load a set of RMPflow parameters from file, and combine with a robot description to create a configuration object for consumption by CreateRmpFlow(). The “end_effector_frame” should correspond to a link name as specified in the original URDF used to create the robot description. All enabled obstacles in world_view will be avoided by the RMPflow policy.

std::unique_ptr<RmpFlowConfig> cumotion::CreateRmpFlowConfigFromMemory( const std::string &rmpflow_config, const RobotDescription &robot_description, const std::string &end_effector_frame, const WorldViewHandle &world_view, )#: Load a set of RMPflow parameters from string, and combine with a robot description to create a configuration object for consumption by CreateRmpFlow(). The “end_effector_frame” should correspond to a link name as specified in the original URDF used to create the robot description. All enabled obstacles in world_view will be avoided by the RMPflow policy.

class RmpFlow#

Interface class for building and evaluating a motion policy in the RMPflow framework.

Public Functions

virtual void setEndEffectorPositionAttractor( const Eigen::Vector3d &position, ) = 0#

Set an end-effector position attractor.

The origin of the end effector frame will be driven towards the specified position.

WARNING: This attractor-specification interface is planned to change in a future release to support multiple task space attractors rather than a single, fixed end-effector frame, and to allow setting relative weights for each attractor. It is recommended that this function be used for setting end-effector position attractors until then.

virtual void clearEndEffectorPositionAttractor() = 0#

Clear end-effector position attractor.

The RMP driving the origin of the end effector frame towards a particular position will be deactivated.

WARNING: This attractor-specification interface is planned to change in a future release to support multiple task space attractors rather than a single, fixed end-effector frame, and to allow setting relative weights for each attractor. It is recommended that this function be used for clearing end-effector position attractors until then.

virtual void setEndEffectorOrientationAttractor( const Rotation3 &orientation, ) = 0#

Set an end-effector orientation attractor.

The orientation of the end effector frame will be driven towards the specified orientation.

WARNING: This attractor-specification interface is planned to change in a future release to support multiple task space attractors rather than a single, fixed end-effector frame, and to allow setting relative weights for each attractor. It is recommended that this function be used for setting orientation attractors until then.

virtual void clearEndEffectorOrientationAttractor() = 0#

Clear end-effector orientation attractor.

The RMPs driving the orientation of the end effector frame towards a particular orientation will be deactivated.

WARNING: This attractor-specification interface is planned to change in a future release to support multiple task space attractors rather than a single, fixed end-effector frame, and to allow setting relative weights for each attractor. It is recommended that this function be used for clearing orientation attractors until then.

virtual void setCSpaceAttractor( const Eigen::VectorXd &cspace_position, ) = 0#

Set an attractor in generalized coordinates (configuration space).

The c-space coordinates will be biased towards the specified configuration.

NOTE: Unlike the end effector attractors, there is always an active c-space attractor (either set using setCSpaceAttractor() or using the default value loaded from the robot description).

WARNING: This attractor-specification interface is planned to change in a future release to support multiple task space attractors rather than a single, fixed end-effector frame, and to allow setting relative weights for each attractor. It is recommended that this function be used for setting c-space attractors until then.

virtual void evalAccel( const Eigen::VectorXd &cspace_position, const Eigen::VectorXd &cspace_velocity, Eigen::Ref<Eigen::VectorXd> cspace_accel, ) const = 0#: Compute configuration-space acceleration from motion policy, given input state. This takes into account the current C-space and/or end-effector targets, as well as any currently-enabled obstacles.

virtual void evalForceAndMetric( const Eigen::VectorXd &cspace_position, const Eigen::VectorXd &cspace_velocity, Eigen::Ref<Eigen::VectorXd> cspace_force, Eigen::Ref<Eigen::MatrixXd> cspace_metric, ) const = 0#: Compute configuration-space force and metric from motion policy, given input state. This takes into account the current C-space and/or end-effector targets, as well as any currently-enabled obstacles.

std::unique_ptr<RmpFlow> cumotion::CreateRmpFlow( const RmpFlowConfig &config, )#: Create an instance of the RmpFlow interface from an RMPflow configuration.

Collision Sphere Generation#

#include <cumotion/collision_sphere_generator.h>

class CollisionSphereGenerator#

The CollisionSphereGenerator generates a set of spheres to approximate the volume enclosed by a triangular mesh.

Public Functions

virtual std::vector<Sphere> generateSpheres( int num_spheres, double radius_offset, ) = 0#

Generate a set of num_spheres that approximate the volume of the specified mesh.

If radius_offset is set to zero, all of the generated spheres will be (approximately) tangent to at least two triangles in the mesh. A positive radius_offset will increase the radii of all spheres by adding radius_offset (e.g., a radius_offset of 0.05 will allow spheres to extend beyond the mesh by up to 0.05). A negative value will shrink the radii of the returned spheres (down to a minimum radius of min_sphere_radius as described in the documentation for setParam()).

virtual bool setParam( const std::string &param_name, ParamValue value, ) = 0#

Set the value of the parameter.

If the value has been successfully updated, true is returned. If an invalid value has been specified for a parameter, a verbose warning is logged, false is returned and the parameter value is NOT updated.

Default parameters are expected to work well for most meshes commonly used to represent robots (i.e., generally “meter scale”). If the meshes are particularly larger or smaller (or represented in units other than meters), then min_sphere_radius, convergence_radius_tol, ‘surface_offset,min_triangle_area, and ‘num_voxels can be scaled appropriately.

Likewise, the default parameters assume a conventional counter-clockwise winding order for vertex indices. If this is not the case, flip_normals can be set to true.

Finally, increasing num_medial_sphere_samples may increase the quality of the selected spheres (at the expense of more processing time).

The full set of parameters that can be set are:

num_medial_sphere_samples [int]

The collision sphere generation begins by sampling medial spheres uniformly from the surface of the provided mesh. The medial axis of a mesh is the set of all points having more than one closest point to the mesh. The medial spheres are spheres centered on the medial axis with radius set to the minimum distance from the mesh. By definition, these medial spheres are tangent to at least two faces of the mesh.
These sampled spheres are the set from which the minimal set of spheres to approximate the mesh will be selected. A larger number of samples may result in a better selection of spheres, and more spheres may be valuable if the mesh is particularly complex.
num_medial_sphere_samples must be positive.
Default value is 500.

flip_normals [bool]

The mesh input to the CollisionSphereGenerator is not required to be strictly watertight, but is expected to generally represent a volume. Spheres will be generated by sampling points on the triangular mesh faces and “growing” the spheres inwards to find a sphere that is tangent to both the original triangle and some other triangle on the mesh.
It is expected that for any point on any triangular face, a sphere that is tangent to that point on the “inside” (negative normal direction) of the triangle with diameter set to the maximum extent on the mesh will intersect at least one other triangle.
The normal is by default computed assuming a counter-clockwise winding direction for vertex indices. If the mesh is input with clockwise winding direction for vertex indices, then flip_normals should be set to true to reverse winding order.
Default value is false.

min_sphere_radius [double]

When generating medial spheres, any spheres that are found to have radius less than min_sphere_radius will be discarded.
NOTE: Discarded spheres still count towards num_medial_sphere_samples. E.g., if generateSpheres() is called with num_medial_sphere_samples = 500, and 20 spheres are discarded for being too small, then the actual number of spheres that will be used to select the final spheres is 480.
min_sphere_radius must be positive.
Default value is 1e-3.

seed [int]

Random seed used for sampling surface points on mesh from which to sample spheres.
seed must be positive.
Default value is 12345.

max_iterations [int]

Maximum number of iterations of binary search used to approximate the radius of each medial sphere.
NOTE: If max_iterations is reached, the sphere will be discarded. Discarded spheres still count towards num_medial_sphere_samples. E.g., if generateSpheres() is called with num_medial_sphere_samples = 500, and 20 spheres are discarded for not converging to the medial sphere radius tolerance within the max_iterations, then the actual number of spheres that will be used to select the final spheres is 480.
max_iterations must be positive.
Default value is 100.

convergence_radius_tol [double]

Convergence criteria for binary search used to approximate the radius of each medial sphere. The search will end when the sphere radius is within convergence_radius_tol of the actual medial sphere radius.
convergence_radius_tol must be positive.
Default value is 1e-3.

surface_offset [double]

When points are sampled from the mesh surface to generate medial spheres, they are offset by surface_offset towards the “inside” of the mesh in order to avoid collisions with faces very close to the sampling face. This is to help avoid very small spheres that would be culled due to the min_sphere_radius.
surface_offset must be positive.
Default value is 1e-4.

min_triangle_area [double]

Triangles with area below min_triangle_area are discarded when creating the mesh for generating medial spheres. This includes degenerate triangles where surface area approaches zero as well as relatively small triangles that will increase computation cost without being likely to improve the set of sampled spheres.
min_triangle_area must be positive.
Default value is 1e-8.

num_voxels [int]

A voxel grid is used to estimate volume additions from each sphere when selecting spheres from the full sample of medial spheres. The num_voxels provides a trade-off between accurate volume approximation (with larger num_voxels) and faster selection (with smaller num_voxels).
num_voxels specifies the number of voxels along the longest dimension of an axis-aligned bounding box (AABB) encompassing the specified mesh. For example, if the AABB has dimensions {20, 50, 30} and `num_voxels is set to 100, then each voxel will have side length 50 / 100 = 0.5. Thus, the number of voxels along each axis of the grid will be [40, 100, 60].
voxel_size must be positive.
Default value is 50.

virtual int numTriangles() = 0#: Return the number of validated triangles that have been included in the mesh used for sampling spheres to approximate volume. NOTE: The number of returned triangles may be less than the number of triangles passed to CreateCollisionSphereGenerator() if triangles are discarded for invalid indices or having an area smaller than min_triangle_area (see setParam() documentation for details).

virtual std::vector<Sphere> getSampledSpheres() = 0#: Return all of the medial axis spheres used to approximate the volume of the mesh. The spheres returned by generateSpheres() are selected from this set. NOTE: This function is intended primarily for debugging functionality and/or tuning parameters in setParam() and is not likely to be needed by most users.

struct ParamValue#

Specify the value for a given parameter.

The required ParamValue constructor for each parameter is detailed in the documentation for setParam().

Public Functions

ParamValue(int value)#: Create ParamValue from int.

ParamValue(double value)#: Create ParamValue from double.

ParamValue(bool value)#: Create ParamValue from bool.

struct Sphere#

Simple representation of a sphere.

Public Members

Eigen::Vector3d center#: 3d coordinates for the center position of the sphere.

double radius#: Radius of the sphere.

std::unique_ptr<CollisionSphereGenerator> cumotion::CreateCollisionSphereGenerator( const std::vector<Eigen::Vector3d> &vertices, const std::vector<Eigen::Vector3i> &triangles, )#

Create a CollisionSphereGenerator to generate spheres that approximate the volume of a mesh represented by vertices and triangles.

Each vertex in vertices is a set of (x,y,z) coordinates that can be referenced by one or more triangles in triangles. Each triangle is a set of indices in the vertices vector.

Vertex indices for a given triangle are considered valid if: [a] The index for each vertex is in range [0, vertices.size()), and [b] The same index is NOT included twice.

Triangles with invalid indices will be discarded with verbose warnings.

Additionally, each triangle in triangles is tested to ensure that its area is greater than or equal to min_triangle_radius (see CollisionSphereGenerator::setParam() documentation for details). Triangles that are too small will be discarded.

By default, the triangle normals will be computed assuming a counter-clockwise winding direction. This convention can be flipped by setting flip_normals (see CollisionSphereGenerator::setParam() documentation for details).

std::vector<CollisionSphereGenerator::Sphere> cumotion::GenerateCollisionSpheres( const std::vector<Eigen::Vector3d> &vertices, const std::vector<Eigen::Vector3i> &triangles, double max_overshoot = kCollisionSphereDefaultMaxOvershoot, int max_num_spheres = kCollisionSphereDefaultMaxNumSpheres, double surface_point_density = kCollisionSphereDefaultSurfacePointDensity, int surface_point_sampling_seed = kCollisionSphereDefaultSurfacePointSamplingSeed, bool flip_normals = kCollisionSphereDefaultFlipNormals, double min_triangle_area = kCollisionSphereDefaultMinTriangleArea, )#

Generate a selection of collision spheres from a grid of points filling a triangle mesh.

NOTE: For most meshes, this GenerateCollisionSpheres() function is expected to perform better than CollisionSphereGenerator() (which is likely to be deprecated).

The mesh is represented by triangles and vertices, where each triangle in triangles is a set of indices referencing points in vertices. At least three vertices and at least one triangle must be provided.

The max_overshoot serves as an upper bound for how far any collision sphere may extend beyond the surface of the mesh. The units of the max_overshoot are the same as the mesh, typically meters for robotics applications. The max_overshoot must be positive.

Larger max_overshoot values will create conservative collision sphere representations with relatively few spheres (increasing performance for motion generation and collision checking). Smaller max_overshoot values will fit spheres more tightly to the mesh and will (in general) require more spheres to fit a given mesh (reducing performance for motion generation and collision checking).

The max_num_spheres sets an upper bound on the number of candidate spheres from which collision spheres will be selected. Specifically, the max_num_spheres is used to generate a grid of points filling the axis-aligned bounding box enclosing the mesh. These points serve as potential sphere centers. The max_num_spheres may need to be increased from the default value for particularly large meshes or for particularly small max_overshoot values. Increasing max_num_spheres will (in general) improve the selection of collision spheres, but result in slower generation time. The max_num_spheres must be positive.

The goal of sphere selection is to select spheres such that the entire mesh surface is enclosed by one or more spheres. As an approximation, points are distributed uniformly on the mesh surface and used to test whether the surface is fully enclosed. The surface_point_density determines how many test point are distributed. The units for surface_point_density are 1 / [mesh-length-unit]^2. For the typical robotics case (with the mesh represented in meters), the default value corresponds to 3 surface points per cm^2. Using higher values for surface_point_density will increase the likelihood of full surface enclosure (at the cost of slower generation time). The surface_point_density must be positive. Changing the parameter surface_point_sampling_seed will result in different surface points being selected and cause different candidate spheres to be selected. This seed may be used, e.g., to randomly generate multiple sphere sets with the same max_overshoot.

The mesh input to GenerateCollisionSpheres() is not required to be strictly watertight but is expected to generally represent a volume. For each triangle, the normal is (by default) computed assuming a counter-clockwise winding direction for vertex indices. If the mesh is input with clockwise winding direction for vertex indices, then flip_normals should be set to true to reverse winding order. The correct surface normal orientation for triangles is required for accurately determining whether grid points are interior or exterior to the mesh.

Very small and/or degenerate triangles are unlikely to aid in collision sphere generation. Thus, triangles with surface area less than min_triangle_area are culled prior to sphere generation and selection. The min_triangle_area must be positive.