#include <trac_ik_kinematics_plugin.hpp>
Public Member Functions | |
const std::vector< std::string > & | getJointNames () const |
const std::vector< std::string > & | getLinkNames () const |
bool | getPositionFK (const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const |
Given a set of joint angles and a set of links, compute their pose. | |
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 |
Given a desired pose of the end-effector, compute the joint angles to reach it. | |
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) |
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 |
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). | |
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 |
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). | |
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 |
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). | |
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 |
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). The consistency_limit specifies that only certain redundancy positions around those specified in the seed state are admissible and need to be searched. | |
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) const |
TRAC_IKKinematicsPlugin () | |
~TRAC_IKKinematicsPlugin () | |
Private Member Functions | |
int | getKDLSegmentIndex (const std::string &name) const |
Private Attributes | |
bool | active_ |
KDL::Chain | chain |
KDL::JntArray | joint_max |
KDL::JntArray | joint_min |
std::vector< std::string > | joint_names_ |
std::vector< std::string > | link_names_ |
uint | num_joints_ |
bool | position_ik_ |
std::string | solve_type |
Definition at line 40 of file trac_ik_kinematics_plugin.hpp.
Definition at line 69 of file trac_ik_kinematics_plugin.hpp.
Definition at line 71 of file trac_ik_kinematics_plugin.hpp.
const std::vector<std::string>& trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin::getJointNames | ( | ) | const [inline, virtual] |
Implements kinematics::KinematicsBase.
Definition at line 56 of file trac_ik_kinematics_plugin.hpp.
int trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin::getKDLSegmentIndex | ( | const std::string & | name | ) | const [private] |
Definition at line 155 of file trac_ik_kinematics_plugin.cpp.
const std::vector<std::string>& trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin::getLinkNames | ( | ) | const [inline, virtual] |
Implements kinematics::KinematicsBase.
Definition at line 60 of file trac_ik_kinematics_plugin.hpp.
bool trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin::getPositionFK | ( | const std::vector< std::string > & | link_names, |
const std::vector< double > & | joint_angles, | ||
std::vector< geometry_msgs::Pose > & | poses | ||
) | const [virtual] |
Given a set of joint angles and a set of links, compute their pose.
This FK routine is only used if 'use_plugin_fk' is set in the 'arm_kinematics_constraint_aware' node, otherwise ROS TF is used to calculate the forward kinematics
link_names | A set of links for which FK needs to be computed |
joint_angles | The state for which FK is being computed |
poses | The resultant set of poses (in the frame returned by getBaseFrame()) |
Implements kinematics::KinematicsBase.
Definition at line 170 of file trac_ik_kinematics_plugin.cpp.
bool trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin::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] |
Given a desired pose of the end-effector, compute the joint angles to reach it.
ik_pose | the desired pose of the link |
ik_seed_state | an initial guess solution for the inverse kinematics |
solution | the solution vector |
error_code | an error code that encodes the reason for failure or success |
Implements kinematics::KinematicsBase.
Definition at line 217 of file trac_ik_kinematics_plugin.cpp.
bool trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin::initialize | ( | const std::string & | robot_description, |
const std::string & | group_name, | ||
const std::string & | base_name, | ||
const std::string & | tip_name, | ||
double | search_discretization | ||
) | [virtual] |
Implements kinematics::KinematicsBase.
Definition at line 45 of file trac_ik_kinematics_plugin.cpp.
bool trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin::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] |
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 |
Implements kinematics::KinematicsBase.
Definition at line 236 of file trac_ik_kinematics_plugin.cpp.
bool trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin::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] |
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 |
the | distance that the redundancy can be from the current position |
Implements kinematics::KinematicsBase.
Definition at line 256 of file trac_ik_kinematics_plugin.cpp.
bool trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin::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] |
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 |
Implements kinematics::KinematicsBase.
Definition at line 275 of file trac_ik_kinematics_plugin.cpp.
bool trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin::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 [virtual] |
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). The consistency_limit specifies that only certain redundancy positions around those specified in the seed state are admissible and need to be searched.
ik_pose | the desired pose of the link |
ik_seed_state | an initial guess solution for the inverse kinematics |
consistency_limit | the distance that the redundancy can be from the current position |
Implements kinematics::KinematicsBase.
Definition at line 294 of file trac_ik_kinematics_plugin.cpp.
bool trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin::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 | ||
) | const |
Definition at line 313 of file trac_ik_kinematics_plugin.cpp.
bool trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin::active_ [private] |
Definition at line 46 of file trac_ik_kinematics_plugin.hpp.
Definition at line 48 of file trac_ik_kinematics_plugin.hpp.
Definition at line 51 of file trac_ik_kinematics_plugin.hpp.
Definition at line 51 of file trac_ik_kinematics_plugin.hpp.
std::vector<std::string> trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin::joint_names_ [private] |
Definition at line 42 of file trac_ik_kinematics_plugin.hpp.
std::vector<std::string> trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin::link_names_ [private] |
Definition at line 43 of file trac_ik_kinematics_plugin.hpp.
Definition at line 45 of file trac_ik_kinematics_plugin.hpp.
Definition at line 49 of file trac_ik_kinematics_plugin.hpp.
std::string trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin::solve_type [private] |
Definition at line 53 of file trac_ik_kinematics_plugin.hpp.