Specific implementation of kinematics using ROS service calls to communicate with external IK solvers. This version can be used with any robot. Supports non-chain kinematic groups. More...

#include <moveit_opw_kinematics_plugin.h>

Inheritance diagram for moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin:

[legend]

Classes
struct	LimitObeyingSol

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

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

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 override

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=kinematics::KinematicsQueryOptions()) const override

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

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

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

	MoveItOPWKinematicsPlugin ()
	Default constructor. More...

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 override

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 override

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 override

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

	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=NULL) 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=NULL) const

virtual	~KinematicsBase ()

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

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

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

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

Private Member Functions
bool	comparePoses (Eigen::Isometry3d &Ta, Eigen::Affine3d &Tb)
	check if two poses are the same within an absolute tolerance of 1e-6 More...

bool	getAllIK (const Eigen::Affine3d &pose, std::vector< std::vector< double >> &joint_poses) const

bool	getIK (const Eigen::Affine3d &pose, const std::vector< double > &seed_state, std::vector< double > &joint_pose) const

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

bool	isRedundantJoint (unsigned int index) const

bool	selfTest ()
	check forward and inverse kinematics consistency More...

bool	setOPWParameters ()

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

Static Private Member Functions
static std::size_t	closestJointPose (const std::vector< double > &target, const std::vector< std::vector< double >> &candidates)

static double	distance (const std::vector< double > &a, const std::vector< double > &b)

Private Attributes
bool	active_

unsigned int	dimension_

moveit_msgs::KinematicSolverInfo	ik_group_info_

robot_model::JointModelGroup *	joint_model_group_

int	num_possible_redundant_joints_

opw_kinematics::Parameters< double >	opw_parameters_

robot_model::RobotModelPtr	robot_model_

robot_state::RobotStatePtr	robot_state_

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

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

static const 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_

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 ROS service calls to communicate with external IK solvers. This version can be used with any robot. Supports non-chain kinematic groups.

Definition at line 32 of file moveit_opw_kinematics_plugin.h.

Constructor & Destructor Documentation

moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::MoveItOPWKinematicsPlugin ( )

Default constructor.

Definition at line 32 of file moveit_opw_kinematics_plugin.cpp.

Member Function Documentation

std::size_t moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::closestJointPose	(	const std::vector< double > &	target,
		const std::vector< std::vector< double >> &	candidates
	)

staticprivate

Definition at line 523 of file moveit_opw_kinematics_plugin.cpp.

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::comparePoses	(	Eigen::Isometry3d &	Ta,
		Eigen::Affine3d &	Tb
	)

private

check if two poses are the same within an absolute tolerance of 1e-6

Definition at line 196 of file moveit_opw_kinematics_plugin.cpp.

double moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::distance	(	const std::vector< double > &	a,
		const std::vector< double > &	b
	)

staticprivate

Definition at line 514 of file moveit_opw_kinematics_plugin.cpp.

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::getAllIK	(	const Eigen::Affine3d &	pose,
		std::vector< std::vector< double >> &	joint_poses
	)		const

private

Definition at line 541 of file moveit_opw_kinematics_plugin.cpp.

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::getIK	(	const Eigen::Affine3d &	pose,
		const std::vector< double > &	seed_state,
		std::vector< double > &	joint_pose
	)		const

private

Definition at line 580 of file moveit_opw_kinematics_plugin.cpp.

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

private

Definition at line 160 of file moveit_opw_kinematics_plugin.cpp.

const std::vector< std::string > & moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::getJointNames ( ) const

overridevirtual

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

Implements kinematics::KinematicsBase.

Definition at line 440 of file moveit_opw_kinematics_plugin.cpp.

const std::vector< std::string > & moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::getLinkNames ( ) const

overridevirtual

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

Implements kinematics::KinematicsBase.

Definition at line 445 of file moveit_opw_kinematics_plugin.cpp.

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::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 408 of file moveit_opw_kinematics_plugin.cpp.

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::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 229 of file moveit_opw_kinematics_plugin.cpp.

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::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 = `kinematics::KinematicsQueryOptions()`
	)		const

overridevirtual

Definition at line 393 of file moveit_opw_kinematics_plugin.cpp.

const std::vector< std::string > & moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::getVariableNames ( ) const

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

Definition at line 450 of file moveit_opw_kinematics_plugin.cpp.

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

inlineoverridevirtual

Implements kinematics::KinematicsBase.

Definition at line 82 of file moveit_opw_kinematics_plugin.h.

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::initialize	(	const std::string &	robot_description,
		const std::string &	group_name,
		const std::string &	base_name,
		const std::vector< std::string > &	tip_frames,
		double	search_discretization
	)

overridevirtual

Reimplemented from kinematics::KinematicsBase.

Definition at line 36 of file moveit_opw_kinematics_plugin.cpp.

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::isRedundantJoint ( unsigned int index ) const

private

Definition at line 152 of file moveit_opw_kinematics_plugin.cpp.

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::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 241 of file moveit_opw_kinematics_plugin.cpp.

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::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 254 of file moveit_opw_kinematics_plugin.cpp.

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::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 266 of file moveit_opw_kinematics_plugin.cpp.

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::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 277 of file moveit_opw_kinematics_plugin.cpp.

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::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

protectedvirtual

Definition at line 288 of file moveit_opw_kinematics_plugin.cpp.

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::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

protectedvirtual

Definition at line 303 of file moveit_opw_kinematics_plugin.cpp.

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::selfTest ( )

private

check forward and inverse kinematics consistency

A basic tests to check if the geometric parameters loaded at initialization are correct.

Definition at line 175 of file moveit_opw_kinematics_plugin.cpp.

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::setOPWParameters ( )

private

Definition at line 455 of file moveit_opw_kinematics_plugin.cpp.

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

overrideprotectedvirtual

Reimplemented from kinematics::KinematicsBase.

Definition at line 134 of file moveit_opw_kinematics_plugin.cpp.

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

private

Definition at line 170 of file moveit_opw_kinematics_plugin.cpp.

Member Data Documentation

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::active_

private

Definition at line 156 of file moveit_opw_kinematics_plugin.h.

unsigned int moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::dimension_

private

Stores information for the inverse kinematics solver

Definition at line 160 of file moveit_opw_kinematics_plugin.h.

moveit_msgs::KinematicSolverInfo moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::ik_group_info_

private

Internal variable that indicates whether solvers are configured and ready

Definition at line 158 of file moveit_opw_kinematics_plugin.h.

robot_model::JointModelGroup* moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::joint_model_group_

private

Definition at line 163 of file moveit_opw_kinematics_plugin.h.

int moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::num_possible_redundant_joints_

private

Definition at line 167 of file moveit_opw_kinematics_plugin.h.

opw_kinematics::Parameters<double> moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::opw_parameters_

private

Definition at line 169 of file moveit_opw_kinematics_plugin.h.

robot_model::RobotModelPtr moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::robot_model_

private

Dimension of the group

Definition at line 162 of file moveit_opw_kinematics_plugin.h.

robot_state::RobotStatePtr moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::robot_state_

private

Definition at line 165 of file moveit_opw_kinematics_plugin.h.

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

Classes

Public Member Functions

Protected Member Functions

Private Member Functions

Static Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation