Specific implementation of kinematics using Levenberg-Marquardt method available at KDL. This version can be used with any robot. More...

#include <lma_kinematics_plugin.h>

Inheritance diagram for lma_kinematics_plugin::LMAKinematicsPlugin:

[legend]

List of all members.

Public Member Functions
const std::vector< std::string > &	getJointNames () const
	Return all the joint names in the order they are used internally.
const std::vector< std::string > &	getLinkNames () const
	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
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
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)
	LMAKinematicsPlugin ()
	Default constructor.
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
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
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
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
Protected Member Functions
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
	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	setRedundantJoints (const std::vector< unsigned int > &redundant_joint_indices)
Private Member Functions
bool	checkConsistency (const KDL::JntArray &seed_state, const std::vector< double > &consistency_limit, const KDL::JntArray &solution) const
	Check whether the solution lies within the consistency limit of the seed state.
int	getJointIndex (const std::string &name) const
int	getKDLSegmentIndex (const std::string &name) const
void	getRandomConfiguration (KDL::JntArray &jnt_array, bool lock_redundancy) const
void	getRandomConfiguration (const KDL::JntArray &seed_state, const std::vector< double > &consistency_limits, KDL::JntArray &jnt_array, bool lock_redundancy) const
	Get a random configuration within joint limits close to the seed state.
bool	isRedundantJoint (unsigned int index) const
bool	timedOut (const ros::WallTime &start_time, double duration) const
Private Attributes
bool	active_
unsigned int	dimension_
double	epsilon_
moveit_msgs::KinematicSolverInfo	fk_chain_info_
moveit_msgs::KinematicSolverInfo	ik_chain_info_
KDL::JntArray	joint_max_
KDL::JntArray	joint_min_
robot_model::JointModelGroup *	joint_model_group_
KDL::Chain	kdl_chain_
double	max_solver_iterations_
std::vector< JointMimic >	mimic_joints_
int	num_possible_redundant_joints_
bool	position_ik_
random_numbers::RandomNumberGenerator	random_number_generator_
std::vector< unsigned int >	redundant_joints_map_index_
robot_model::RobotModelPtr	robot_model_
robot_state::RobotStatePtr	state_
robot_state::RobotStatePtr	state_2_

Detailed Description

Specific implementation of kinematics using Levenberg-Marquardt method available at KDL. This version can be used with any robot.

Definition at line 74 of file lma_kinematics_plugin.h.

Constructor & Destructor Documentation

lma_kinematics_plugin::LMAKinematicsPlugin::LMAKinematicsPlugin ( )

Default constructor.

Definition at line 54 of file lma_kinematics_plugin.cpp.

Member Function Documentation

bool lma_kinematics_plugin::LMAKinematicsPlugin::checkConsistency	(	const KDL::JntArray &	seed_state,
		const std::vector< double > &	consistency_limit,
		const KDL::JntArray &	solution
	)		const `[private]`

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

Parameters:

seed_state	Seed state
redundancy	Index of the redundant joint within the chain
consistency_limit	The returned state for redundant joint should be in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
solution	solution configuration

Returns:: true if check succeeds

Definition at line 117 of file lma_kinematics_plugin.cpp.

int lma_kinematics_plugin::LMAKinematicsPlugin::getJointIndex ( const std::string & name ) const [private]

Definition at line 347 of file lma_kinematics_plugin.cpp.

const std::vector< std::string > & lma_kinematics_plugin::LMAKinematicsPlugin::getJointNames ( ) const [virtual]

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

Implements kinematics::KinematicsBase.

Definition at line 613 of file lma_kinematics_plugin.cpp.

int lma_kinematics_plugin::LMAKinematicsPlugin::getKDLSegmentIndex ( const std::string & name ) const [private]

Definition at line 357 of file lma_kinematics_plugin.cpp.

const std::vector< std::string > & lma_kinematics_plugin::LMAKinematicsPlugin::getLinkNames ( ) const [virtual]

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

Implements kinematics::KinematicsBase.

Definition at line 618 of file lma_kinematics_plugin.cpp.

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

Implements kinematics::KinematicsBase.

Definition at line 567 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::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]`

Implements kinematics::KinematicsBase.

Definition at line 376 of file lma_kinematics_plugin.cpp.

void lma_kinematics_plugin::LMAKinematicsPlugin::getRandomConfiguration	(	KDL::JntArray &	jnt_array,
		bool	lock_redundancy
	)		const `[private]`

Definition at line 58 of file lma_kinematics_plugin.cpp.

void lma_kinematics_plugin::LMAKinematicsPlugin::getRandomConfiguration	(	const KDL::JntArray &	seed_state,
		const std::vector< double > &	consistency_limits,
		KDL::JntArray &	jnt_array,
		bool	lock_redundancy
	)		const `[private]`

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

Parameters:

seed_state	Seed state
redundancy	Index of the redundant joint within the chain
consistency_limit	The returned state will contain a value for the redundant joint in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
jnt_array	Returned random configuration

Definition at line 80 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::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]`

Implements kinematics::KinematicsBase.

Definition at line 127 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::isRedundantJoint ( unsigned int index ) const [private]

Definition at line 72 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::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]`

Implements kinematics::KinematicsBase.

Definition at line 387 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::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]`

Implements kinematics::KinematicsBase.

Definition at line 399 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::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]`

Implements kinematics::KinematicsBase.

Definition at line 409 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::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]`

Implements kinematics::KinematicsBase.

Definition at line 420 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::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 `[protected]`

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
check_consistency	Set to true if consistency check needs to be performed
redundancy	The index of the redundant joint
consistency_limit	The returned solutuion will contain a value for the redundant joint in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]

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

Definition at line 430 of file lma_kinematics_plugin.cpp.

bool lma_kinematics_plugin::LMAKinematicsPlugin::setRedundantJoints ( const std::vector< unsigned int > & redundant_joint_indices ) [protected, virtual]

Reimplemented from kinematics::KinematicsBase.

Definition at line 289 of file lma_kinematics_plugin.cpp.