#include <kdl_manager.hpp>

Inheritance diagram for generic_control_toolbox::KDLManager:

Public Member Functions
bool	addSegment (const std::string &end_effector_link, KDL::Segment &new_segment)

bool	checkStateMessage (const std::string &end_effector_link, const sensor_msgs::JointState &state) const

bool	createJointState (const std::string &end_effector_link, const Eigen::VectorXd &q, const Eigen::VectorXd &qdot, sensor_msgs::JointState &state) const

bool	getCoriolis (const std::string &end_effector_link, const sensor_msgs::JointState &state, Eigen::MatrixXd &coriolis)

bool	getEefPose (const std::string &end_effector_link, const sensor_msgs::JointState &state, KDL::Frame &out) const

bool	getEefTwist (const std::string &end_effector_link, const sensor_msgs::JointState &state, KDL::FrameVel &out) const

bool	getGravity (const std::string &end_effector_link, const sensor_msgs::JointState &state, Eigen::MatrixXd &g)

bool	getGrippingPoint (const std::string &end_effector_link, const sensor_msgs::JointState &state, KDL::Frame &out) const

bool	getGrippingPoseIK (const std::string &end_effector_link, const sensor_msgs::JointState &state, const KDL::Frame &in, KDL::JntArray &out) const

bool	getGrippingTwist (const std::string &end_effector_link, const sensor_msgs::JointState &state, KDL::Twist &out) const

bool	getGrippingVelIK (const std::string &end_effector_link, const sensor_msgs::JointState &state, const KDL::Twist &in, KDL::JntArray &out) const

bool	getInertia (const std::string &end_effector_link, const sensor_msgs::JointState &state, Eigen::MatrixXd &H)

bool	getJacobian (const std::string &end_effector_link, const sensor_msgs::JointState &state, KDL::Jacobian &out) const

bool	getJointLimits (const std::string &end_effector_link, KDL::JntArray &q_min, KDL::JntArray &q_max, KDL::JntArray &q_vel_lim) const

bool	getJointPositions (const std::string &end_effector_link, const sensor_msgs::JointState &state, KDL::JntArray &q) const

bool	getJointPositions (const std::string &end_effector_link, const sensor_msgs::JointState &state, Eigen::VectorXd &q) const

bool	getJointState (const std::string &end_effector_link, const Eigen::VectorXd &qdot, sensor_msgs::JointState &state) const

bool	getJointState (const std::string &end_effector_link, const Eigen::VectorXd &q, const Eigen::VectorXd &qdot, sensor_msgs::JointState &state) const

bool	getJointState (const std::string &end_effector_link, const Eigen::VectorXd &q, const Eigen::VectorXd &qdot, const Eigen::VectorXd &effort, sensor_msgs::JointState &state) const

bool	getJointVelocities (const std::string &end_effector_link, const sensor_msgs::JointState &state, KDL::JntArray &q_dot) const

bool	getNumJoints (const std::string &end_effector_link, unsigned int &num_joints) const

bool	getPoseFK (const std::string &end_effector_link, const sensor_msgs::JointState &state, const KDL::JntArray &in, KDL::Frame &out) const

bool	getPoseIK (const std::string &end_effector_link, const sensor_msgs::JointState &state, const KDL::Frame &in, KDL::JntArray &out) const

bool	getSensorPoint (const std::string &end_effector_link, const sensor_msgs::JointState &state, KDL::Frame &out) const

bool	getVelIK (const std::string &end_effector_link, const sensor_msgs::JointState &state, const KDL::Twist &in, KDL::JntArray &out) const

bool	grippingTwistToEef (const std::string &end_effector_link, const sensor_msgs::JointState &state, const KDL::Twist &in, KDL::Twist &out) const

bool	initializeArm (const std::string &end_effector_link)

bool	isInitialized (const std::string &end_effector_link) const

	KDLManager (const std::string &chain_base_link, ros::NodeHandle nh=ros::NodeHandle("~"))

bool	setGrippingPoint (const std::string &end_effector_link, const std::string &gripping_point_frame)

bool	setSensorPoint (const std::string &end_effector_link, const std::string &sensor_point_frame)

bool	verifyPose (const std::string &end_effector_link, const KDL::Frame &in) const

	~KDLManager ()

Public Member Functions inherited from generic_control_toolbox::ManagerBase
	ManagerBase ()

virtual	~ManagerBase ()

Private Types
typedef std::shared_ptr< KDL::ChainDynParam >	ChainDynParamPtr

typedef KDL::ChainFkSolverPos_recursive	FkSolverPos

typedef std::shared_ptr< FkSolverPos >	FkSolverPosPtr

typedef KDL::ChainFkSolverVel_recursive	FkSolverVel

typedef std::shared_ptr< FkSolverVel >	FkSolverVelPtr

typedef KDL::ChainIkSolverPos_LMA	IkSolverPos

typedef std::shared_ptr< IkSolverPos >	IkSolverPosPtr

typedef KDL::ChainIkSolverVel	IkSolverVel

typedef std::shared_ptr< IkSolverVel >	IkSolverVelPtr

typedef KDL::ChainJntToJacSolver	JacSolver

typedef std::shared_ptr< JacSolver >	JacSolverPtr

Private Member Functions
bool	getChainJointState (const sensor_msgs::JointState &current_state, const std::string &end_effector_link, KDL::JntArray &positions, KDL::JntArrayVel &velocities) const

bool	getParam ()

bool	getRigidTransform (const std::string &base_frame, const std::string &target_frame, KDL::Frame &out) const

bool	hasJoint (const KDL::Chain &chain, const std::string &joint_name) const

bool	initializeArmCommon (const std::string &end_effector_link)

void	updateSolvers ()

Private Attributes
std::map< std::string, std::vector< std::string > >	actuated_joint_names_

std::map< std::string, KDL::Chain >	chain_

std::string	chain_base_link_
	list of actuated joints per arm More...

std::map< std::string, ChainDynParamPtr >	dynamic_chain_

std::map< std::string, KDL::Frame >	eef_to_gripping_point_

std::map< std::string, KDL::Frame >	eef_to_sensor_point_

double	eps_

std::map< std::string, FkSolverPosPtr >	fkpos_

std::map< std::string, FkSolverVelPtr >	fkvel_

KDL::Vector	gravity_in_chain_base_link_

double	ik_angle_tolerance_

double	ik_pos_tolerance_

std::map< std::string, IkSolverPosPtr >	ikpos_

std::map< std::string, IkSolverVelPtr >	ikvel_

std::string	ikvel_solver_

std::map< std::string, JacSolverPtr >	jac_solver_

tf::TransformListener	listener_

double	max_tf_attempts_

urdf::Model	model_

ros::NodeHandle	nh_

double	nso_weight_

Detailed Description

Loads and maintains a URDF robot description and provides access to relevant KDL objects/solvers. Supports n end-effectors

Definition at line 35 of file kdl_manager.hpp.

Member Typedef Documentation

◆ ChainDynParamPtr

typedef std::shared_ptr<KDL::ChainDynParam> generic_control_toolbox::KDLManager::ChainDynParamPtr

private

Definition at line 434 of file kdl_manager.hpp.

◆ FkSolverPos

typedef KDL::ChainFkSolverPos_recursive generic_control_toolbox::KDLManager::FkSolverPos

private

Definition at line 425 of file kdl_manager.hpp.

◆ FkSolverPosPtr

typedef std::shared_ptr<FkSolverPos> generic_control_toolbox::KDLManager::FkSolverPosPtr

private

Definition at line 431 of file kdl_manager.hpp.

◆ FkSolverVel

typedef KDL::ChainFkSolverVel_recursive generic_control_toolbox::KDLManager::FkSolverVel

private

Definition at line 426 of file kdl_manager.hpp.

◆ FkSolverVelPtr

typedef std::shared_ptr<FkSolverVel> generic_control_toolbox::KDLManager::FkSolverVelPtr

private

Definition at line 432 of file kdl_manager.hpp.

◆ IkSolverPos

typedef KDL::ChainIkSolverPos_LMA generic_control_toolbox::KDLManager::IkSolverPos

private

Definition at line 424 of file kdl_manager.hpp.

◆ IkSolverPosPtr

typedef std::shared_ptr<IkSolverPos> generic_control_toolbox::KDLManager::IkSolverPosPtr

private

Definition at line 430 of file kdl_manager.hpp.

◆ IkSolverVel

typedef KDL::ChainIkSolverVel generic_control_toolbox::KDLManager::IkSolverVel

private

Definition at line 423 of file kdl_manager.hpp.

◆ IkSolverVelPtr

typedef std::shared_ptr<IkSolverVel> generic_control_toolbox::KDLManager::IkSolverVelPtr

private

Definition at line 429 of file kdl_manager.hpp.

◆ JacSolver

typedef KDL::ChainJntToJacSolver generic_control_toolbox::KDLManager::JacSolver

private

Definition at line 427 of file kdl_manager.hpp.

◆ JacSolverPtr

typedef std::shared_ptr<JacSolver> generic_control_toolbox::KDLManager::JacSolverPtr

private

Definition at line 433 of file kdl_manager.hpp.

Constructor & Destructor Documentation

◆ KDLManager()

generic_control_toolbox::KDLManager::KDLManager	(	const std::string &	chain_base_link,
		ros::NodeHandle	nh = `ros::NodeHandle("~")`
	)

Definition at line 5 of file kdl_manager.cpp.

◆ ~KDLManager()

generic_control_toolbox::KDLManager::~KDLManager ( )

Definition at line 17 of file kdl_manager.cpp.

Member Function Documentation

◆ addSegment()

bool generic_control_toolbox::KDLManager::addSegment	(	const std::string &	end_effector_link,
		KDL::Segment &	new_segment
	)

Adds a new segment at the end of the requested chain. WARNING: The intended use case is to facilitate dynamic computations when the robot has a load that is not included in the chain's definition, e.g., a complex gripper. KDL does not support removing segments, so this should not be used to set a variable load.

Parameters

end_effector_link	The name of the requested end-effector.
new_segment	A new segment to be added at the end of a chain.

Definition at line 1117 of file kdl_manager.cpp.

◆ checkStateMessage()

bool generic_control_toolbox::KDLManager::checkStateMessage	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state
	)		const

Checks if the provided joint state message has the required information for the kinematic chain indexed by the given end effector link. Useful with the joint states topic does not contain consistently the same information.

Parameters

end_effector_link	The end_effector link name.
state	The joint state message to verify.

Returns: True if the joint state message contains valid information for the requested chain, false otherwise.

Definition at line 1002 of file kdl_manager.cpp.

◆ createJointState()

bool generic_control_toolbox::KDLManager::createJointState	(	const std::string &	end_effector_link,
		const Eigen::VectorXd &	q,
		const Eigen::VectorXd &	qdot,
		sensor_msgs::JointState &	state
	)		const

Create a joint state message for the given end-effector link with the desired joint position and velocities.

Parameters

end_effector_link	The chain's end_effector.
qdot	The chain's joint velocities.
state	The generated joint state message.

Returns: False if something goes wrond.

Definition at line 307 of file kdl_manager.cpp.

◆ getChainJointState()

bool generic_control_toolbox::KDLManager::getChainJointState	(	const sensor_msgs::JointState &	current_state,
		const std::string &	end_effector_link,
		KDL::JntArray &	positions,
		KDL::JntArrayVel &	velocities
	)		const

private

Fills in the joint arrays with the state of a given kinematic chain. The joint state might include joints outside of the kinematic chain, so there is the need to process it.

Parameters

current_state	The robot joint state.
arm	The target arm index.
positions	The joint positions of the kinematic chain.
velocities	The joint velocities of the kinematic chain.

Returns: True if the full joint chain was found in the current state, false otherwise.

Definition at line 1044 of file kdl_manager.cpp.

◆ getCoriolis()

bool generic_control_toolbox::KDLManager::getCoriolis	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		Eigen::MatrixXd &	coriolis
	)

Computes the coriolis forces exerted in the given chain.

Parameters

end_effector_link	The name of the requested end-effector.
state	The current joint state.
coriolis	The output coriolis effort.

Returns: False in case something goes wrong, true otherwise.

Definition at line 731 of file kdl_manager.cpp.

◆ getEefPose()

bool generic_control_toolbox::KDLManager::getEefPose	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		KDL::Frame &	out
	)		const

Returns the pose of the requested end-effector, given a joint state.

Parameters

end_effector_link	The name of the requested end-effector.
state	The current robot joint state.
out	The resulting eef pose.

Returns: False in case something goes wrong, true otherwise.

Definition at line 538 of file kdl_manager.cpp.

◆ getEefTwist()

bool generic_control_toolbox::KDLManager::getEefTwist	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		KDL::FrameVel &	out
	)		const

Returns the twist of the requested end-effector, given a joint state.

Parameters

end_effector_link	The name of the requested end-effector.
state	The current robot joint state.
out	The resulting eef twist.

Returns: False in case something goes wrong, true otherwise.

Definition at line 559 of file kdl_manager.cpp.

◆ getGravity()

bool generic_control_toolbox::KDLManager::getGravity	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		Eigen::MatrixXd &	g
	)

Computes the predicted gravity effect on the given chain.

Parameters

end_effector_link	The name of the requested end-effector.
state	The current joint state.
g	The output gravity effort.

Returns: False in case something goes wrong, true otherwise.

Definition at line 707 of file kdl_manager.cpp.

◆ getGrippingPoint()

bool generic_control_toolbox::KDLManager::getGrippingPoint	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		KDL::Frame &	out
	)		const

Returns the gripping point of the chosen arm. By default this is set to be the end-effector pose.

Parameters

end_effector_link	The arm's end-effector name.
state	The robot joint state.
out	The resulting gripping point frame.

Returns: False if something goes wrong, true otherwise.

Definition at line 457 of file kdl_manager.cpp.

◆ getGrippingPoseIK()

bool generic_control_toolbox::KDLManager::getGrippingPoseIK	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		const KDL::Frame &	in,
		KDL::JntArray &	out
	)		const

Returns the inverse kinematics of a chain gripping point , given a desired pose.

Parameters

end_effector_link	The name of the requested end-effector.
state	The current robot joint state.
in	Desired eef pose.
out	Joint state for the desired gripping pose.

Returns: False in case something goes wrong or if the solver did not converge to the desired pose, true otherwise.

Definition at line 874 of file kdl_manager.cpp.

◆ getGrippingTwist()

bool generic_control_toolbox::KDLManager::getGrippingTwist	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		KDL::Twist &	out
	)		const

Returns the twist of the requested end-effector's gripping point. By default this is set to be the end-effector's twist.

Parameters

end_effector_link	The name of the requested end-effector.
state	The current robot joint state.
out	The resulting eef twist.

Returns: False in case something goes wrong, true otherwise.

Definition at line 495 of file kdl_manager.cpp.

◆ getGrippingVelIK()

bool generic_control_toolbox::KDLManager::getGrippingVelIK	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		const KDL::Twist &	in,
		KDL::JntArray &	out
	)		const

Returns the inverse differential kinematics of the requested gripping point, given a desired twist.

Parameters

end_effector_link	The name of the requested end-effector.
state	The current robot joint state.
in	Desired gripping point twist, in the gripping point frame.
out	Joint state for the desired twist.

Returns: False in case something goes wrong, true otherwise.

Definition at line 903 of file kdl_manager.cpp.

◆ getInertia()

bool generic_control_toolbox::KDLManager::getInertia	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		Eigen::MatrixXd &	H
	)

Computes the inertia matrix of the kinematic chain.

Parameters

end_effector_link	The name of the requested end-effector.
state	The current joint state.
H	The output inertia matrix.

Returns: False in case something goes wrong, true otherwise.

Definition at line 683 of file kdl_manager.cpp.

◆ getJacobian()

bool generic_control_toolbox::KDLManager::getJacobian	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		KDL::Jacobian &	out
	)		const

Returns the jacobian the requested end-effector's chain.

Parameters

end_effector_link	The name of the requested end-effector.
state	The current robot joint state.
out	End-effector's jacobian.

Returns: False in case something goes wrong, true otherwise.

Definition at line 981 of file kdl_manager.cpp.

◆ getJointLimits()

bool generic_control_toolbox::KDLManager::getJointLimits	(	const std::string &	end_effector_link,
		KDL::JntArray &	q_min,
		KDL::JntArray &	q_max,
		KDL::JntArray &	q_vel_lim
	)		const

Returns the joint limits for all the actuated joints in the eef kinematic chain.

Parameters

end_effector_link	The name of the requested end-effector.
q_min	The lower joint position limit.
q_max	The upper joint position limit.
q_vel_lim	The maximum joint velocity.

Returns: False in case something goes wrong, true otherwise.

Definition at line 579 of file kdl_manager.cpp.

◆ getJointPositions() [1/2]

bool generic_control_toolbox::KDLManager::getJointPositions	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		KDL::JntArray &	q
	)		const

Returns the current joint positions in the KDL format.

Parameters

end_effector_link	The name of the requested end-effector.
state	The current joint state
q	The joint positions in KDL format

Returns: False in case something goes wrong, true otherwise.

Definition at line 623 of file kdl_manager.cpp.

◆ getJointPositions() [2/2]

bool generic_control_toolbox::KDLManager::getJointPositions	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		Eigen::VectorXd &	q
	)		const

Definition at line 642 of file kdl_manager.cpp.

◆ getJointState() [1/3]

bool generic_control_toolbox::KDLManager::getJointState	(	const std::string &	end_effector_link,
		const Eigen::VectorXd &	qdot,
		sensor_msgs::JointState &	state
	)		const

Fills the joint state message given only joint velocities. Joint positions are taken from the joint state message.

Parameters

end_effector_link	The chain's end_effector.
qdot	The chain's joint velocities.
state	The generated joint state message.

Returns: False if something goes wrong.

Definition at line 262 of file kdl_manager.cpp.

◆ getJointState() [2/3]

bool generic_control_toolbox::KDLManager::getJointState	(	const std::string &	end_effector_link,
		const Eigen::VectorXd &	q,
		const Eigen::VectorXd &	qdot,
		sensor_msgs::JointState &	state
	)		const

Fills the joint state message appropriately given the joint positions and velocities of the selected end-effector's joint chain.

Parameters

end_effector_link	The chain's end_effector.
q	The chain's joint posisitions.
qdot	The chain's joint velocities.
state	The generated joint state message.

Returns: False if something goes wrong.

Definition at line 350 of file kdl_manager.cpp.

◆ getJointState() [3/3]

bool generic_control_toolbox::KDLManager::getJointState	(	const std::string &	end_effector_link,
		const Eigen::VectorXd &	q,
		const Eigen::VectorXd &	qdot,
		const Eigen::VectorXd &	effort,
		sensor_msgs::JointState &	state
	)		const

Fills the joint state message appropriately given the joint positions, velocities and efforts of the selected end-effector's joint chain.

Parameters

end_effector_link	The chain's end_effector.
q	The chain's joint posisitions.
qdot	The chain's joint velocities.
effort	The chain's joint efforts.
state	The generated joint state message.

Returns: False if something goes wrong.

Definition at line 406 of file kdl_manager.cpp.

◆ getJointVelocities()

bool generic_control_toolbox::KDLManager::getJointVelocities	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		KDL::JntArray &	q_dot
	)		const

Returns the current joint velocities in the KDL format.

Parameters

end_effector_link	The name of the requested end-effector.
state	The current joint state.
q_dot	The joint velocities in the KDL format.

Returns: False in case something goes wrong, true otherwise.

Definition at line 662 of file kdl_manager.cpp.

◆ getNumJoints()

bool generic_control_toolbox::KDLManager::getNumJoints	(	const std::string &	end_effector_link,
		unsigned int &	num_joints
	)		const

Returns the number of joints for a given chain.

Parameters

end_effector_link	The name of the requested end-effector.
num_joints	The number of joints of the associated kinematic chain.

Returns: False in case something goes wrong, true otherwise.

Definition at line 1105 of file kdl_manager.cpp.

◆ getParam()

bool generic_control_toolbox::KDLManager::getParam ( )

private

Loads the manager parameters

Returns: False in case something goes wrong, true otherwise

Definition at line 19 of file kdl_manager.cpp.

◆ getPoseFK()

bool generic_control_toolbox::KDLManager::getPoseFK	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		const KDL::JntArray &	in,
		KDL::Frame &	out
	)		const

Returns the forward kinematics of the requested end-effector, given a desired joint state.

Parameters

end_effector_link	The name of the requested end-effector.
state	The current robot joint state.
in	Desired eef joint state.
out	Pose for the desired joint state.

Returns: False in case something goes wrong, true otherwise.

Definition at line 890 of file kdl_manager.cpp.

◆ getPoseIK()

bool generic_control_toolbox::KDLManager::getPoseIK	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		const KDL::Frame &	in,
		KDL::JntArray &	out
	)		const

Returns the inverse kinematics of the requested end-effector, given a desired pose.

Parameters

end_effector_link	The name of the requested end-effector.
state	The current robot joint state.
in	Desired eef pose.
out	Joint state for the desired pose.

Returns: False in case something goes wrong or if the solver did not converge to the desired pose, true otherwise.

Definition at line 823 of file kdl_manager.cpp.

◆ getRigidTransform()

bool generic_control_toolbox::KDLManager::getRigidTransform	(	const std::string &	base_frame,
		const std::string &	target_frame,
		KDL::Frame &	out
	)		const

private

Queries TF to get the rigid transform between two frames

Parameters

base_frame	The base frame of the transform.
target_frame	The target frame of the transform.
out	The rigid transform between the two frames in the KDL format.

Returns: False in case something goes wrong, true otherwise.

Definition at line 753 of file kdl_manager.cpp.

◆ getSensorPoint()

bool generic_control_toolbox::KDLManager::getSensorPoint	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		KDL::Frame &	out
	)		const

Returns the sensor point of the chosen arm. By default this is set to be the end-effector pose.

Parameters

end_effector_link	The arm's end-effector name.
state	The robot joint state.
out	The resulting sensor point frame.

Returns: False if something goes wrong, true otherwise.

Definition at line 476 of file kdl_manager.cpp.

◆ getVelIK()

bool generic_control_toolbox::KDLManager::getVelIK	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		const KDL::Twist &	in,
		KDL::JntArray &	out
	)		const

Returns the inverse differential kinematics of the requested end-effector, given a desired twist.

Parameters

end_effector_link	The name of the requested end-effector.
state	The current robot joint state.
in	Desired eef twist.
out	Joint state for the desired twist.

Returns: False in case something goes wrong, true otherwise.

Definition at line 960 of file kdl_manager.cpp.

◆ grippingTwistToEef()

bool generic_control_toolbox::KDLManager::grippingTwistToEef	(	const std::string &	end_effector_link,
		const sensor_msgs::JointState &	state,
		const KDL::Twist &	in,
		KDL::Twist &	out
	)		const

Convert an input twist expressed in the chain's gripping frame to an equivalent twist at the end-effector. Assumes rigid linkage between the two frames.

Parameters

end_effector_link	The name of the requested end-effector.
state	The current robot_joint state
in	Gripping point twist, in the gripping point frame.
out	End-effector twist, in the end-effector frame.

Returns: False in case something goes wrong, true otherwise.

Definition at line 923 of file kdl_manager.cpp.

◆ hasJoint()

bool generic_control_toolbox::KDLManager::hasJoint	(	const KDL::Chain &	chain,
		const std::string &	joint_name
	)		const

private

Check if a chain has the given joint_name.

Parameters

chain	The kinematic chain where to look for the joint.
joint_name	The joint name we wish to check.

Returns: True if the joint was found in the kinematic chain, false otherwise.

Definition at line 1091 of file kdl_manager.cpp.

◆ initializeArm()

bool generic_control_toolbox::KDLManager::initializeArm ( const std::string & end_effector_link )

Initialize the kinematic chain, solvers and joint arrays for an arm defined by its end-effector link. The kinematic chain is assumed to start at chain_base_link_.

Parameters

end_effector_link The final link of the kinematic chain.

Returns: false if it is not possible to initialize.

Definition at line 96 of file kdl_manager.cpp.

◆ initializeArmCommon()

bool generic_control_toolbox::KDLManager::initializeArmCommon ( const std::string & end_effector_link )

private

Common part of the initialize arm methods.

Parameters

end_effector_link The name of the requested end-effector.

Returns: False in case something goes wrong, true otherwise.

Definition at line 152 of file kdl_manager.cpp.

◆ isInitialized()

bool generic_control_toolbox::KDLManager::isInitialized ( const std::string & end_effector_link ) const

Checks whether a kinematic chain with the given end-effector link was initialized in a KDL manager instance.

Parameters

end_effector_link The final link of the kinematic chain.

Returns: False if the chain is not initialized, true otherwise.

Definition at line 147 of file kdl_manager.cpp.

◆ setGrippingPoint()

bool generic_control_toolbox::KDLManager::setGrippingPoint	(	const std::string &	end_effector_link,
		const std::string &	gripping_point_frame
	)

Queries TF for the rigid transform between the end-effector link frame and the gripping point frame and stores it as a KDL Frame.

Parameters

end_effector_link	The end-effector link name.
gripping_point_frame	TF name of the gripping point frame.

Returns: False if transform is not found or something else goes wrong, true otherwise.

Definition at line 221 of file kdl_manager.cpp.

◆ setSensorPoint()

bool generic_control_toolbox::KDLManager::setSensorPoint	(	const std::string &	end_effector_link,
		const std::string &	sensor_point_frame
	)

Queries TF for the rigid transform between the end-effector link frame and a sensor point frame, and stores it as a KDL Frame.

Parameters

end_effector_link	The end-effector link name.
sensor_point_frame	TF name of the sensor point frame.

Returns: False if transform is not found or something else goes wrong, true otherwise.

Definition at line 242 of file kdl_manager.cpp.

◆ updateSolvers()

void generic_control_toolbox::KDLManager::updateSolvers ( )

private

Updates the internal data structures of all solvers.

Definition at line 1130 of file kdl_manager.cpp.

◆ verifyPose()

bool generic_control_toolbox::KDLManager::verifyPose	(	const std::string &	end_effector_link,
		const KDL::Frame &	in
	)		const

Verifies if a given pose is reachable by the requested end-effector.

Parameters

end_effector_link	The name of the requested end-effector.
in	The pose that is to be verified.

Returns: True if the pose is reachable, false otherwise or in case of error.

Definition at line 795 of file kdl_manager.cpp.

Member Data Documentation

◆ actuated_joint_names_

std::map<std::string, std::vector<std::string> > generic_control_toolbox::KDLManager::actuated_joint_names_

private

Definition at line 451 of file kdl_manager.hpp.

◆ chain_

std::map<std::string, KDL::Chain> generic_control_toolbox::KDLManager::chain_

private

Definition at line 443 of file kdl_manager.hpp.

◆ chain_base_link_

std::string generic_control_toolbox::KDLManager::chain_base_link_

private

list of actuated joints per arm

Definition at line 452 of file kdl_manager.hpp.

◆ dynamic_chain_

std::map<std::string, ChainDynParamPtr> generic_control_toolbox::KDLManager::dynamic_chain_

private

Definition at line 444 of file kdl_manager.hpp.

◆ eef_to_gripping_point_

std::map<std::string, KDL::Frame> generic_control_toolbox::KDLManager::eef_to_gripping_point_

private

Definition at line 441 of file kdl_manager.hpp.

◆ eef_to_sensor_point_

std::map<std::string, KDL::Frame> generic_control_toolbox::KDLManager::eef_to_sensor_point_

private

Definition at line 442 of file kdl_manager.hpp.

◆ eps_

double generic_control_toolbox::KDLManager::eps_

private

Definition at line 453 of file kdl_manager.hpp.

◆ fkpos_

std::map<std::string, FkSolverPosPtr> generic_control_toolbox::KDLManager::fkpos_

private

Definition at line 438 of file kdl_manager.hpp.

◆ fkvel_

std::map<std::string, FkSolverVelPtr> generic_control_toolbox::KDLManager::fkvel_

private

Definition at line 439 of file kdl_manager.hpp.

◆ gravity_in_chain_base_link_

KDL::Vector generic_control_toolbox::KDLManager::gravity_in_chain_base_link_

private

Definition at line 449 of file kdl_manager.hpp.

◆ ik_angle_tolerance_

double generic_control_toolbox::KDLManager::ik_angle_tolerance_

private

Definition at line 453 of file kdl_manager.hpp.

◆ ik_pos_tolerance_

double generic_control_toolbox::KDLManager::ik_pos_tolerance_

private

Definition at line 453 of file kdl_manager.hpp.

◆ ikpos_

std::map<std::string, IkSolverPosPtr> generic_control_toolbox::KDLManager::ikpos_

private

Definition at line 437 of file kdl_manager.hpp.

◆ ikvel_

std::map<std::string, IkSolverVelPtr> generic_control_toolbox::KDLManager::ikvel_

private

Definition at line 436 of file kdl_manager.hpp.

◆ ikvel_solver_

std::string generic_control_toolbox::KDLManager::ikvel_solver_

private

Definition at line 452 of file kdl_manager.hpp.

◆ jac_solver_

std::map<std::string, JacSolverPtr> generic_control_toolbox::KDLManager::jac_solver_

private

Definition at line 440 of file kdl_manager.hpp.

◆ listener_

tf::TransformListener generic_control_toolbox::KDLManager::listener_

private

Definition at line 448 of file kdl_manager.hpp.

◆ max_tf_attempts_

double generic_control_toolbox::KDLManager::max_tf_attempts_

private

Definition at line 453 of file kdl_manager.hpp.

◆ model_

urdf::Model generic_control_toolbox::KDLManager::model_

private

Definition at line 446 of file kdl_manager.hpp.

◆ nh_

ros::NodeHandle generic_control_toolbox::KDLManager::nh_

private

Definition at line 447 of file kdl_manager.hpp.

◆ nso_weight_

double generic_control_toolbox::KDLManager::nso_weight_

private

Definition at line 453 of file kdl_manager.hpp.

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

Public Member Functions

Private Types

Private Member Functions

Private Attributes

Detailed Description

Member Typedef Documentation

◆ ChainDynParamPtr

◆ FkSolverPos

◆ FkSolverPosPtr

◆ FkSolverVel

◆ FkSolverVelPtr

◆ IkSolverPos

◆ IkSolverPosPtr

◆ IkSolverVel

◆ IkSolverVelPtr

◆ JacSolver

◆ JacSolverPtr

Constructor & Destructor Documentation

◆ KDLManager()

◆ ~KDLManager()

Member Function Documentation

◆ addSegment()

◆ checkStateMessage()

◆ createJointState()

◆ getChainJointState()

◆ getCoriolis()

◆ getEefPose()

◆ getEefTwist()

◆ getGravity()

◆ getGrippingPoint()

◆ getGrippingPoseIK()

◆ getGrippingTwist()

◆ getGrippingVelIK()

◆ getInertia()

◆ getJacobian()

◆ getJointLimits()

◆ getJointPositions() [1/2]

◆ getJointPositions() [2/2]

◆ getJointState() [1/3]

◆ getJointState() [2/3]

◆ getJointState() [3/3]

◆ getJointVelocities()

◆ getNumJoints()

◆ getParam()

◆ getPoseFK()

◆ getPoseIK()

◆ getRigidTransform()

◆ getSensorPoint()

◆ getVelIK()

◆ grippingTwistToEef()

◆ hasJoint()

◆ initializeArm()

◆ initializeArmCommon()

◆ isInitialized()

◆ setGrippingPoint()

◆ setSensorPoint()

◆ updateSolvers()

◆ verifyPose()

Member Data Documentation

◆ actuated_joint_names_

◆ chain_

◆ chain_base_link_

◆ dynamic_chain_

◆ eef_to_gripping_point_

◆ eef_to_sensor_point_

◆ eps_

◆ fkpos_

◆ fkvel_

◆ gravity_in_chain_base_link_

◆ ik_angle_tolerance_

◆ ik_pos_tolerance_

◆ ikpos_

◆ ikvel_

◆ ikvel_solver_

◆ jac_solver_

◆ listener_

◆ max_tf_attempts_

◆ model_

◆ nh_

◆ nso_weight_