Specific implementation of kinematics using KDL. This version supports any kinematic chain, also including mimic joints. More...

#include <kdl_kinematics_plugin.h>

Inheritance diagram for kdl_kinematics_plugin::KDLKinematicsPlugin:

[legend]

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

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

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

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

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

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

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

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

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

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)

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

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 Types
typedef Eigen::Matrix< double, 6, 1 >	Twist

Protected Member Functions
int	CartToJnt (KDL::ChainIkSolverVelMimicSVD &ik_solver, const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out, const unsigned int max_iter, const Eigen::VectorXd &joint_weights, const Twist &cartesian_weights) const
	Solve position IK given initial joint values. More...

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 Eigen::VectorXd &seed_state, const std::vector< double > &consistency_limits, const Eigen::VectorXd &solution) const
	Check whether the solution lies within the consistency limits of the seed state. More...

void	clipToJointLimits (const KDL::JntArray &q, KDL::JntArray &q_delta, Eigen::ArrayXd &weighting) const
	clip q_delta such that joint limits will not be violated More...

void	getJointWeights ()

void	getRandomConfiguration (const Eigen::VectorXd &seed_state, const std::vector< double > &consistency_limits, Eigen::VectorXd &jnt_array) const
	Get a random configuration within consistency limits close to the seed state. More...

void	getRandomConfiguration (Eigen::VectorXd &jnt_array) const

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

Private Attributes
unsigned int	dimension_
	Dimension of the group. More...

double	epsilon_

std::unique_ptr< KDL::ChainFkSolverPos >	fk_solver_

bool	initialized_
	Internal variable that indicates whether solver is configured and ready. More...

Eigen::VectorXd	joint_max_
	joint limits More...

Eigen::VectorXd	joint_min_

const moveit::core::JointModelGroup *	joint_model_group_

std::vector< double >	joint_weights_

KDL::Chain	kdl_chain_

int	max_solver_iterations_

std::vector< JointMimic >	mimic_joints_

double	orientation_vs_position_weight_

moveit_msgs::KinematicSolverInfo	solver_info_
	Stores information for the inverse kinematics solver. More...

moveit::core::RobotStatePtr	state_

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 supports any kinematic chain, also including mimic joints.

Definition at line 72 of file kdl_kinematics_plugin.h.

Member Typedef Documentation

◆ Twist

typedef Eigen::Matrix<double, 6, 1> kdl_kinematics_plugin::KDLKinematicsPlugin::Twist

protected

Definition at line 125 of file kdl_kinematics_plugin.h.

Constructor & Destructor Documentation

◆ KDLKinematicsPlugin()

kdl_kinematics_plugin::KDLKinematicsPlugin::KDLKinematicsPlugin ( )

Default constructor.

Definition at line 86 of file kdl_kinematics_plugin.cpp.

Member Function Documentation

◆ CartToJnt()

int kdl_kinematics_plugin::KDLKinematicsPlugin::CartToJnt	(	KDL::ChainIkSolverVelMimicSVD &	ik_solver,
		const KDL::JntArray &	q_init,
		const KDL::Frame &	p_in,
		KDL::JntArray &	q_out,
		const unsigned int	max_iter,
		const Eigen::VectorXd &	joint_weights,
		const Twist &	cartesian_weights
	)		const

protected

Solve position IK given initial joint values.

Definition at line 454 of file kdl_kinematics_plugin.cpp.

◆ checkConsistency()

bool kdl_kinematics_plugin::KDLKinematicsPlugin::checkConsistency	(	const Eigen::VectorXd &	seed_state,
		const std::vector< double > &	consistency_limits,
		const Eigen::VectorXd &	solution
	)		const

private

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

Parameters

seed_state	Seed state
consistency_limits
solution	solution configuration

Returns: true if check succeeds

Definition at line 104 of file kdl_kinematics_plugin.cpp.

◆ clipToJointLimits()

void kdl_kinematics_plugin::KDLKinematicsPlugin::clipToJointLimits	(	const KDL::JntArray &	q,
		KDL::JntArray &	q_delta,
		Eigen::ArrayXd &	weighting
	)		const

private

clip q_delta such that joint limits will not be violated

Definition at line 535 of file kdl_kinematics_plugin.cpp.

◆ getJointNames()

const std::vector< std::string > & kdl_kinematics_plugin::KDLKinematicsPlugin::getJointNames ( ) const

overridevirtual

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

Implements kinematics::KinematicsBase.

Definition at line 590 of file kdl_kinematics_plugin.cpp.

◆ getJointWeights()

void kdl_kinematics_plugin::KDLKinematicsPlugin::getJointWeights ( )

private

Definition at line 114 of file kdl_kinematics_plugin.cpp.

◆ getLinkNames()

const std::vector< std::string > & kdl_kinematics_plugin::KDLKinematicsPlugin::getLinkNames ( ) const

overridevirtual

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

Implements kinematics::KinematicsBase.

Definition at line 595 of file kdl_kinematics_plugin.cpp.

◆ getPositionFK()

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

overridevirtual

Implements kinematics::KinematicsBase.

Definition at line 554 of file kdl_kinematics_plugin.cpp.

◆ getPositionIK()

bool kdl_kinematics_plugin::KDLKinematicsPlugin::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

Implements kinematics::KinematicsBase.

Definition at line 306 of file kdl_kinematics_plugin.cpp.

◆ getRandomConfiguration() [1/2]

void kdl_kinematics_plugin::KDLKinematicsPlugin::getRandomConfiguration	(	const Eigen::VectorXd &	seed_state,
		const std::vector< double > &	consistency_limits,
		Eigen::VectorXd &	jnt_array
	)		const

private

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

Parameters

seed_state	Seed state
consistency_limits
jnt_array	Returned random configuration

Definition at line 96 of file kdl_kinematics_plugin.cpp.

◆ getRandomConfiguration() [2/2]

void kdl_kinematics_plugin::KDLKinematicsPlugin::getRandomConfiguration ( Eigen::VectorXd & jnt_array ) const

private

Definition at line 90 of file kdl_kinematics_plugin.cpp.

◆ initialize()

bool kdl_kinematics_plugin::KDLKinematicsPlugin::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
	)

overridevirtual

Reimplemented from kinematics::KinematicsBase.

Definition at line 169 of file kdl_kinematics_plugin.cpp.

◆ searchPositionIK() [1/4]

bool kdl_kinematics_plugin::KDLKinematicsPlugin::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

Implements kinematics::KinematicsBase.

Definition at line 348 of file kdl_kinematics_plugin.cpp.

◆ searchPositionIK() [2/4]

bool kdl_kinematics_plugin::KDLKinematicsPlugin::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

Implements kinematics::KinematicsBase.

Definition at line 328 of file kdl_kinematics_plugin.cpp.

◆ searchPositionIK() [3/4]

bool kdl_kinematics_plugin::KDLKinematicsPlugin::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

Implements kinematics::KinematicsBase.

Definition at line 337 of file kdl_kinematics_plugin.cpp.

◆ searchPositionIK() [4/4]

bool kdl_kinematics_plugin::KDLKinematicsPlugin::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

Implements kinematics::KinematicsBase.

Definition at line 317 of file kdl_kinematics_plugin.cpp.

◆ timedOut()

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

private

Definition at line 301 of file kdl_kinematics_plugin.cpp.

Member Data Documentation

◆ dimension_

unsigned int kdl_kinematics_plugin::KDLKinematicsPlugin::dimension_

private

Dimension of the group.

Definition at line 161 of file kdl_kinematics_plugin.h.

◆ epsilon_

double kdl_kinematics_plugin::KDLKinematicsPlugin::epsilon_

private

Definition at line 173 of file kdl_kinematics_plugin.h.

◆ fk_solver_

std::unique_ptr<KDL::ChainFkSolverPos> kdl_kinematics_plugin::KDLKinematicsPlugin::fk_solver_

private

Definition at line 167 of file kdl_kinematics_plugin.h.

◆ initialized_

bool kdl_kinematics_plugin::KDLKinematicsPlugin::initialized_

private

Internal variable that indicates whether solver is configured and ready.

Definition at line 159 of file kdl_kinematics_plugin.h.

◆ joint_max_

Eigen::VectorXd kdl_kinematics_plugin::KDLKinematicsPlugin::joint_max_

private

joint limits

Definition at line 170 of file kdl_kinematics_plugin.h.

◆ joint_min_

Eigen::VectorXd kdl_kinematics_plugin::KDLKinematicsPlugin::joint_min_

private

Definition at line 170 of file kdl_kinematics_plugin.h.

◆ joint_model_group_

const moveit::core::JointModelGroup* kdl_kinematics_plugin::KDLKinematicsPlugin::joint_model_group_

private

Definition at line 164 of file kdl_kinematics_plugin.h.

◆ joint_weights_

std::vector<double> kdl_kinematics_plugin::KDLKinematicsPlugin::joint_weights_

private

Definition at line 169 of file kdl_kinematics_plugin.h.

◆ kdl_chain_

KDL::Chain kdl_kinematics_plugin::KDLKinematicsPlugin::kdl_chain_

private

Definition at line 166 of file kdl_kinematics_plugin.h.

◆ max_solver_iterations_

int kdl_kinematics_plugin::KDLKinematicsPlugin::max_solver_iterations_

private

Definition at line 172 of file kdl_kinematics_plugin.h.

◆ mimic_joints_

std::vector<JointMimic> kdl_kinematics_plugin::KDLKinematicsPlugin::mimic_joints_

private

Definition at line 168 of file kdl_kinematics_plugin.h.

◆ orientation_vs_position_weight_

double kdl_kinematics_plugin::KDLKinematicsPlugin::orientation_vs_position_weight_

private

weight of orientation error vs position error

< 1.0: orientation has less importance than position

1.0: orientation has more importance than position

= 0.0: perform position-only IK

Definition at line 179 of file kdl_kinematics_plugin.h.

◆ solver_info_

moveit_msgs::KinematicSolverInfo kdl_kinematics_plugin::KDLKinematicsPlugin::solver_info_

private

Stores information for the inverse kinematics solver.

Definition at line 162 of file kdl_kinematics_plugin.h.

◆ state_

moveit::core::RobotStatePtr kdl_kinematics_plugin::KDLKinematicsPlugin::state_

private

Definition at line 165 of file kdl_kinematics_plugin.h.

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

Public Member Functions

Protected Types

Protected Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ Twist

Constructor & Destructor Documentation

◆ KDLKinematicsPlugin()

Member Function Documentation

◆ CartToJnt()

◆ checkConsistency()

◆ clipToJointLimits()

◆ getJointNames()

◆ getJointWeights()

◆ getLinkNames()

◆ getPositionFK()

◆ getPositionIK()

◆ getRandomConfiguration() [1/2]

◆ getRandomConfiguration() [2/2]

◆ initialize()

◆ searchPositionIK() [1/4]

◆ searchPositionIK() [2/4]

◆ searchPositionIK() [3/4]

◆ searchPositionIK() [4/4]

◆ timedOut()

Member Data Documentation

◆ dimension_

◆ epsilon_

◆ fk_solver_

◆ initialized_

◆ joint_max_

◆ joint_min_

◆ joint_model_group_

◆ joint_weights_

◆ kdl_chain_

◆ max_solver_iterations_

◆ mimic_joints_

◆ orientation_vs_position_weight_

◆ solver_info_

◆ state_