Specific implementation of kinematics using KDL. This version can be used with any robot. More...
#include <kdl_kinematics_plugin.h>
Public Member Functions | |
const std::vector< std::string > & | getJointNames () const |
Return all the joint names in the order they are used internally. More... | |
const std::vector< std::string > & | getLinkNames () const |
Return all the link names in the order they are represented internally. More... | |
virtual bool | getPositionFK (const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const |
virtual bool | getPositionIK (const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const |
virtual bool | initialize (const std::string &robot_description, const std::string &group_name, const std::string &base_name, const std::string &tip_name, double search_discretization) |
KDLKinematicsPlugin () | |
Default constructor. More... | |
virtual bool | searchPositionIK (const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const |
virtual bool | searchPositionIK (const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, const std::vector< double > &consistency_limits, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const |
virtual bool | searchPositionIK (const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, const IKCallbackFn &solution_callback, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const |
virtual bool | searchPositionIK (const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, const std::vector< double > &consistency_limits, std::vector< double > &solution, const IKCallbackFn &solution_callback, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const |
Public Member Functions inherited from kinematics::KinematicsBase | |
virtual const std::string & | getBaseFrame () const |
double | getDefaultTimeout () const |
virtual const std::string & | getGroupName () const |
virtual bool | getPositionIK (const std::vector< geometry_msgs::Pose > &ik_poses, const std::vector< double > &ik_seed_state, std::vector< std::vector< double > > &solutions, KinematicsResult &result, const kinematics::KinematicsQueryOptions &options) const |
virtual void | getRedundantJoints (std::vector< unsigned int > &redundant_joint_indices) const |
double | getSearchDiscretization (int joint_index=0) const |
std::vector< DiscretizationMethod > | getSupportedDiscretizationMethods () const |
virtual const std::string & | getTipFrame () const |
virtual const std::vector< std::string > & | getTipFrames () const |
virtual bool | initialize (const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization) |
KinematicsBase () | |
virtual bool | searchPositionIK (const std::vector< geometry_msgs::Pose > &ik_poses, const std::vector< double > &ik_seed_state, double timeout, const std::vector< double > &consistency_limits, std::vector< double > &solution, const IKCallbackFn &solution_callback, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const moveit::core::RobotState *context_state=NULL) const |
void | setDefaultTimeout (double timeout) |
bool | setRedundantJoints (const std::vector< std::string > &redundant_joint_names) |
void | setSearchDiscretization (const std::map< int, double > &discretization) |
void | setSearchDiscretization (double sd) |
virtual void | setValues (const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::string &tip_frame, double search_discretization) |
virtual void | setValues (const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization) |
virtual bool | supportsGroup (const moveit::core::JointModelGroup *jmg, std::string *error_text_out=NULL) const |
virtual | ~KinematicsBase () |
Protected Member Functions | |
bool | searchPositionIK (const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, const IKCallbackFn &solution_callback, moveit_msgs::MoveItErrorCodes &error_code, const std::vector< double > &consistency_limits, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const |
Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines). More... | |
virtual bool | setRedundantJoints (const std::vector< unsigned int > &redundant_joint_indices) |
Protected Member Functions inherited from kinematics::KinematicsBase | |
bool | lookupParam (const std::string ¶m, T &val, const T &default_val) const |
Private Member Functions | |
bool | checkConsistency (const KDL::JntArray &seed_state, const std::vector< double > &consistency_limit, const KDL::JntArray &solution) const |
Check whether the solution lies within the consistency limit of the seed state. More... | |
int | getJointIndex (const std::string &name) const |
int | getKDLSegmentIndex (const std::string &name) const |
void | getRandomConfiguration (KDL::JntArray &jnt_array, bool lock_redundancy) const |
void | getRandomConfiguration (const KDL::JntArray &seed_state, const std::vector< double > &consistency_limits, KDL::JntArray &jnt_array, bool lock_redundancy) const |
Get a random configuration within joint limits close to the seed state. More... | |
bool | isRedundantJoint (unsigned int index) const |
bool | timedOut (const ros::WallTime &start_time, double duration) const |
Private Attributes | |
bool | active_ |
unsigned int | dimension_ |
double | epsilon_ |
moveit_msgs::KinematicSolverInfo | fk_chain_info_ |
moveit_msgs::KinematicSolverInfo | ik_chain_info_ |
KDL::JntArray | joint_max_ |
KDL::JntArray | joint_min_ |
robot_model::JointModelGroup * | joint_model_group_ |
KDL::Chain | kdl_chain_ |
double | max_solver_iterations_ |
std::vector< JointMimic > | mimic_joints_ |
int | num_possible_redundant_joints_ |
bool | position_ik_ |
random_numbers::RandomNumberGenerator | random_number_generator_ |
std::vector< unsigned int > | redundant_joints_map_index_ |
robot_model::RobotModelPtr | robot_model_ |
robot_state::RobotStatePtr | state_ |
robot_state::RobotStatePtr | state_2_ |
Additional Inherited Members | |
Public Types inherited from kinematics::KinematicsBase | |
typedef boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> | IKCallbackFn |
Static Public Attributes inherited from kinematics::KinematicsBase | |
static const double | DEFAULT_SEARCH_DISCRETIZATION |
static const double | DEFAULT_TIMEOUT |
Protected Attributes inherited from kinematics::KinematicsBase | |
std::string | base_frame_ |
double | default_timeout_ |
std::string | group_name_ |
std::map< int, double > | redundant_joint_discretization_ |
std::vector< unsigned int > | redundant_joint_indices_ |
std::string | robot_description_ |
double | search_discretization_ |
std::vector< DiscretizationMethod > | supported_methods_ |
std::string | tip_frame_ |
std::vector< std::string > | tip_frames_ |
Specific implementation of kinematics using KDL. This version can be used with any robot.
Definition at line 70 of file kdl_kinematics_plugin.h.
kdl_kinematics_plugin::KDLKinematicsPlugin::KDLKinematicsPlugin | ( | ) |
Default constructor.
Definition at line 55 of file kdl_kinematics_plugin.cpp.
|
private |
Check whether the solution lies within the consistency limit of the seed state.
seed_state | Seed state |
redundancy | Index of the redundant joint within the chain |
consistency_limit | The returned state for redundant joint should be in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit] |
solution | solution configuration |
Definition at line 118 of file kdl_kinematics_plugin.cpp.
|
private |
Definition at line 348 of file kdl_kinematics_plugin.cpp.
|
virtual |
Return all the joint names in the order they are used internally.
Implements kinematics::KinematicsBase.
Definition at line 603 of file kdl_kinematics_plugin.cpp.
|
private |
Definition at line 358 of file kdl_kinematics_plugin.cpp.
|
virtual |
Return all the link names in the order they are represented internally.
Implements kinematics::KinematicsBase.
Definition at line 608 of file kdl_kinematics_plugin.cpp.
|
virtual |
Implements kinematics::KinematicsBase.
Definition at line 558 of file kdl_kinematics_plugin.cpp.
|
virtual |
Implements kinematics::KinematicsBase.
Definition at line 377 of file kdl_kinematics_plugin.cpp.
|
private |
Definition at line 59 of file kdl_kinematics_plugin.cpp.
|
private |
Get a random configuration within joint limits close to the seed state.
seed_state | Seed state |
redundancy | Index of the redundant joint within the chain |
consistency_limit | The returned state will contain a value for the redundant joint in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit] |
jnt_array | Returned random configuration |
Definition at line 81 of file kdl_kinematics_plugin.cpp.
|
virtual |
Implements kinematics::KinematicsBase.
Definition at line 128 of file kdl_kinematics_plugin.cpp.
|
private |
Definition at line 73 of file kdl_kinematics_plugin.cpp.
|
virtual |
Implements kinematics::KinematicsBase.
Definition at line 388 of file kdl_kinematics_plugin.cpp.
|
virtual |
Implements kinematics::KinematicsBase.
Definition at line 400 of file kdl_kinematics_plugin.cpp.
|
virtual |
Implements kinematics::KinematicsBase.
Definition at line 410 of file kdl_kinematics_plugin.cpp.
|
virtual |
Implements kinematics::KinematicsBase.
Definition at line 421 of file kdl_kinematics_plugin.cpp.
|
protected |
Given a desired pose of the end-effector, search for the joint angles required to reach it. This particular method is intended for "searching" for a solutions by stepping through the redundancy (or other numerical routines).
ik_pose | the desired pose of the link |
ik_seed_state | an initial guess solution for the inverse kinematics |
timeout | The amount of time (in seconds) available to the solver |
solution | the solution vector |
solution_callback | A callback solution for the IK solution |
error_code | an error code that encodes the reason for failure or success |
check_consistency | Set to true if consistency check needs to be performed |
redundancy | The index of the redundant joint |
consistency_limit | The returned solutuion will contain a value for the redundant joint in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit] |
Definition at line 431 of file kdl_kinematics_plugin.cpp.
|
protectedvirtual |
Reimplemented from kinematics::KinematicsBase.
Definition at line 289 of file kdl_kinematics_plugin.cpp.
|
private |
Definition at line 372 of file kdl_kinematics_plugin.cpp.
|
private |
Definition at line 178 of file kdl_kinematics_plugin.h.
|
private |
Definition at line 186 of file kdl_kinematics_plugin.h.
|
private |
Definition at line 203 of file kdl_kinematics_plugin.h.
|
private |
Stores information for the inverse kinematics solver
Definition at line 182 of file kdl_kinematics_plugin.h.
|
private |
Internal variable that indicates whether solvers are configured and ready
Definition at line 180 of file kdl_kinematics_plugin.h.
|
private |
Definition at line 188 of file kdl_kinematics_plugin.h.
|
private |
Dimension of the group
Definition at line 188 of file kdl_kinematics_plugin.h.
|
private |
Definition at line 201 of file kdl_kinematics_plugin.h.
|
private |
Store information for the forward kinematics solver
Definition at line 184 of file kdl_kinematics_plugin.h.
|
private |
Definition at line 202 of file kdl_kinematics_plugin.h.
|
private |
Definition at line 204 of file kdl_kinematics_plugin.h.
|
private |
Definition at line 196 of file kdl_kinematics_plugin.h.
|
private |
Definition at line 200 of file kdl_kinematics_plugin.h.
|
mutableprivate |
Joint limits
Definition at line 190 of file kdl_kinematics_plugin.h.
|
private |
Definition at line 197 of file kdl_kinematics_plugin.h.
|
private |
Definition at line 192 of file kdl_kinematics_plugin.h.
|
private |
Definition at line 194 of file kdl_kinematics_plugin.h.
|
private |
Definition at line 194 of file kdl_kinematics_plugin.h.