`path_ik` Namespace

Definition

namespace path_ik { ... }

Namespaces Index

namespace	internal

Classes Index

struct	CartesianLimits
	Cartesian velocity and acceleration limits for the trapezoidal profile. More...

struct	CollisionCheckOptions
	Contains options for path collision checking. More...

struct	ErrorType


struct	GoalTolerance
	Tolerances that the final path waypoint (the goal) must satisfy. More...

class	PartialPathError


struct	PathCollisionInfo
	Struct to hold information about path collisions. More...

struct	PathIKOptions
	Options passed to the Path Inverse Kinematics solver. More...

struct	ResampledTrajectory
	Resampled trajectory data at uniform control rate. More...

struct	SdfControlPoint
	A single collision proxy that the SDF nullspace task should push away from obstacles. More...

struct	SdfRepulsionParameters
	Tunables for the SDF repulsion nullspace task. More...

struct	TrajectoryOptions
	Options for creating a time-parameterized trajectory from waypoints. More...

struct	TrapezoidalProfileError

Typedefs Index

using	NullspaceOverrideFunction = std::function< Eigen::VectorXd(const Eigen::VectorXd &current_nullspace_component, const Eigen::VectorXd &current_joint_values)>
	Function signature for a custom nullspace task. More...

using	JointSpacePath = std::vector< Eigen::VectorXd >


using	Path = std::vector< Eigen::Isometry3d >


using	CartesianTipVector = Eigen::Vectord< 6 *kMaxNumTips >


using	TipJointMap = Eigen::StaticMatrix< bool, kMaxNumTips, kMaxNumJoints >

Enumerations Index

enum class	SaturationPhase { ... }


enum class	TipConstraint { ... }
	Possible task-space constraints when solving kinematics for a path. More...

Functions Index

tl::expected< ResampledTrajectory, TrapezoidalProfileError >	fitCartesianTrapezoidalVelocityProfile (const Path &cartesian_path, const JointSpacePath &joint_path, const CartesianLimits &cartesian_limits, const Eigen::VectorXd &max_joint_velocities, const Eigen::VectorXd &max_joint_accelerations, int control_rate)
	Assign timestamps to a dense joint-space path so the Cartesian-space trajectory follows a trapezoidal velocity profile. More...

tl::expected< trajectory_msgs::msg::JointTrajectory, std::string >	toJointTrajectoryMessage (const ResampledTrajectory &trajectory, const std::vector< std::string > &joint_names)
	Convert a ResampledTrajectory into a JointTrajectory message. More...

template <int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
auto	dampedLeastSquaresInverse (const Eigen::Matrix< double, Rows, Cols > &matrix, double damping) -> Eigen::Matrix< double, Cols, Rows >
	Compute the Damped Least Squares inverse of a matrix. More...

tl::expected< Path, ErrorType >	interpolate (const Eigen::Isometry3d &start, const Eigen::Isometry3d &end, double max_translational_step, double max_angular_step)
	Interpolate a Cartesian segment at a given density. More...

tl::expected< Path, ErrorType >	interpolate (const std::vector< Eigen::Isometry3d > &path, double max_translational_step, double max_angular_step, double blending_radius)
	Interpolate a Cartesian path at a given density. More...

std::vector< Eigen::Vector3d >	interpolateBetweenPoints (const Eigen::Vector3d &point_a, const Eigen::Vector3d &point_b, double spacing)
	Interpolate linearly between two points in 3D space. More...

template <typename MatrixType>
auto	computeJacobianForChain (const moveit_pro::base::RobotState &state, const moveit_pro::base::JointModelGroup &group, const moveit_pro::base::LinkModel &tip_link, const Eigen::Vector3d &tip_offset) -> tl::expected< MatrixType, std::string >
	Compute the Jacobian matrix for a given tip link in a kinematic chain. More...

tl::expected< Eigen::MatrixNd, std::string >	computeMultiTipJacobian (const moveit_pro::base::RobotState &state, const moveit_pro::base::JointModelGroup &group, std::span< const moveit_pro::base::LinkModel *const > tip_links, const std::vector< Eigen::Vector3d > &tip_offsets)
	Compute the Jacobian matrix for a kinematic tree with multiple tips. More...

Eigen::Isometry3d	getWorldPoseInRootCoordinates (const moveit_pro::base::RobotState &state, const moveit_pro::base::JointModelGroup &group)
	Get the pose of the world frame given in the group root link frame, i.e. `root_pose_world. at the given state. More...

Eigen::Vector3d	rotationToAxisAngleVector (const Eigen::Quaterniond &rotation)
	Converts a 3D rotation given by a quaternion into its axis-angle (u-theta) representation. More...

Eigen::Vector6d	poseToPoseVector (const Eigen::Isometry3d &pose)
	Converts a 3D Pose into its 6D vector representation (t, u-theta). More...

Eigen::Vector6d	poseError (const Eigen::Isometry3d &a, const Eigen::Isometry3d &b)
	Computes the 3D error (translation and orientation) between two poses. More...

template <class LinearPart>
Eigen::Matrix6d	twistRotationMatrix (const LinearPart &rotation)
	Computes the 6x6 twist rotation matrix corresponding to a rotation. More...

template <class LinearPart>
Eigen::MatrixNd	multiTipTwistRotationMatrix (const LinearPart &rotation, const size_t num_tips)
	Computes the NxN twist rotation matrix corresponding to a rotation. More...

Eigen::Vector6d	transformTwist (const Eigen::Vector6d &twist_a, const Eigen::Isometry3d &a_pose_b)
	Re-expresses a body twist from frame a to a rigidly-attached frame b, accounting for the lever-arm (omega x r) shift between the two origins. More...

double	angleDifference (const double angle_a, const double angle_b)
	Compute the unsigned angle difference between two angles. More...

Eigen::VectorXd	jointLimitAvoidanceTask (const Eigen::VectorXd &lower_limits, const Eigen::VectorXd &upper_limits, const Eigen::VectorXd &joint_values, double activation_fraction, double gain)
	A nullspace task to stay away from joint limits. More...

tl::expected< JointSpacePath, ErrorType >	pathIK (moveit_pro::base::RobotState &robot_state, const moveit_pro::base::JointModelGroup &group, const std::vector< const moveit_pro::base::LinkModel * > &tip_links, const Path &reference_path, const PathIKOptions &options)
	Computes the incremental Inverse Kinematics over a given Cartesian path, for multiple tips. More...

tl::expected< JointSpacePath, ErrorType >	pathIK (moveit_pro::base::RobotState &robot_state, const moveit_pro::base::JointModelGroup &group, const moveit_pro::base::LinkModel &tip_link, const Path &reference_path, const PathIKOptions &options)
	Computes the incremental Inverse Kinematics over a given Cartesian path, for a single tip. More...

tl::expected< std::vector< Eigen::VectorXd >, std::string >	downsamplePath (const moveit_pro::base::JointModelGroup &group, const JointSpacePath &path, const moveit_pro::base::RobotState &reference_robot_state, double joint_space_step, double cartesian_space_step, std::vector< std::size_t > *mapping=nullptr)
	Downsamples a given path to a given resolution. More...

tl::expected< JointSpacePath, std::string >	interpolateJointPath (const JointSpacePath &path, double max_joint_step)
	Densifies a joint-space path by linear interpolation, bounded by a per-joint step. More...

bool	isPathInCollision (const moveit_pro::base::JointModelGroup &group, const JointSpacePath &path, const CollisionCheckOptions &options, moveit_pro::base::planning_scene::PlanningScene &planning_scene, PathCollisionInfo &collision_info)
	Checks if a given path is in collision. More...

tl::expected< Path, std::string >	computeTipPathFromJointSpacePath (moveit_pro::base::RobotState &robot_state, const moveit_pro::base::JointModelGroup &group, const moveit_pro::base::LinkModel &tip_link, const Eigen::Isometry3d &tip_offset, const JointSpacePath &joint_path)
	Compute the Cartesian path traced by a tip link as the robot moves through a joint-space path. More...

Eigen::VectorXd	sdfRepulsionTask (const moveit_pro::base::distance_field::DistanceField &sdf, moveit_pro::base::RobotState &fk_state, const moveit_pro::base::JointModelGroup &group, const Eigen::VectorXd &current_joint_values, std::span< const SdfControlPoint > control_points, const SdfRepulsionParameters &params)
	Compute a nullspace joint-space nudge that pushes each control-point sphere away from the nearest obstacle in sdf. More...

tl::expected< moveit_pro::base::trajectory_processing::Trajectory, std::string >	createTrajectoryFromWaypoints (const std::vector< Eigen::VectorXd > &waypoints, const Eigen::VectorXd &max_joint_velocities, const Eigen::VectorXd &max_joint_accelerations, const TrajectoryOptions &options)
	Create a trajectory from a list of waypoints using the trajectory_processing library. More...

tl::expected< moveit_pro::base::trajectory_processing::Trajectory, std::string >	createTrajectoryFromWaypoints (const std::vector< Eigen::VectorXd > &waypoints, const Eigen::VectorXd &max_joint_velocities, const Eigen::VectorXd &max_joint_accelerations, double velocity_scale_factor, double acceleration_scale_factor)


tl::expected< trajectory_msgs::msg::JointTrajectory, std::string >	createTrajectoryFromWaypoints (const moveit_pro::base::JointModelGroup &group, const std::vector< Eigen::VectorXd > &waypoints, const TrajectoryOptions &options)
	Create a trajectory message from a list of waypoints. More...

tl::expected< trajectory_msgs::msg::JointTrajectory, std::string >	createTrajectoryFromWaypoints (const moveit_pro::base::JointModelGroup &group, const std::vector< Eigen::VectorXd > &waypoints, double velocity_scale_factor, double acceleration_scale_factor, int sampling_rate)


tl::expected< void, std::string >	appendToTrajectoryMessage (const std::vector< std::string > &joint_names, const moveit_pro::base::trajectory_processing::Trajectory &trajectory, int sampling_rate, trajectory_msgs::msg::JointTrajectory &trajectory_msg)
	Sample a moveit_pro::base::trajectory_processing::Trajectory at a frequency, and append to a JointTrajectory message. More...

tl::unexpected< ErrorType >	errorWithMessage (const std::string &error_message)


template < ... >
auto	velocityInverseKinematics (const Eigen::MatrixBase< Eigen::Matrix< Scalar, Rows, Cols, Options, MaxRows, MaxCols > > &jacobian, double damping, const Eigen::Matrix< Scalar, Rows, 1, Options, MaxRows, 1 > &cartesian_velocity, const Eigen::MatrixBase< Eigen::Matrix< Scalar, Cols, 1, Options, MaxCols, 1 > > &nullspace_component) -> Eigen::Matrix< Scalar, Cols, 1, Options, MaxCols, 1 >
	Compute joint velocities corresponding to a given Cartesian velocity. More...

tl::expected< Eigen::MatrixNd, std::string >	computeMultiTipJacobian (const moveit_pro::base::RobotState &state, const moveit_pro::base::JointModelGroup &group, const std::span< const LinkModel *const > tip_links, const std::vector< Eigen::Vector3d > &tip_offsets)


list[ndarray]	path_ik (RobotState robot_state, JointModelGroup group, list[str] tip_links, list[ndarray] reference_path, PathIKOptions\|None options=None)

Variables Index

constexpr double	kDefaultPathIkTranslationalStep = 0.001


constexpr double	kDefaultPathIkAngularStep = 0.001


constexpr int	kMaxNumTips = 4


constexpr int	kMaxNumJoints = 256


constexpr double	kDistanceToJointLimitThreshold = 0.001


constexpr double	kConvergenceThreshold = 1e-4


constexpr double	kGoalRefinementMinErrorImprovement = 1e-6


constexpr int	kMaxGoalRefinementIterations = 100

Typedefs

CartesianTipVector

using path_ik::CartesianTipVector = typedef Eigen::Vectord<6 * kMaxNumTips>

Definition at line 61 of file types.hpp.

JointSpacePath

using path_ik::JointSpacePath = typedef std::vector<Eigen::VectorXd>

Definition at line 27 of file types.hpp.

NullspaceOverrideFunction

using path_ik::NullspaceOverrideFunction = typedef std::function<Eigen::VectorXd(const Eigen::VectorXd& current_nullspace_component, const Eigen::VectorXd& current_joint_values)>

Function signature for a custom nullspace task.

This function is called during the IK optimization process and allows users to inject custom nullspace costs. The function receives the current nullspace component (computed from built-in tasks like joint limit avoidance) and the current joint values, and should return the modified nullspace component. It gives users the opportunity to modify or fully override the nullspace behavior of the solver.

Parameters

current_nullspace_component	The nullspace component computed from built-in tasks (joint limits, orientation tracking, etc.).
current_joint_values	The current joint values for the group being solved.

Returns: Modified nullspace component to be used in the velocity IK.

Definition at line 49 of file path_ik.hpp.

Path

using path_ik::Path = typedef std::vector<Eigen::Isometry3d>

Definition at line 58 of file types.hpp.

TipJointMap

using path_ik::TipJointMap = typedef Eigen::StaticMatrix<bool, kMaxNumTips, kMaxNumJoints>

Definition at line 64 of file types.hpp.

Enumerations

SaturationPhase

enum class path_ik::SaturationPhase

strong

Enumeration values

kRamp	Acceleration/deceleration ramp: the tangential acceleration limit binds
kCruise	Constant-Cartesian-speed cruise: path curvature binds, not the acceleration limit

Phase of the trapezoidal profile in which a joint-acceleration violation occurred. The phase decides the remedy: a ramp violation is driven by the Cartesian acceleration limit, while a cruise violation comes from path curvature at constant speed and is unaffected by that limit.

Definition at line 34 of file cartesian_timing.hpp.

TipConstraint

enum class path_ik::TipConstraint

strong

Possible task-space constraints when solving kinematics for a path.

Enumeration values

kPositionOnly
kPositionAndOrientation

A constraint needs to be always satisfied by the solution at every waypoint.

Definition at line 31 of file path_ik.hpp.

Functions

angleDifference()

double path_ik::angleDifference (const double angle_a, const double angle_b)

Compute the unsigned angle difference between two angles.

Parameters

angle_a	The first angle in radians.
angle_b	The second angle in radians.

Returns: The unsigned smallest angle difference in radians.

Definition at line 75 of file math.cpp.

appendToTrajectoryMessage()

tl::expected< void, std::string > path_ik::appendToTrajectoryMessage (const std::vector< std::string > & joint_names, const moveit_pro::base::trajectory_processing::Trajectory & trajectory, int sampling_rate, trajectory_msgs::msg::JointTrajectory & trajectory_msg)

Sample a moveit_pro::base::trajectory_processing::Trajectory at a frequency, and append to a JointTrajectory message.

Parameters

joint_names	The joint names to use for the trajectory.
trajectory	The moveit_pro::base::trajectory_processing::Trajectory to append.
sampling_rate	The sampling rate to use for the trajectory.
trajectory_msg	The JointTrajectory message to append to.

Returns: An error message if the trajectory could not be appended.

Definition at line 137 of file trajectory_utils.cpp.

computeJacobianForChain()

template <typename MatrixType>

tl::expected< MatrixType, std::string > path_ik::computeJacobianForChain (const moveit_pro::base::RobotState & state, const moveit_pro::base::JointModelGroup & group, const moveit_pro::base::LinkModel & tip_link, const Eigen::Vector3d & tip_offset)

Compute the Jacobian matrix for a given tip link in a kinematic chain.

This function computes the Jacobian matrix for a kinematic chain. The Jacobian matrix is computed for given tip link and an optional position offset. The Jacobian matrix is computed in the frame of the root link of the kinematic chain. The columns in the Jacobian are ordered by the active joints in the joint model group, one column per active variable, i.e. the layout RobotState::setJointGroupActivePositions() expects. The number of rows will always be 6 (translation / axis-angle rotation). Therefore, the Jacobian will be a 6 x M matrix, where M is the number of active variables. The Cartesian velocity at the tips can then be computed as:

v = J * q_dot

Mimic joints get no column of their own: the motion each one induces at the tip is folded, scaled by the mimic multiplier, into the column of the active joint that drives it. This follows every mimic of an active joint, whether or not the mimic joint is a member of the group, which is exactly the set RobotState moves when that joint moves, so the Jacobian agrees with forward kinematics. A mimic joint in the group whose driving joint is not an active joint of the group does not move when the group's joints move, so it contributes nothing. Note that an active joint that is not an ancestor of tip_link still gets a non-zero column when it drives a mimic joint that is: its own contribution is zero, but the mimic joint's is not.

The function is templated to allow for different matrix types, such as Eigen::MatrixXd (dynamic) or Eigen::MatrixNd (statically-allocated), suitable for real-time use.

Parameters

state	The current robot state, needs to be up to date (not dirty), or the function will return error.
group	The joint model group for which to compute the Jacobian. It needs to be a kinematic chain, or the function will return an error.
tip_link	The link for which to compute the Jacobian. Must be part of the group or a descendant link.
tip_offset	Translational offset from the tip link to the point for which the Jacobian is computed. The offsets are expressed in the frame of the tip link.

Returns: the Jacobian matrix if successful, or an error message if the computation failed.

Definition at line 286 of file jacobian.hpp.

computeMultiTipJacobian()

tl::expected< Eigen::MatrixNd, std::string > path_ik::computeMultiTipJacobian (const moveit_pro::base::RobotState & state, const moveit_pro::base::JointModelGroup & group, std::span< const moveit_pro::base::LinkModel *const > tip_links, const std::vector< Eigen::Vector3d > & tip_offsets)

Compute the Jacobian matrix for a kinematic tree with multiple tips.

This function computes the Jacobian matrix for a kinematic tree with multiple tips. The Jacobian matrix is computed for the tips and the given offsets. The Jacobian matrix is computed in the frame of the root link of the kinematic tree. The columns in the Jacobian are ordered by the active joints in the joint model group, one column per active variable, i.e. the layout RobotState::setJointGroupActivePositions() expects. The number of rows will depend on the number of tips (each tip will add 6 rows to the Jacobian). Therefore, the multi-tip Jacobian will be a 6N x M matrix, where N is the number of tips and M the number of active variables. Cartesian velocities for the tips can then be computed as: [v1; v2; ...; vN] = J * q_dot

Mimic joints are folded into the column of the active joint that drives them, scaled by the mimic multiplier, exactly as in computeJacobianForChain(): an active joint that does not move a tip itself still gets a non-zero column for that tip when it drives a mimic joint that does.

The function uses statically allocated matrices and returns an error if the number of tip links, robot joints, or group joint variables exceeds their capacities (kMaxNumTips, kMaxNumJoints, and Eigen::kMaxEigenVectorCapacity respectively).

Parameters

state	The current robot state, needs to be up to date (not dirty), or the function will return error.
group	The joint model group for which to compute the Jacobian. It needs to be a kinematic tree, i.e. all joints are connected to a single root joint.
tip_links	The links for which to compute the Jacobian. The links must be part of the joint group.
tip_offsets	Translational offsets from the tip links to the point for which the Jacobian is computed. The offsets are expressed in the frame of the tip links.

Returns: the Jacobian matrix if successful, or an error message if the computation failed.

Definition at line 88 of file jacobian.hpp.

computeMultiTipJacobian()

tl::expected< Eigen::MatrixNd, std::string > path_ik::computeMultiTipJacobian (const moveit_pro::base::RobotState & state, const moveit_pro::base::JointModelGroup & group, const std::span< const LinkModel *const > tip_links, const std::vector< Eigen::Vector3d > & tip_offsets)

Definition at line 75 of file jacobian.cpp.

computeTipPathFromJointSpacePath()

tl::expected< Path, std::string > path_ik::computeTipPathFromJointSpacePath (moveit_pro::base::RobotState & robot_state, const moveit_pro::base::JointModelGroup & group, const moveit_pro::base::LinkModel & tip_link, const Eigen::Isometry3d & tip_offset, const JointSpacePath & joint_path)

Compute the Cartesian path traced by a tip link as the robot moves through a joint-space path.

Applies forward kinematics to each joint-space waypoint and records the (offset) tip link pose, producing exactly one Cartesian pose per joint-space waypoint. Use this to build the Cartesian path that matches a joint-space path 1:1, e.g. for fitCartesianTrapezoidalVelocityProfile().

robot_state is used as scratch space for the forward kinematics, so its joint positions are modified.

Parameters

robot_state	The robot state used to compute forward kinematics.
group	The joint model group the path is defined on.
tip_link	The link whose poses are computed.
tip_offset	Offset applied to the tip link frame at each waypoint.
joint_path	The joint-space path. Every waypoint must match the group's active variable count.

Returns: The Cartesian path of the (offset) tip link, with the same size as joint_path, or an error message if a waypoint dimension doesn't match the group.

Definition at line 297 of file path_utils.cpp.

createTrajectoryFromWaypoints()

tl::expected< moveit_pro::base::trajectory_processing::Trajectory, std::string > path_ik::createTrajectoryFromWaypoints (const std::vector< Eigen::VectorXd > & waypoints, const Eigen::VectorXd & max_joint_velocities, const Eigen::VectorXd & max_joint_accelerations, const TrajectoryOptions & options)

Create a trajectory from a list of waypoints using the trajectory_processing library.

Parameters

waypoints	List of waypoints to create a trajectory from.
max_joint_velocities	Maximum joint velocities for the robot.
max_joint_accelerations	Maximum joint accelerations for the robot.
options	Trajectory creation options. sampling_rate is unused because this overload returns a continuous trajectory rather than a sampled message. max_deviation is measured in radians for revolute joints or meters for prismatic joints and must be greater than zero so the blended path is differentiable. Smaller values keep the trajectory closer to the waypoints at the cost of slower corners.

Returns: A timed trajectory if successful, or an error message if the trajectory could not be created.

Definition at line 21 of file trajectory_utils.cpp.

createTrajectoryFromWaypoints()

Definition at line 57 of file trajectory_utils.cpp.

createTrajectoryFromWaypoints()

tl::expected< trajectory_msgs::msg::JointTrajectory, std::string > path_ik::createTrajectoryFromWaypoints (const moveit_pro::base::JointModelGroup & group, const std::vector< Eigen::VectorXd > & waypoints, const TrajectoryOptions & options)

Create a trajectory message from a list of waypoints.

Maximum joint velocities / accelerations are obtained from the given joint model group.

Parameters

group	The joint model group to use for the trajectory.
waypoints	List of waypoints to create a trajectory from.
options	Trajectory creation options. max_deviation is measured in radians for revolute joints or meters for prismatic joints and must be greater than zero so the blended path is differentiable. Smaller values keep the trajectory closer to the waypoints at the cost of slower corners.

Returns: A JointTrajectory message if successful, or an error message if the trajectory could not be created.

Definition at line 68 of file trajectory_utils.cpp.

createTrajectoryFromWaypoints()

Definition at line 126 of file trajectory_utils.cpp.

dampedLeastSquaresInverse()

template <int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>

Eigen::Matrix< double, Cols, Rows > path_ik::dampedLeastSquaresInverse (const Eigen::Matrix< double, Rows, Cols > & matrix, double damping)

Compute the Damped Least Squares inverse of a matrix.

This function is templated on the number of rows and columns, which allows using static Eigen types if desired. 'damping' must be positive or zero. A very small value may cause instabilities around singularities. A larger value will behave better at singularities at the cost of deviations and longer convergence times. A value around 0.01 is normally good enough for servoing/control applications. Planning applications (e.g. IK, etc.) may use lower values for faster convergence. For reference see: http://graphics.cs.cmu.edu/nsp/course/15464-s17/lectures/iksurvey.pdf

Definition at line 26 of file damped_least_squares_inverse.hpp.

downsamplePath()

tl::expected< std::vector< Eigen::VectorXd >, std::string > path_ik::downsamplePath (const moveit_pro::base::JointModelGroup & group, const JointSpacePath & path, const moveit_pro::base::RobotState & reference_robot_state, double joint_space_step, double cartesian_space_step, std::vector< std::size_t > * mapping=nullptr)

Downsamples a given path to a given resolution.

Given a dense path, this function removes waypoints so that any two adjacent waypoints are not closer than joint_space_step in joint-space (L1 norm) or cartesian_space_step in Cartesian space (Euclidean distance on the translation at the last link of the group). This can be helpful to downsample a dense path (e.g. from a trajectory) but keeping a desired 'resolution' both in joint-space and Cartesian space, for instance for a dense-enough collision check.

The first and last waypoints of the path are always included in the output. All other waypoints in the output were also part of the original path, i.e. this function doesn't create new waypoints, just removes some, and keeps others. If the waypoints in the input are already separated more than joint_space_step and cartesian_space_step, the path won't be modified.

reference_robot_state is a RobotState to be used as reference when computing forward kinematics.

If a mapping argument is provided, it will be filled in with the mapping between the new path indexes and the previous path indexes, i.e. mapping[i] will contain the original path index that corresponds to index i in the new path.

Returns: A downsampled version of the path, where adjacent waypoints are spaced not closer than joint_space_step or cartesian_space_step.

Definition at line 152 of file path_utils.cpp.

errorWithMessage()

tl::unexpected< ErrorType > path_ik::errorWithMessage (const std::string & error_message)

inline

Definition at line 52 of file types.hpp.

fitCartesianTrapezoidalVelocityProfile()

tl::expected< ResampledTrajectory, TrapezoidalProfileError > path_ik::fitCartesianTrapezoidalVelocityProfile (const Path & cartesian_path, const JointSpacePath & joint_path, const CartesianLimits & cartesian_limits, const Eigen::VectorXd & max_joint_velocities, const Eigen::VectorXd & max_joint_accelerations, int control_rate)

Assign timestamps to a dense joint-space path so the Cartesian-space trajectory follows a trapezoidal velocity profile.

The Cartesian path and joint path must have the same size (N waypoints). The joint path is typically the output of pathIK(). The trapezoidal profile respects both translational and rotational Cartesian limits using a rate-limited arc length parameterization. Joint velocities and accelerations are computed analytically from the profile.

If the resulting joint-space velocities or accelerations exceed the given limits, the function returns a TrapezoidalProfileError containing the required time-dilation factor.

Parameters

cartesian_path	Dense Cartesian path (N 6-DOF poses).
joint_path	Dense joint-space path (N joint vectors, from pathIK).
cartesian_limits	Translational and rotational velocity/acceleration limits.
max_joint_velocities	Per-joint velocity limits (rad/s).
max_joint_accelerations	Per-joint acceleration limits (rad/s^2).
control_rate	Trajectory sampling rate in Hz.

Returns: A uniformly sampled ResampledTrajectory, or a TrapezoidalProfileError.

Definition at line 376 of file cartesian_timing.cpp.

getWorldPoseInRootCoordinates()

Eigen::Isometry3d path_ik::getWorldPoseInRootCoordinates (const moveit_pro::base::RobotState & state, const moveit_pro::base::JointModelGroup & group)

Get the pose of the world frame given in the group root link frame, i.e. `root_pose_world. at the given state.

Definition at line 63 of file jacobian.cpp.

interpolate()

tl::expected< Path, ErrorType > path_ik::interpolate (const Eigen::Isometry3d & start, const Eigen::Isometry3d & end, double max_translational_step, double max_angular_step)

Interpolate a Cartesian segment at a given density.

This function will interpolate in Cartesian space between the given 'start' and 'end' poses in such a way that adjacent waypoints will always be closer than 'max_translational_step' in 3D translation (meters) and 'max_angular_step' in 3D rotation (radians). 'max_translational_step' and 'max_angular_step' must always be non-zero and positive. Otherwise an error will be returned.

Returns: The interpolated path as a sequence of poses, or an error.

Definition at line 94 of file interpolate.cpp.

interpolate()

tl::expected< Path, ErrorType > path_ik::interpolate (const std::vector< Eigen::Isometry3d > & path, double max_translational_step, double max_angular_step, double blending_radius)

Interpolate a Cartesian path at a given density.

This function will interpolate in Cartesian space along the given waypoints in such a way that:

Adjacent waypoints will always be closer than 'max_translational_step' in 3D translation (meters) and 'max_angular_step' in 3D rotation (radians) for the linear segments of the output path (not necessarily at the corners).
Every intermediate waypoint will be 'blended' according to the given 'blending_radius' in order to generate a smooth curve that can be executed continuously by the robot.
The initial and final waypoints of the path will be reached exactly.

'max_translational_step', 'max_angular_step' and 'blending_radius' must always be non-zero and positive. 'blending_radius' must also be smaller than half the distance between the closest two waypoints in the input path, i.e. it must be possible to apply the given 'blending_radius'. Otherwise an error will be returned.

Returns: The interpolated path as a sequence of poses, or an error.

Definition at line 145 of file interpolate.cpp.

interpolateBetweenPoints()

std::vector< Eigen::Vector3d > path_ik::interpolateBetweenPoints (const Eigen::Vector3d & point_a, const Eigen::Vector3d & point_b, double spacing)

Interpolate linearly between two points in 3D space.

This function will return a vector of points that are evenly spaced between the given 'point_a' and 'point_b', with the specified 'spacing' in meters. The input points will be included in the result. This function asserts that 'point_a' and 'point_b' are not the same point, and that 'spacing' is positive.

Parameters

point_a	The first point in 3D space.
point_b	The second point in 3D space.
spacing	The distance between adjacent points in meters.

Returns: A vector of points that are evenly spaced between 'point_a' and 'point_b'.

Definition at line 221 of file interpolate.cpp.

interpolateJointPath()

tl::expected< JointSpacePath, std::string > path_ik::interpolateJointPath (const JointSpacePath & path, double max_joint_step)

Densifies a joint-space path by linear interpolation, bounded by a per-joint step.

Given a sparse path, this function inserts linearly interpolated waypoints between every pair of consecutive input waypoints so that no single joint moves more than max_joint_step (radians for revolute joints, meters for prismatic joints) between adjacent output waypoints. The L-infinity (cwiseAbs().maxCoeff()) metric on the joint-delta vector is what bounds the step.

The first and last waypoints of each input segment are preserved exactly in the output, and the function does not remove any input waypoint. Intended for densifying a sparse RRT/connector path for finer collision checking or visualization.

Parameters

path	Input joint-space path. Must have at least two waypoints, all of the same non-zero dimension, with finite joint values.
max_joint_step	Maximum allowed per-joint displacement between adjacent output waypoints. Must be positive, finite, and large enough that no segment requires more than one million interpolation steps.

Returns: The densified joint-space path, or an error message describing why the input is invalid.

Definition at line 213 of file path_utils.cpp.

isPathInCollision()

bool path_ik::isPathInCollision (const moveit_pro::base::JointModelGroup & group, const JointSpacePath & path, const CollisionCheckOptions & options, moveit_pro::base::planning_scene::PlanningScene & planning_scene, PathCollisionInfo & collision_info)

Checks if a given path is in collision.

Given a joint-space path, this function checks if any waypoint in that path is in collision with the given planning_scene. This function does discrete collision-checking at every waypoint, i.e. it doesn't perform a swept-volume check between two adjacent waypoints.

Collision checking is done according to the options specified in the options parameter.

Throws std::invalid_argument if the path has fewer than two waypoints, or if any waypoint does not hold exactly one value per active joint variable of group. Both are caller errors rather than runtime conditions, and this function's bool return has no channel to report them through: a false would read as "no collision" for a path that was never checked.

Returns: true if the path is in collision at any waypoint along the path, and fills in collision_info with information about the collision (e.g. path index where the collision was found).

Definition at line 269 of file path_utils.cpp.

jointLimitAvoidanceTask()

Eigen::VectorXd path_ik::jointLimitAvoidanceTask (const Eigen::VectorXd & lower_limits, const Eigen::VectorXd & upper_limits, const Eigen::VectorXd & joint_values, double activation_fraction, double gain)

A nullspace task to stay away from joint limits.

This function computes the gradient of a distance-to-joint-limits cost function. The cost function is defined as follows:

Given the lower limits (ll) and the upper limits (ul).
Given an activation distance (ad), defined as a distance to the joint limits.
We compute the lower and upper activation thresholds (lt and ut), as: lt = ll + ad ut - ul - ad
Given the current joint values (q), and a cost gain 'k', the cost is defined as: 0 if lt <= q <= ut k * (q - ut)^2 if q > ut k * (q - lt)^2 if q < lt

The cost is zero if the joint values are further than the activation distance from the limits. If the joint values become closer than the activation distance to a limit, the cost increases quadratically with the encroachment on the activation threshold. The gain 'k' determines how quickly the cost increases.

The gradient of this cost function is given by: 0 if lt <= q <= ut k * (q - ut) if q > ut k * (ut - q) if q < lt

lower_limits and upper_limits are the joints lower and upper position limits. joint_values contains the current joint values. activation_fraction determines the activation thresholds, as a fraction of the joint range, e.g. a value of 0.1 will activate the cost when a joint value is closer to the limit than 10% of the joint range. gain controls how quickly the cost increases with the encroachment into the activation region.

Definition at line 14 of file nullspace_tasks.cpp.

multiTipTwistRotationMatrix()

template <class LinearPart>

Eigen::MatrixNd path_ik::multiTipTwistRotationMatrix (const LinearPart & rotation, const size_t num_tips)

Computes the NxN twist rotation matrix corresponding to a rotation.

Parameters

rotation	The stacked 3x3 rotation matrix.
num_tips	How many tips to create the rotation matrix for.

Returns: a NxN matrix that rotates a twist by the given rotation, where N=6*num_tips.

Definition at line 57 of file math.hpp.

path_ik()

list[ndarray] path_ik.path_ik (RobotState robot_state, JointModelGroup group, list tip_links=[str], list reference_path=[ndarray], PathIKOptions|None options=None)

Compute a joint-space path that follows the given Cartesian reference path using inverse kinematics.
Args:
    robot_state: The initial robot state
    group: The name of the planning group to use for IK.
    tip_link: The name of the tip link for the IK chain.
    reference_path: A list of Cartesian poses (or waypoints) to follow.
    options (PathIKOptions, optional): Additional options for path IK. If None, defaults are used.
Returns:
    list: A list of joint-space states corresponding to the reference path.
Raises:
    PartialPathError: If the IK solver cannot compute a solution for the entire path. The exception contains
        the partial path that was successfully computed before failure.
    Message: If other errors occur during IK computation.

Definition at line 21 of file path_ik.py.

pathIK()

tl::expected< JointSpacePath, ErrorType > path_ik::pathIK (moveit_pro::base::RobotState & robot_state, const moveit_pro::base::JointModelGroup & group, const std::vector< const moveit_pro::base::LinkModel * > & tip_links, const Path & reference_path, const PathIKOptions & options)

Computes the incremental Inverse Kinematics over a given Cartesian path, for multiple tips.

The initial joint-space configuration and starting pose are given by 'robot_state', which needs to be set accordingly before this call. 'reference_path' is the path to follow in Cartesian space. It should be given with respect to the group root link. This function will densely sample the space along the reference path, and compute the multi-tip incremental inverse kinematics. The intermediate waypoints will be 'blended' with the given radius (in 'options') in order to create a smooth dense path. The solution is guaranteed to not contain large joint-space reconfigurations. The output of the function is still a path, it won't contain any timing information. Before its execution, the joint-space path will need to be timed via a trajectory timing algorithm.

The reference_path doesn't need to start at any of the given tip links. It can be anywhere in the space, but the given tip_links will move relative to the given reference path, based on their relative pose to the reference path in the given initial robot_state.

Parameters

robot_state	The current robot state, with the initial joint-space configuration. The current robot state is used to determine the initial pose of the tips to be controlled with respect to the given reference_path. On success it is left at the final solved configuration; on failure it is left at the last attempted configuration, which may be one step beyond the returned partial path.
group	The joint model group to solve the kinematics for. It can be a chain or a tree if solving for multiple tips. Groups containing mimic joints are supported: the solve works over the group's active variables and the mimic joints follow their driving joint. A mimic joint's own position limits are not enforced, only its driving joint's.
tip_links	The links to move according to the given reference_path. The links must be part of the joint group.
reference_path	The path to follow in Cartesian space. It should be given with respect to the group root link.
options	The options to use when solving the kinematics. See PathIKOptions for more information.

Returns: A dense joint-space path to move the robot along the reference path, or an error if the kinematics couldn't be solved. In that case, ErrorType may contain the section of the path that could be solved, if a solvable section was found.

Definition at line 452 of file path_ik.cpp.

pathIK()

tl::expected< JointSpacePath, ErrorType > path_ik::pathIK (moveit_pro::base::RobotState & robot_state, const moveit_pro::base::JointModelGroup & group, const moveit_pro::base::LinkModel & tip_link, const Path & reference_path, const PathIKOptions & options)

Computes the incremental Inverse Kinematics over a given Cartesian path, for a single tip.

See the multi-tip version above for details.

Parameters

robot_state	The current robot state, with the initial joint-space configuration.
group	The joint model group to solve the kinematics for.
tip_link	The link to move according to the given reference_path.
reference_path	The path to follow in Cartesian space.
options	The options to use when solving the kinematics.

Returns: A dense joint-space path to move the robot along the reference path, or an error if the kinematics couldn't be solved. In that case, ErrorType may contain the section of the path that could be solved, if a solvable section was found.

Definition at line 442 of file path_ik.cpp.

poseError()

Eigen::Vector6d path_ik::poseError (const Eigen::Isometry3d & a, const Eigen::Isometry3d & b)

Computes the 3D error (translation and orientation) between two poses.

The error is given as a 6D vector (t u-theta), with the first 3 components containing the translation error, and the last three components containing the orientation error in an axis-angle representation.

Returns: a 6D vector with the translation and orientation error (t u-theta).

Definition at line 52 of file math.cpp.

poseToPoseVector()

Eigen::Vector6d path_ik::poseToPoseVector (const Eigen::Isometry3d & pose)

Converts a 3D Pose into its 6D vector representation (t, u-theta).

Returns: a 6D vector where the translation part is directly the translation part of the input pose, and the rotation part is the axis-angle representation of the rotation.

Definition at line 43 of file math.cpp.

rotationToAxisAngleVector()

Eigen::Vector3d path_ik::rotationToAxisAngleVector (const Eigen::Quaterniond & rotation)

Converts a 3D rotation given by a quaternion into its axis-angle (u-theta) representation.

Returns: a 3D vector with the axis-angle representation of the given quaternion.

Definition at line 18 of file math.cpp.

sdfRepulsionTask()

Eigen::VectorXd path_ik::sdfRepulsionTask (const moveit_pro::base::distance_field::DistanceField & sdf, moveit_pro::base::RobotState & fk_state, const moveit_pro::base::JointModelGroup & group, const Eigen::VectorXd & current_joint_values, std::span< const SdfControlPoint > control_points, const SdfRepulsionParameters & params)

Compute a nullspace joint-space nudge that pushes each control-point sphere away from the nearest obstacle in sdf.

Intended to be called inside a PathIKOptions::setNullspaceOverrideFunction(...) lambda; how the returned nudge is combined with the existing nullspace component (added, assigned, scaled) is the caller's choice.

For each control point: queries the SDF at the sphere center, computes the surface clearance (distance - radius), and if it's below params.activation_distance builds a Cartesian repulsion velocity (gain * penetration along the SDF gradient direction). That Cartesian velocity is mapped to joint deltas via the damped 3xN pseudoinverse of the translational Jacobian rows. Contributions across control points sum.

Preconditions (enforced via RUNTIME_ASSERT; programmer error if violated):

current_joint_values.size() == group.getActiveVariableCount()
params.{activation_distance, gain, damping} are all finite, and params.damping > 0
every SdfControlPoint has a non-null jacobian_tip_link, finite radius, and finite offset_in_tip_link
computeJacobianForChain succeeds for every control point's jacobian_tip_link

Per-control-point safe-skip (other points still contribute):

the sphere center is outside the SDF bounds
the sphere is farther from any obstacle than params.activation_distance
the SDF gradient at the query point has zero magnitude

warning

The SDF must propagate to at least max(radius) + activation_distance past obstacles across all control points. Otherwise the gradient at the query point is invalid before the sphere surface clearance becomes small enough to trigger repulsion.

warning

Construct the PropagationDistanceField with propagate_negative_distances=true (the default is false). Without it, the gradient inside obstacle cells is unsigned and may push the sphere further into the obstacle rather than out of it.

Parameters

sdf	The signed (or unsigned) distance field. Queried but not modified.
fk_state	Robot state used for FK and Jacobian. Mutated: joint group positions are set to current_joint_values and update() is called. Pass a copy not shared with the IK solver to keep the solver's state untouched.
group	Joint model group being solved. jacobian_tip_link of every control point must be in this group (or be a descendant of the chain).
current_joint_values	Active-joint configuration of the group.
control_points	One or more sphere proxies to keep away from obstacles. Empty span returns zero.
params	Activation distance, gain, and damping.

Returns: Joint-space nudge sized to group.getActiveVariableCount().

Definition at line 22 of file sdf_repulsion_task.cpp.

toJointTrajectoryMessage()

tl::expected< trajectory_msgs::msg::JointTrajectory, std::string > path_ik::toJointTrajectoryMessage (const ResampledTrajectory & trajectory, const std::vector< std::string > & joint_names)

Convert a ResampledTrajectory into a JointTrajectory message.

Parameters

trajectory	The resampled trajectory, e.g. from fitCartesianTrapezoidalVelocityProfile().
joint_names	The joint names to write into the message. The size must match the trajectory waypoint dimension.

Returns: A JointTrajectory message, or an error message if the trajectory is empty or has inconsistent sizes.

Definition at line 464 of file cartesian_timing.cpp.

transformTwist()

Eigen::Vector6d path_ik::transformTwist (const Eigen::Vector6d & twist_a, const Eigen::Isometry3d & a_pose_b)

Re-expresses a body twist from frame a to a rigidly-attached frame b, accounting for the lever-arm (omega x r) shift between the two origins.

Given a body twist V_a expressed at the origin of frame a (in a-frame coordinates), and the rigid pose of frame b in frame a's coordinates, returns the equivalent body twist V_b expressed at b's origin in b-frame coordinates. This is the SE(3) body-twist adjoint Ad(b_pose_a) V_a, where b_pose_a = a_pose_b.inverse().

Parameters

twist_a	The body twist in frame a (linear; angular).
a_pose_b	The pose of frame b expressed in frame a (i.e., b's origin and orientation in a-coords). For any parent frame p, p_pose_a * a_pose_b == p_pose_b.

Returns: The equivalent body twist V_b in frame b.

Definition at line 57 of file math.cpp.

twistRotationMatrix()

template <class LinearPart>

Eigen::Matrix6d path_ik::twistRotationMatrix (const LinearPart & rotation)

Computes the 6x6 twist rotation matrix corresponding to a rotation.

Parameters

rotation

The 3x3 rotation matrix.

Returns: a 6x6 matrix that rotates a twist by the given rotation.

Definition at line 43 of file math.hpp.

velocityInverseKinematics()

template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>

Eigen::Matrix< Scalar, Cols, 1, Options, MaxCols, 1 > path_ik::velocityInverseKinematics (const Eigen::MatrixBase< Eigen::Matrix< Scalar, Rows, Cols, Options, MaxRows, MaxCols > > & jacobian, double damping, const Eigen::Matrix< Scalar, Rows, 1, Options, MaxRows, 1 > & cartesian_velocity, const Eigen::MatrixBase< Eigen::Matrix< Scalar, Cols, 1, Options, MaxCols, 1 > > & nullspace_component)

Compute joint velocities corresponding to a given Cartesian velocity.

Given a cartesian_velocity, compute and return the corresponding joint velocities. Use nullspace_component to include joint-space costs to the minimization. nullspace_component can be seen as joint-space velocities that will be achieved in a best-effort way, i.e. only if it's kinematically possible. This function is templated on the Eigen matrix template parameters, which allows using static Eigen types if desired, and ensures the input types are compatible. The function is similar to path_ik::dampedLeastSquaresInverse() but it solves for the Cartesian velocity instead of the full Jacobian, which should be faster. See that function for more details.

Definition at line 31 of file velocity_inverse_kinematics.hpp.

Variables

kConvergenceThreshold

constexpr double path_ik::kConvergenceThreshold = 1e-4

constexpr

Definition at line 34 of file path_ik.cpp.

kDefaultPathIkAngularStep

constexpr double path_ik::kDefaultPathIkAngularStep = 0.001

constexpr

Definition at line 25 of file path_ik.hpp.

kDefaultPathIkTranslationalStep

constexpr double path_ik::kDefaultPathIkTranslationalStep = 0.001

constexpr

Definition at line 24 of file path_ik.hpp.

kDistanceToJointLimitThreshold

constexpr double path_ik::kDistanceToJointLimitThreshold = 0.001

constexpr

Definition at line 29 of file path_ik.cpp.

kGoalRefinementMinErrorImprovement

constexpr double path_ik::kGoalRefinementMinErrorImprovement = 1e-6

constexpr

Definition at line 39 of file path_ik.cpp.

kMaxGoalRefinementIterations

constexpr int path_ik::kMaxGoalRefinementIterations = 100

constexpr

Definition at line 43 of file path_ik.cpp.

kMaxNumJoints

constexpr int path_ik::kMaxNumJoints = 256

constexpr

Definition at line 22 of file types.hpp.

kMaxNumTips

constexpr int path_ik::kMaxNumTips = 4

constexpr

Definition at line 21 of file types.hpp.

The documentation for this namespace was generated from the following files:

Generated via doxygen2docusaurus 2.2.2 by Doxygen 1.9.8.

Definition​

Namespaces Index​

Classes Index​

Typedefs Index​

Enumerations Index​

Functions Index​

Variables Index​

Typedefs​

CartesianTipVector​

JointSpacePath​

NullspaceOverrideFunction​

Path​

TipJointMap​

Enumerations​

SaturationPhase​

TipConstraint​

Functions​

angleDifference()​

appendToTrajectoryMessage()​

computeJacobianForChain()​

computeMultiTipJacobian()​

computeMultiTipJacobian()​

computeTipPathFromJointSpacePath()​

createTrajectoryFromWaypoints()​

createTrajectoryFromWaypoints()​

createTrajectoryFromWaypoints()​

createTrajectoryFromWaypoints()​

dampedLeastSquaresInverse()​

downsamplePath()​

errorWithMessage()​

fitCartesianTrapezoidalVelocityProfile()​

getWorldPoseInRootCoordinates()​

interpolate()​

interpolate()​

interpolateBetweenPoints()​

interpolateJointPath()​

isPathInCollision()​

jointLimitAvoidanceTask()​

multiTipTwistRotationMatrix()​

path_ik()​

pathIK()​

pathIK()​

poseError()​

poseToPoseVector()​

rotationToAxisAngleVector()​

sdfRepulsionTask()​

toJointTrajectoryMessage()​

transformTwist()​

twistRotationMatrix()​

velocityInverseKinematics()​

Variables​

kConvergenceThreshold​

kDefaultPathIkAngularStep​

kDefaultPathIkTranslationalStep​

kDistanceToJointLimitThreshold​

kGoalRefinementMinErrorImprovement​

kMaxGoalRefinementIterations​

kMaxNumJoints​

kMaxNumTips​

Definition

Namespaces Index

Classes Index

Typedefs Index

Enumerations Index

Functions Index

Variables Index

Typedefs

CartesianTipVector

JointSpacePath

NullspaceOverrideFunction

Path

TipJointMap

Enumerations

SaturationPhase

TipConstraint

Functions

angleDifference()

appendToTrajectoryMessage()

computeJacobianForChain()

computeMultiTipJacobian()

computeMultiTipJacobian()

computeTipPathFromJointSpacePath()

createTrajectoryFromWaypoints()

createTrajectoryFromWaypoints()

createTrajectoryFromWaypoints()

createTrajectoryFromWaypoints()

dampedLeastSquaresInverse()

downsamplePath()

errorWithMessage()

fitCartesianTrapezoidalVelocityProfile()

getWorldPoseInRootCoordinates()

interpolate()

interpolate()

interpolateBetweenPoints()

interpolateJointPath()

isPathInCollision()

jointLimitAvoidanceTask()

multiTipTwistRotationMatrix()

path_ik()

pathIK()

pathIK()

poseError()

poseToPoseVector()

rotationToAxisAngleVector()

sdfRepulsionTask()

toJointTrajectoryMessage()

transformTwist()

twistRotationMatrix()

velocityInverseKinematics()

Variables

kConvergenceThreshold

kDefaultPathIkAngularStep

kDefaultPathIkTranslationalStep

kDistanceToJointLimitThreshold

kGoalRefinementMinErrorImprovement

kMaxGoalRefinementIterations

kMaxNumJoints

kMaxNumTips