Basic low-level kinematics functions. More...

#include <basic_kin.h>

Public Member Functions
bool	calcFwdKin (const Eigen::VectorXd &joint_angles, Eigen::Affine3d &pose) const
	Calculates tool pose of robot chain.
bool	calcFwdKin (const Eigen::VectorXd &joint_angles, const std::string &base, const std::string &tip, KDL::Frame &pose) const
	Creates chain and calculates tool pose relative to root New chain is not stored permanently, but subchain_fk_solver_ is updated.
bool	calcJacobian (const Eigen::VectorXd &joint_angles, Eigen::MatrixXd &jacobian) const
	Calculated jacobian of robot given joint angles.
bool	checkInitialized () const
	Checks if BasicKin is initialized (init() has been run: urdf model loaded, etc.)
bool	checkJoints (const Eigen::VectorXd &vec) const
	Check for consistency in # and limits of joints.
const moveit::core::JointModelGroup *	getJointModelGroup () const
	Get the name of the kinematic group.
bool	getJointNames (std::vector< std::string > &names) const
	Get list of joint names for robot.
Eigen::MatrixXd	getLimits () const
	Getter for joint_limits_.
bool	getLinkNames (std::vector< std::string > &names) const
	Get list of link names for robot.
std::string	getRobotBaseLinkName () const
	getter for the robot base link name
std::string	getRobotTipLinkName () const
	getter for the robot tip link name
bool	getSubChain (const std::string link_name, KDL::Chain &chain) const
	Get a subchain of the kinematic group.
bool	init (const moveit::core::JointModelGroup *group)
	Initializes BasicKin Creates KDL::Chain from urdf::Model, populates joint_list_, joint_limits_, and link_list_.
bool	linkTransforms (const Eigen::VectorXd &joint_angles, std::vector< KDL::Frame > &poses, const std::vector< std::string > &link_names=std::vector< std::string >()) const
	Calculates transforms of each link relative to base (not including base) If link_names is specified, only listed links will be returned. Otherwise all links in link_list_ will be returned.
unsigned int	numJoints () const
	Number of joints in robot.
BasicKin &	operator= (const BasicKin &rhs)
	Assigns values from another BasicKin to this.
bool	solvePInv (const Eigen::MatrixXd &A, const Eigen::VectorXd &b, Eigen::VectorXd &x) const
	Solve equation Ax=b for x Use this SVD to compute A+ (pseudoinverse of A). Weighting still TBD.
Static Public Member Functions
static bool	dampedPInv (const Eigen::MatrixXd &A, Eigen::MatrixXd &P, const double eps=0.011, const double lambda=0.01)
	Calculate Damped Pseudoinverse Use this SVD to compute A+ (pseudoinverse of A). Weighting still TBD.
static void	EigenToKDL (const Eigen::VectorXd &vec, KDL::JntArray &joints)
	Convert Eigen::Vector to KDL::JntArray.
static void	KDLToEigen (const KDL::Frame &frame, Eigen::Affine3d &transform)
	Convert KDL::Frame to Eigen::Affine3d.
static void	KDLToEigen (const KDL::Jacobian &jacobian, Eigen::MatrixXd &matrix)
	Convert KDL::Jacobian to Eigen::Matrix.
Private Member Functions
int	getJointNum (const std::string &joint_name) const
	Get joint number of given joint in initialized robot.
int	getLinkNum (const std::string &link_name) const
	Get link number of given joint in initialized robot.
Private Attributes
std::string	base_name_
boost::scoped_ptr < KDL::ChainFkSolverPos_recursive >	fk_solver_
const moveit::core::JointModelGroup *	group_
bool	initialized_
boost::scoped_ptr < KDL::ChainJntToJacSolver >	jac_solver_
Eigen::Matrix< double, Eigen::Dynamic, 2 >	joint_limits_
std::vector< std::string >	joint_list_
KDL::Tree	kdl_tree_
std::vector< std::string >	link_list_
KDL::Chain	robot_chain_
std::string	tip_name_

Detailed Description

Basic low-level kinematics functions.

Typically, just wrappers around the equivalent KDL calls.

Definition at line 53 of file basic_kin.h.

Member Function Documentation

bool constrained_ik::basic_kin::BasicKin::calcFwdKin	(	const Eigen::VectorXd &	joint_angles,
		Eigen::Affine3d &	pose
	)		const

Calculates tool pose of robot chain.

Parameters:

joint_angles	Vector of joint angles (size must match number of joints in robot chain)
pose	Transform of end-of-tip relative to root

Returns:: True if calculation successful, False if anything is wrong (including uninitialized BasicKin)

Definition at line 44 of file basic_kin.cpp.

bool constrained_ik::basic_kin::BasicKin::calcFwdKin	(	const Eigen::VectorXd &	joint_angles,
		const std::string &	base,
		const std::string &	tip,
		KDL::Frame &	pose
	)		const

Creates chain and calculates tool pose relative to root New chain is not stored permanently, but subchain_fk_solver_ is updated.

Parameters:

joint_angles	Vector of joint angles (size must match number of joints in chain)
base	Name of base link for new chain
tip	Name of tip link for new chain
pose	Transform of end-of-tip relative to base

Returns:: True if calculation successful, False if anything is wrong (including uninitialized BasicKin)

Definition at line 66 of file basic_kin.cpp.

bool constrained_ik::basic_kin::BasicKin::calcJacobian	(	const Eigen::VectorXd &	joint_angles,
		Eigen::MatrixXd &	jacobian
	)		const

Calculated jacobian of robot given joint angles.

Parameters:

joint_angles	Input vector of joint angles
jacobian	Output jacobian

Returns:: True if calculation successful, False if anything is wrong (including uninitialized BasicKin)

Definition at line 96 of file basic_kin.cpp.

bool constrained_ik::basic_kin::BasicKin::checkInitialized ( ) const [inline]

Checks if BasicKin is initialized (init() has been run: urdf model loaded, etc.)

Returns:: True if init() has completed successfully

Definition at line 100 of file basic_kin.h.

bool constrained_ik::basic_kin::BasicKin::checkJoints ( const Eigen::VectorXd & vec ) const

Check for consistency in # and limits of joints.

Parameters:

vec	Vector of joint values

Returns:: True if size of vec matches # of robot joints and all joints are within limits

Definition at line 113 of file basic_kin.cpp.

bool constrained_ik::basic_kin::BasicKin::dampedPInv	(	const Eigen::MatrixXd &	A,
		Eigen::MatrixXd &	P,
		const double	eps = `0.011`,
		const double	lambda = `0.01`
	)		`[static]`

Calculate Damped Pseudoinverse Use this SVD to compute A+ (pseudoinverse of A). Weighting still TBD.

Parameters:

A	Input matrix (represents Jacobian)
P	Output matrix (represents pseudoinverse of A)
eps	Singular value threshold
lambda	Damping factor

Returns:: True if Pseudoinverse completes properly

Definition at line 371 of file basic_kin.cpp.

static void constrained_ik::basic_kin::BasicKin::EigenToKDL	(	const Eigen::VectorXd &	vec,
		KDL::JntArray &	joints
	)		`[inline, static]`

Convert Eigen::Vector to KDL::JntArray.

Parameters:

vec	Input Eigen vector
joints	Output KDL joint array

Definition at line 231 of file basic_kin.h.

const moveit::core::JointModelGroup* constrained_ik::basic_kin::BasicKin::getJointModelGroup ( ) const [inline]

Get the name of the kinematic group.

Returns:: string with the group name

Definition at line 144 of file basic_kin.h.

bool constrained_ik::basic_kin::BasicKin::getJointNames ( std::vector< std::string > & names ) const

Get list of joint names for robot.

Parameters:

names Output vector of joint names, copied from joint_list_ created in init()

Returns:: True if BasicKin has been successfully initialized

Definition at line 135 of file basic_kin.cpp.

int constrained_ik::basic_kin::BasicKin::getJointNum ( const std::string & joint_name ) const [private]

Get joint number of given joint in initialized robot.

Parameters:

joint_name Input name of joint

Returns:: joint index if joint_name part of joint_list_, n+1 otherwise

Definition at line 157 of file basic_kin.cpp.

Eigen::MatrixXd constrained_ik::basic_kin::BasicKin::getLimits ( ) const [inline]

Getter for joint_limits_.

Returns:: Matrix of joint limits

Definition at line 122 of file basic_kin.h.

bool constrained_ik::basic_kin::BasicKin::getLinkNames ( std::vector< std::string > & names ) const

Get list of link names for robot.

Parameters:

names Output vector of names, copied from link_list_ created in init()

Returns:: True if BasicKin has been successfully initialized

Definition at line 146 of file basic_kin.cpp.

int constrained_ik::basic_kin::BasicKin::getLinkNum ( const std::string & link_name ) const [private]

Get link number of given joint in initialized robot.

Parameters:

link_name Input name of link

Returns:: link index if link_name part of link_list_, l+1 otherwise

Definition at line 167 of file basic_kin.cpp.

std::string constrained_ik::basic_kin::BasicKin::getRobotBaseLinkName ( ) const [inline]

getter for the robot base link name

Returns:: std::string base_name_

Definition at line 176 of file basic_kin.h.

std::string constrained_ik::basic_kin::BasicKin::getRobotTipLinkName ( ) const [inline]

getter for the robot tip link name

Returns:: std::string tip_name_

Definition at line 182 of file basic_kin.h.

bool constrained_ik::basic_kin::BasicKin::getSubChain	(	const std::string	link_name,
		KDL::Chain &	chain
	)		const

Get a subchain of the kinematic group.

Parameters:

link_name	Name of final link in chain
chain	Output kinematic chain

Returns:: True if the subchain was successfully created

Definition at line 259 of file basic_kin.cpp.

bool constrained_ik::basic_kin::BasicKin::init ( const moveit::core::JointModelGroup * group )

Initializes BasicKin Creates KDL::Chain from urdf::Model, populates joint_list_, joint_limits_, and link_list_.

Parameters:

group Input kinematic joint model group

Returns:: True if init() completes successfully

Definition at line 177 of file basic_kin.cpp.

void constrained_ik::basic_kin::BasicKin::KDLToEigen	(	const KDL::Frame &	frame,
		Eigen::Affine3d &	transform
	)		`[static]`

Convert KDL::Frame to Eigen::Affine3d.

Parameters:

frame	Input KDL Frame
transform	Output Eigen transform (Affine3d)

Definition at line 237 of file basic_kin.cpp.

void constrained_ik::basic_kin::BasicKin::KDLToEigen	(	const KDL::Jacobian &	jacobian,
		Eigen::MatrixXd &	matrix
	)		`[static]`

Convert KDL::Jacobian to Eigen::Matrix.

Parameters:

jacobian	Input KDL Jacobian
matrix	Output Eigen MatrixXd

Definition at line 250 of file basic_kin.cpp.

bool constrained_ik::basic_kin::BasicKin::linkTransforms	(	const Eigen::VectorXd &	joint_angles,
		std::vector< KDL::Frame > &	poses,
		const std::vector< std::string > &	link_names = `std::vector<std::string>()`
	)		const

Calculates transforms of each link relative to base (not including base) If link_names is specified, only listed links will be returned. Otherwise all links in link_list_ will be returned.

Parameters:

joint_angles	Input vector of joint values
poses	Output poses of listed links
link_names	Optional input list of links to calculate transforms for

Returns:: True if all requested links have poses calculated

Definition at line 273 of file basic_kin.cpp.

unsigned int constrained_ik::basic_kin::BasicKin::numJoints ( ) const [inline]

Number of joints in robot.

Returns:: Number of joints in robot

Definition at line 150 of file basic_kin.h.

BasicKin & constrained_ik::basic_kin::BasicKin::operator= ( const BasicKin & rhs )

Assigns values from another BasicKin to this.

Parameters:

rhs	Input BasicKin object to copy from

Returns:: reference to this BasicKin object

Definition at line 314 of file basic_kin.cpp.

bool constrained_ik::basic_kin::BasicKin::solvePInv	(	const Eigen::MatrixXd &	A,
		const Eigen::VectorXd &	b,
		Eigen::VectorXd &	x
	)		const

Solve equation Ax=b for x Use this SVD to compute A+ (pseudoinverse of A). Weighting still TBD.

Parameters:

A	Input matrix (represents Jacobian)
b	Input vector (represents desired pose)
x	Output vector (represents joint values)

Returns:: True if solver completes properly

Definition at line 331 of file basic_kin.cpp.

Member Data Documentation

std::string constrained_ik::basic_kin::BasicKin::base_name_ [private]

Link name of first link in the kinematic chain

Definition at line 238 of file basic_kin.h.

boost::scoped_ptr<KDL::ChainFkSolverPos_recursive> constrained_ik::basic_kin::BasicKin::fk_solver_ [private]

KDL Forward Kinematic Solver

Definition at line 243 of file basic_kin.h.

const moveit::core::JointModelGroup* constrained_ik::basic_kin::BasicKin::group_ [private]

Move group

Definition at line 235 of file basic_kin.h.

bool constrained_ik::basic_kin::BasicKin::initialized_ [private]

Identifies if the object has been initialized

Definition at line 234 of file basic_kin.h.

boost::scoped_ptr<KDL::ChainJntToJacSolver> constrained_ik::basic_kin::BasicKin::jac_solver_ [private]

KDL Jacobian Solver

Definition at line 244 of file basic_kin.h.

Eigen::Matrix<double, Eigen::Dynamic, 2> constrained_ik::basic_kin::BasicKin::joint_limits_ [private]

Joint limits

Definition at line 242 of file basic_kin.h.

std::vector<std::string> constrained_ik::basic_kin::BasicKin::joint_list_ [private]

List of joint names

Definition at line 240 of file basic_kin.h.

KDL::Tree constrained_ik::basic_kin::BasicKin::kdl_tree_ [private]

KDL tree object

Definition at line 237 of file basic_kin.h.

std::vector<std::string> constrained_ik::basic_kin::BasicKin::link_list_ [private]

List of link names

Definition at line 241 of file basic_kin.h.

KDL::Chain constrained_ik::basic_kin::BasicKin::robot_chain_ [private]

KDL Chain object

Definition at line 236 of file basic_kin.h.

std::string constrained_ik::basic_kin::BasicKin::tip_name_ [private]

Link name of last kink in the kinematic chain

Definition at line 239 of file basic_kin.h.

The documentation for this class was generated from the following files:

Public Member Functions

Static Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Member Function Documentation

Member Data Documentation