Provides an interface for kinematics solvers. More...

#include <kinematics_base.h>

Inheritance diagram for kinematics::KinematicsBase:

Public Types
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.
Public Member Functions
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.
double	getDefaultTimeout () const
	For functions that require a timeout specified but one is not specified using arguments, this default timeout is used.
virtual const std::string &	getGroupName () const
	Return the name of the group that the solver is operating on.
virtual const std::vector < std::string > &	getJointNames () const =0
	Return all the joint names in the order they are used internally.
virtual const std::vector < std::string > &	getLinkNames () const =0
	Return all the link names in the order they are represented internally.
virtual bool	getPositionFK (const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const =0
	Given a set of joint angles and a set of links, compute their pose.
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 =0
	Given a desired pose of the end-effector, compute the joint angles to reach it.
virtual void	getRedundantJoints (std::vector< unsigned int > &redundant_joint_indices) const
	Get the set of redundant joints.
double	getSearchDiscretization () const
	Get the value of the search discretization.
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.
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.
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)=0
	Initialization function for the kinematics, for use with kinematic chain IK solvers.
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.
	KinematicsBase ()
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 =0
	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).
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 =0
	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).
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 =0
	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).
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 =0
	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).
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).
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)
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!
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()
void	setSearchDiscretization (double sd)
	Set the search 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)
	Set the parameters for the solver, for use with kinematic chain IK solvers.
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.
virtual const bool	supportsGroup (const moveit::core::JointModelGroup jmg, std::string error_text_out=NULL) const
	Check if this solver supports a given JointModelGroup.
virtual	~KinematicsBase ()
	Virtual destructor for the interface.
Static Public Attributes
static const double	DEFAULT_SEARCH_DISCRETIZATION = 0.1
static const double	DEFAULT_TIMEOUT = 1.0
Protected Attributes
std::string	base_frame_
double	default_timeout_
std::string	group_name_
std::vector< unsigned int >	redundant_joint_indices_
std::string	robot_description_
double	search_discretization_
std::string	tip_frame_
std::vector< std::string >	tip_frames_
Private Member Functions
std::string	removeSlash (const std::string &str) const

Detailed Description

Provides an interface for kinematics solvers.

Definition at line 80 of file kinematics_base.h.

Member Typedef Documentation

typedef boost::function<void(const geometry_msgs::Pose &ik_pose, const std::vector<double> &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> kinematics::KinematicsBase::IKCallbackFn

The signature for a callback that can compute IK.

Definition at line 88 of file kinematics_base.h.

Constructor & Destructor Documentation

virtual kinematics::KinematicsBase::~KinematicsBase ( ) [inline, virtual]

Virtual destructor for the interface.

Definition at line 479 of file kinematics_base.h.

kinematics::KinematicsBase::KinematicsBase ( ) [inline]

Definition at line 481 of file kinematics_base.h.

Member Function Documentation

virtual const std::string& kinematics::KinematicsBase::getBaseFrame ( ) const [inline, virtual]

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.

Returns:: The string name of the frame in which the solver is operating

Definition at line 360 of file kinematics_base.h.

double kinematics::KinematicsBase::getDefaultTimeout ( ) const [inline]

For functions that require a timeout specified but one is not specified using arguments, this default timeout is used.

Definition at line 471 of file kinematics_base.h.

virtual const std::string& kinematics::KinematicsBase::getGroupName ( ) const [inline, virtual]

Return the name of the group that the solver is operating on.

Returns:: The string name of the group that the solver is operating on

Definition at line 350 of file kinematics_base.h.

virtual const std::vector<std::string>& kinematics::KinematicsBase::getJointNames ( ) const [pure virtual]

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

Implemented in pr2_arm_kinematics::PR2ArmKinematicsPlugin.

virtual const std::vector<std::string>& kinematics::KinematicsBase::getLinkNames ( ) const [pure virtual]

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

Implemented in pr2_arm_kinematics::PR2ArmKinematicsPlugin.

virtual bool kinematics::KinematicsBase::getPositionFK	(	const std::vector< std::string > &	link_names,
		const std::vector< double > &	joint_angles,
		std::vector< geometry_msgs::Pose > &	poses
	)		const `[pure virtual]`

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

Parameters:

link_names	A set of links for which FK needs to be computed
joint_angles	The state for which FK is being computed
poses	The resultant set of poses (in the frame returned by getBaseFrame())

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

Implemented in pr2_arm_kinematics::PR2ArmKinematicsPlugin.

virtual bool kinematics::KinematicsBase::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 `[pure virtual]`

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

Parameters:

ik_pose	the desired pose of the link
ik_seed_state	an initial guess solution for the inverse kinematics
solution	the solution vector
error_code	an error code that encodes the reason for failure or success
lock_redundant_joints	if setRedundantJoints() was previously called, keep the values of the joints marked as redundant the same as in the seed

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

Implemented in pr2_arm_kinematics::PR2ArmKinematicsPlugin.

virtual void kinematics::KinematicsBase::getRedundantJoints ( std::vector< unsigned int > & redundant_joint_indices ) const [inline, virtual]

Get the set of redundant joints.

Definition at line 411 of file kinematics_base.h.

double kinematics::KinematicsBase::getSearchDiscretization ( ) const [inline]

Get the value of the search discretization.

Definition at line 457 of file kinematics_base.h.

virtual const std::string& kinematics::KinematicsBase::getTipFrame ( ) const [inline, virtual]

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.

Returns:: The string name of the tip frame of the chain on which the solver is operating

Definition at line 371 of file kinematics_base.h.

virtual const std::vector<std::string>& kinematics::KinematicsBase::getTipFrames ( ) const [inline, virtual]

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.

Returns:: The vector of names of the tip frames of the kinematic tree on which the solver is operating

Definition at line 384 of file kinematics_base.h.

virtual bool kinematics::KinematicsBase::initialize	(	const std::string &	robot_description,
		const std::string &	group_name,
		const std::string &	base_frame,
		const std::string &	tip_frame,
		double	search_discretization
	)		`[pure virtual]`

Initialization function for the kinematics, for use with kinematic chain IK solvers.

Parameters:

robot_description	This parameter can be used as an identifier for the robot kinematics it is computed for; For example, the name of the ROS parameter that contains the robot description;
group_name	The group for which this solver is being configured
base_frame	The base frame in which all input poses are expected. This may (or may not) be the root frame of the chain that the solver operates on
tip_frame	The tip of the chain
search_discretization	The discretization of the search when the solver steps through the redundancy

Returns:: True if initialization was successful, false otherwise

Implemented in pr2_arm_kinematics::PR2ArmKinematicsPlugin.

virtual bool kinematics::KinematicsBase::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
	)		`[inline, virtual]`

Initialization function for the kinematics, for use with non-chain IK solvers.

Parameters:

robot_description	This parameter can be used as an identifier for the robot kinematics is computed for; For example, rhe name of the ROS parameter that contains the robot description;
group_name	The group for which this solver is being configured
base_frame	The base frame in which all input poses are expected. This may (or may not) be the root frame of the chain that the solver operates on
tip_frames	A vector of tips of the kinematic tree
search_discretization	The discretization of the search when the solver steps through the redundancy

Returns:: True if initialization was successful, false otherwise

Definition at line 326 of file kinematics_base.h.

std::string kinematics::KinematicsBase::removeSlash ( const std::string & str ) const [private]

Definition at line 105 of file kinematics_base.cpp.

virtual bool kinematics::KinematicsBase::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 `[pure 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
timeout	The amount of time (in seconds) available to the solver
solution	the solution vector
error_code	an error code that encodes the reason for failure or success
lock_redundant_joints	if setRedundantJoints() was previously called, keep the values of the joints marked as redundant the same as in the seed

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

Implemented in pr2_arm_kinematics::PR2ArmKinematicsPlugin.

virtual bool kinematics::KinematicsBase::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 `[pure 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
timeout	The amount of time (in seconds) available to the solver
consistency_limits	the distance that any joint in the solution can be from the corresponding joints in the current seed state
solution	the solution vector
error_code	an error code that encodes the reason for failure or success
lock_redundant_joints	if setRedundantJoints() was previously called, keep the values of the joints marked as redundant the same as in the seed

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

Implemented in pr2_arm_kinematics::PR2ArmKinematicsPlugin.

virtual bool kinematics::KinematicsBase::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 `[pure 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
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
lock_redundant_joints	if setRedundantJoints() was previously called, keep the values of the joints marked as redundant the same as in the seed

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

Implemented in pr2_arm_kinematics::PR2ArmKinematicsPlugin.

virtual bool kinematics::KinematicsBase::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 `[pure 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
timeout	The amount of time (in seconds) available to the solver
consistency_limits	the distance that any joint in the solution can be from the corresponding joints in the current seed state
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
lock_redundant_joints	if setRedundantJoints() was previously called, keep the values of the joints marked as redundant the same as in the seed

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

Implemented in pr2_arm_kinematics::PR2ArmKinematicsPlugin.

virtual bool kinematics::KinematicsBase::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 `[inline, virtual]`

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

Parameters:

ik_poses	the desired pose of each tip link
ik_seed_state	an initial guess solution for the inverse kinematics
timeout	The amount of time (in seconds) available to the solver
consistency_limits	the distance that any joint in the solution can be from the corresponding joints in the current seed state
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
options	container for other IK options
context_state	(optional) the context in which this request is being made. The position values corresponding to joints in the current group may not match those in ik_seed_state. The values in ik_seed_state are the ones to use. This is passed just to provide the other joint values, in case they are needed for context, like with an IK solver that computes a balanced result for a biped.

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

Definition at line 213 of file kinematics_base.h.

void kinematics::KinematicsBase::setDefaultTimeout ( double timeout ) [inline]

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)

Definition at line 464 of file kinematics_base.h.

bool kinematics::KinematicsBase::setRedundantJoints ( const std::vector< unsigned int > & redundant_joint_indices ) [virtual]

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!

Parameters:

redundant_joint_indices The set of redundant joint indices (corresponding to the list of joints you get from getJointNames()).

Returns:: False if any of the input joint indices are invalid (exceed number of joints)

Definition at line 78 of file kinematics_base.cpp.

bool kinematics::KinematicsBase::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()

Parameters:

redundant_joint_names The set of redundant joint names.

Returns:: False if any of the input joint indices are invalid (exceed number of joints)

Definition at line 91 of file kinematics_base.cpp.

void kinematics::KinematicsBase::setSearchDiscretization ( double sd ) [inline]

Set the search discretization.

Definition at line 449 of file kinematics_base.h.

void kinematics::KinematicsBase::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]`

Set the parameters for the solver, for use with kinematic chain IK solvers.

Parameters:

robot_description	This parameter can be used as an identifier for the robot kinematics it is computed for; For example, the name of the ROS parameter that contains the robot description;
group_name	The group for which this solver is being configured
base_frame	The base frame in which all input poses are expected. This may (or may not) be the root frame of the chain that the solver operates on
tip_frame	The tip of the chain
search_discretization	The discretization of the search when the solver steps through the redundancy

Definition at line 43 of file kinematics_base.cpp.

void kinematics::KinematicsBase::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]`

Set the parameters for the solver, for use with non-chain IK solvers.

Parameters:

robot_description	This parameter can be used as an identifier for the robot kinematics it is computed for; For example, the name of the ROS parameter that contains the robot description;
group_name	The group for which this solver is being configured
base_frame	The base frame in which all input poses are expected. This may (or may not) be the root frame of the chain that the solver operates on
tip_frames	A vector of tips of the kinematic tree
search_discretization	The discretization of the search when the solver steps through the redundancy

Definition at line 57 of file kinematics_base.cpp.

const bool kinematics::KinematicsBase::supportsGroup	(	const moveit::core::JointModelGroup *	jmg,
		std::string *	error_text_out = `NULL`
	)		const `[virtual]`

Check if this solver supports a given JointModelGroup.

Override this function to check if your kinematics solver implementation supports the given group.

The default implementation just returns jmg->isChain(), since solvers written before this function was added all supported only chain groups.

Parameters:

jmg	the planning group being proposed to be solved by this IK solver
error_text_out	If this pointer is non-null and the group is not supported, this is filled with a description of why it's not supported.