#include <place_tester_fast.h>
Public Member Functions | |
PlaceTesterFast (planning_environment::CollisionModels *cm=NULL, const std::string &plugin_name="pr2_arm_kinematics/PR2ArmKinematicsPlugin") | |
Also adds a grasp marker at the pre-grasp location. | |
void | setPlanningSceneState (planning_models::KinematicState *state) |
void | testPlaces (const object_manipulation_msgs::PlaceGoal &place_goal, const std::vector< geometry_msgs::PoseStamped > &place_locations, std::vector< PlaceExecutionInfo > &execution_info, bool return_on_first_hit) |
Tests a set of place locations and provides their execution info. | |
~PlaceTesterFast () | |
Public Attributes | |
pluginlib::ClassLoader < kinematics::KinematicsBase > | kinematics_loader_ |
Protected Member Functions | |
geometry_msgs::Vector3 | doNegate (const geometry_msgs::Vector3 &vec) |
planning_environment::CollisionModels * | getCollisionModels () |
void | getGroupLinks (const std::string &group_name, std::vector< std::string > &group_links) |
bool | getInterpolatedIK (const std::string &arm_name, const tf::Transform &first_pose, const tf::Vector3 &direction, const double &distance, const std::vector< double > &ik_solution, const bool &reverse, const bool &premultiply, trajectory_msgs::JointTrajectory &traj) |
planning_models::KinematicState * | getPlanningSceneState () |
std::vector < arm_navigation_msgs::LinkPadding > | linkPaddingForPlace (const object_manipulation_msgs::PlaceGoal &place_goal) |
Dynamic link padding to be used for grasp operation. | |
virtual void | testPlace (const object_manipulation_msgs::PlaceGoal &placre_goal, const geometry_msgs::PoseStamped &place_locations, PlaceExecutionInfo &execution_info) |
Tests a single place location. | |
Protected Attributes | |
planning_environment::CollisionModels * | cm_ |
double | consistent_angle_ |
std::map< std::string, arm_kinematics_constraint_aware::ArmKinematicsSolverConstraintAware * > | ik_solver_map_ |
unsigned int | num_points_ |
unsigned int | redundancy_ |
planning_models::KinematicState * | state_ |
ros::Publisher | vis_marker_array_publisher_ |
ros::Publisher | vis_marker_publisher_ |
Definition at line 47 of file place_tester_fast.h.
object_manipulator::PlaceTesterFast::PlaceTesterFast | ( | planning_environment::CollisionModels * | cm = NULL , |
const std::string & | plugin_name = "pr2_arm_kinematics/PR2ArmKinematicsPlugin" |
||
) |
Also adds a grasp marker at the pre-grasp location.
Definition at line 47 of file place_tester_fast.cpp.
Definition at line 89 of file place_tester_fast.cpp.
geometry_msgs::Vector3 object_manipulator::PlaceTesterFast::doNegate | ( | const geometry_msgs::Vector3 & | vec | ) | [inline, protected] |
Definition at line 51 of file place_tester_fast.h.
planning_environment::CollisionModels * object_manipulator::PlaceTesterFast::getCollisionModels | ( | void | ) | [protected] |
Definition at line 98 of file place_tester_fast.cpp.
void object_manipulator::PlaceTesterFast::getGroupLinks | ( | const std::string & | group_name, |
std::vector< std::string > & | group_links | ||
) | [protected] |
Definition at line 135 of file place_tester_fast.cpp.
bool object_manipulator::PlaceTesterFast::getInterpolatedIK | ( | const std::string & | arm_name, |
const tf::Transform & | first_pose, | ||
const tf::Vector3 & | direction, | ||
const double & | distance, | ||
const std::vector< double > & | ik_solution, | ||
const bool & | reverse, | ||
const bool & | premultiply, | ||
trajectory_msgs::JointTrajectory & | traj | ||
) | [protected] |
Definition at line 145 of file place_tester_fast.cpp.
planning_models::KinematicState * object_manipulator::PlaceTesterFast::getPlanningSceneState | ( | ) | [protected] |
Definition at line 106 of file place_tester_fast.cpp.
std::vector< arm_navigation_msgs::LinkPadding > object_manipulator::PlaceTesterFast::linkPaddingForPlace | ( | const object_manipulation_msgs::PlaceGoal & | place_goal | ) | [protected] |
Dynamic link padding to be used for grasp operation.
Zero padding on fingertip links
Definition at line 124 of file place_tester_fast.cpp.
void object_manipulator::PlaceTesterFast::setPlanningSceneState | ( | planning_models::KinematicState * | state | ) | [inline] |
Definition at line 102 of file place_tester_fast.h.
void object_manipulator::PlaceTesterFast::testPlace | ( | const object_manipulation_msgs::PlaceGoal & | place_goal, |
const geometry_msgs::PoseStamped & | place_location, | ||
PlaceExecutionInfo & | execution_info | ||
) | [protected, virtual] |
Tests a single place location.
Implements object_manipulator::PlaceTester.
Definition at line 182 of file place_tester_fast.cpp.
void object_manipulator::PlaceTesterFast::testPlaces | ( | const object_manipulation_msgs::PlaceGoal & | place_goal, |
const std::vector< geometry_msgs::PoseStamped > & | place_locations, | ||
std::vector< PlaceExecutionInfo > & | execution_info, | ||
bool | return_on_first_hit | ||
) | [virtual] |
Tests a set of place locations and provides their execution info.
Reimplemented from object_manipulator::PlaceTester.
Definition at line 188 of file place_tester_fast.cpp.
Definition at line 92 of file place_tester_fast.h.
double object_manipulator::PlaceTesterFast::consistent_angle_ [protected] |
Definition at line 82 of file place_tester_fast.h.
std::map<std::string, arm_kinematics_constraint_aware::ArmKinematicsSolverConstraintAware*> object_manipulator::PlaceTesterFast::ik_solver_map_ [protected] |
Definition at line 79 of file place_tester_fast.h.
pluginlib::ClassLoader<kinematics::KinematicsBase> object_manipulator::PlaceTesterFast::kinematics_loader_ |
Definition at line 111 of file place_tester_fast.h.
unsigned int object_manipulator::PlaceTesterFast::num_points_ [protected] |
Definition at line 83 of file place_tester_fast.h.
unsigned int object_manipulator::PlaceTesterFast::redundancy_ [protected] |
Definition at line 84 of file place_tester_fast.h.
Definition at line 93 of file place_tester_fast.h.
Definition at line 86 of file place_tester_fast.h.
Definition at line 87 of file place_tester_fast.h.