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
00037
00038 #ifndef PR2_ARM_IK_CONSTRAINT_AWARE_H
00039 #define PR2_ARM_IK_CONSTRAINT_AWARE_H
00040
00041 #include <angles/angles.h>
00042 #include <pr2_arm_kinematics/pr2_arm_kinematics.h>
00043
00044
00045 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
00046 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00047 #include <kinematics_msgs/GetPositionFK.h>
00048
00049 #include <boost/shared_ptr.hpp>
00050
00051 #include <planning_environment/models/collision_models_interface.h>
00052
00053 #include <arm_navigation_msgs/DisplayTrajectory.h>
00054 #include <arm_navigation_msgs/ArmNavigationErrorCodes.h>
00055
00056 #include <urdf/model.h>
00057
00058 namespace pr2_arm_kinematics
00059 {
00060 class PR2ArmIKConstraintAware : public pr2_arm_kinematics::PR2ArmKinematics
00061 {
00062 public:
00063
00074 PR2ArmIKConstraintAware();
00075
00076 virtual ~PR2ArmIKConstraintAware()
00077 {
00078 if (collision_models_interface_)
00079 delete collision_models_interface_;
00080 };
00081
00092 int CartToJntSearch(const KDL::JntArray& q_in,
00093 const KDL::Frame& p_in,
00094 KDL::JntArray &q_out,
00095 const double &timeout,
00096 arm_navigation_msgs::ArmNavigationErrorCodes& error_code);
00097
00108 bool getConstraintAwarePositionIK(kinematics_msgs::GetConstraintAwarePositionIK::Request &request,
00109 kinematics_msgs::GetConstraintAwarePositionIK::Response &response);
00110
00111 virtual bool getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
00112 kinematics_msgs::GetPositionIK::Response &response);
00113
00114 protected:
00115
00116 virtual bool transformPose(const std::string& des_frame,
00117 const geometry_msgs::PoseStamped& pose_in,
00118 geometry_msgs::PoseStamped& pose_out);
00119
00120
00121 private:
00122
00123 ros::ServiceServer ik_collision_service_;
00124 planning_environment::CollisionModelsInterface *collision_models_interface_;
00125 std::string group_;
00126 bool use_collision_map_;
00127 ros::Publisher vis_marker_publisher_;
00128 ros::Publisher vis_marker_array_publisher_;
00129 void contactFound(collision_space::EnvironmentModel::Contact &contact);
00130 std::vector<std::string> end_effector_collision_links_;
00131 std::vector<std::string> arm_links_;
00132 void initialPoseCheck(const KDL::JntArray &jnt_array,
00133 const KDL::Frame &p_in,
00134 arm_navigation_msgs::ArmNavigationErrorCodes &error_code);
00135 void collisionCheck(const KDL::JntArray &jnt_array,
00136 const KDL::Frame &p_in,
00137 arm_navigation_msgs::ArmNavigationErrorCodes &error_code);
00138 void printStringVec(const std::string &prefix, const std::vector<std::string> &string_vector);
00139 ros::Publisher display_trajectory_publisher_;
00140 bool visualize_solution_;
00141 kinematics_msgs::PositionIKRequest ik_request_;
00142 ros::Time last_planning_scene_drop_;
00143 arm_navigation_msgs::Constraints constraints_;
00144 void advertiseIK();
00145
00146 bool isReady(arm_navigation_msgs::ArmNavigationErrorCodes &error_code);
00147 void sendEndEffectorPose(const planning_models::KinematicState* state, bool valid);
00148 };
00149 }
00150 #endif