moveit2
The MoveIt Motion Planning Framework for ROS 2.
Public Member Functions | Protected Types | Protected Member Functions | List of all members
kdl_kinematics_plugin::KDLKinematicsPlugin Class Reference

Specific implementation of kinematics using KDL. This version supports any kinematic chain, also including mimic joints. More...

#include <kdl_kinematics_plugin.h>

Inheritance diagram for kdl_kinematics_plugin::KDLKinematicsPlugin:
Inheritance graph
[legend]
Collaboration diagram for kdl_kinematics_plugin::KDLKinematicsPlugin:
Collaboration graph
[legend]

Public Member Functions

 KDLKinematicsPlugin ()
 Default constructor. More...
 
bool getPositionIK (const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, std::vector< double > &solution, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
 Given a desired pose of the end-effector, compute the joint angles to reach it. More...
 
bool searchPositionIK (const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
 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 solution by stepping through the redundancy (or other numerical routines). More...
 
bool searchPositionIK (const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, const std::vector< double > &consistency_limits, std::vector< double > &solution, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
 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 solution by stepping through the redundancy (or other numerical routines). More...
 
bool searchPositionIK (const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, const IKCallbackFn &solution_callback, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
 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 solution by stepping through the redundancy (or other numerical routines). More...
 
bool searchPositionIK (const geometry_msgs::msg::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::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
 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 solution by stepping through the redundancy (or other numerical routines). More...
 
bool getPositionFK (const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::msg::Pose > &poses) const override
 Given a set of joint angles and a set of links, compute their pose. More...
 
bool initialize (const rclcpp::Node::SharedPtr &node, const moveit::core::RobotModel &robot_model, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization) override
 Initialization function for the kinematics, for use with kinematic chain IK solvers. More...
 
const std::vector< std::string > & getJointNames () const override
 Return all the joint names in the order they are used internally. More...
 
const std::vector< std::string > & getLinkNames () const override
 Return all the link names in the order they are represented internally. More...
 
- Public Member Functions inherited from kinematics::KinematicsBase
virtual bool getPositionIK (const std::vector< geometry_msgs::msg::Pose > &ik_poses, const std::vector< double > &ik_seed_state, std::vector< std::vector< double > > &solutions, KinematicsResult &result, const kinematics::KinematicsQueryOptions &options) const
 Given the desired poses of all end-effectors, compute joint angles that are able to reach it. More...
 
virtual bool searchPositionIK (const std::vector< geometry_msgs::msg::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::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const moveit::core::RobotState *context_state=nullptr) const
 Given a set of desired poses for a planning group with multiple end-effectors, search for the joint angles required to reach them. This is useful for e.g. biped robots that need to perform whole-body IK. Not necessary for most robots that have kinematic chains. This particular method is intended for "searching" for a solution by stepping through the redundancy (or other numerical routines). More...
 
virtual bool searchPositionIK (const std::vector< geometry_msgs::msg::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, const IKCostFn &cost_function, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const moveit::core::RobotState *context_state=nullptr) const
 Given a set of desired poses for a planning group with multiple end-effectors, search for the joint angles required to reach them. This is useful for e.g. biped robots that need to perform whole-body IK. Not necessary for most robots that have kinematic chains. This particular method is intended for "searching" for a solution by stepping through the redundancy (or other numerical routines). More...
 
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)
 Set the parameters for the solver, for use with non-chain IK solvers. More...
 
virtual const std::string & getGroupName () const
 Return the name of the group that the solver is operating on. More...
 
virtual const std::string & getBaseFrame () const
 Return the name of the frame in which the solver is operating. This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. More...
 
virtual const std::string & getTipFrame () const
 Return the name of the tip frame of the chain on which the solver is operating. This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. More...
 
virtual const std::vector< std::string > & getTipFrames () const
 Return the names of the tip frames of the kinematic tree on which the solver is operating. This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. More...
 
virtual bool setRedundantJoints (const std::vector< unsigned int > &redundant_joint_indices)
 Set a set of redundant joints for the kinematics solver to use. This can fail, depending on the IK solver and choice of redundant joints!. Also, it sets the discretization values for each redundant joint to a default value. More...
 
bool setRedundantJoints (const std::vector< std::string > &redundant_joint_names)
 Set a set of redundant joints for the kinematics solver to use. This function is just a convenience function that calls the previous definition of setRedundantJoints() More...
 
virtual void getRedundantJoints (std::vector< unsigned int > &redundant_joint_indices) const
 Get the set of redundant joints. More...
 
virtual bool supportsGroup (const moveit::core::JointModelGroup *jmg, std::string *error_text_out=nullptr) const
 Check if this solver supports a given JointModelGroup. More...
 
void setSearchDiscretization (double sd)
 Set the search discretization value for all the redundant joints. More...
 
void setSearchDiscretization (const std::map< int, double > &discretization)
 Sets individual discretization values for each redundant joint. More...
 
double getSearchDiscretization (int joint_index=0) const
 Get the value of the search discretization. More...
 
std::vector< DiscretizationMethodgetSupportedDiscretizationMethods () const
 Returns the set of supported kinematics discretization search types. This implementation only supports the DiscretizationMethods::ONE search. More...
 
void setDefaultTimeout (double timeout)
 For functions that require a timeout specified but one is not specified using arguments, a default timeout is used, as set by this function (and initialized to KinematicsBase::DEFAULT_TIMEOUT) More...
 
double getDefaultTimeout () const
 For functions that require a timeout specified but one is not specified using arguments, this default timeout is used. More...
 
virtual ~KinematicsBase ()
 Virtual destructor for the interface. More...
 
 KinematicsBase ()
 

Protected Types

typedef Eigen::Matrix< double, 6, 1 > Twist
 

Protected Member Functions

int CartToJnt (KDL::ChainIkSolverVelMimicSVD &ik_solver, const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out, const unsigned int max_iter, const Eigen::VectorXd &joint_weights, const Twist &cartesian_weights) const
 Solve position IK given initial joint values. More...
 
- Protected Member Functions inherited from kinematics::KinematicsBase
template<typename T >
bool lookupParam (const rclcpp::Node::SharedPtr &node, const std::string &param, T &val, const T &default_val) const
 Enables kinematics plugins access to parameters that are defined for the private namespace and inside 'robot_description_kinematics'. Parameters are searched in the following locations and order. More...
 
void storeValues (const moveit::core::RobotModel &robot_model, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization)
 

Additional Inherited Members

- Public Types inherited from kinematics::KinematicsBase
using IKCallbackFn = std::function< void(const geometry_msgs::msg::Pose &, const std::vector< double > &, moveit_msgs::msg::MoveItErrorCodes &)>
 Signature for a callback to validate an IK solution. Typically used for collision checking. More...
 
using IKCostFn = std::function< double(const geometry_msgs::msg::Pose &, const moveit::core::RobotState &, moveit::core::JointModelGroup const *, const std::vector< double > &)>
 Signature for a cost function used to evaluate IK solutions. More...
 
- Static Public Attributes inherited from kinematics::KinematicsBase
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_kinematics_base.kinematics_base")
 
static const double DEFAULT_SEARCH_DISCRETIZATION = 0.1
 
static const double DEFAULT_TIMEOUT = 1.0
 
- Protected Attributes inherited from kinematics::KinematicsBase
rclcpp::Node::SharedPtr node_
 
moveit::core::RobotModelConstPtr robot_model_
 
std::string robot_description_
 
std::string group_name_
 
std::string base_frame_
 
std::vector< std::string > tip_frames_
 
double default_timeout_
 
std::vector< unsigned int > redundant_joint_indices_
 
std::map< int, double > redundant_joint_discretization_
 
std::vector< DiscretizationMethodsupported_methods_
 

Detailed Description

Specific implementation of kinematics using KDL. This version supports any kinematic chain, also including mimic joints.

Definition at line 74 of file kdl_kinematics_plugin.h.

Member Typedef Documentation

◆ Twist

typedef Eigen::Matrix<double, 6, 1> kdl_kinematics_plugin::KDLKinematicsPlugin::Twist
protected

Definition at line 128 of file kdl_kinematics_plugin.h.

Constructor & Destructor Documentation

◆ KDLKinematicsPlugin()

kdl_kinematics_plugin::KDLKinematicsPlugin::KDLKinematicsPlugin ( )

Default constructor.

Definition at line 54 of file kdl_kinematics_plugin.cpp.

Member Function Documentation

◆ CartToJnt()

int kdl_kinematics_plugin::KDLKinematicsPlugin::CartToJnt ( KDL::ChainIkSolverVelMimicSVD ik_solver,
const KDL::JntArray &  q_init,
const KDL::Frame &  p_in,
KDL::JntArray &  q_out,
const unsigned int  max_iter,
const Eigen::VectorXd &  joint_weights,
const Twist cartesian_weights 
) const
protected

Solve position IK given initial joint values.

Definition at line 427 of file kdl_kinematics_plugin.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getJointNames()

const std::vector< std::string > & kdl_kinematics_plugin::KDLKinematicsPlugin::getJointNames ( ) const
overridevirtual

Return all the joint names in the order they are used internally.

Implements kinematics::KinematicsBase.

Definition at line 563 of file kdl_kinematics_plugin.cpp.

◆ getLinkNames()

const std::vector< std::string > & kdl_kinematics_plugin::KDLKinematicsPlugin::getLinkNames ( ) const
overridevirtual

Return all the link names in the order they are represented internally.

Implements kinematics::KinematicsBase.

Definition at line 568 of file kdl_kinematics_plugin.cpp.

◆ getPositionFK()

bool kdl_kinematics_plugin::KDLKinematicsPlugin::getPositionFK ( const std::vector< std::string > &  link_names,
const std::vector< double > &  joint_angles,
std::vector< geometry_msgs::msg::Pose > &  poses 
) const
overridevirtual

Given a set of joint angles and a set of links, compute their pose.

Parameters
link_namesA set of links for which FK needs to be computed
joint_anglesThe state for which FK is being computed
posesThe resultant set of poses (in the frame returned by getBaseFrame())
Returns
True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 527 of file kdl_kinematics_plugin.cpp.

Here is the caller graph for this function:

◆ getPositionIK()

bool kdl_kinematics_plugin::KDLKinematicsPlugin::getPositionIK ( const geometry_msgs::msg::Pose &  ik_pose,
const std::vector< double > &  ik_seed_state,
std::vector< double > &  solution,
moveit_msgs::msg::MoveItErrorCodes &  error_code,
const kinematics::KinematicsQueryOptions options = kinematics::KinematicsQueryOptions() 
) const
overridevirtual

Given a desired pose of the end-effector, compute the joint angles to reach it.

In contrast to the searchPositionIK methods, this one is expected to return the solution closest to the seed state. Randomly re-seeding is explicitly not allowed.

Parameters
ik_posethe desired pose of the link
ik_seed_statean initial guess solution for the inverse kinematics
solutionthe solution vector
error_codean error code that encodes the reason for failure or success
optionscontainer for other IK options. See definition of KinematicsQueryOptions for details.
Returns
True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 276 of file kdl_kinematics_plugin.cpp.

Here is the call graph for this function:

◆ initialize()

bool kdl_kinematics_plugin::KDLKinematicsPlugin::initialize ( const rclcpp::Node::SharedPtr &  node,
const moveit::core::RobotModel robot_model,
const std::string &  group_name,
const std::string &  base_frame,
const std::vector< std::string > &  tip_frames,
double  search_discretization 
)
overridevirtual

Initialization function for the kinematics, for use with kinematic chain IK solvers.

Parameters
robot_model- allow the URDF to be loaded much quicker by passing in a pre-parsed model of the robot
group_nameThe group for which this solver is being configured
base_frameThe base frame in which all input poses are expected. This may (or may not) be the root frame of the chain that the solver operates on
tip_framesThe tip of the chain
search_discretizationThe discretization of the search when the solver steps through the redundancy
Returns
true if initialization was successful, false otherwise

Default implementation returns false and issues a warning to implement this new API. TODO: Make this method purely virtual after some soaking time, replacing the fallback.

Reimplemented from kinematics::KinematicsBase.

Definition at line 138 of file kdl_kinematics_plugin.cpp.

Here is the call graph for this function:

◆ searchPositionIK() [1/4]

bool kdl_kinematics_plugin::KDLKinematicsPlugin::searchPositionIK ( const geometry_msgs::msg::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::msg::MoveItErrorCodes &  error_code,
const kinematics::KinematicsQueryOptions options = kinematics::KinematicsQueryOptions() 
) const
overridevirtual

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 solution by stepping through the redundancy (or other numerical routines).

Parameters
ik_posethe desired pose of the link
ik_seed_statean initial guess solution for the inverse kinematics
timeoutThe amount of time (in seconds) available to the solver
consistency_limitsthe distance that any joint in the solution can be from the corresponding joints in the current seed state
solutionthe solution vector
solution_callbackA callback to validate an IK solution
error_codean error code that encodes the reason for failure or success
optionscontainer for other IK options. See definition of KinematicsQueryOptions for details.
Returns
True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 321 of file kdl_kinematics_plugin.cpp.

Here is the call graph for this function:

◆ searchPositionIK() [2/4]

bool kdl_kinematics_plugin::KDLKinematicsPlugin::searchPositionIK ( const geometry_msgs::msg::Pose &  ik_pose,
const std::vector< double > &  ik_seed_state,
double  timeout,
const std::vector< double > &  consistency_limits,
std::vector< double > &  solution,
moveit_msgs::msg::MoveItErrorCodes &  error_code,
const kinematics::KinematicsQueryOptions options = kinematics::KinematicsQueryOptions() 
) const
overridevirtual

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 solution by stepping through the redundancy (or other numerical routines).

Parameters
ik_posethe desired pose of the link
ik_seed_statean initial guess solution for the inverse kinematics
timeoutThe amount of time (in seconds) available to the solver
consistency_limitsthe distance that any joint in the solution can be from the corresponding joints in the current seed state
solutionthe solution vector
error_codean error code that encodes the reason for failure or success
optionscontainer for other IK options. See definition of KinematicsQueryOptions for details.
Returns
True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 300 of file kdl_kinematics_plugin.cpp.

Here is the call graph for this function:

◆ searchPositionIK() [3/4]

bool kdl_kinematics_plugin::KDLKinematicsPlugin::searchPositionIK ( const geometry_msgs::msg::Pose &  ik_pose,
const std::vector< double > &  ik_seed_state,
double  timeout,
std::vector< double > &  solution,
const IKCallbackFn solution_callback,
moveit_msgs::msg::MoveItErrorCodes &  error_code,
const kinematics::KinematicsQueryOptions options = kinematics::KinematicsQueryOptions() 
) const
overridevirtual

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 solution by stepping through the redundancy (or other numerical routines).

Parameters
ik_posethe desired pose of the link
ik_seed_statean initial guess solution for the inverse kinematics
timeoutThe amount of time (in seconds) available to the solver
solutionthe solution vector
solution_callbackA callback to validate an IK solution
error_codean error code that encodes the reason for failure or success
optionscontainer for other IK options. See definition of KinematicsQueryOptions for details.
Returns
True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 310 of file kdl_kinematics_plugin.cpp.

Here is the call graph for this function:

◆ searchPositionIK() [4/4]

bool kdl_kinematics_plugin::KDLKinematicsPlugin::searchPositionIK ( const geometry_msgs::msg::Pose &  ik_pose,
const std::vector< double > &  ik_seed_state,
double  timeout,
std::vector< double > &  solution,
moveit_msgs::msg::MoveItErrorCodes &  error_code,
const kinematics::KinematicsQueryOptions options = kinematics::KinematicsQueryOptions() 
) const
overridevirtual

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 solution by stepping through the redundancy (or other numerical routines).

Parameters
ik_posethe desired pose of the link
ik_seed_statean initial guess solution for the inverse kinematics
timeoutThe amount of time (in seconds) available to the solver
solutionthe solution vector
error_codean error code that encodes the reason for failure or success
optionscontainer for other IK options. See definition of KinematicsQueryOptions for details.
Returns
True if a valid solution was found, false otherwise

Implements kinematics::KinematicsBase.

Definition at line 288 of file kdl_kinematics_plugin.cpp.

Here is the caller graph for this function:

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