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

	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

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

void	setDefaultTimeout (double timeout)

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

void	setSearchDiscretization (double sd)

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

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

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	comparePoses (Eigen::Isometry3d &Ta, Eigen::Isometry3d &Tb)
	check if two poses are the same within an absolute tolerance of 1e-6 More...

void	expandIKSolutions (std::vector< std::vector< double >> &solutions) const
	append IK solutions by adding +-2pi More...

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

bool	getIK (const Eigen::Isometry3d &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 (const std::vector< double > &joint_angles)
	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_

const robot_model::JointModelGroup *	joint_model_group_

int	num_possible_redundant_joints_

opw_kinematics::Parameters< double >	opw_parameters_

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_

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

◆ MoveItOPWKinematicsPlugin()

moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::MoveItOPWKinematicsPlugin ( )

Default constructor.

Definition at line 28 of file moveit_opw_kinematics_plugin.cpp.

Member Function Documentation

◆ closestJointPose()

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 554 of file moveit_opw_kinematics_plugin.cpp.

◆ comparePoses()

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

private

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

Definition at line 181 of file moveit_opw_kinematics_plugin.cpp.

◆ distance()

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

staticprivate

Definition at line 545 of file moveit_opw_kinematics_plugin.cpp.

◆ expandIKSolutions()

void moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::expandIKSolutions ( std::vector< std::vector< double >> & solutions ) const

private

append IK solutions by adding +-2pi

For all solutions, check if solution +-360° is still inside limits An opw solution might be outside the joint limits, while the extended one is inside (e.g. asymmetric limits) therefore this just extends the solution space, need to apply joint limits separately

Definition at line 289 of file moveit_opw_kinematics_plugin.cpp.

◆ getAllIK()

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

private

Definition at line 572 of file moveit_opw_kinematics_plugin.cpp.

◆ getIK()

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

private

Definition at line 607 of file moveit_opw_kinematics_plugin.cpp.

◆ getJointIndex()

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

private

Definition at line 147 of file moveit_opw_kinematics_plugin.cpp.

◆ getJointNames()

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 471 of file moveit_opw_kinematics_plugin.cpp.

◆ getLinkNames()

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 476 of file moveit_opw_kinematics_plugin.cpp.

◆ getPositionFK()

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 439 of file moveit_opw_kinematics_plugin.cpp.

◆ getPositionIK() [1/2]

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 215 of file moveit_opw_kinematics_plugin.cpp.

◆ getPositionIK() [2/2]

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 424 of file moveit_opw_kinematics_plugin.cpp.

◆ getVariableNames()

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 481 of file moveit_opw_kinematics_plugin.cpp.

◆ initialize()

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

◆ isRedundantJoint()

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

private

Definition at line 139 of file moveit_opw_kinematics_plugin.cpp.

◆ searchPositionIK() [1/6]

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 227 of file moveit_opw_kinematics_plugin.cpp.

◆ searchPositionIK() [2/6]

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 240 of file moveit_opw_kinematics_plugin.cpp.

◆ searchPositionIK() [3/6]

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 252 of file moveit_opw_kinematics_plugin.cpp.

◆ searchPositionIK() [4/6]

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 263 of file moveit_opw_kinematics_plugin.cpp.

◆ searchPositionIK() [5/6]

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 274 of file moveit_opw_kinematics_plugin.cpp.

◆ searchPositionIK() [6/6]

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 327 of file moveit_opw_kinematics_plugin.cpp.

◆ selfTest()

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::selfTest ( const std::vector< double > & joint_angles )

private

check forward and inverse kinematics consistency

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

Definition at line 162 of file moveit_opw_kinematics_plugin.cpp.

◆ setOPWParameters()

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::setOPWParameters ( )

private

Definition at line 486 of file moveit_opw_kinematics_plugin.cpp.

◆ setRedundantJoints()

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

overrideprotectedvirtual

Reimplemented from kinematics::KinematicsBase.

Definition at line 121 of file moveit_opw_kinematics_plugin.cpp.

◆ timedOut()

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

private

Definition at line 157 of file moveit_opw_kinematics_plugin.cpp.

Member Data Documentation

◆ active_

bool moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::active_

private

Definition at line 158 of file moveit_opw_kinematics_plugin.h.

◆ dimension_

unsigned int moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::dimension_

private

Stores information for the inverse kinematics solver

Definition at line 162 of file moveit_opw_kinematics_plugin.h.

◆ ik_group_info_

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 160 of file moveit_opw_kinematics_plugin.h.

◆ joint_model_group_

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

private

Dimension of the group

Definition at line 164 of file moveit_opw_kinematics_plugin.h.

◆ num_possible_redundant_joints_

int moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::num_possible_redundant_joints_

private

Definition at line 168 of file moveit_opw_kinematics_plugin.h.

◆ opw_parameters_

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

private

Definition at line 170 of file moveit_opw_kinematics_plugin.h.

◆ robot_state_

robot_state::RobotStatePtr moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin::robot_state_

private

Definition at line 166 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

◆ MoveItOPWKinematicsPlugin()

Member Function Documentation

◆ closestJointPose()

◆ comparePoses()

◆ distance()

◆ expandIKSolutions()

◆ getAllIK()

◆ getIK()

◆ getJointIndex()

◆ getJointNames()

◆ getLinkNames()

◆ getPositionFK()

◆ getPositionIK() [1/2]

◆ getPositionIK() [2/2]

◆ getVariableNames()

◆ initialize()

◆ isRedundantJoint()

◆ searchPositionIK() [1/6]

◆ searchPositionIK() [2/6]

◆ searchPositionIK() [3/6]

◆ searchPositionIK() [4/6]

◆ searchPositionIK() [5/6]

◆ searchPositionIK() [6/6]

◆ selfTest()

◆ setOPWParameters()

◆ setRedundantJoints()

◆ timedOut()

Member Data Documentation

◆ active_

◆ dimension_

◆ ik_group_info_

◆ joint_model_group_

◆ num_possible_redundant_joints_

◆ opw_parameters_

◆ robot_state_