Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #ifndef _GRASP_TESTER_FAST_
00037 #define _GRASP_TESTER_FAST_
00038
00039 #include "object_manipulator/grasp_execution/approach_lift_grasp.h"
00040 #include <arm_kinematics_constraint_aware/arm_kinematics_solver_constraint_aware.h>
00041 #include <pluginlib/class_loader.h>
00042
00043
00044
00045 namespace object_manipulator {
00046
00048
00066 inline geometry_msgs::Vector3 doNegate(const geometry_msgs::Vector3& vec) {
00067 geometry_msgs::Vector3 v;
00068 v.x = - vec.x;
00069 v.y = - vec.y;
00070 v.z = - vec.z;
00071 return v;
00072 }
00073
00074 class GraspTesterFast : public GraspTester
00075 {
00076 protected:
00077 virtual void testGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00078 const manipulation_msgs::Grasp &grasp,
00079 GraspExecutionInfo &execution_info);
00080
00082 virtual std::vector<arm_navigation_msgs::LinkPadding>
00083 linkPaddingForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal);
00084
00085 bool getInterpolatedIK(const std::string& arm_name,
00086 const tf::Transform& first_pose,
00087 const tf::Vector3& direction,
00088 const double& distance,
00089 const std::vector<double>& ik_solution,
00090 const bool& reverse,
00091 const bool& premultiply,
00092 trajectory_msgs::JointTrajectory& traj);
00093
00094
00095
00096 std::map<std::string, arm_kinematics_constraint_aware::ArmKinematicsSolverConstraintAware*> ik_solver_map_;
00097
00098
00099 double consistent_angle_;
00100 unsigned int num_points_;
00101 unsigned int redundancy_;
00102
00103 ros::Publisher vis_marker_array_publisher_;
00104 ros::Publisher vis_marker_publisher_;
00105
00106 planning_environment::CollisionModels* getCollisionModels();
00107 planning_models::KinematicState* getPlanningSceneState();
00108
00109 planning_environment::CollisionModels* cm_;
00110 planning_models::KinematicState* state_;
00111
00112 public:
00113
00114 pluginlib::ClassLoader<kinematics::KinematicsBase> kinematics_loader_;
00115
00117 GraspTesterFast(planning_environment::CollisionModels* cm = NULL,
00118 const std::string& plugin_name="pr2_arm_kinematics/PR2ArmKinematicsPlugin");
00119
00120 ~GraspTesterFast();
00121
00122 void setPlanningSceneState(planning_models::KinematicState* state) {
00123 state_ = state;
00124 }
00125
00126 void getGroupJoints(const std::string& group_name,
00127 std::vector<std::string>& group_links);
00128
00129 void getGroupLinks(const std::string& group_name,
00130 std::vector<std::string>& group_links);
00131
00132 virtual void testGrasps(const object_manipulation_msgs::PickupGoal &pickup_goal,
00133 const std::vector<manipulation_msgs::Grasp> &grasps,
00134 std::vector<GraspExecutionInfo> &execution_info,
00135 bool return_on_first_hit);
00136 };
00137
00138 }
00139
00140 #endif