#include <cartesian_interpolator.h>

Classes
struct	Distance

struct	Percentage

Static Public Member Functions
static Distance	computeCartesianPath (RobotState start_state, const JointModelGroup group, std::vector< std::shared_ptr< RobotState >> &traj, const LinkModel *link, const Eigen::Vector3d &translation, bool global_reference_frame, const MaxEEFStep &max_step, const JumpThreshold &jump_threshold, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const kinematics::KinematicsBase::IKCostFn &cost_function=kinematics::KinematicsBase::IKCostFn())
	Compute the sequence of joint values that correspond to a straight Cartesian path for a particular link. More...

static Distance	computeCartesianPath (RobotState start_state, const JointModelGroup group, std::vector< std::shared_ptr< RobotState >> &traj, const LinkModel *link, const Eigen::Vector3d &direction, bool global_reference_frame, double distance, const MaxEEFStep &max_step, const JumpThreshold &jump_threshold, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const kinematics::KinematicsBase::IKCostFn &cost_function=kinematics::KinematicsBase::IKCostFn())
	Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular link. More...

static Percentage	computeCartesianPath (RobotState start_state, const JointModelGroup group, std::vector< std::shared_ptr< RobotState >> &traj, const LinkModel *link, const Eigen::Isometry3d &target, bool global_reference_frame, const MaxEEFStep &max_step, const JumpThreshold &jump_threshold, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const kinematics::KinematicsBase::IKCostFn &cost_function=kinematics::KinematicsBase::IKCostFn(), const Eigen::Isometry3d &link_offset=Eigen::Isometry3d::Identity())
	Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular frame. More...

static Percentage	computeCartesianPath (RobotState start_state, const JointModelGroup group, std::vector< std::shared_ptr< RobotState >> &traj, const LinkModel *link, const EigenSTL::vector_Isometry3d &waypoints, bool global_reference_frame, const MaxEEFStep &max_step, const JumpThreshold &jump_threshold, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const kinematics::KinematicsBase::IKCostFn &cost_function=kinematics::KinematicsBase::IKCostFn(), const Eigen::Isometry3d &link_offset=Eigen::Isometry3d::Identity())
	Compute the sequence of joint values that perform a general Cartesian path. More...

static Percentage	checkJointSpaceJump (const JointModelGroup *group, std::vector< std::shared_ptr< RobotState >> &traj, const JumpThreshold &jump_threshold)
	Tests joint space jumps of a trajectory. More...

static Percentage	checkRelativeJointSpaceJump (const JointModelGroup *group, std::vector< std::shared_ptr< RobotState >> &traj, double jump_threshold_factor)
	Tests for relative joint space jumps of the trajectory traj. More...

static Percentage	checkAbsoluteJointSpaceJump (const JointModelGroup *group, std::vector< std::shared_ptr< RobotState >> &traj, double revolute_jump_threshold, double prismatic_jump_threshold)
	Tests for absolute joint space jumps of the trajectory traj. More...

Detailed Description

Definition at line 93 of file cartesian_interpolator.h.

Member Function Documentation

◆ checkAbsoluteJointSpaceJump()

CartesianInterpolator::Percentage moveit::core::CartesianInterpolator::checkAbsoluteJointSpaceJump	(	const JointModelGroup *	group,
		std::vector< std::shared_ptr< RobotState >> &	traj,
		double	revolute_jump_threshold,
		double	prismatic_jump_threshold
	)

static

Tests for absolute joint space jumps of the trajectory traj.

The joint-space difference between consecutive waypoints is computed for each active joint and compared to the absolute thresholds revolute_jump_threshold for revolute joints and prismatic_jump_threshold for prismatic joints. If these thresholds are exceeded, the trajectory is truncated.

Parameters

group	The joint model group of the robot state.
traj	The trajectory that should be tested.
revolute_jump_threshold	Absolute joint-space threshold for revolute joints.
prismatic_jump_threshold	Absolute joint-space threshold for prismatic joints.

Returns: The fraction of the trajectory that passed.

TODO: move to more appropriate location

Definition at line 270 of file cartesian_interpolator.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ checkJointSpaceJump()

CartesianInterpolator::Percentage moveit::core::CartesianInterpolator::checkJointSpaceJump	(	const JointModelGroup *	group,
		std::vector< std::shared_ptr< RobotState >> &	traj,
		const JumpThreshold &	jump_threshold
	)

static

Tests joint space jumps of a trajectory.

If jump_threshold_factor is non-zero, we test for relative jumps. If revolute_jump_threshold or prismatic_jump_threshold are non-zero, we test for absolute jumps. Both tests can be combined. If all params are zero, jump detection is disabled. For relative jump detection, the average joint-space distance between consecutive points in the trajectory is computed. If any individual joint-space motion delta is larger then this average distance by a factor of jump_threshold_factor, this step is considered a failure and the returned path is truncated up to just before the jump.

Parameters

group	The joint model group of the robot state.
traj	The trajectory that should be tested.
jump_threshold	The struct holding jump thresholds to determine if a joint space jump has occurred.

Returns: The fraction of the trajectory that passed.

TODO: move to more appropriate location

Definition at line 216 of file cartesian_interpolator.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ checkRelativeJointSpaceJump()

CartesianInterpolator::Percentage moveit::core::CartesianInterpolator::checkRelativeJointSpaceJump	(	const JointModelGroup *	group,
		std::vector< std::shared_ptr< RobotState >> &	traj,
		double	jump_threshold_factor
	)

static

Tests for relative joint space jumps of the trajectory traj.

First, the average distance between adjacent trajectory points is computed. If two adjacent trajectory points have distance > jump_threshold_factor * average, the trajectory is truncated at this point.

Parameters

group	The joint model group of the robot state.
traj	The trajectory that should be tested.
jump_threshold_factor	The threshold to determine if a joint space jump has occurred .

Returns: The fraction of the trajectory that passed.

TODO: move to more appropriate location

Definition at line 233 of file cartesian_interpolator.cpp.

Here is the caller graph for this function:

◆ computeCartesianPath() [1/4]

static Percentage moveit::core::CartesianInterpolator::computeCartesianPath	(	RobotState *	start_state,
		const JointModelGroup *	group,
		std::vector< std::shared_ptr< RobotState >> &	traj,
		const LinkModel *	link,
		const Eigen::Isometry3d &	target,
		bool	global_reference_frame,
		const MaxEEFStep &	max_step,
		const JumpThreshold &	jump_threshold,
		const GroupStateValidityCallbackFn &	validCallback = `GroupStateValidityCallbackFn()`,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`,
		const kinematics::KinematicsBase::IKCostFn &	cost_function = `kinematics::KinematicsBase::IKCostFn()`,
		const Eigen::Isometry3d &	link_offset = `Eigen::Isometry3d::Identity()`
	)

static

Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular frame.

In contrast to the previous function, the Cartesian path is specified as a target frame to be reached (target) for a virtual frame attached to the robot link with the given link_offset. The target frame is assumed to be specified either w.r.t. to the global reference frame or the virtual link frame (global_reference_frame is false). This function returns the percentage (0..1) of the path that was achieved. All other comments from the previous function apply.

◆ computeCartesianPath() [2/4]

static Distance moveit::core::CartesianInterpolator::computeCartesianPath	(	RobotState *	start_state,
		const JointModelGroup *	group,
		std::vector< std::shared_ptr< RobotState >> &	traj,
		const LinkModel *	link,
		const Eigen::Vector3d &	direction,
		bool	global_reference_frame,
		double	distance,
		const MaxEEFStep &	max_step,
		const JumpThreshold &	jump_threshold,
		const GroupStateValidityCallbackFn &	validCallback = `GroupStateValidityCallbackFn()`,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`,
		const kinematics::KinematicsBase::IKCostFn &	cost_function = `kinematics::KinematicsBase::IKCostFn()`
	)

inlinestatic

Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular link.

In contrast to the previous function, the translation vector is specified as a (unit) direction vector and a distance.

Definition at line 185 of file cartesian_interpolator.h.

Here is the call graph for this function:

◆ computeCartesianPath() [3/4]

static Distance moveit::core::CartesianInterpolator::computeCartesianPath	(	RobotState *	start_state,
		const JointModelGroup *	group,
		std::vector< std::shared_ptr< RobotState >> &	traj,
		const LinkModel *	link,
		const Eigen::Vector3d &	translation,
		bool	global_reference_frame,
		const MaxEEFStep &	max_step,
		const JumpThreshold &	jump_threshold,
		const GroupStateValidityCallbackFn &	validCallback = `GroupStateValidityCallbackFn()`,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`,
		const kinematics::KinematicsBase::IKCostFn &	cost_function = `kinematics::KinematicsBase::IKCostFn()`
	)

static

Compute the sequence of joint values that correspond to a straight Cartesian path for a particular link.

The Cartesian path to be followed is specified as a translation vector to be followed by the robot link. This vector is assumed to be specified either in the global reference frame or in the local reference frame of the link (global_reference_frame is false). The resulting joint values are stored in the vector traj, one by one. The maximum distance in Cartesian space between consecutive points on the resulting path is specified in the MaxEEFStep struct which provides two fields: translation and rotation. If a validCallback is specified, this is passed to the internal call to setFromIK(). In case of IK failure, the computation of the path stops and the value returned corresponds to the distance that was achieved and for which corresponding states were added to the path. At the end of the function call, the state of the group corresponds to the last attempted Cartesian pose.

During the computation of the trajectory, it is usually preferred if consecutive joint values do not 'jump' by a large amount in joint space, even if the Cartesian distance between the corresponding points is small as expected. To account for this, the jump_threshold struct is provided, which comprises three fields: jump_threshold_factor, revolute_jump_threshold and prismatic_jump_threshold. If either revolute_jump_threshold or prismatic_jump_threshold are non-zero, we test for absolute jumps. If jump_threshold_factor is non-zero, we test for relative jumps. Otherwise (all params are zero), jump detection is disabled.

For relative jump detection, the average joint-space distance between consecutive points in the trajectory is computed. If any individual joint-space motion delta is larger then this average distance by a factor of jump_threshold_factor, this step is considered a failure and the returned path is truncated up to just before the jump.

For absolute jump thresholds, if any individual joint-space motion delta is larger then revolute_jump_threshold for revolute joints or prismatic_jump_threshold for prismatic joints then this step is considered a failure and the returned path is truncated up to just before the jump.

Kinematics solvers may use cost functions to prioritize certain solutions, which may be specified with cost_function.

Here is the caller graph for this function:

◆ computeCartesianPath() [4/4]

static Percentage moveit::core::CartesianInterpolator::computeCartesianPath	(	RobotState *	start_state,
		const JointModelGroup *	group,
		std::vector< std::shared_ptr< RobotState >> &	traj,
		const LinkModel *	link,
		const EigenSTL::vector_Isometry3d &	waypoints,
		bool	global_reference_frame,
		const MaxEEFStep &	max_step,
		const JumpThreshold &	jump_threshold,
		const GroupStateValidityCallbackFn &	validCallback = `GroupStateValidityCallbackFn()`,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`,
		const kinematics::KinematicsBase::IKCostFn &	cost_function = `kinematics::KinematicsBase::IKCostFn()`,
		const Eigen::Isometry3d &	link_offset = `Eigen::Isometry3d::Identity()`
	)

static

Compute the sequence of joint values that perform a general Cartesian path.

In contrast to the previous functions, the Cartesian path is specified as a set of waypoints to be sequentially reached by the virtual frame attached to the robot link. The waypoints are transforms given either w.r.t. the global reference frame or the virtual frame at the immediately preceding waypoint. The virtual frame needs to move in a straight line between two consecutive waypoints. All other comments apply.

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

moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h
moveit_core/robot_state/src/cartesian_interpolator.cpp

Classes

Static Public Member Functions

Detailed Description

Member Function Documentation

◆ checkAbsoluteJointSpaceJump()

◆ checkJointSpaceJump()

◆ checkRelativeJointSpaceJump()

◆ computeCartesianPath() [1/4]

◆ computeCartesianPath() [2/4]

◆ computeCartesianPath() [3/4]

◆ computeCartesianPath() [4/4]