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/monitors/planning_monitor.h>
00052 #include <planning_models/kinematic_model.h>
00053
00054 #include <motion_planning_msgs/DisplayTrajectory.h>
00055 #include <motion_planning_msgs/LinkPadding.h>
00056 #include <motion_planning_msgs/ArmNavigationErrorCodes.h>
00057
00058 #include <urdf/model.h>
00059
00060 namespace pr2_arm_kinematics
00061 {
00062 class PR2ArmIKConstraintAware : public pr2_arm_kinematics::PR2ArmKinematics
00063 {
00064 public:
00065
00076 PR2ArmIKConstraintAware();
00077
00078 virtual ~PR2ArmIKConstraintAware()
00079 {
00080 if (planning_monitor_)
00081 delete planning_monitor_;
00082 if (collision_models_)
00083 delete collision_models_;
00084 };
00085
00096 int CartToJntSearch(const KDL::JntArray& q_in,
00097 const KDL::Frame& p_in,
00098 KDL::JntArray &q_out,
00099 const double &timeout,
00100 motion_planning_msgs::ArmNavigationErrorCodes& error_code);
00101
00112 bool getConstraintAwarePositionIK(kinematics_msgs::GetConstraintAwarePositionIK::Request &request,
00113 kinematics_msgs::GetConstraintAwarePositionIK::Response &response);
00114 private:
00115
00116 ros::ServiceServer ik_collision_service_;
00117 planning_environment::CollisionModels *collision_models_;
00118 planning_environment::PlanningMonitor *planning_monitor_;
00119 planning_models::KinematicState* kinematic_state_;
00120 std::string group_;
00121 bool use_collision_map_;
00122 ros::Publisher vis_marker_publisher_;
00123 ros::Publisher vis_marker_array_publisher_;
00124 void contactFound(collision_space::EnvironmentModel::Contact &contact);
00125 std::vector<std::string> default_collision_links_;
00126 std::vector<std::string> end_effector_collision_links_;
00127 std::vector<std::string> arm_links_;
00128 void initialPoseCheck(const KDL::JntArray &jnt_array,
00129 const KDL::Frame &p_in,
00130 motion_planning_msgs::ArmNavigationErrorCodes &error_code);
00131 void collisionCheck(const KDL::JntArray &jnt_array,
00132 const KDL::Frame &p_in,
00133 motion_planning_msgs::ArmNavigationErrorCodes &error_code);
00134 void printStringVec(const std::string &prefix, const std::vector<std::string> &string_vector);
00135 ros::Publisher display_trajectory_publisher_;
00136 bool visualize_solution_;
00137 kinematics_msgs::PositionIKRequest ik_request_;
00138 motion_planning_msgs::OrderedCollisionOperations collision_operations_;
00139 std::vector<motion_planning_msgs::LinkPadding> link_padding_;
00140 std::vector<motion_planning_msgs::AllowedContactSpecification> allowed_contacts_;
00141 motion_planning_msgs::Constraints constraints_;
00142 bool setup_collision_environment_;
00143 bool setupCollisionEnvironment(void);
00144 void advertiseIK();
00145
00146 bool isReady(motion_planning_msgs::ArmNavigationErrorCodes &error_code);
00147 void sendEndEffectorPose(const planning_models::KinematicState* state, bool valid);
00148
00150 urdf::Model robot_model_;
00152 bool robot_model_initialized_;
00153
00154
00155 };
00156 }
00157 #endif