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 ARM_KINEMATICS_CONSTRAINT_AWARE_H
00039 #define ARM_KINEMATICS_CONSTRAINT_AWARE_H
00040
00041
00042 #include <boost/shared_ptr.hpp>
00043
00044
00045 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
00046 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00047 #include <kinematics_msgs/GetPositionFK.h>
00048
00049 #include <motion_planning_msgs/ArmNavigationErrorCodes.h>
00050 #include <motion_planning_msgs/DisplayTrajectory.h>
00051 #include <motion_planning_msgs/LinkPadding.h>
00052
00053
00054 #include <arm_kinematics_constraint_aware/arm_kinematics_constraint_aware_utils.h>
00055 #include <planning_environment/monitors/planning_monitor.h>
00056 #include <planning_models/kinematic_model.h>
00057 #include <kdl/jntarray.hpp>
00058 #include <angles/angles.h>
00059 #include <urdf/model.h>
00060
00061
00062 #include <pluginlib/class_loader.h>
00063 #include <kinematics_base/kinematics_base.h>
00064
00065
00066 namespace arm_kinematics_constraint_aware
00067 {
00068 class ArmKinematicsConstraintAware
00069 {
00070 public:
00071
00082 ArmKinematicsConstraintAware();
00083
00084 virtual ~ArmKinematicsConstraintAware()
00085 {
00086 if (planning_monitor_)
00087 delete planning_monitor_;
00088 if (collision_models_)
00089 delete collision_models_;
00090 };
00091
00102 bool getConstraintAwarePositionIK(kinematics_msgs::GetConstraintAwarePositionIK::Request &request,
00103 kinematics_msgs::GetConstraintAwarePositionIK::Response &response);
00104
00105 bool getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
00106 kinematics_msgs::GetPositionIK::Response &response);
00107
00108 bool getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
00109 kinematics_msgs::GetKinematicSolverInfo::Response &response);
00110
00111 bool getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
00112 kinematics_msgs::GetKinematicSolverInfo::Response &response);
00113
00114 bool getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
00115 kinematics_msgs::GetPositionFK::Response &response);
00116
00117 bool isActive(){ return active_;}
00118 private:
00119
00120 pluginlib::ClassLoader<kinematics::KinematicsBase> kinematics_loader_;
00121 kinematics::KinematicsBase* kinematics_solver_;
00122 bool active_;
00123
00124 ros::NodeHandle node_handle_,root_handle_;
00125 ros::ServiceServer ik_collision_service_, ik_service_, fk_service_, ik_solver_info_service_, fk_solver_info_service_;
00126 planning_environment::CollisionModels *collision_models_;
00127 planning_environment::PlanningMonitor *planning_monitor_;
00128 planning_models::KinematicState* kinematic_state_;
00129 std::string group_,root_name_;
00130 bool use_collision_map_;
00131 ros::Publisher vis_marker_publisher_;
00132 ros::Publisher vis_marker_array_publisher_;
00133 void contactFound(collision_space::EnvironmentModel::Contact &contact);
00134 std::vector<std::string> default_collision_links_;
00135 std::vector<std::string> end_effector_collision_links_;
00136 std::vector<std::string> arm_links_;
00137 void collisionCheck(const geometry_msgs::Pose &ik_pose,
00138 const std::vector<double> &ik_solution,
00139 int &error_code);
00140 void initialPoseCheck(const geometry_msgs::Pose &ik_pose,
00141 const std::vector<double> &ik_solution,
00142 int &error_code);
00143 void printStringVec(const std::string &prefix, const std::vector<std::string> &string_vector);
00144 ros::Publisher display_trajectory_publisher_;
00145 bool visualize_solution_;
00146 kinematics_msgs::PositionIKRequest ik_request_;
00147 motion_planning_msgs::OrderedCollisionOperations collision_operations_;
00148 std::vector<motion_planning_msgs::LinkPadding> link_padding_;
00149 std::vector<motion_planning_msgs::AllowedContactSpecification> allowed_contacts_;
00150 motion_planning_msgs::Constraints constraints_;
00151 bool setup_collision_environment_;
00152 bool setupCollisionEnvironment(void);
00153
00154 void advertiseBaseKinematicsServices();
00155 void advertiseConstraintIKService();
00156
00157 bool isReady(motion_planning_msgs::ArmNavigationErrorCodes &error_code);
00158 void sendEndEffectorPose(const planning_models::KinematicState* state, bool valid);
00159
00160 kinematics_msgs::KinematicSolverInfo chain_info_;
00161
00163 urdf::Model robot_model_;
00165 bool robot_model_initialized_;
00166 tf::TransformListener tf_;
00167 };
00168 }
00169 #endif