#include <pr2_arm_kinematics_plugin.h>

Inheritance diagram for pr2_arm_kinematics::PR2ArmKinematicsPlugin:

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
	Given a set of joint angles and a set of links, compute their pose. More...

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
	Given a desired pose of the end-effector, compute the joint angles to reach it. More...

virtual bool	initialize (const std::string &robot_description, const std::string &group_name, const std::string &base_name, const std::string &tip_name, double search_discretization)
	Initialization function for the kinematics. More...

bool	isActive ()
	Specifies if the node is active or not. More...

	PR2ArmKinematicsPlugin ()

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

void	setRobotModel (urdf::ModelInterfaceSharedPtr &robot_model)

Public Member Functions inherited from kinematics::KinematicsBase
virtual const std::string &	getBaseFrame () const
	Return the name of the frame in which the solver is operating. This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. More...

double	getDefaultTimeout () const
	For functions that require a timeout specified but one is not specified using arguments, this default timeout is used. More...

virtual const std::string &	getGroupName () const
	Return the name of the group that the solver is operating on. More...

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
	Given a desired pose of the end-effector, compute the set joint angles solutions that are able to reach it. More...

virtual void	getRedundantJoints (std::vector< unsigned int > &redundant_joint_indices) const
	Get the set of redundant joints. More...

double	getSearchDiscretization (int joint_index=0) const
	Get the value of the search discretization. More...

std::vector< DiscretizationMethod >	getSupportedDiscretizationMethods () const
	Returns the set of supported kinematics discretization search types. This implementation only supports the DiscretizationMethods::ONE search. More...

virtual const std::string &	getTipFrame () const
	Return the name of the tip frame of the chain on which the solver is operating. This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. Deprecated in favor of getTipFrames(), but will remain for foreseeable future for backwards compatibility. More...

virtual const std::vector< std::string > &	getTipFrames () const
	Return the names of the tip frames of the kinematic tree on which the solver is operating. This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. More...

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)
	Initialization function for the kinematics, for use with non-chain IK solvers. More...

	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
	Given a set of desired poses for a planning group with multiple end-effectors, search for the joint angles required to reach them. This is useful for e.g. biped robots that need to perform whole-body IK. Not necessary for most robots that have kinematic chains. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines). More...

void	setDefaultTimeout (double timeout)
	For functions that require a timeout specified but one is not specified using arguments, a default timeout is used, as set by this function (and initialized to KinematicsBase::DEFAULT_TIMEOUT) More...

virtual bool	setRedundantJoints (const std::vector< unsigned int > &redundant_joint_indices)
	Set a set of redundant joints for the kinematics solver to use. This can fail, depending on the IK solver and choice of redundant joints!. Also, it sets the discretization values for each redundant joint to a default value. More...

bool	setRedundantJoints (const std::vector< std::string > &redundant_joint_names)
	Set a set of redundant joints for the kinematics solver to use. This function is just a convenience function that calls the previous definition of setRedundantJoints() More...

void	setSearchDiscretization (double sd)
	Set the search discretization value for all the redundant joints. More...

void	setSearchDiscretization (const std::map< int, double > &discretization)
	Sets individual discretization values for each redundant joint. More...

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)
	Set the parameters for the solver, for use with kinematic chain IK solvers. More...

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)
	Set the parameters for the solver, for use with non-chain IK solvers. More...

virtual bool	supportsGroup (const moveit::core::JointModelGroup jmg, std::string error_text_out=NULL) const
	Check if this solver supports a given JointModelGroup. More...

virtual	~KinematicsBase ()
	Virtual destructor for the interface. More...

Protected Member Functions
void	desiredPoseCallback (const KDL::JntArray &jnt_array, const KDL::Frame &ik_pose, moveit_msgs::MoveItErrorCodes &error_code) const

void	jointSolutionCallback (const KDL::JntArray &jnt_array, const KDL::Frame &ik_pose, moveit_msgs::MoveItErrorCodes &error_code) const

Protected Member Functions inherited from kinematics::KinematicsBase
template<typename T >
bool	lookupParam (const std::string &param, T &val, const T &default_val) const
	Enables kinematics plugins access to parameters that are defined for the private namespace and inside 'robot_description_kinematics'. Parameters are searched in the following locations and order. More...

Protected Attributes
bool	active_

IKCallbackFn	desiredPoseCallback_

int	dimension_

moveit_msgs::KinematicSolverInfo	fk_solver_info_

int	free_angle_

moveit_msgs::KinematicSolverInfo	ik_solver_info_

std::shared_ptr< KDL::ChainFkSolverPos_recursive >	jnt_to_pose_solver_

KDL::Chain	kdl_chain_

pr2_arm_kinematics::PR2ArmIKSolverPtr	pr2_arm_ik_solver_

urdf::ModelInterfaceSharedPtr	robot_model_

std::string	root_name_

IKCallbackFn	solutionCallback_

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_

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
	The signature for a callback that can compute IK. More...

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

static const double	DEFAULT_TIMEOUT = 1.0

Detailed Description

Definition at line 125 of file pr2_arm_kinematics_plugin.h.

Constructor & Destructor Documentation

pr2_arm_kinematics::PR2ArmKinematicsPlugin::PR2ArmKinematicsPlugin ( )

Definition at line 253 of file pr2_arm_kinematics_plugin.cpp.

Member Function Documentation

void pr2_arm_kinematics::PR2ArmKinematicsPlugin::desiredPoseCallback	(	const KDL::JntArray &	jnt_array,
		const KDL::Frame &	ik_pose,
		moveit_msgs::MoveItErrorCodes &	error_code
	)		const

protected

const std::vector< std::string > & pr2_arm_kinematics::PR2ArmKinematicsPlugin::getJointNames ( ) const

virtual

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

Implements kinematics::KinematicsBase.

Definition at line 414 of file pr2_arm_kinematics_plugin.cpp.

const std::vector< std::string > & pr2_arm_kinematics::PR2ArmKinematicsPlugin::getLinkNames ( ) const

virtual

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

Implements kinematics::KinematicsBase.

Definition at line 423 of file pr2_arm_kinematics_plugin.cpp.

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

virtual

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

Parameters

request	- the request contains the joint angles, set of links for which poses are to be computed and a timeout
response	- the response contains stamped pose information for all the requested links

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

Implements kinematics::KinematicsBase.

Definition at line 407 of file pr2_arm_kinematics_plugin.cpp.

bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::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

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

Parameters

ik_link_name	- the name of the link for which IK is being computed
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 322 of file pr2_arm_kinematics_plugin.cpp.

bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::initialize	(	const std::string &	robot_description,
		const std::string &	group_name,
		const std::string &	base_name,
		const std::string &	tip_name,
		double	search_discretization
	)

virtual

Initialization function for the kinematics.

Returns: True if initialization was successful, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 267 of file pr2_arm_kinematics_plugin.cpp.

bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::isActive ( )

Specifies if the node is active or not.

Returns: True if the node is active, false otherwise.

Definition at line 257 of file pr2_arm_kinematics_plugin.cpp.

void pr2_arm_kinematics::PR2ArmKinematicsPlugin::jointSolutionCallback	(	const KDL::JntArray &	jnt_array,
		const KDL::Frame &	ik_pose,
		moveit_msgs::MoveItErrorCodes &	error_code
	)		const

protected

bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::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

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 329 of file pr2_arm_kinematics_plugin.cpp.

bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::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

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
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 379 of file pr2_arm_kinematics_plugin.cpp.

bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::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

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 388 of file pr2_arm_kinematics_plugin.cpp.

bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::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

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