Public Member Functions | Private Types | Private Member Functions | Private Attributes
generic_control_toolbox::KDLManager Class Reference

#include <kdl_manager.hpp>

Inheritance diagram for generic_control_toolbox::KDLManager:
Inheritance graph
[legend]

List of all members.

Public Member Functions

bool checkStateMessage (const std::string &end_effector_link, const 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 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 ()

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)

Private Attributes

std::map< std::string,
std::vector< std::string > > 
actuated_joint_names_
std::map< std::string, KDL::Chainchain_
std::string chain_base_link_
 list of actuated joints per arm
std::map< std::string,
ChainDynParamPtr
dynamic_chain_
std::map< std::string, KDL::Frameeef_to_gripping_point_
std::map< std::string, KDL::Frameeef_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

Definition at line 394 of file kdl_manager.hpp.

Definition at line 385 of file kdl_manager.hpp.

Definition at line 391 of file kdl_manager.hpp.

Definition at line 386 of file kdl_manager.hpp.

Definition at line 392 of file kdl_manager.hpp.

Definition at line 384 of file kdl_manager.hpp.

Definition at line 390 of file kdl_manager.hpp.

Definition at line 383 of file kdl_manager.hpp.

Definition at line 389 of file kdl_manager.hpp.

Definition at line 387 of file kdl_manager.hpp.

Definition at line 393 of file kdl_manager.hpp.


Constructor & Destructor Documentation

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.

Definition at line 17 of file kdl_manager.cpp.


Member Function Documentation

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_linkThe end_effector link name.
stateThe joint state message to verify.
Returns:
True if the joint state message contains valid information for the requested chain, false otherwise.

Definition at line 934 of file kdl_manager.cpp.

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_stateThe robot joint state.
armThe target arm index.
positionsThe joint positions of the kinematic chain.
velocitiesThe joint velocities of the kinematic chain.
Returns:
True if the full joint chain was found in the current state, false otherwise.

Definition at line 976 of file kdl_manager.cpp.

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_linkThe name of the requested end-effector.
stateThe current joint state.
coriolisThe output coriolis effort.
Returns:
False in case something goes wrong, true otherwise.

Definition at line 675 of file kdl_manager.cpp.

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_linkThe name of the requested end-effector.
stateThe current robot joint state.
outThe resulting eef pose.
Returns:
False in case something goes wrong, true otherwise.

Definition at line 489 of file kdl_manager.cpp.

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_linkThe name of the requested end-effector.
stateThe current robot joint state.
outThe resulting eef twist.
Returns:
False in case something goes wrong, true otherwise.

Definition at line 510 of file kdl_manager.cpp.

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_linkThe name of the requested end-effector.
stateThe current joint state.
gThe output gravity effort.
Returns:
False in case something goes wrong, true otherwise.

Definition at line 651 of file kdl_manager.cpp.

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_linkThe arm's end-effector name.
stateThe robot joint state.
outThe resulting gripping point frame.
Returns:
False if something goes wrong, true otherwise.

Definition at line 408 of file kdl_manager.cpp.

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_linkThe name of the requested end-effector.
stateThe current robot joint state.
inDesired eef pose.
outJoint 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 818 of file kdl_manager.cpp.

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_linkThe name of the requested end-effector.
stateThe current robot joint state.
outThe resulting eef twist.
Returns:
False in case something goes wrong, true otherwise.

Definition at line 446 of file kdl_manager.cpp.

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_linkThe name of the requested end-effector.
stateThe current robot joint state.
inDesired gripping point twist, in the gripping point frame.
outJoint state for the desired twist.
Returns:
False in case something goes wrong, true otherwise.

Definition at line 847 of file kdl_manager.cpp.

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_linkThe name of the requested end-effector.
stateThe current joint state.
HThe output inertia matrix.
Returns:
False in case something goes wrong, true otherwise.

Definition at line 634 of file kdl_manager.cpp.

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_linkThe name of the requested end-effector.
stateThe current robot joint state.
outEnd-effector's jacobian.
Returns:
False in case something goes wrong, true otherwise.

Definition at line 913 of file kdl_manager.cpp.

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_linkThe name of the requested end-effector.
q_minThe lower joint position limit.
q_maxThe upper joint position limit.
q_vel_limThe maximum joint velocity.
Returns:
False in case something goes wrong, true otherwise.

Definition at line 530 of file kdl_manager.cpp.

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_linkThe name of the requested end-effector.
stateThe current joint state
qThe joint positions in KDL format
Returns:
False in case something goes wrong, true otherwise.

Definition at line 574 of file kdl_manager.cpp.

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

Definition at line 593 of file kdl_manager.cpp.

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_linkThe chain's end_effector.
qdotThe chain's joint velocities.
stateThe generated joint state message.
Returns:
False if something goes wrong.

Definition at line 256 of file kdl_manager.cpp.

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_linkThe chain's end_effector.
qThe chain's joint posisitions.
qdotThe chain's joint velocities.
stateThe generated joint state message.
Returns:
False if something goes wrong.

Definition at line 301 of file kdl_manager.cpp.

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_linkThe chain's end_effector.
qThe chain's joint posisitions.
qdotThe chain's joint velocities.
effortThe chain's joint efforts.
stateThe generated joint state message.
Returns:
False if something goes wrong.

Definition at line 357 of file kdl_manager.cpp.

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_linkThe name of the requested end-effector.
stateThe current joint state.
q_dotThe joint velocities in the KDL format.
Returns:
False in case something goes wrong, true otherwise.

Definition at line 613 of file kdl_manager.cpp.

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_linkThe name of the requested end-effector.
num_jointsThe number of joints of the associated kinematic chain.
Returns:
False in case something goes wrong, true otherwise.

Definition at line 1036 of file kdl_manager.cpp.

Loads the manager parameters

Returns:
False in case something goes wrong, true otherwise

Definition at line 19 of file kdl_manager.cpp.

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_linkThe name of the requested end-effector.
stateThe current robot joint state.
inDesired eef joint state.
outPose for the desired joint state.
Returns:
False in case something goes wrong, true otherwise.

Definition at line 834 of file kdl_manager.cpp.

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_linkThe name of the requested end-effector.
stateThe current robot joint state.
inDesired eef pose.
outJoint 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 767 of file kdl_manager.cpp.

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_frameThe base frame of the transform.
target_frameThe target frame of the transform.
outThe rigid transform between the two frames in the KDL format.
Returns:
False in case something goes wrong, true otherwise.

Definition at line 697 of file kdl_manager.cpp.

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_linkThe arm's end-effector name.
stateThe robot joint state.
outThe resulting sensor point frame.
Returns:
False if something goes wrong, true otherwise.

Definition at line 427 of file kdl_manager.cpp.

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_linkThe name of the requested end-effector.
stateThe current robot joint state.
inDesired eef twist.
outJoint state for the desired twist.
Returns:
False in case something goes wrong, true otherwise.

Definition at line 892 of file kdl_manager.cpp.

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:
chainThe kinematic chain where to look for the joint.
joint_nameThe joint name we wish to check.
Returns:
True if the joint was found in the kinematic chain, false otherwise.

Definition at line 1022 of file kdl_manager.cpp.

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_linkThe final link of the kinematic chain.
Returns:
false if it is not possible to initialize.

Definition at line 96 of file kdl_manager.cpp.

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

Common part of the initialize arm methods.

Parameters:
end_effector_linkThe name of the requested end-effector.
Returns:
False in case something goes wrong, true otherwise.

Definition at line 152 of file kdl_manager.cpp.

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_linkThe 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.

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_linkThe end-effector link name.
gripping_point_frameTF name of the gripping point frame.
Returns:
False if transform is not found or something else goes wrong, true otherwise.

Definition at line 215 of file kdl_manager.cpp.

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_linkThe end-effector link name.
sensor_point_frameTF name of the sensor point frame.
Returns:
False if transform is not found or something else goes wrong, true otherwise.

Definition at line 236 of file kdl_manager.cpp.

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_linkThe name of the requested end-effector.
inThe pose that is to be verified.
Returns:
True if the pose is reachable, false otherwise or in case of error.

Definition at line 739 of file kdl_manager.cpp.


Member Data Documentation

std::map<std::string, std::vector<std::string> > generic_control_toolbox::KDLManager::actuated_joint_names_ [private]

Definition at line 411 of file kdl_manager.hpp.

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

Definition at line 403 of file kdl_manager.hpp.

list of actuated joints per arm

Definition at line 412 of file kdl_manager.hpp.

Definition at line 404 of file kdl_manager.hpp.

Definition at line 401 of file kdl_manager.hpp.

Definition at line 402 of file kdl_manager.hpp.

Definition at line 413 of file kdl_manager.hpp.

Definition at line 398 of file kdl_manager.hpp.

Definition at line 399 of file kdl_manager.hpp.

Definition at line 409 of file kdl_manager.hpp.

Definition at line 413 of file kdl_manager.hpp.

Definition at line 413 of file kdl_manager.hpp.

Definition at line 397 of file kdl_manager.hpp.

Definition at line 396 of file kdl_manager.hpp.

Definition at line 412 of file kdl_manager.hpp.

Definition at line 400 of file kdl_manager.hpp.

Definition at line 408 of file kdl_manager.hpp.

Definition at line 413 of file kdl_manager.hpp.

Definition at line 406 of file kdl_manager.hpp.

Definition at line 407 of file kdl_manager.hpp.

Definition at line 413 of file kdl_manager.hpp.


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


generic_control_toolbox
Author(s): diogo
autogenerated on Thu Jun 6 2019 18:02:57