$search
#include <pr2_arm_kinematics_plugin.h>
Public Member Functions | |
std::string | getBaseFrame () |
Return the frame in which the kinematics is operating. | |
std::vector< std::string > | getJointNames () |
Return all the joint names in the order they are used internally. | |
std::vector< std::string > | getLinkNames () |
Return all the link names in the order they are represented internally. | |
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. | |
std::string | getToolFrame () |
Return the links for which kinematics can be computed. | |
bool | initialize (std::string name) |
Initialization function for the kinematics. | |
bool | isActive () |
Specifies if the node is active or not. | |
PR2ArmKinematicsPlugin () | |
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, 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). | |
Protected Member Functions | |
void | desiredPoseCallback (const KDL::JntArray &jnt_array, const KDL::Frame &ik_pose, arm_navigation_msgs::ArmNavigationErrorCodes &error_code) |
void | jointSolutionCallback (const KDL::JntArray &jnt_array, const KDL::Frame &ik_pose, arm_navigation_msgs::ArmNavigationErrorCodes &error_code) |
Protected Attributes | |
bool | active_ |
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, int &error_code)> | desiredPoseCallback_ |
int | dimension_ |
ros::ServiceServer | fk_service_ |
kinematics_msgs::KinematicSolverInfo | fk_solver_info_ |
ros::ServiceServer | fk_solver_info_service_ |
int | free_angle_ |
ros::ServiceServer | ik_service_ |
kinematics_msgs::KinematicSolverInfo | ik_solver_info_ |
ros::ServiceServer | ik_solver_info_service_ |
boost::shared_ptr < KDL::ChainFkSolverPos_recursive > | jnt_to_pose_solver_ |
KDL::Chain | kdl_chain_ |
ros::NodeHandle | node_handle_ |
boost::shared_ptr < pr2_arm_kinematics::PR2ArmIKSolver > | pr2_arm_ik_solver_ |
urdf::Model | robot_model_ |
ros::NodeHandle | root_handle_ |
std::string | root_name_ |
double | search_discretization_ |
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, int &error_code)> | solutionCallback_ |
Definition at line 62 of file pr2_arm_kinematics_plugin.h.
pr2_arm_kinematics::PR2ArmKinematicsPlugin::PR2ArmKinematicsPlugin | ( | ) |
Definition at line 54 of file pr2_arm_kinematics_plugin.cpp.
void pr2_arm_kinematics::PR2ArmKinematicsPlugin::desiredPoseCallback | ( | const KDL::JntArray & | jnt_array, | |
const KDL::Frame & | ik_pose, | |||
arm_navigation_msgs::ArmNavigationErrorCodes & | error_code | |||
) | [protected] |
Definition at line 220 of file pr2_arm_kinematics_plugin.cpp.
std::string pr2_arm_kinematics::PR2ArmKinematicsPlugin::getBaseFrame | ( | ) | [virtual] |
Return the frame in which the kinematics is operating.
Implements kinematics::KinematicsBase.
Definition at line 359 of file pr2_arm_kinematics_plugin.cpp.
std::vector< std::string > pr2_arm_kinematics::PR2ArmKinematicsPlugin::getJointNames | ( | ) | [virtual] |
Return all the joint names in the order they are used internally.
Implements kinematics::KinematicsBase.
Definition at line 379 of file pr2_arm_kinematics_plugin.cpp.
std::vector< std::string > pr2_arm_kinematics::PR2ArmKinematicsPlugin::getLinkNames | ( | ) | [virtual] |
Return all the link names in the order they are represented internally.
Implements kinematics::KinematicsBase.
Definition at line 390 of file pr2_arm_kinematics_plugin.cpp.
bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::getPositionFK | ( | const std::vector< std::string > & | link_names, | |
const std::vector< double > & | joint_angles, | |||
std::vector< geometry_msgs::Pose > & | poses | |||
) | [virtual] |
Given a set of joint angles and a set of links, compute their pose.
request | - the request contains the joint angles, set of links for which poses are to be computed and a timeout | |
response | - the response contains stamped pose information for all the requested links |
Implements kinematics::KinematicsBase.
bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::getPositionIK | ( | const geometry_msgs::Pose & | ik_pose, | |
const std::vector< double > & | ik_seed_state, | |||
std::vector< double > & | solution, | |||
int & | error_code | |||
) | [virtual] |
Given a desired pose of the end-effector, compute the joint angles to reach it.
ik_link_name | - the name of the link for which IK is being computed | |
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 117 of file pr2_arm_kinematics_plugin.cpp.
std::string pr2_arm_kinematics::PR2ArmKinematicsPlugin::getToolFrame | ( | ) | [virtual] |
Return the links for which kinematics can be computed.
Implements kinematics::KinematicsBase.
Definition at line 369 of file pr2_arm_kinematics_plugin.cpp.
bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::initialize | ( | std::string | name | ) | [virtual] |
Initialization function for the kinematics.
Implements kinematics::KinematicsBase.
Definition at line 63 of file pr2_arm_kinematics_plugin.cpp.
bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::isActive | ( | ) |
Specifies if the node is active or not.
Definition at line 56 of file pr2_arm_kinematics_plugin.cpp.
void pr2_arm_kinematics::PR2ArmKinematicsPlugin::jointSolutionCallback | ( | const KDL::JntArray & | jnt_array, | |
const KDL::Frame & | ik_pose, | |||
arm_navigation_msgs::ArmNavigationErrorCodes & | error_code | |||
) | [protected] |
Definition at line 241 of file pr2_arm_kinematics_plugin.cpp.
bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::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 | |||
) | [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.
bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::searchPositionIK | ( | const geometry_msgs::Pose & | ik_pose, | |
const std::vector< double > & | ik_seed_state, | |||
const double & | timeout, | |||
std::vector< double > & | solution, | |||
int & | error_code | |||
) | [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 168 of file pr2_arm_kinematics_plugin.cpp.
bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::active_ [protected] |
Definition at line 157 of file pr2_arm_kinematics_plugin.h.
boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> pr2_arm_kinematics::PR2ArmKinematicsPlugin::desiredPoseCallback_ [protected] |
Definition at line 171 of file pr2_arm_kinematics_plugin.h.
int pr2_arm_kinematics::PR2ArmKinematicsPlugin::dimension_ [protected] |
Definition at line 166 of file pr2_arm_kinematics_plugin.h.
Definition at line 163 of file pr2_arm_kinematics_plugin.h.
kinematics_msgs::KinematicSolverInfo pr2_arm_kinematics::PR2ArmKinematicsPlugin::fk_solver_info_ [protected] |
Definition at line 169 of file pr2_arm_kinematics_plugin.h.
Definition at line 163 of file pr2_arm_kinematics_plugin.h.
int pr2_arm_kinematics::PR2ArmKinematicsPlugin::free_angle_ [protected] |
Definition at line 158 of file pr2_arm_kinematics_plugin.h.
Definition at line 163 of file pr2_arm_kinematics_plugin.h.
kinematics_msgs::KinematicSolverInfo pr2_arm_kinematics::PR2ArmKinematicsPlugin::ik_solver_info_ [protected] |
Definition at line 169 of file pr2_arm_kinematics_plugin.h.
Definition at line 163 of file pr2_arm_kinematics_plugin.h.
boost::shared_ptr<KDL::ChainFkSolverPos_recursive> pr2_arm_kinematics::PR2ArmKinematicsPlugin::jnt_to_pose_solver_ [protected] |
Definition at line 167 of file pr2_arm_kinematics_plugin.h.
Definition at line 168 of file pr2_arm_kinematics_plugin.h.
Definition at line 161 of file pr2_arm_kinematics_plugin.h.
boost::shared_ptr<pr2_arm_kinematics::PR2ArmIKSolver> pr2_arm_kinematics::PR2ArmKinematicsPlugin::pr2_arm_ik_solver_ [protected] |
Definition at line 162 of file pr2_arm_kinematics_plugin.h.
Definition at line 159 of file pr2_arm_kinematics_plugin.h.
Definition at line 161 of file pr2_arm_kinematics_plugin.h.
std::string pr2_arm_kinematics::PR2ArmKinematicsPlugin::root_name_ [protected] |
Definition at line 165 of file pr2_arm_kinematics_plugin.h.
double pr2_arm_kinematics::PR2ArmKinematicsPlugin::search_discretization_ [protected] |
Definition at line 160 of file pr2_arm_kinematics_plugin.h.
boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> pr2_arm_kinematics::PR2ArmKinematicsPlugin::solutionCallback_ [protected] |
Definition at line 172 of file pr2_arm_kinematics_plugin.h.