Public Member Functions | |
bool | getPositionFK (const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) |
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, int &error_code) |
Given a desired pose of the end-effector, compute the joint angles to reach it. | |
IKFastKinematicsPlugin () | |
bool | searchPositionIK (const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, const double &timeout, std::vector< double > &solution, int &error_code) |
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, const double &timeout, const unsigned int &redundancy, const double &consistency_limit, std::vector< double > &solution, int &error_code) |
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, const double &timeout, std::vector< double > &solution, const boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, int &error_code)> &desired_pose_callback, const boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, int &error_code)> &solution_callback, int &error_code) |
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, const double &timeout, const unsigned int &redundancy, const double &consistency_limit, std::vector< double > &solution, const boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, int &error_code)> &desired_pose_callback, const boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, int &error_code)> &solution_callback, int &error_code) |
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. | |
Private Member Functions | |
void | fillFreeParams (int count, int *array) |
void | getClosestSolution (const std::vector< double > &ik_seed_state, std::vector< double > &solution) |
int | getClosestSolutionIndex (const std::vector< double > &ik_seed_state, const std::vector< std::vector< double > > &solutions) |
bool | getCount (int &count, const int &max_count, const int &min_count) |
const std::vector< std::string > & | getJointNames () const |
const std::vector< std::string > & | getLinkNames () const |
void | getSolution (int i, std::vector< double > &solution) |
Gets a specific solution from the set. | |
double | harmonize (const std::vector< double > &ik_seed_state, std::vector< double > &solution) |
double | harmonize_old (const std::vector< double > &ik_seed_state, std::vector< double > &solution) |
bool | initialize (const std::string &group_name, const std::string &base_name, const std::string &tip_name, const double &search_discretization) |
int | solve (KDL::Frame &pose_frame, const std::vector< double > &vfree) |
Calls the IK solver from IKFast. | |
Private Attributes | |
std::vector< int > | free_params_ |
std::vector< bool > | joint_has_limits_vector_ |
std::vector< double > | joint_max_vector_ |
std::vector< double > | joint_min_vector_ |
std::vector< std::string > | joint_names_ |
std::vector< std::string > | link_names_ |
size_t | num_joints_ |
IkSolutionList< IkReal > | solutions_ |
Definition at line 78 of file M16iB20_manipulator_ikfast_plugin.cpp.
Definition at line 99 of file M16iB20_manipulator_ikfast_plugin.cpp.
void M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::fillFreeParams | ( | int | count, |
int * | array | ||
) | [private] |
Definition at line 446 of file M16iB20_manipulator_ikfast_plugin.cpp.
void M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::getClosestSolution | ( | const std::vector< double > & | ik_seed_state, |
std::vector< double > & | solution | ||
) | [private] |
Definition at line 401 of file M16iB20_manipulator_ikfast_plugin.cpp.
int M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::getClosestSolutionIndex | ( | const std::vector< double > & | ik_seed_state, |
const std::vector< std::vector< double > > & | solutions | ||
) | [private] |
Definition at line 425 of file M16iB20_manipulator_ikfast_plugin.cpp.
bool M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::getCount | ( | int & | count, |
const int & | max_count, | ||
const int & | min_count | ||
) | [private] |
Definition at line 452 of file M16iB20_manipulator_ikfast_plugin.cpp.
const std::vector<std::string>& M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::getJointNames | ( | ) | const [inline, private] |
Definition at line 91 of file M16iB20_manipulator_ikfast_plugin.cpp.
const std::vector<std::string>& M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::getLinkNames | ( | ) | const [inline, private] |
Definition at line 92 of file M16iB20_manipulator_ikfast_plugin.cpp.
bool M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::getPositionFK | ( | const std::vector< std::string > & | link_names, |
const std::vector< double > & | joint_angles, | ||
std::vector< geometry_msgs::Pose > & | poses | ||
) |
Given a set of joint angles and a set of links, compute their pose.
link_names | - set of links for which poses are to be computed |
joint_angles | - current joint angles the response contains stamped pose information for all the requested links |
Definition at line 488 of file M16iB20_manipulator_ikfast_plugin.cpp.
bool M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::getPositionIK | ( | const geometry_msgs::Pose & | ik_pose, |
const std::vector< double > & | ik_seed_state, | ||
std::vector< double > & | solution, | ||
int & | error_code | ||
) |
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 |
Definition at line 520 of file M16iB20_manipulator_ikfast_plugin.cpp.
void M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::getSolution | ( | int | i, |
std::vector< double > & | solution | ||
) | [private] |
Gets a specific solution from the set.
Definition at line 350 of file M16iB20_manipulator_ikfast_plugin.cpp.
double M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::harmonize | ( | const std::vector< double > & | ik_seed_state, |
std::vector< double > & | solution | ||
) | [private] |
Definition at line 391 of file M16iB20_manipulator_ikfast_plugin.cpp.
double M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::harmonize_old | ( | const std::vector< double > & | ik_seed_state, |
std::vector< double > & | solution | ||
) | [private] |
Definition at line 368 of file M16iB20_manipulator_ikfast_plugin.cpp.
bool M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::initialize | ( | const std::string & | group_name, |
const std::string & | base_name, | ||
const std::string & | tip_name, | ||
const double & | search_discretization | ||
) | [private] |
Definition at line 225 of file M16iB20_manipulator_ikfast_plugin.cpp.
bool M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::searchPositionIK | ( | const geometry_msgs::Pose & | ik_pose, |
const std::vector< double > & | ik_seed_state, | ||
const double & | timeout, | ||
std::vector< double > & | solution, | ||
int & | error_code | ||
) |
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 |
Definition at line 582 of file M16iB20_manipulator_ikfast_plugin.cpp.
bool M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::searchPositionIK | ( | const geometry_msgs::Pose & | ik_pose, |
const std::vector< double > & | ik_seed_state, | ||
const double & | timeout, | ||
const unsigned int & | redundancy, | ||
const double & | consistency_limit, | ||
std::vector< double > & | solution, | ||
int & | error_code | ||
) |
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 |
Definition at line 671 of file M16iB20_manipulator_ikfast_plugin.cpp.
bool M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::searchPositionIK | ( | const geometry_msgs::Pose & | ik_pose, |
const std::vector< double > & | ik_seed_state, | ||
const double & | timeout, | ||
std::vector< double > & | solution, | ||
const boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, int &error_code)> & | desired_pose_callback, | ||
const boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, int &error_code)> & | solution_callback, | ||
int & | error_code | ||
) |
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 |
Definition at line 774 of file M16iB20_manipulator_ikfast_plugin.cpp.
bool M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::searchPositionIK | ( | const geometry_msgs::Pose & | ik_pose, |
const std::vector< double > & | ik_seed_state, | ||
const double & | timeout, | ||
const unsigned int & | redundancy, | ||
const double & | consistency_limit, | ||
std::vector< double > & | solution, | ||
const boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, int &error_code)> & | desired_pose_callback, | ||
const boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, int &error_code)> & | solution_callback, | ||
int & | error_code | ||
) |
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 |
Definition at line 883 of file M16iB20_manipulator_ikfast_plugin.cpp.
int M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::solve | ( | KDL::Frame & | pose_frame, |
const std::vector< double > & | vfree | ||
) | [private] |
Calls the IK solver from IKFast.
Definition at line 320 of file M16iB20_manipulator_ikfast_plugin.cpp.
std::vector<int> M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::free_params_ [private] |
Definition at line 86 of file M16iB20_manipulator_ikfast_plugin.cpp.
std::vector<bool> M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::joint_has_limits_vector_ [private] |
Definition at line 83 of file M16iB20_manipulator_ikfast_plugin.cpp.
std::vector<double> M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::joint_max_vector_ [private] |
Definition at line 82 of file M16iB20_manipulator_ikfast_plugin.cpp.
std::vector<double> M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::joint_min_vector_ [private] |
Definition at line 81 of file M16iB20_manipulator_ikfast_plugin.cpp.
std::vector<std::string> M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::joint_names_ [private] |
Definition at line 80 of file M16iB20_manipulator_ikfast_plugin.cpp.
std::vector<std::string> M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::link_names_ [private] |
Definition at line 84 of file M16iB20_manipulator_ikfast_plugin.cpp.
size_t M16iB20_manipulator_kinematics::IKFastKinematicsPlugin::num_joints_ [private] |
Definition at line 85 of file M16iB20_manipulator_ikfast_plugin.cpp.
Definition at line 89 of file M16iB20_manipulator_ikfast_plugin.cpp.