Basic low-level kinematics functions. Typically, just wrappers around the equivalent KDL calls. More...

#include <basic_kin.h>

Public Member Functions
	BasicKin ()
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)
	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.
bool	getJointNames (std::vector< std::string > &names) const
	Get list of joint names for robot.
Eigen::MatrixXd	getLimits () const
	Get list of joint names for specific robot chain Crawls chain to create list.
bool	getLinkNames (std::vector< std::string > &names) const
	Get list of link names for robot.
bool	init (const urdf::Model &robot, const std::string &base_name, const std::string &tip_name)
	Get list of link names for specific robot chain Crawls chain to create 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.
	~BasicKin ()
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.
Static Private Member Functions
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)
Private Attributes
boost::scoped_ptr < KDL::ChainFkSolverPos_recursive >	fk_solver_
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_
boost::scoped_ptr < KDL::ChainFkSolverPos_recursive >	subchain_fk_solver_

Detailed Description

Basic low-level kinematics functions. Typically, just wrappers around the equivalent KDL calls.

Definition at line 44 of file basic_kin.h.

Constructor & Destructor Documentation

constrained_ik::basic_kin::BasicKin::BasicKin ( ) [inline]

Definition at line 47 of file basic_kin.h.

constrained_ik::basic_kin::BasicKin::~BasicKin ( ) [inline]

Definition at line 48 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)

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

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)

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 85 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 81 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 102 of file basic_kin.cpp.

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

Convert Eigen::Vector to KDL::JntArray.

Parameters:

vec	Input Eigen vector
joints	Output KDL joint array

Definition at line 181 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 124 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 185 of file basic_kin.cpp.

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

Get list of joint names for specific robot chain Crawls chain to create list.

Parameters:

chain	Input robot chain to retrieve list from
names	Output vector of joint names

Returns:: True if BasicKin has been successfully initializedGetter for joint_limits_; Matrix of joint limits

Definition at line 109 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 156 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 195 of file basic_kin.cpp.

bool constrained_ik::basic_kin::BasicKin::init	(	const urdf::Model &	robot,
		const std::string &	base_name,
		const std::string &	tip_name
	)

Get list of link names for specific robot chain Crawls chain to create list.

Parameters:

chain	Input robot chain to retrieve list from
names	Output vector of link names

Returns:: True if BasicKin has been successfully initializedInitializes BasicKin Creates KDL::Chain from urdf::Model, populates joint_list_, joint_limits_, and link_list_

Parameters:

robot	Input model containing robot information
base_name	Input name of base link
tip_name	Input name of tip link

Returns:: True if init() completes successfully

Definition at line 205 of file basic_kin.cpp.

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

Convert KDL::Frame to Eigen::Affine3d.

Parameters:

frame	Input KDL Frame
transform	Output Eigen transform (Affine3d)

Definition at line 261 of file basic_kin.cpp.

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

brief Convert KDL::Jacobian to Eigen::Matrix

Parameters:

jacobian	Input KDL Jacobian
matrix	Output Eigen MatrixXd

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 283 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 139 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 316 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 330 of file basic_kin.cpp.

Member Data Documentation

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

Definition at line 174 of file basic_kin.h.

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

Definition at line 169 of file basic_kin.h.

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

Definition at line 175 of file basic_kin.h.

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

Definition at line 173 of file basic_kin.h.

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

Definition at line 172 of file basic_kin.h.

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

Definition at line 171 of file basic_kin.h.

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

Definition at line 172 of file basic_kin.h.

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

Definition at line 170 of file basic_kin.h.

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

Definition at line 174 of file basic_kin.h.

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

Public Member Functions

Private Member Functions

Static Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation