$search
#include <pr2_arm_ik_solver.h>
Public Member Functions | |
int | CartToJnt (const KDL::JntArray &q_init, const KDL::Frame &p_in, std::vector< KDL::JntArray > &q_out) |
An extension of the KDL solver interface to return all solutions found. NOTE: This method only returns a solution if it exists for the free parameter value passed in. To search for a solution in the entire workspace use the CartToJntSearch method detailed below. | |
int | CartToJnt (const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out) |
The KDL solver interface that is required to be implemented. NOTE: This method only returns a solution if it exists for the free parameter value passed in. To search for a solution in the entire workspace use the CartToJntSearch method detailed below. | |
int | CartToJntSearch (const KDL::JntArray &q_in, const KDL::Frame &p_in, KDL::JntArray &q_out, const double &timeout, arm_navigation_msgs::ArmNavigationErrorCodes &error_code, const boost::function< void(const KDL::JntArray &, const KDL::Frame &, arm_navigation_msgs::ArmNavigationErrorCodes &)> &desired_pose_callback, const boost::function< void(const KDL::JntArray &, const KDL::Frame &, arm_navigation_msgs::ArmNavigationErrorCodes &)> &solution_callback) |
This method searches for and returns the first solution it finds that also satisifies both user defined callbacks. | |
int | CartToJntSearch (const KDL::JntArray &q_in, const KDL::Frame &p_in, KDL::JntArray &q_out, const double &timeout) |
This method searches for and returns the closest solution to the initial guess in the first set of solutions it finds. | |
int | CartToJntSearch (const KDL::JntArray &q_in, const KDL::Frame &p_in, std::vector< KDL::JntArray > &q_out, const double &timeout) |
This method searches for and returns the first set of solutions it finds. | |
std::string | getFrameId () |
void | getSolverInfo (kinematics_msgs::KinematicSolverInfo &response) |
A method to get chain information about the serial chain that the IK operates on. | |
PR2ArmIKSolver (const urdf::Model &robot_model, const std::string &root_frame_name, const std::string &tip_frame_name, const double &search_discretization_angle, const int &free_angle) | |
~PR2ArmIKSolver () | |
Public Attributes | |
bool | active_ |
Indicates whether the solver has been successfully initialized. | |
PR2ArmIK | pr2_arm_ik_ |
The PR2 inverse kinematics solver. | |
Private Member Functions | |
bool | getCount (int &count, const int &max_count, const int &min_count) |
Private Attributes | |
int | free_angle_ |
std::string | root_frame_name_ |
double | search_discretization_angle_ |
Definition at line 52 of file pr2_arm_ik_solver.h.
PR2ArmIKSolver::PR2ArmIKSolver | ( | const urdf::Model & | robot_model, | |
const std::string & | root_frame_name, | |||
const std::string & | tip_frame_name, | |||
const double & | search_discretization_angle, | |||
const int & | free_angle | |||
) |
Definition at line 38 of file pr2_arm_ik_solver.cpp.
pr2_arm_kinematics::PR2ArmIKSolver::~PR2ArmIKSolver | ( | ) | [inline] |
Definition at line 71 of file pr2_arm_ik_solver.h.
int PR2ArmIKSolver::CartToJnt | ( | const KDL::JntArray & | q_init, | |
const KDL::Frame & | p_in, | |||
std::vector< KDL::JntArray > & | q_out | |||
) |
An extension of the KDL solver interface to return all solutions found. NOTE: This method only returns a solution if it exists for the free parameter value passed in. To search for a solution in the entire workspace use the CartToJntSearch method detailed below.
q_init | The initial guess for the inverse kinematics solution. The solver uses the joint value q_init(pr2_ik_->free_angle_) as as an input to the inverse kinematics. pr2_ik_->free_angle_ can either be 0 or 2 corresponding to the shoulder pan or shoulder roll angle | |
p_in | A KDL::Frame representation of the position of the end-effector for which the IK is being solved. | |
q_out | A std::vector of KDL::JntArray containing all found solutions. |
Definition at line 112 of file pr2_arm_ik_solver.cpp.
int PR2ArmIKSolver::CartToJnt | ( | const KDL::JntArray & | q_init, | |
const KDL::Frame & | p_in, | |||
KDL::JntArray & | q_out | |||
) |
The KDL solver interface that is required to be implemented. NOTE: This method only returns a solution if it exists for the free parameter value passed in. To search for a solution in the entire workspace use the CartToJntSearch method detailed below.
q_init | The initial guess for the inverse kinematics solution. The solver uses the joint value q_init(pr2_ik_->free_angle_) as as an input to the inverse kinematics. pr2_ik_->free_angle_ can either be 0 or 2 corresponding to the shoulder pan or shoulder roll angle | |
p_in | A KDL::Frame representation of the position of the end-effector for which the IK is being solved. | |
q_out | A single inverse kinematic solution (if it exists). |
Definition at line 58 of file pr2_arm_ik_solver.cpp.
int PR2ArmIKSolver::CartToJntSearch | ( | const KDL::JntArray & | q_in, | |
const KDL::Frame & | p_in, | |||
KDL::JntArray & | q_out, | |||
const double & | timeout, | |||
arm_navigation_msgs::ArmNavigationErrorCodes & | error_code, | |||
const boost::function< void(const KDL::JntArray &, const KDL::Frame &, arm_navigation_msgs::ArmNavigationErrorCodes &)> & | desired_pose_callback, | |||
const boost::function< void(const KDL::JntArray &, const KDL::Frame &, arm_navigation_msgs::ArmNavigationErrorCodes &)> & | solution_callback | |||
) |
This method searches for and returns the first solution it finds that also satisifies both user defined callbacks.
q_init | The initial guess for the inverse kinematics solution. The solver uses the joint value q_init(pr2_ik_->free_angle_) as | |
p_in | A KDL::Frame representation of the position of the end-effector for which the IK is being solved. | |
q_out | A std::vector of KDL::JntArray containing all found solutions. | |
desired_pose_callback | A callback function to which the desired pose is passed in | |
solution_callback | A callback function to which IK solutions are passed in |
Definition at line 261 of file pr2_arm_ik_solver.cpp.
int PR2ArmIKSolver::CartToJntSearch | ( | const KDL::JntArray & | q_in, | |
const KDL::Frame & | p_in, | |||
KDL::JntArray & | q_out, | |||
const double & | timeout | |||
) |
This method searches for and returns the closest solution to the initial guess in the first set of solutions it finds.
q_in | The initial guess for the inverse kinematics solution. The solver uses the joint value q_init(pr2_ik_->free_angle_) as as an input to the inverse kinematics. pr2_ik_->free_angle_ can either be 0 or 2 corresponding to the shoulder pan or shoulder roll angle | |
p_in | A KDL::Frame representation of the position of the end-effector for which the IK is being solved. | |
q_out | A std::vector of KDL::JntArray containing all found solutions. | |
timeout | The amount of time (in seconds) to spend looking for a solution. |
Definition at line 222 of file pr2_arm_ik_solver.cpp.
int PR2ArmIKSolver::CartToJntSearch | ( | const KDL::JntArray & | q_in, | |
const KDL::Frame & | p_in, | |||
std::vector< KDL::JntArray > & | q_out, | |||
const double & | timeout | |||
) |
This method searches for and returns the first set of solutions it finds.
q_in | The initial guess for the inverse kinematics solution. The solver uses the joint value q_init(pr2_ik_->free_angle_) as as an input to the inverse kinematics. pr2_ik_->free_angle_ can either be 0 or 2 corresponding to the shoulder pan or shoulder roll angle | |
p_in | A KDL::Frame representation of the position of the end-effector for which the IK is being solved. | |
q_out | A std::vector of KDL::JntArray containing all found solutions. | |
timeout | The amount of time (in seconds) to spend looking for a solution. |
Definition at line 183 of file pr2_arm_ik_solver.cpp.
bool PR2ArmIKSolver::getCount | ( | int & | count, | |
const int & | max_count, | |||
const int & | min_count | |||
) | [private] |
Definition at line 145 of file pr2_arm_ik_solver.cpp.
std::string PR2ArmIKSolver::getFrameId | ( | ) |
Definition at line 331 of file pr2_arm_ik_solver.cpp.
void PR2ArmIKSolver::getSolverInfo | ( | kinematics_msgs::KinematicSolverInfo & | response | ) |
A method to get chain information about the serial chain that the IK operates on.
response | This class gets populated with information about the joints that IK operates on, including joint names and limits. |
Definition at line 53 of file pr2_arm_ik_solver.cpp.
Indicates whether the solver has been successfully initialized.
Definition at line 81 of file pr2_arm_ik_solver.h.
int pr2_arm_kinematics::PR2ArmIKSolver::free_angle_ [private] |
Definition at line 174 of file pr2_arm_ik_solver.h.
The PR2 inverse kinematics solver.
Definition at line 71 of file pr2_arm_ik_solver.h.
std::string pr2_arm_kinematics::PR2ArmIKSolver::root_frame_name_ [private] |
Definition at line 176 of file pr2_arm_ik_solver.h.
double pr2_arm_kinematics::PR2ArmIKSolver::search_discretization_angle_ [private] |
Definition at line 172 of file pr2_arm_ik_solver.h.