This class offers services for forward and inverse kinematics calculations for tree kinematic structures. More...

#include <tree_kinematics.h>

Public Member Functions
bool	getPositionFk (kinematics_msgs::GetPositionFK::Request &request, kinematics_msgs::GetPositionFK::Response &response)
	Uses the given joint positions to calculate the position(s) of the specified end point(s) in Cartesian coordinates.
bool	getPositionIk (tree_kinematics::get_tree_position_ik::Request &request, tree_kinematics::get_tree_position_ik::Response &response)
	Uses the given end point position(s) in Cartesian coordinates to calculate new joint positions.
bool	init ()
	Configures the tree_kinematics class.
	TreeKinematics ()
	~TreeKinematics ()
Private Member Functions
int	getJointIndex (const std::string &name)
	Returns the index of the joint.
bool	loadModel (const std::string xml, KDL::Tree &kdl_tree, std::string &tree_root_name, unsigned int &nr_of_jnts, KDL::JntArray &joint_min, KDL::JntArray &joint_max, KDL::JntArray &joint_vel_max)
	Initialises the robot model and KDL::Tree and calls readJoints(...)
bool	readJoints (urdf::Model &robot_model, KDL::Tree &kdl_tree, std::string &tree_root_name, unsigned int &nr_of_jnts, KDL::JntArray &joint_min, KDL::JntArray &joint_max, KDL::JntArray &joint_vel_max)
	Retrieves information about all joints in the given model.
Private Attributes
ros::ServiceServer	fk_service_
boost::scoped_ptr < KDL::TreeFkSolverPos >	fk_solver_
double	ik_duration_
double	ik_duration_median_
boost::scoped_ptr < KDL::TreeIkSolverPos_Online >	ik_pos_solver_
ros::ServiceServer	ik_service_
double	ik_srv_duration_
double	ik_srv_duration_median_
boost::scoped_ptr < KDL::TreeIkSolverVel_wdls >	ik_vel_solver_
kinematics_msgs::KinematicSolverInfo	info_
KDL::MatrixXd	js_w_matr_
KDL::Tree	kdl_tree_
double	lambda_
unsigned int	loop_count_
ros::NodeHandle	nh_
ros::NodeHandle	nh_private_
unsigned int	nr_of_jnts_
int	srv_call_frequency_
tf::TransformListener	tf_listener_
std::string	tree_root_name_
KDL::MatrixXd	ts_w_matr_

Detailed Description

This class offers services for forward and inverse kinematics calculations for tree kinematic structures.

This class is basically a wrapper around KDL's solvers for forward and inverse kinematics specialised on tree kinematic structures. Once configured, services are offered for conveniently executing forward and inverse kinematics calculations. This class follows the structure of the pr2_arm_kinematics and the generic arm_kinematics package, which also offer similar services although for kinematic chains.

Definition at line 68 of file tree_kinematics.h.

Constructor & Destructor Documentation

tree_kinematics::TreeKinematics::TreeKinematics ( ) [inline]

Definition at line 71 of file tree_kinematics.h.

tree_kinematics::TreeKinematics::~TreeKinematics ( ) [inline]

Definition at line 80 of file tree_kinematics.h.

Member Function Documentation

int tree_kinematics::TreeKinematics::getJointIndex ( const std::string & name ) [private]

Returns the index of the joint.

Returns the index of the joint; used by getPositionFk(...) and getPositionIk (...);

Parameters:

name	name of the joint which index should be returned

Returns:: the index of the joint; -1, if joint could not be found

Definition at line 330 of file tree_kinematics.cpp.

bool tree_kinematics::TreeKinematics::getPositionFk	(	kinematics_msgs::GetPositionFK::Request &	request,
		kinematics_msgs::GetPositionFK::Response &	response
	)

Uses the given joint positions to calculate the position(s) of the specified end point(s) in Cartesian coordinates.

This method is basically a wrapper for KDL's FK solver to get the end point position in Cartesian coordinates based on the given joint positions, which are retrieved from the model.

@ param request contains the the name of the joints and the names of end points for which the position shall be calculated for @ param response contains the position(s) of the specified end point(s) in Cartesian coordinates

@ return false, if no index could be found for a given joint name or if the transformation from pose message to KDL::Frame threw a exception; true otherwise

Definition at line 341 of file tree_kinematics.cpp.

bool tree_kinematics::TreeKinematics::getPositionIk	(	tree_kinematics::get_tree_position_ik::Request &	request,
		tree_kinematics::get_tree_position_ik::Response &	response
	)

Uses the given end point position(s) in Cartesian coordinates to calculate new joint positions.

This method is basically a wrapper for KDL's IK solver to calculated the necessary joint positions to reach the given end point position(s).

@ param request contains the current joint positions and the name(s) and desired position(s) of the end point(s) @ param response contains the new joint positions as well as average joint velocities to reach these new joint positions in the time given by the service call frequency

@ return false, if no index could be found for a given joint name or if the transformation from pose message to KDL::Frame threw a exception; true otherwise

Definition at line 406 of file tree_kinematics.cpp.

bool tree_kinematics::TreeKinematics::init ( )

Configures the tree_kinematics class.

This methods loads a lot of parameters from the parameter server. Most of them have hard-coded default parameters. These are used, when the specific parameter could be found on the parameter server. Furthermore services get registered, the robot model is created and its parameters read (i.e. joint position limits) to configure the utilized kinematics solver. Note: The other methods will not work properly, if this function has not been called first!

@ return false, if critical parameters could not be found, true otherwise

Definition at line 39 of file tree_kinematics.cpp.

bool tree_kinematics::TreeKinematics::loadModel	(	const std::string	xml,
		KDL::Tree &	kdl_tree,
		std::string &	tree_root_name,
		unsigned int &	nr_of_jnts,
		KDL::JntArray &	joint_min,
		KDL::JntArray &	joint_max,
		KDL::JntArray &	joint_vel_max
	)		`[private]`

Initialises the robot model and KDL::Tree and calls readJoints(...)

This method initialises the robot model and KDL::Tree and calls readJoints(...).

Parameters:

xml	the xml description of the robot model
kdl_tree	pointer to the KDL::Tree
tree_root_name	pointer to the root name, which defines the (sub)tree
nr_of_jnts	pointer to the number of joints
joint_min	pointer to an array containing the minimum position limits of the joints
joint_max	pointer to an array containing the maximum position limits of the joints
joint_vel_max	pointer to an array containing the maximum absolute velocity limits of the joints

Returns:: false, if the robot model or KDL::Tree could not be initialised or readJoint(...) returned false, true otherwise

Definition at line 202 of file tree_kinematics.cpp.

bool tree_kinematics::TreeKinematics::readJoints	(	urdf::Model &	robot_model,
		KDL::Tree &	kdl_tree,
		std::string &	tree_root_name,
		unsigned int &	nr_of_jnts,
		KDL::JntArray &	joint_min,
		KDL::JntArray &	joint_max,
		KDL::JntArray &	joint_vel_max
	)		`[private]`

Retrieves information about all joints in the given model.

Using the provided root a subtree is extracted. This subtree is then searched for the all joints and their position and velocity limits. This method uses readJoints(...).

Parameters:

xml	pointer to the robot's model containing the desired joint information
kdl_tree	pointer to the KDL::Tree
tree_root_name	pointer to the root name, which defines the (sub)tree
nr_of_jnts	pointer to the number of joints
joint_min	pointer to an array containing the minimum position limits of the joints
joint_max	pointer to an array containing the maximum position limits of the joints
joint_vel_max	pointer to an array containing the maximum absolute velocity limits of the joints