#include <pr2_arm_kinematics_plugin.h>
Public Member Functions | |
const std::vector< std::string > & | getJointNames () const |
Return all the joint names in the order they are used internally. | |
const std::vector< std::string > & | getLinkNames () const |
Return all the link names in the order they are represented internally. | |
virtual 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. | |
virtual 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. | |
virtual 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) |
Initialization function for the kinematics. | |
bool | isActive () |
Specifies if the node is active or not. | |
PR2ArmKinematicsPlugin () | |
virtual 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). | |
virtual 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). | |
virtual 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). | |
virtual 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. | |
void | setRobotModel (boost::shared_ptr< urdf::ModelInterface > &robot_model) |
Protected Member Functions | |
void | desiredPoseCallback (const KDL::JntArray &jnt_array, const KDL::Frame &ik_pose, moveit_msgs::MoveItErrorCodes &error_code) const |
void | jointSolutionCallback (const KDL::JntArray &jnt_array, const KDL::Frame &ik_pose, moveit_msgs::MoveItErrorCodes &error_code) const |
Protected Attributes | |
bool | active_ |
IKCallbackFn | desiredPoseCallback_ |
int | dimension_ |
moveit_msgs::KinematicSolverInfo | fk_solver_info_ |
int | free_angle_ |
moveit_msgs::KinematicSolverInfo | ik_solver_info_ |
boost::shared_ptr < KDL::ChainFkSolverPos_recursive > | jnt_to_pose_solver_ |
KDL::Chain | kdl_chain_ |
boost::shared_ptr < pr2_arm_kinematics::PR2ArmIKSolver > | pr2_arm_ik_solver_ |
boost::shared_ptr < urdf::ModelInterface > | robot_model_ |
std::string | root_name_ |
IKCallbackFn | solutionCallback_ |
Definition at line 130 of file pr2_arm_kinematics_plugin.h.
Definition at line 248 of file pr2_arm_kinematics_plugin.cpp.
void pr2_arm_kinematics::PR2ArmKinematicsPlugin::desiredPoseCallback | ( | const KDL::JntArray & | jnt_array, |
const KDL::Frame & | ik_pose, | ||
moveit_msgs::MoveItErrorCodes & | error_code | ||
) | const [protected] |
const std::vector< std::string > & pr2_arm_kinematics::PR2ArmKinematicsPlugin::getJointNames | ( | ) | const [virtual] |
Return all the joint names in the order they are used internally.
Implements kinematics::KinematicsBase.
Definition at line 417 of file pr2_arm_kinematics_plugin.cpp.
const std::vector< std::string > & pr2_arm_kinematics::PR2ArmKinematicsPlugin::getLinkNames | ( | ) | const [virtual] |
Return all the link names in the order they are represented internally.
Implements kinematics::KinematicsBase.
Definition at line 426 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 | ||
) | const [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.
Definition at line 410 of file pr2_arm_kinematics_plugin.cpp.
bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::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_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 312 of file pr2_arm_kinematics_plugin.cpp.
bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::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] |
Initialization function for the kinematics.
Implements kinematics::KinematicsBase.
Definition at line 262 of file pr2_arm_kinematics_plugin.cpp.
Specifies if the node is active or not.
Definition at line 250 of file pr2_arm_kinematics_plugin.cpp.
void pr2_arm_kinematics::PR2ArmKinematicsPlugin::jointSolutionCallback | ( | const KDL::JntArray & | jnt_array, |
const KDL::Frame & | ik_pose, | ||
moveit_msgs::MoveItErrorCodes & | error_code | ||
) | const [protected] |
bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::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 321 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, | ||
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 |
consistency_limit | the distance that the redundancy can be from the current position |
Implements kinematics::KinematicsBase.
Definition at line 376 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, | ||
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 387 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, | ||
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 398 of file pr2_arm_kinematics_plugin.cpp.
void pr2_arm_kinematics::PR2ArmKinematicsPlugin::setRobotModel | ( | boost::shared_ptr< urdf::ModelInterface > & | robot_model | ) |
Definition at line 257 of file pr2_arm_kinematics_plugin.cpp.
bool pr2_arm_kinematics::PR2ArmKinematicsPlugin::active_ [protected] |
Definition at line 258 of file pr2_arm_kinematics_plugin.h.
IKCallbackFn pr2_arm_kinematics::PR2ArmKinematicsPlugin::desiredPoseCallback_ [mutable, protected] |
Definition at line 268 of file pr2_arm_kinematics_plugin.h.
int pr2_arm_kinematics::PR2ArmKinematicsPlugin::dimension_ [protected] |
Definition at line 263 of file pr2_arm_kinematics_plugin.h.
moveit_msgs::KinematicSolverInfo pr2_arm_kinematics::PR2ArmKinematicsPlugin::fk_solver_info_ [protected] |
Definition at line 266 of file pr2_arm_kinematics_plugin.h.
int pr2_arm_kinematics::PR2ArmKinematicsPlugin::free_angle_ [protected] |
Definition at line 259 of file pr2_arm_kinematics_plugin.h.
moveit_msgs::KinematicSolverInfo pr2_arm_kinematics::PR2ArmKinematicsPlugin::ik_solver_info_ [protected] |
Definition at line 266 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 264 of file pr2_arm_kinematics_plugin.h.
Definition at line 265 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 261 of file pr2_arm_kinematics_plugin.h.
boost::shared_ptr<urdf::ModelInterface> pr2_arm_kinematics::PR2ArmKinematicsPlugin::robot_model_ [protected] |
Definition at line 260 of file pr2_arm_kinematics_plugin.h.
std::string pr2_arm_kinematics::PR2ArmKinematicsPlugin::root_name_ [protected] |
Definition at line 262 of file pr2_arm_kinematics_plugin.h.
IKCallbackFn pr2_arm_kinematics::PR2ArmKinematicsPlugin::solutionCallback_ [mutable, protected] |
Definition at line 269 of file pr2_arm_kinematics_plugin.h.