Specific implementation of kinematics using KDL. This version can be used with any robot. More...

#include <ur_moveit_plugin.h>

Inheritance diagram for ur_kinematics::URKinematicsPlugin:

Public Member Functions
const std::vector< std::string > &	getJointNames () const
	Return all the joint names in the order they are used internally. More...

const std::vector< std::string > &	getLinkNames () const
	Return all the link names in the order they are represented internally. More...

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

virtual 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

virtual 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)

virtual 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

virtual 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

virtual 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

virtual 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

	URKinematicsPlugin ()
	Default constructor. 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 ()

Protected Member Functions
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 std::vector< double > &consistency_limits, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const
	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...

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

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)

Private Member Functions
bool	checkConsistency (const KDL::JntArray &seed_state, const std::vector< double > &consistency_limit, const KDL::JntArray &solution) const
	Check whether the solution lies within the consistency limit of the seed state. More...

int	getJointIndex (const std::string &name) const

int	getKDLSegmentIndex (const std::string &name) const

void	getRandomConfiguration (const KDL::JntArray &seed_state, const std::vector< double > &consistency_limits, KDL::JntArray &jnt_array, bool lock_redundancy) const
	Get a random configuration within joint limits close to the seed state. More...

void	getRandomConfiguration (KDL::JntArray &jnt_array, bool lock_redundancy) const

bool	isRedundantJoint (unsigned int index) const

bool	timedOut (const ros::WallTime &start_time, double duration) const

Private Attributes
bool	active_

std::string	arm_prefix_

unsigned int	dimension_

double	epsilon_

moveit_msgs::KinematicSolverInfo	fk_chain_info_

moveit_msgs::KinematicSolverInfo	ik_chain_info_

std::vector< double >	ik_weights_

KDL::JntArray	joint_max_

KDL::JntArray	joint_min_

const robot_model::JointModelGroup *	joint_model_group_

KDL::Chain	kdl_base_chain_

KDL::Chain	kdl_chain_

KDL::Chain	kdl_tip_chain_

double	max_solver_iterations_

std::vector< kdl_kinematics_plugin::JointMimic >	mimic_joints_

int	num_possible_redundant_joints_

bool	position_ik_

random_numbers::RandomNumberGenerator	random_number_generator_

std::vector< unsigned int >	redundant_joints_map_index_

robot_state::RobotStatePtr	state_

robot_state::RobotStatePtr	state_2_

int	ur_joint_inds_start_

std::vector< std::string >	ur_joint_names_

std::vector< std::string >	ur_link_names_

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 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

Specific implementation of kinematics using KDL. This version can be used with any robot.

Definition at line 173 of file ur_moveit_plugin.h.

Constructor & Destructor Documentation

◆ URKinematicsPlugin()

ur_kinematics::URKinematicsPlugin::URKinematicsPlugin ( )

Default constructor.

Definition at line 159 of file ur_moveit_plugin.cpp.

Member Function Documentation

◆ checkConsistency()

bool ur_kinematics::URKinematicsPlugin::checkConsistency	(	const KDL::JntArray &	seed_state,
		const std::vector< double > &	consistency_limit,
		const KDL::JntArray &	solution
	)		const

private

Check whether the solution lies within the consistency limit of the seed state.

Parameters

seed_state	Seed state
redundancy	Index of the redundant joint within the chain
consistency_limit	The returned state for redundant joint should be in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
solution	solution configuration

Returns: true if check succeeds

Definition at line 220 of file ur_moveit_plugin.cpp.

◆ getJointIndex()

int ur_kinematics::URKinematicsPlugin::getJointIndex ( const std::string & name ) const

private

Definition at line 476 of file ur_moveit_plugin.cpp.

◆ getJointNames()

const std::vector< std::string > & ur_kinematics::URKinematicsPlugin::getJointNames ( ) const

virtual

Return all the joint names in the order they are used internally.

Implements kinematics::KinematicsBase.

Definition at line 869 of file ur_moveit_plugin.cpp.

◆ getKDLSegmentIndex()

int ur_kinematics::URKinematicsPlugin::getKDLSegmentIndex ( const std::string & name ) const

private

Definition at line 485 of file ur_moveit_plugin.cpp.

◆ getLinkNames()

const std::vector< std::string > & ur_kinematics::URKinematicsPlugin::getLinkNames ( ) const

virtual

Return all the link names in the order they are represented internally.

Implements kinematics::KinematicsBase.

Definition at line 874 of file ur_moveit_plugin.cpp.

◆ getPositionFK()

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

virtual

Implements kinematics::KinematicsBase.

Definition at line 823 of file ur_moveit_plugin.cpp.

◆ getPositionIK()

bool ur_kinematics::URKinematicsPlugin::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

virtual

Implements kinematics::KinematicsBase.

Definition at line 502 of file ur_moveit_plugin.cpp.

◆ getRandomConfiguration() [1/2]

void ur_kinematics::URKinematicsPlugin::getRandomConfiguration	(	const KDL::JntArray &	seed_state,
		const std::vector< double > &	consistency_limits,
		KDL::JntArray &	jnt_array,
		bool	lock_redundancy
	)		const

private

Get a random configuration within joint limits close to the seed state.

Parameters

seed_state	Seed state
redundancy	Index of the redundant joint within the chain
consistency_limit	The returned state will contain a value for the redundant joint in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
jnt_array	Returned random configuration

Definition at line 183 of file ur_moveit_plugin.cpp.

◆ getRandomConfiguration() [2/2]

void ur_kinematics::URKinematicsPlugin::getRandomConfiguration	(	KDL::JntArray &	jnt_array,
		bool	lock_redundancy
	)		const

private

Definition at line 161 of file ur_moveit_plugin.cpp.

◆ initialize()

bool ur_kinematics::URKinematicsPlugin::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
	)

virtual

Reimplemented from kinematics::KinematicsBase.

Definition at line 230 of file ur_moveit_plugin.cpp.

◆ isRedundantJoint()

bool ur_kinematics::URKinematicsPlugin::isRedundantJoint ( unsigned int index ) const

private

Definition at line 175 of file ur_moveit_plugin.cpp.

◆ searchPositionIK() [1/5]

bool ur_kinematics::URKinematicsPlugin::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

virtual

Implements kinematics::KinematicsBase.

Definition at line 579 of file ur_moveit_plugin.cpp.

◆ searchPositionIK() [2/5]

bool ur_kinematics::URKinematicsPlugin::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

virtual

Implements kinematics::KinematicsBase.

Definition at line 541 of file ur_moveit_plugin.cpp.

◆ searchPositionIK() [3/5]

bool ur_kinematics::URKinematicsPlugin::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

virtual

Implements kinematics::KinematicsBase.

Definition at line 560 of file ur_moveit_plugin.cpp.

◆ searchPositionIK() [4/5]

bool ur_kinematics::URKinematicsPlugin::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 std::vector< double > &	consistency_limits,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`
	)		const

protected

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
timeout	The amount of time (in seconds) available to the solver
solution	the solution vector
solution_callback	A callback solution for the IK solution
error_code	an error code that encodes the reason for failure or success
check_consistency	Set to true if consistency check needs to be performed
redundancy	The index of the redundant joint
consistency_limit	The returned solutuion will contain a value for the redundant joint in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]

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

Definition at line 602 of file ur_moveit_plugin.cpp.

◆ searchPositionIK() [5/5]

bool ur_kinematics::URKinematicsPlugin::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

virtual

Implements kinematics::KinematicsBase.

Definition at line 521 of file ur_moveit_plugin.cpp.

◆ setRedundantJoints()

bool ur_kinematics::URKinematicsPlugin::setRedundantJoints ( const std::vector< unsigned int > & redundant_joint_indices )

protectedvirtual

Reimplemented from kinematics::KinematicsBase.

Definition at line 432 of file ur_moveit_plugin.cpp.

◆ timedOut()

bool ur_kinematics::URKinematicsPlugin::timedOut	(	const ros::WallTime &	start_time,
		double	duration
	)		const

private

Definition at line 497 of file ur_moveit_plugin.cpp.

Member Data Documentation

◆ active_

bool ur_kinematics::URKinematicsPlugin::active_

private

Definition at line 368 of file ur_moveit_plugin.h.

◆ arm_prefix_

std::string ur_kinematics::URKinematicsPlugin::arm_prefix_

private

Definition at line 398 of file ur_moveit_plugin.h.

◆ dimension_

unsigned int ur_kinematics::URKinematicsPlugin::dimension_

private

Definition at line 376 of file ur_moveit_plugin.h.

◆ epsilon_

double ur_kinematics::URKinematicsPlugin::epsilon_

private

Definition at line 391 of file ur_moveit_plugin.h.

◆ fk_chain_info_

moveit_msgs::KinematicSolverInfo ur_kinematics::URKinematicsPlugin::fk_chain_info_

private

Stores information for the inverse kinematics solver

Definition at line 372 of file ur_moveit_plugin.h.

◆ ik_chain_info_

moveit_msgs::KinematicSolverInfo ur_kinematics::URKinematicsPlugin::ik_chain_info_

private

Internal variable that indicates whether solvers are configured and ready

Definition at line 370 of file ur_moveit_plugin.h.

◆ ik_weights_

std::vector<double> ur_kinematics::URKinematicsPlugin::ik_weights_

private

Definition at line 394 of file ur_moveit_plugin.h.

◆ joint_max_

KDL::JntArray ur_kinematics::URKinematicsPlugin::joint_max_

private

Definition at line 378 of file ur_moveit_plugin.h.

◆ joint_min_

KDL::JntArray ur_kinematics::URKinematicsPlugin::joint_min_

private

Dimension of the group

Definition at line 378 of file ur_moveit_plugin.h.

◆ joint_model_group_

const robot_model::JointModelGroup* ur_kinematics::URKinematicsPlugin::joint_model_group_

private

Definition at line 389 of file ur_moveit_plugin.h.

◆ kdl_base_chain_

KDL::Chain ur_kinematics::URKinematicsPlugin::kdl_base_chain_

private

Definition at line 402 of file ur_moveit_plugin.h.

◆ kdl_chain_

KDL::Chain ur_kinematics::URKinematicsPlugin::kdl_chain_

private

Store information for the forward kinematics solver

Definition at line 374 of file ur_moveit_plugin.h.

◆ kdl_tip_chain_

KDL::Chain ur_kinematics::URKinematicsPlugin::kdl_tip_chain_

private

Definition at line 403 of file ur_moveit_plugin.h.

◆ max_solver_iterations_

double ur_kinematics::URKinematicsPlugin::max_solver_iterations_

private

Definition at line 390 of file ur_moveit_plugin.h.

◆ mimic_joints_

std::vector<kdl_kinematics_plugin::JointMimic> ur_kinematics::URKinematicsPlugin::mimic_joints_

private

Definition at line 392 of file ur_moveit_plugin.h.

◆ num_possible_redundant_joints_

int ur_kinematics::URKinematicsPlugin::num_possible_redundant_joints_

private

Definition at line 384 of file ur_moveit_plugin.h.

◆ position_ik_

bool ur_kinematics::URKinematicsPlugin::position_ik_

private

Definition at line 388 of file ur_moveit_plugin.h.

◆ random_number_generator_

random_numbers::RandomNumberGenerator ur_kinematics::URKinematicsPlugin::random_number_generator_

mutableprivate

Joint limits

Definition at line 380 of file ur_moveit_plugin.h.

◆ redundant_joints_map_index_

std::vector<unsigned int> ur_kinematics::URKinematicsPlugin::redundant_joints_map_index_

private

Definition at line 385 of file ur_moveit_plugin.h.

◆ state_

robot_state::RobotStatePtr ur_kinematics::URKinematicsPlugin::state_

private

Definition at line 382 of file ur_moveit_plugin.h.

◆ state_2_

robot_state::RobotStatePtr ur_kinematics::URKinematicsPlugin::state_2_

private

Definition at line 382 of file ur_moveit_plugin.h.

◆ ur_joint_inds_start_

int ur_kinematics::URKinematicsPlugin::ur_joint_inds_start_

private

Definition at line 397 of file ur_moveit_plugin.h.

◆ ur_joint_names_

std::vector<std::string> ur_kinematics::URKinematicsPlugin::ur_joint_names_

private

Definition at line 395 of file ur_moveit_plugin.h.

◆ ur_link_names_

std::vector<std::string> ur_kinematics::URKinematicsPlugin::ur_link_names_

private

Definition at line 396 of file ur_moveit_plugin.h.

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

Public Member Functions

Protected Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ URKinematicsPlugin()

Member Function Documentation

◆ checkConsistency()

◆ getJointIndex()

◆ getJointNames()

◆ getKDLSegmentIndex()

◆ getLinkNames()

◆ getPositionFK()

◆ getPositionIK()

◆ getRandomConfiguration() [1/2]

◆ getRandomConfiguration() [2/2]

◆ initialize()

◆ isRedundantJoint()

◆ searchPositionIK() [1/5]

◆ searchPositionIK() [2/5]

◆ searchPositionIK() [3/5]

◆ searchPositionIK() [4/5]

◆ searchPositionIK() [5/5]

◆ setRedundantJoints()

◆ timedOut()

Member Data Documentation

◆ active_

◆ arm_prefix_

◆ dimension_

◆ epsilon_

◆ fk_chain_info_

◆ ik_chain_info_

◆ ik_weights_

◆ joint_max_

◆ joint_min_

◆ joint_model_group_

◆ kdl_base_chain_

◆ kdl_chain_

◆ kdl_tip_chain_

◆ max_solver_iterations_

◆ mimic_joints_

◆ num_possible_redundant_joints_

◆ position_ik_

◆ random_number_generator_

◆ redundant_joints_map_index_

◆ state_

◆ state_2_

◆ ur_joint_inds_start_

◆ ur_joint_names_

◆ ur_link_names_