Inheritance diagram for _NAMESPACE_::IKFastKinematicsPlugin:

Public Member Functions
bool	getPositionFK (const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const override
	Given a set of joint angles and a set of links, compute their pose. More...

bool	getPositionIK (const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
	Given a desired pose of the end-effector, compute the joint angles to reach it. More...

bool	getPositionIK (const std::vector< geometry_msgs::Pose > &ik_poses, const std::vector< double > &ik_seed_state, std::vector< std::vector< double >> &solutions, kinematics::KinematicsResult &result, const kinematics::KinematicsQueryOptions &options) const override
	Given a desired pose of the end-effector, compute the set joint angles solutions that are able to reach it. More...

	IKFastKinematicsPlugin ()
	Interface for an IKFast kinematics plugin. More...

bool	searchPositionIK (const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, const std::vector< double > &consistency_limits, std::vector< double > &solution, const IKCallbackFn &solution_callback, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
	Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines). The consistency_limit specifies that only certain redundancy positions around those specified in the seed state are admissible and need to be searched. More...

bool	searchPositionIK (const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, const std::vector< double > &consistency_limits, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
	Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines). More...

bool	searchPositionIK (const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, const IKCallbackFn &solution_callback, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
	Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines). More...

bool	searchPositionIK (const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
	Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines). More...

bool	setRedundantJoints (const std::vector< unsigned int > &redundant_joint_indices) override
	Overrides the default method to prevent changing the redundant joints. More...

void	setSearchDiscretization (const std::map< unsigned int, double > &discretization)
	Sets the discretization value for the redundant joint. More...

Public Member Functions inherited from kinematics::KinematicsBase
virtual const std::string &	getBaseFrame () const

double	getDefaultTimeout () const

virtual const std::string &	getGroupName () const

virtual bool	getPositionIK (const std::vector< geometry_msgs::Pose > &ik_poses, const std::vector< double > &ik_seed_state, std::vector< std::vector< double > > &solutions, KinematicsResult &result, const kinematics::KinematicsQueryOptions &options) const

virtual void	getRedundantJoints (std::vector< unsigned int > &redundant_joint_indices) const

double	getSearchDiscretization (int joint_index=0) const

std::vector< DiscretizationMethod >	getSupportedDiscretizationMethods () const

virtual const std::string &	getTipFrame () const

virtual const std::vector< std::string > &	getTipFrames () const

virtual bool	initialize (const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::string &tip_frame, double search_discretization)

virtual bool	initialize (const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization)

	KinematicsBase ()

virtual bool	searchPositionIK (const std::vector< geometry_msgs::Pose > &ik_poses, const std::vector< double > &ik_seed_state, double timeout, const std::vector< double > &consistency_limits, std::vector< double > &solution, const IKCallbackFn &solution_callback, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const moveit::core::RobotState *context_state=nullptr) const

void	setDefaultTimeout (double timeout)

bool	setRedundantJoints (const std::vector< std::string > &redundant_joint_names)

void	setSearchDiscretization (const std::map< int, double > &discretization)

void	setSearchDiscretization (double sd)

virtual void	setValues (const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::string &tip_frame, double search_discretization)

virtual void	setValues (const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization)

virtual bool	supportsGroup (const moveit::core::JointModelGroup jmg, std::string error_text_out=nullptr) const

virtual	~KinematicsBase ()

Private Member Functions
bool	computeRelativeTransform (const std::string &from, const std::string &to, Eigen::Isometry3d &transform, bool &differs_from_identity)
	Validate that we can compute a fixed transform between from and to links. More...

double	enforceLimits (double val, double min, double max) const
	If the value is outside of min/max then it tries to +/- 2 * pi to put the value into the range. More...

void	fillFreeParams (int count, int *array)

bool	getCount (int &count, const int &max_count, const int &min_count) const

const std::vector< std::string > &	getJointNames () const override

const std::vector< std::string > &	getLinkNames () const override

void	getSolution (const IkSolutionList< IkReal > &solutions, const std::vector< double > &ik_seed_state, int i, std::vector< double > &solution) const
	Gets a specific solution from the set with joints rotated 360° to be near seed state where possible. More...

void	getSolution (const IkSolutionList< IkReal > &solutions, int i, std::vector< double > &solution) const
	Gets a specific solution from the set. More...

bool	initialize (const moveit::core::RobotModel &robot_model, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization) override

bool	sampleRedundantJoint (kinematics::DiscretizationMethod method, std::vector< double > &sampled_joint_vals) const
	samples the designated redundant joint using the chosen discretization method More...

size_t	solve (KDL::Frame &pose_frame, const std::vector< double > &vfree, IkSolutionList< IkReal > &solutions) const
	Calls the IK solver from IKFast. More...

void	transformToChainFrame (const geometry_msgs::Pose &ik_pose, KDL::Frame &ik_pose_chain) const
	Transforms the input pose to the correct frame for the solver. This assumes that the group includes the entire solver chain and that any joints outside of the solver chain within the group are are fixed. More...

Private Attributes
bool	base_transform_required_

Eigen::Isometry3d	chain_base_to_group_base_

std::vector< int >	free_params_

Eigen::Isometry3d	group_tip_to_chain_tip_

const std::string	IKFAST_BASE_FRAME_ = "_BASE_LINK_"

const std::string	IKFAST_TIP_FRAME_ = "_EEF_LINK_"

bool	initialized_

std::vector< bool >	joint_has_limits_vector_

std::vector< double >	joint_max_vector_

std::vector< double >	joint_min_vector_

std::vector< std::string >	joint_names_

std::vector< std::string >	link_names_

std::string	link_prefix_

const std::string	name_ { "ikfast" }

const size_t	num_joints_

bool	tip_transform_required_

Additional Inherited Members
Public Types inherited from kinematics::KinematicsBase
typedef boost::function< void(const geometry_msgs::Pose &, const std::vector< double > &, moveit_msgs::MoveItErrorCodes &)>	IKCallbackFn

Static Public Attributes inherited from kinematics::KinematicsBase
static const MOVEIT_KINEMATICS_BASE_EXPORT double	DEFAULT_SEARCH_DISCRETIZATION

static const MOVEIT_KINEMATICS_BASE_EXPORT double	DEFAULT_TIMEOUT

Protected Member Functions inherited from kinematics::KinematicsBase
bool	lookupParam (const std::string &param, T &val, const T &default_val) const

void	storeValues (const moveit::core::RobotModel &robot_model, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization)

Protected Attributes inherited from kinematics::KinematicsBase
std::string	base_frame_

double	default_timeout_

std::string	group_name_

std::map< int, double >	redundant_joint_discretization_

std::vector< unsigned int >	redundant_joint_indices_

std::string	robot_description_

moveit::core::RobotModelConstPtr	robot_model_

double	search_discretization_

std::vector< DiscretizationMethod >	supported_methods_

std::string	tip_frame_

std::vector< std::string >	tip_frames_

Detailed Description

Definition at line 153 of file ikfast61_moveit_plugin_template.cpp.

Constructor & Destructor Documentation

◆ IKFastKinematicsPlugin()

_NAMESPACE_::IKFastKinematicsPlugin::IKFastKinematicsPlugin ( )

inline

Interface for an IKFast kinematics plugin.

Definition at line 197 of file ikfast61_moveit_plugin_template.cpp.

Member Function Documentation

◆ computeRelativeTransform()

bool _NAMESPACE_::IKFastKinematicsPlugin::computeRelativeTransform	(	const std::string &	from,
		const std::string &	to,
		Eigen::Isometry3d &	transform,
		bool &	differs_from_identity
	)

private

Validate that we can compute a fixed transform between from and to links.

Definition at line 373 of file ikfast61_moveit_plugin_template.cpp.

◆ enforceLimits()

double _NAMESPACE_::IKFastKinematicsPlugin::enforceLimits	(	double	val,
		double	min,
		double	max
	)		const

private

If the value is outside of min/max then it tries to +/- 2 * pi to put the value into the range.

Definition at line 726 of file ikfast61_moveit_plugin_template.cpp.

◆ fillFreeParams()

void _NAMESPACE_::IKFastKinematicsPlugin::fillFreeParams	(	int	count,
		int *	array
	)

private

Definition at line 742 of file ikfast61_moveit_plugin_template.cpp.

◆ getCount()

bool _NAMESPACE_::IKFastKinematicsPlugin::getCount	(	int &	count,
		const int &	max_count,
		const int &	min_count
	)		const

private

Definition at line 749 of file ikfast61_moveit_plugin_template.cpp.

◆ getJointNames()

const std::vector<std::string>& _NAMESPACE_::IKFastKinematicsPlugin::getJointNames ( ) const

inlineoverrideprivatevirtual

Implements kinematics::KinematicsBase.

Definition at line 186 of file ikfast61_moveit_plugin_template.cpp.

◆ getLinkNames()

const std::vector<std::string>& _NAMESPACE_::IKFastKinematicsPlugin::getLinkNames ( ) const

inlineoverrideprivatevirtual

Implements kinematics::KinematicsBase.

Definition at line 190 of file ikfast61_moveit_plugin_template.cpp.

◆ getPositionFK()

bool _NAMESPACE_::IKFastKinematicsPlugin::getPositionFK	(	const std::vector< std::string > &	link_names,
		const std::vector< double > &	joint_angles,
		std::vector< geometry_msgs::Pose > &	poses
	)		const

overridevirtual

Given a set of joint angles and a set of links, compute their pose.

Parameters

link_names	A set of links for which FK needs to be computed
joint_angles	The state for which FK is being computed
poses	The resultant set of poses (in the frame returned by getBaseFrame())

Returns: True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 785 of file ikfast61_moveit_plugin_template.cpp.

◆ getPositionIK() [1/2]

bool _NAMESPACE_::IKFastKinematicsPlugin::getPositionIK	(	const geometry_msgs::Pose &	ik_pose,
		const std::vector< double > &	ik_seed_state,
		std::vector< double > &	solution,
		moveit_msgs::MoveItErrorCodes &	error_code,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`
	)		const

overridevirtual

Given a desired pose of the end-effector, compute the joint angles to reach it.

Parameters

ik_pose	the desired pose of the link
ik_seed_state	an initial guess solution for the inverse kinematics
solution	the solution vector
error_code	an error code that encodes the reason for failure or success

Returns: True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 1103 of file ikfast61_moveit_plugin_template.cpp.

◆ getPositionIK() [2/2]

bool _NAMESPACE_::IKFastKinematicsPlugin::getPositionIK	(	const std::vector< geometry_msgs::Pose > &	ik_poses,
		const std::vector< double > &	ik_seed_state,
		std::vector< std::vector< double >> &	solutions,
		kinematics::KinematicsResult &	result,
		const kinematics::KinematicsQueryOptions &	options
	)		const

override

Given a desired pose of the end-effector, compute the set joint angles solutions that are able to reach it.

This is a default implementation that returns only one solution and so its result is equivalent to calling 'getPositionIK(...)' with a zero initialized seed.

Parameters

ik_poses	The desired pose of each tip link
ik_seed_state	an initial guess solution for the inverse kinematics
solutions	A vector of vectors where each entry is a valid joint solution
result	A struct that reports the results of the query
options	An option struct which contains the type of redundancy discretization used. This default implementation only supports the KinmaticSearches::NO_DISCRETIZATION method; requesting any other will result in failure.

Returns: True if a valid set of solutions was found, false otherwise.

Definition at line 1212 of file ikfast61_moveit_plugin_template.cpp.

◆ getSolution() [1/2]

void _NAMESPACE_::IKFastKinematicsPlugin::getSolution	(	const IkSolutionList< IkReal > &	solutions,
		const std::vector< double > &	ik_seed_state,
		int	i,
		std::vector< double > &	solution
	)		const

private

Gets a specific solution from the set with joints rotated 360° to be near seed state where possible.

Definition at line 693 of file ikfast61_moveit_plugin_template.cpp.

◆ getSolution() [2/2]

void _NAMESPACE_::IKFastKinematicsPlugin::getSolution	(	const IkSolutionList< IkReal > &	solutions,
		int	i,
		std::vector< double > &	solution
	)		const

private

Gets a specific solution from the set.

Definition at line 673 of file ikfast61_moveit_plugin_template.cpp.

◆ initialize()

bool _NAMESPACE_::IKFastKinematicsPlugin::initialize	(	const moveit::core::RobotModel &	robot_model,
		const std::string &	group_name,
		const std::string &	base_frame,
		const std::vector< std::string > &	tip_frames,
		double	search_discretization
	)

overrideprivatevirtual

Reimplemented from kinematics::KinematicsBase.

Definition at line 398 of file ikfast61_moveit_plugin_template.cpp.

◆ sampleRedundantJoint()

bool _NAMESPACE_::IKFastKinematicsPlugin::sampleRedundantJoint	(	kinematics::DiscretizationMethod	method,
		std::vector< double > &	sampled_joint_vals
	)		const

private

samples the designated redundant joint using the chosen discretization method

Parameters

method	An enumeration flag indicating the discretization method to be used
sampled_joint_vals	Sampled joint values for the redundant joint

Returns: True if sampling succeeded.

Definition at line 1355 of file ikfast61_moveit_plugin_template.cpp.

◆ searchPositionIK() [1/4]

bool _NAMESPACE_::IKFastKinematicsPlugin::searchPositionIK	(	const geometry_msgs::Pose &	ik_pose,
		const std::vector< double > &	ik_seed_state,
		double	timeout,
		const std::vector< double > &	consistency_limits,
		std::vector< double > &	solution,
		const IKCallbackFn &	solution_callback,
		moveit_msgs::MoveItErrorCodes &	error_code,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`
	)		const

overridevirtual

Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines). The consistency_limit specifies that only certain redundancy positions around those specified in the seed state are admissible and need to be searched.

Parameters

ik_pose	the desired pose of the link
ik_seed_state	an initial guess solution for the inverse kinematics
consistency_limit	the distance that the redundancy can be from the current position

Returns: True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 872 of file ikfast61_moveit_plugin_template.cpp.

◆ searchPositionIK() [2/4]

bool _NAMESPACE_::IKFastKinematicsPlugin::searchPositionIK	(	const geometry_msgs::Pose &	ik_pose,
		const std::vector< double > &	ik_seed_state,
		double	timeout,
		const std::vector< double > &	consistency_limits,
		std::vector< double > &	solution,
		moveit_msgs::MoveItErrorCodes &	error_code,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`
	)		const

overridevirtual

Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines).

Parameters

ik_pose	the desired pose of the link
ik_seed_state	an initial guess solution for the inverse kinematics
the	distance that the redundancy can be from the current position

Returns: True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 851 of file ikfast61_moveit_plugin_template.cpp.

◆ searchPositionIK() [3/4]

bool _NAMESPACE_::IKFastKinematicsPlugin::searchPositionIK	(	const geometry_msgs::Pose &	ik_pose,
		const std::vector< double > &	ik_seed_state,
		double	timeout,
		std::vector< double > &	solution,
		const IKCallbackFn &	solution_callback,
		moveit_msgs::MoveItErrorCodes &	error_code,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`
	)		const

overridevirtual

Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines).

Parameters

ik_pose	the desired pose of the link
ik_seed_state	an initial guess solution for the inverse kinematics

Returns: True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 861 of file ikfast61_moveit_plugin_template.cpp.

◆ searchPositionIK() [4/4]

bool _NAMESPACE_::IKFastKinematicsPlugin::searchPositionIK	(	const geometry_msgs::Pose &	ik_pose,
		const std::vector< double > &	ik_seed_state,
		double	timeout,
		std::vector< double > &	solution,
		moveit_msgs::MoveItErrorCodes &	error_code,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`
	)		const

overridevirtual

Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines).

Parameters

ik_pose	the desired pose of the link
ik_seed_state	an initial guess solution for the inverse kinematics

Returns: True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 841 of file ikfast61_moveit_plugin_template.cpp.

◆ setRedundantJoints()

bool _NAMESPACE_::IKFastKinematicsPlugin::setRedundantJoints ( const std::vector< unsigned int > & redundant_joint_indices )

overridevirtual

Overrides the default method to prevent changing the redundant joints.

Reimplemented from kinematics::KinematicsBase.

Definition at line 543 of file ikfast61_moveit_plugin_template.cpp.

◆ setSearchDiscretization()

void _NAMESPACE_::IKFastKinematicsPlugin::setSearchDiscretization ( const std::map< unsigned int, double > & discretization )

Sets the discretization value for the redundant joint.

Since this ikfast implementation allows for one redundant joint then only the first entry will be in the discretization map will be used. Calling this method replaces previous discretization settings.

Parameters

discretization a map of joint indices and discretization value pairs.

Definition at line 510 of file ikfast61_moveit_plugin_template.cpp.

◆ solve()

size_t _NAMESPACE_::IKFastKinematicsPlugin::solve	(	KDL::Frame &	pose_frame,
		const std::vector< double > &	vfree,
		IkSolutionList< IkReal > &	solutions
	)		const

private

Calls the IK solver from IKFast.

Returns: The number of solutions found

Definition at line 549 of file ikfast61_moveit_plugin_template.cpp.

◆ transformToChainFrame()

void _NAMESPACE_::IKFastKinematicsPlugin::transformToChainFrame	(	const geometry_msgs::Pose &	ik_pose,
		KDL::Frame &	ik_pose_chain
	)		const

private

Transforms the input pose to the correct frame for the solver. This assumes that the group includes the entire solver chain and that any joints outside of the solver chain within the group are are fixed.

Parameters

ik_pose	The pose to be transformed which should be in the correct frame for the group.
ik_pose_chain	The ik_pose to be populated with the apropriate pose for the solver

Definition at line 1398 of file ikfast61_moveit_plugin_template.cpp.