#include <pr2_arm_kinematics_constraint_aware.h>
Public Member Functions | |
| int | CartToJntSearch (const KDL::JntArray &q_in, const KDL::Frame &p_in, KDL::JntArray &q_out, const double &timeout, motion_planning_msgs::ArmNavigationErrorCodes &error_code) |
| This method searches for and returns the closest solution to the initial guess in the first set of solutions it finds. | |
| bool | getConstraintAwarePositionIK (kinematics_msgs::GetConstraintAwarePositionIK::Request &request, kinematics_msgs::GetConstraintAwarePositionIK::Response &response) |
| This method searches for and returns the closest solution to the initial guess in the first set of solutions it finds. | |
| PR2ArmIKConstraintAware () | |
| virtual | ~PR2ArmIKConstraintAware () |
Private Member Functions | |
| void | advertiseIK () |
| void | collisionCheck (const KDL::JntArray &jnt_array, const KDL::Frame &p_in, motion_planning_msgs::ArmNavigationErrorCodes &error_code) |
| void | contactFound (collision_space::EnvironmentModel::Contact &contact) |
| The ccost and display arguments should be bound by the caller. This is a callback function that gets called by the planning environment when a collision is found. | |
| void | initialPoseCheck (const KDL::JntArray &jnt_array, const KDL::Frame &p_in, motion_planning_msgs::ArmNavigationErrorCodes &error_code) |
| bool | isReady (motion_planning_msgs::ArmNavigationErrorCodes &error_code) |
| void | printStringVec (const std::string &prefix, const std::vector< std::string > &string_vector) |
| void | sendEndEffectorPose (const planning_models::KinematicState *state, bool valid) |
| bool | setupCollisionEnvironment (void) |
Private Attributes | |
| std::vector < motion_planning_msgs::AllowedContactSpecification > | allowed_contacts_ |
| std::vector< std::string > | arm_links_ |
| planning_environment::CollisionModels * | collision_models_ |
| motion_planning_msgs::OrderedCollisionOperations | collision_operations_ |
| motion_planning_msgs::Constraints | constraints_ |
| std::vector< std::string > | default_collision_links_ |
| ros::Publisher | display_trajectory_publisher_ |
| std::vector< std::string > | end_effector_collision_links_ |
| std::string | group_ |
| ros::ServiceServer | ik_collision_service_ |
| kinematics_msgs::PositionIKRequest | ik_request_ |
| planning_models::KinematicState * | kinematic_state_ |
| std::vector < motion_planning_msgs::LinkPadding > | link_padding_ |
| planning_environment::PlanningMonitor * | planning_monitor_ |
| urdf::Model | robot_model_ |
| A model of the robot to see which joints wrap around. | |
| bool | robot_model_initialized_ |
| Flag that tells us if the robot model was initialized successfully. | |
| bool | setup_collision_environment_ |
| bool | use_collision_map_ |
| ros::Publisher | vis_marker_array_publisher_ |
| ros::Publisher | vis_marker_publisher_ |
| bool | visualize_solution_ |
Definition at line 44 of file pr2_arm_kinematics_constraint_aware.h.
| pr2_arm_kinematics::PR2ArmIKConstraintAware::PR2ArmIKConstraintAware | ( | ) |
Definition at line 59 of file pr2_arm_kinematics_constraint_aware.cpp.
| virtual pr2_arm_kinematics::PR2ArmIKConstraintAware::~PR2ArmIKConstraintAware | ( | ) | [inline, virtual] |
Definition at line 58 of file pr2_arm_kinematics_constraint_aware.h.
| void pr2_arm_kinematics::PR2ArmIKConstraintAware::advertiseIK | ( | ) | [private] |
Definition at line 80 of file pr2_arm_kinematics_constraint_aware.cpp.
| int pr2_arm_kinematics::PR2ArmIKConstraintAware::CartToJntSearch | ( | const KDL::JntArray & | q_in, | |
| const KDL::Frame & | p_in, | |||
| KDL::JntArray & | q_out, | |||
| const double & | timeout, | |||
| motion_planning_msgs::ArmNavigationErrorCodes & | error_code | |||
| ) |
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 112 of file pr2_arm_kinematics_constraint_aware.cpp.
| void pr2_arm_kinematics::PR2ArmIKConstraintAware::collisionCheck | ( | const KDL::JntArray & | jnt_array, | |
| const KDL::Frame & | p_in, | |||
| motion_planning_msgs::ArmNavigationErrorCodes & | error_code | |||
| ) | [private] |
Definition at line 243 of file pr2_arm_kinematics_constraint_aware.cpp.
| void pr2_arm_kinematics::PR2ArmIKConstraintAware::contactFound | ( | collision_space::EnvironmentModel::Contact & | contact | ) | [private] |
The ccost and display arguments should be bound by the caller. This is a callback function that gets called by the planning environment when a collision is found.
Definition at line 513 of file pr2_arm_kinematics_constraint_aware.cpp.
| bool pr2_arm_kinematics::PR2ArmIKConstraintAware::getConstraintAwarePositionIK | ( | kinematics_msgs::GetConstraintAwarePositionIK::Request & | request, | |
| kinematics_msgs::GetConstraintAwarePositionIK::Response & | response | |||
| ) |
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 131 of file pr2_arm_kinematics_constraint_aware.cpp.
| void pr2_arm_kinematics::PR2ArmIKConstraintAware::initialPoseCheck | ( | const KDL::JntArray & | jnt_array, | |
| const KDL::Frame & | p_in, | |||
| motion_planning_msgs::ArmNavigationErrorCodes & | error_code | |||
| ) | [private] |
Definition at line 270 of file pr2_arm_kinematics_constraint_aware.cpp.
| bool pr2_arm_kinematics::PR2ArmIKConstraintAware::isReady | ( | motion_planning_msgs::ArmNavigationErrorCodes & | error_code | ) | [private] |
Definition at line 86 of file pr2_arm_kinematics_constraint_aware.cpp.
| void pr2_arm_kinematics::PR2ArmIKConstraintAware::printStringVec | ( | const std::string & | prefix, | |
| const std::vector< std::string > & | string_vector | |||
| ) | [private] |
Definition at line 405 of file pr2_arm_kinematics_constraint_aware.cpp.
| void pr2_arm_kinematics::PR2ArmIKConstraintAware::sendEndEffectorPose | ( | const planning_models::KinematicState * | state, | |
| bool | valid | |||
| ) | [private] |
Definition at line 349 of file pr2_arm_kinematics_constraint_aware.cpp.
| bool pr2_arm_kinematics::PR2ArmIKConstraintAware::setupCollisionEnvironment | ( | void | ) | [private] |
Definition at line 414 of file pr2_arm_kinematics_constraint_aware.cpp.
std::vector<motion_planning_msgs::AllowedContactSpecification> pr2_arm_kinematics::PR2ArmIKConstraintAware::allowed_contacts_ [private] |
Definition at line 120 of file pr2_arm_kinematics_constraint_aware.h.
std::vector<std::string> pr2_arm_kinematics::PR2ArmIKConstraintAware::arm_links_ [private] |
Definition at line 107 of file pr2_arm_kinematics_constraint_aware.h.
planning_environment::CollisionModels* pr2_arm_kinematics::PR2ArmIKConstraintAware::collision_models_ [private] |
Definition at line 97 of file pr2_arm_kinematics_constraint_aware.h.
motion_planning_msgs::OrderedCollisionOperations pr2_arm_kinematics::PR2ArmIKConstraintAware::collision_operations_ [private] |
Definition at line 118 of file pr2_arm_kinematics_constraint_aware.h.
motion_planning_msgs::Constraints pr2_arm_kinematics::PR2ArmIKConstraintAware::constraints_ [private] |
Definition at line 121 of file pr2_arm_kinematics_constraint_aware.h.
std::vector<std::string> pr2_arm_kinematics::PR2ArmIKConstraintAware::default_collision_links_ [private] |
Definition at line 105 of file pr2_arm_kinematics_constraint_aware.h.
ros::Publisher pr2_arm_kinematics::PR2ArmIKConstraintAware::display_trajectory_publisher_ [private] |
Definition at line 115 of file pr2_arm_kinematics_constraint_aware.h.
std::vector<std::string> pr2_arm_kinematics::PR2ArmIKConstraintAware::end_effector_collision_links_ [private] |
Definition at line 106 of file pr2_arm_kinematics_constraint_aware.h.
std::string pr2_arm_kinematics::PR2ArmIKConstraintAware::group_ [private] |
Definition at line 100 of file pr2_arm_kinematics_constraint_aware.h.
ros::ServiceServer pr2_arm_kinematics::PR2ArmIKConstraintAware::ik_collision_service_ [private] |
Definition at line 96 of file pr2_arm_kinematics_constraint_aware.h.
kinematics_msgs::PositionIKRequest pr2_arm_kinematics::PR2ArmIKConstraintAware::ik_request_ [private] |
Definition at line 117 of file pr2_arm_kinematics_constraint_aware.h.
planning_models::KinematicState* pr2_arm_kinematics::PR2ArmIKConstraintAware::kinematic_state_ [private] |
Definition at line 99 of file pr2_arm_kinematics_constraint_aware.h.
std::vector<motion_planning_msgs::LinkPadding> pr2_arm_kinematics::PR2ArmIKConstraintAware::link_padding_ [private] |
Definition at line 119 of file pr2_arm_kinematics_constraint_aware.h.
planning_environment::PlanningMonitor* pr2_arm_kinematics::PR2ArmIKConstraintAware::planning_monitor_ [private] |
Definition at line 98 of file pr2_arm_kinematics_constraint_aware.h.
urdf::Model pr2_arm_kinematics::PR2ArmIKConstraintAware::robot_model_ [private] |
A model of the robot to see which joints wrap around.
Definition at line 130 of file pr2_arm_kinematics_constraint_aware.h.
Flag that tells us if the robot model was initialized successfully.
Definition at line 132 of file pr2_arm_kinematics_constraint_aware.h.
Definition at line 122 of file pr2_arm_kinematics_constraint_aware.h.
Definition at line 101 of file pr2_arm_kinematics_constraint_aware.h.
ros::Publisher pr2_arm_kinematics::PR2ArmIKConstraintAware::vis_marker_array_publisher_ [private] |
Definition at line 103 of file pr2_arm_kinematics_constraint_aware.h.
ros::Publisher pr2_arm_kinematics::PR2ArmIKConstraintAware::vis_marker_publisher_ [private] |
Definition at line 102 of file pr2_arm_kinematics_constraint_aware.h.
Definition at line 116 of file pr2_arm_kinematics_constraint_aware.h.
