37 #ifndef MOVEIT_PR2_ARM_IK_SOLVER_
38 #define MOVEIT_PR2_ARM_IK_SOLVER_
42 #include <kdl/chainiksolver.hpp>
46 #include <moveit_msgs/PositionIKRequest.h>
47 #include <geometry_msgs/PoseStamped.h>
56 class PR2ArmIKSolver :
public KDL::ChainIkSolverPos
66 const std::string &root_frame_name,
67 const std::string &tip_frame_name,
68 const double &search_discretization_angle,
69 const int &free_angle);
97 int CartToJnt(
const KDL::JntArray& q_init,
98 const KDL::Frame& p_in,
99 KDL::JntArray& q_out);
113 int CartToJnt(
const KDL::JntArray& q_init,
114 const KDL::Frame& p_in,
115 std::vector<KDL::JntArray> &q_out);
128 int CartToJntSearch(
const KDL::JntArray& q_in,
129 const KDL::Frame& p_in,
130 std::vector<KDL::JntArray> &q_out,
131 const double &timeout);
166 int CartToJntSearch(
const KDL::JntArray& q_in,
167 const KDL::Frame& p_in,
168 KDL::JntArray &q_out,
169 const double &timeout,
170 const double& consistency_limit);
185 int CartToJntSearch(
const KDL::JntArray& q_in,
186 const KDL::Frame& p_in,
187 KDL::JntArray &q_out,
188 const double &timeout,
189 moveit_msgs::MoveItErrorCodes &error_code,
206 int CartToJntSearch(
const KDL::JntArray& q_in,
207 const KDL::Frame& p_in,
208 KDL::JntArray &q_out,
209 const double &timeout,
210 const double& consistency_limit,
211 moveit_msgs::MoveItErrorCodes &error_code,
219 void getSolverInfo(moveit_msgs::KinematicSolverInfo &response);
221 std::string getFrameId();
223 unsigned int getFreeAngle()
const {
227 void setFreeAngle(
const unsigned int& free_angle) {
248 int CartToJntSearch(
const KDL::JntArray& q_in,
249 const KDL::Frame& p_in,
250 KDL::JntArray &q_out,
251 const double &timeout,
252 bool use_consistency_limit,
253 const double& consistency_limit,
254 moveit_msgs::MoveItErrorCodes &error_code,
257 bool getCount(
int &count,
const int &max_count,
const int &min_count);
266 #endif// PR2_ARM_IK_SOLVER_H