Damped Least-Squares Inverse Kinematic Solution. More...

#include <constrained_ik.h>

Public Member Functions
virtual void	addConstraint (Constraint *constraint, ConstraintTypes constraint_type)
	Add a new constraint to this IK solver.
virtual void	addConstraintsFromParamServer (const std::string &parameter_name)
	Add constraints from the ROS Parameter Server This allows for the constraints to be defined in a yaml file and loaded at runtime using the plugin interface.
virtual Eigen::MatrixXd	calcDampedPseudoinverse (const Eigen::MatrixXd &J) const
	Calculate the damped pseudo inverse of a matrix.
virtual bool	calcInvKin (const Eigen::Affine3d &goal, const Eigen::VectorXd &joint_seed, Eigen::VectorXd &joint_angles) const
	computes the inverse kinematics for the given pose of the tip link
virtual bool	calcInvKin (const Eigen::Affine3d &goal, const Eigen::VectorXd &joint_seed, const planning_scene::PlanningSceneConstPtr planning_scene, Eigen::VectorXd &joint_angles) const
	computes the inverse kinematics for the given pose of the tip link
virtual Eigen::MatrixXd	calcNullspaceProjection (const Eigen::MatrixXd &J) const
	Calculates the null space projection matrix using J^-1 * J.
virtual Eigen::MatrixXd	calcNullspaceProjectionTheRightWay (const Eigen::MatrixXd &A) const
	Calculates the null space projection matrix using SVD.
virtual initialization_state::InitializationState	checkInitialized () const
	Checks to see if object is initialized (ie: init() has been called)
virtual void	clearConstraintList ()
	Delete all constraints.
virtual bool	getJointNames (std::vector< std::string > &names) const
	Getter for joint names.
virtual const basic_kin::BasicKin &	getKin () const
	Getter for kinematics object.
virtual bool	getLinkNames (std::vector< std::string > &names) const
	Getter for link names.
virtual ConstrainedIKConfiguration	getSolverConfiguration () const
	Getter for solver configuration.
virtual void	init (const basic_kin::BasicKin &kin)
	Initializes object with kinematic model of robot.
bool	isInitialized () const
	Check if solver has been initialized.
virtual bool	linkTransforms (const Eigen::VectorXd &joints, std::vector< KDL::Frame > &poses, const std::vector< std::string > link_names=std::vector< std::string >()) const
	computes and returns the link transfoms of the named joints
virtual void	loadDefaultSolverConfiguration ()
	This will load the default solver configuration parameters.
virtual unsigned int	numJoints () const
	Getter for BasicKin numJoints.
virtual void	setSolverConfiguration (const ConstrainedIKConfiguration &config)
	Setter for solver configuration.
Static Public Member Functions
static double	rangedAngle (double angle)
	Translates an angle to lie between +/-PI.
Protected Member Functions
virtual SolverStatus	checkStatus (const constrained_ik::SolverState &state, const constrained_ik::ConstraintResults &primary, const constrained_ik::ConstraintResults &auxiliary) const
	Method determine convergence when both primary and auxiliary constraints are present.
virtual void	clipToJointLimits (Eigen::VectorXd &joints) const
	This function clips the joints within the joint limits.
virtual constrained_ik::ConstraintResults	evalConstraint (constraint_types::ConstraintTypes constraint_type, const constrained_ik::SolverState &state) const
	Calculating error, jacobian & status for all constraints specified.
virtual constrained_ik::SolverState	getState (const Eigen::Affine3d &goal, const Eigen::VectorXd &joint_seed) const
	Creates a new SolverState and checks key elements.
virtual void	updateState (constrained_ik::SolverState &state, const Eigen::VectorXd &joints) const
	Method update an existing SolverState provided new joint positions.
Protected Attributes
ConstraintGroup	auxiliary_constraints_
ConstrainedIKConfiguration	config_
bool	initialized_
basic_kin::BasicKin	kin_
ros::NodeHandle	nh_
ConstraintGroup	primary_constraints_

Detailed Description

Damped Least-Squares Inverse Kinematic Solution.

Todo:: Remove calcNullspaceProjection function and rename calcNullspaceProjectionTheRightWay to calcNullspaceProjection

Definition at line 57 of file constrained_ik.h.

Member Function Documentation

virtual void constrained_ik::Constrained_IK::addConstraint	(	Constraint *	constraint,
		ConstraintTypes	constraint_type
	)		`[inline, virtual]`

Add a new constraint to this IK solver.

Parameters:

constraint	Constraint to limit IK solution
constraint_type	Contraint type (primary or auxiliary)

Definition at line 82 of file constrained_ik.h.

void constrained_ik::Constrained_IK::addConstraintsFromParamServer ( const std::string & parameter_name ) [virtual]

Add constraints from the ROS Parameter Server This allows for the constraints to be defined in a yaml file and loaded at runtime using the plugin interface.

Parameters:

parameter_name the complete parameter name on the ROS parameter server Example: parameter_name="/namespace/namespace2/array_parameter"

Definition at line 47 of file constrained_ik.cpp.

Eigen::MatrixXd constrained_ik::Constrained_IK::calcDampedPseudoinverse ( const Eigen::MatrixXd & J ) const [virtual]

Calculate the damped pseudo inverse of a matrix.

Parameters:

J	matrix to compute inverse of

Returns:: inverse of J

Definition at line 186 of file constrained_ik.cpp.

bool constrained_ik::Constrained_IK::calcInvKin	(	const Eigen::Affine3d &	goal,
		const Eigen::VectorXd &	joint_seed,
		Eigen::VectorXd &	joint_angles
	)		const `[virtual]`

computes the inverse kinematics for the given pose of the tip link

Parameters:

goal	cartesian pose to solve the inverse kinematics about
joint_seed	joint values that is used as the initial guess
joint_angles	The joint pose that places the tip link to the desired pose.

Returns:: True if valid IK solution is found, otherwise false

Definition at line 201 of file constrained_ik.cpp.

bool constrained_ik::Constrained_IK::calcInvKin	(	const Eigen::Affine3d &	goal,
		const Eigen::VectorXd &	joint_seed,
		const planning_scene::PlanningSceneConstPtr	planning_scene,
		Eigen::VectorXd &	joint_angles
	)		const `[virtual]`

computes the inverse kinematics for the given pose of the tip link

Parameters:

goal	cartesian pose to solve the inverse kinematics about
joint_seed	joint values that is used as the initial guess
planning_scene	pointer to a planning scene that holds all the object in the environment. Use by the solver to check for collision; if a null pointer is passed then collisions are ignored.
joint_angles	The joint pose that places the tip link to the desired pose.

Returns:: True if valid IK solution is found, otherwise false

Definition at line 208 of file constrained_ik.cpp.

Eigen::MatrixXd constrained_ik::Constrained_IK::calcNullspaceProjection ( const Eigen::MatrixXd & J ) const [virtual]

Calculates the null space projection matrix using J^-1 * J.

Parameters:

J	matrix to compute null space projection matrix from

Returns:: null space projection matrix

Definition at line 146 of file constrained_ik.cpp.

Eigen::MatrixXd constrained_ik::Constrained_IK::calcNullspaceProjectionTheRightWay ( const Eigen::MatrixXd & A ) const [virtual]

Calculates the null space projection matrix using SVD.

Parameters:

A	matrix to compute null space projection matrix from

Returns:: null space projection matrix

Definition at line 155 of file constrained_ik.cpp.

virtual initialization_state::InitializationState constrained_ik::Constrained_IK::checkInitialized ( ) const [inline, virtual]

Checks to see if object is initialized (ie: init() has been called)

Returns:: InitializationState

Definition at line 134 of file constrained_ik.h.

SolverStatus constrained_ik::Constrained_IK::checkStatus	(	const constrained_ik::SolverState &	state,
		const constrained_ik::ConstraintResults &	primary,
		const constrained_ik::ConstraintResults &	auxiliary
	)		const `[protected, virtual]`

Method determine convergence when both primary and auxiliary constraints are present.

Parameters:

state	current state of the solver
primary	constraint results
auxiliary	constraint results

Returns:: SolverStatus

Definition at line 333 of file constrained_ik.cpp.

void constrained_ik::Constrained_IK::clipToJointLimits ( Eigen::VectorXd & joints ) const [protected, virtual]

This function clips the joints within the joint limits.

Parameters:

joints a Eigen::VectorXd passed by reference

Definition at line 406 of file constrained_ik.cpp.

constrained_ik::ConstraintResults constrained_ik::Constrained_IK::evalConstraint	(	constraint_types::ConstraintTypes	constraint_type,
		const constrained_ik::SolverState &	state
	)		const `[protected, virtual]`

Calculating error, jacobian & status for all constraints specified.

Parameters:

constraint_type	Contraint type (primary or auxiliary)
state	The state of the current solver

Returns:: ConstraintResults

Definition at line 135 of file constrained_ik.cpp.

virtual bool constrained_ik::Constrained_IK::getJointNames ( std::vector< std::string > & names ) const [inline, virtual]

Getter for joint names.

Parameters:

names Output vector of strings naming all joints in robot

Returns:: True if BasicKin object is initialized

Definition at line 165 of file constrained_ik.h.

virtual const basic_kin::BasicKin& constrained_ik::Constrained_IK::getKin ( ) const [inline, virtual]

Getter for kinematics object.

Returns:: Reference to active kinematics object

Definition at line 171 of file constrained_ik.h.

virtual bool constrained_ik::Constrained_IK::getLinkNames ( std::vector< std::string > & names ) const [inline, virtual]

Getter for link names.

Parameters:

names Output vector of strings naming all links in robot

Returns:: True is BasicKin object is initialized

Definition at line 178 of file constrained_ik.h.

virtual ConstrainedIKConfiguration constrained_ik::Constrained_IK::getSolverConfiguration ( ) const [inline, virtual]

Getter for solver configuration.

Returns:: ConstrainedIkConfiguration

Definition at line 184 of file constrained_ik.h.

constrained_ik::SolverState constrained_ik::Constrained_IK::getState	(	const Eigen::Affine3d &	goal,
		const Eigen::VectorXd &	joint_seed
	)		const `[protected, virtual]`

Creates a new SolverState and checks key elements.

Parameters:

goal	cartesian pose to solve the inverse kinematics about
joint_seed	inital joint position for the solver.

Returns:: SolverState

Definition at line 442 of file constrained_ik.cpp.

void constrained_ik::Constrained_IK::init ( const basic_kin::BasicKin & kin ) [virtual]

Initializes object with kinematic model of robot.

Parameters:

kin	BasicKin object with robot info

Definition at line 422 of file constrained_ik.cpp.

bool constrained_ik::Constrained_IK::isInitialized ( ) const [inline]

Check if solver has been initialized.

Returns:: True if initialized, otherwise false

Definition at line 240 of file constrained_ik.h.

virtual bool constrained_ik::Constrained_IK::linkTransforms	(	const Eigen::VectorXd &	joints,
		std::vector< KDL::Frame > &	poses,
		const std::vector< std::string >	link_names = `std::vector<std::string>()`
	)		const `[inline, virtual]`

computes and returns the link transfoms of the named joints

Parameters:

joints	a vector of joints
poses	the desired pose transforms
link_names	the names of the links

Returns:: true on success

Definition at line 72 of file constrained_ik.h.

virtual unsigned int constrained_ik::Constrained_IK::numJoints ( ) const [inline, virtual]

Getter for BasicKin numJoints.

Returns:: Number of variable joints in robot

Definition at line 205 of file constrained_ik.h.

double constrained_ik::Constrained_IK::rangedAngle ( double angle ) [static]

Translates an angle to lie between +/-PI.

Todo:: Should be moved to the utils class

Parameters:

angle Input angle, radians

Returns:: Output angle within range of +/- PI

Definition at line 434 of file constrained_ik.cpp.

void constrained_ik::Constrained_IK::setSolverConfiguration ( const ConstrainedIKConfiguration & config ) [virtual]

Setter for solver configuration.

Parameters:

config new object for config_

Definition at line 129 of file constrained_ik.cpp.

void constrained_ik::Constrained_IK::updateState	(	constrained_ik::SolverState &	state,
		const Eigen::VectorXd &	joints
	)		const `[protected, virtual]`

Method update an existing SolverState provided new joint positions.

Parameters:

state	solvers current state
joints	new joint position to be used by the solver

Definition at line 453 of file constrained_ik.cpp.

Member Data Documentation

ConstraintGroup constrained_ik::Constrained_IK::auxiliary_constraints_ [protected]

Array of auxiliary constraints

Definition at line 249 of file constrained_ik.h.

ConstrainedIKConfiguration constrained_ik::Constrained_IK::config_ [protected]

Solver configuration parameters

Definition at line 245 of file constrained_ik.h.

bool constrained_ik::Constrained_IK::initialized_ [protected]

True if solver is intialized, otherwise false

Definition at line 252 of file constrained_ik.h.

basic_kin::BasicKin constrained_ik::Constrained_IK::kin_ [protected]

Solver kinematic model

Definition at line 253 of file constrained_ik.h.

ros::NodeHandle constrained_ik::Constrained_IK::nh_ [protected]

ROS node handle

Definition at line 244 of file constrained_ik.h.

ConstraintGroup constrained_ik::Constrained_IK::primary_constraints_ [protected]

Array of primary constraints

Definition at line 248 of file constrained_ik.h.

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

Public Member Functions

Static Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Function Documentation

Member Data Documentation