37 #ifndef PR2_ARM_IK_NODE_H 38 #define PR2_ARM_IK_NODE_H 40 #include <kdl/config.h> 41 #include <kdl/frames.hpp> 42 #include <kdl/jntarray.hpp> 43 #include <kdl/tree.hpp> 47 #include <moveit_msgs/GetPositionFK.h> 48 #include <moveit_msgs/GetPositionIK.h> 49 #include <moveit_msgs/KinematicSolverInfo.h> 50 #include <moveit_msgs/MoveItErrorCodes.h> 52 #include <kdl/chainfksolverpos_recursive.hpp> 84 const std::string& tip_frame_name,
const double& search_discretization_angle,
const int& free_angle);
89 #define KDL_VERSION_LESS(a, b, c) ((KDL_VERSION) < ((a << 16) | (b << 8) | c)) 90 #if KDL_VERSION_LESS(1, 4, 0) 95 #undef KDL_VERSION_LESS 117 bool getCount(
int& count,
const int& max_count,
const int& min_count);
154 getPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
155 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
166 bool searchPositionIK(
167 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
168 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
179 bool searchPositionIK(
180 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
181 const std::vector<double>& consistency_limits, std::vector<double>& solution,
182 moveit_msgs::MoveItErrorCodes& error_code,
193 bool searchPositionIK(
194 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
195 std::vector<double>& solution,
const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
208 bool searchPositionIK(
209 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
210 const std::vector<double>& consistency_limits, std::vector<double>& solution,
211 const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
221 bool getPositionFK(
const std::vector<std::string>& link_names,
const std::vector<double>& joint_angles,
222 std::vector<geometry_msgs::Pose>& poses)
const override;
229 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
230 double search_discretization)
override;
235 const std::vector<std::string>& getJointNames()
const override;
240 const std::vector<std::string>& getLinkNames()
const override;
256 moveit_msgs::MoveItErrorCodes& error_code)
const;
259 moveit_msgs::MoveItErrorCodes& error_code)
const;
double search_discretization_angle_
A set of options for the kinematics solver.
Core components of MoveIt!
IKCallbackFn desiredPoseCallback_
MOVEIT_CLASS_FORWARD(PR2ArmIKSolver)
ROSCONSOLE_DECL void initialize()
pr2_arm_kinematics::PR2ArmIKSolverPtr pr2_arm_ik_solver_
bool getCount(int &count, const int &max_count, const int &min_count)
std::string root_frame_name_
static const int NO_IK_SOLUTION
std::shared_ptr< KDL::ChainFkSolverPos_recursive > jnt_to_pose_solver_
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
void getKDLChainInfo(const KDL::Chain &chain, moveit_msgs::KinematicSolverInfo &chain_info)
void updateInternalDataStructures()
Provides an interface for kinematics solvers.
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
PR2ArmIK pr2_arm_ik_
The PR2 inverse kinematics solver.
~PR2ArmIKSolver() override
bool active_
Indicates whether the solver has been successfully initialized.
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
Signature for a callback to validate an IK solution. Typically used for collision checking...
moveit_msgs::KinematicSolverInfo ik_solver_info_
void getSolverInfo(moveit_msgs::KinematicSolverInfo &info)
get chain information about the arm. This populates the IK query response, filling in joint level inf...
IKCallbackFn solutionCallback_
PR2ArmIKSolver(const urdf::ModelInterface &robot_model, const std::string &root_frame_name, const std::string &tip_frame_name, const double &search_discretization_angle, const int &free_angle)
int cartToJntSearch(const KDL::JntArray &q_in, const KDL::Frame &p_in, KDL::JntArray &q_out, const double &timeout)
void getSolverInfo(moveit_msgs::KinematicSolverInfo &response)
static const int TIMED_OUT
int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out) override
double computeEuclideanDistance(const std::vector< double > &array_1, const KDL::JntArray &array_2)
Eigen::Isometry3f KDLToEigenMatrix(const KDL::Frame &p)