44 #include <geometry_msgs/Pose.h>
45 #include <moveit_msgs/GetPositionFK.h>
46 #include <moveit_msgs/GetPositionIK.h>
47 #include <moveit_msgs/KinematicSolverInfo.h>
48 #include <moveit_msgs/MoveItErrorCodes.h>
51 #include <kdl/config.h>
52 #include <kdl/chainfksolver.hpp>
53 #include <kdl/chainiksolver.hpp>
75 getPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
76 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
80 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
81 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
85 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
86 const std::vector<double>& consistency_limits, std::vector<double>& solution,
87 moveit_msgs::MoveItErrorCodes& error_code,
91 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
92 std::vector<double>& solution,
const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
96 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
97 const std::vector<double>& consistency_limits, std::vector<double>& solution,
98 const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
101 bool getPositionFK(
const std::vector<std::string>& link_names,
const std::vector<double>& joint_angles,
102 std::vector<geometry_msgs::Pose>& poses)
const override;
105 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
106 double search_discretization)
override;
111 const std::vector<std::string>&
getJointNames()
const override;
116 const std::vector<std::string>&
getLinkNames()
const override;
127 bool checkConsistency(
const Eigen::VectorXd& seed_state,
const std::vector<double>& consistency_limits,
128 const Eigen::VectorXd& solution)
const;
130 bool obeysLimits(
const Eigen::VectorXd& values)
const;
132 void harmonize(Eigen::VectorXd& values)
const;
141 void getRandomConfiguration(
const Eigen::VectorXd& seed_state,
const std::vector<double>& consistency_limits,
142 Eigen::VectorXd& jnt_array)
const;
150 moveit::core::RobotStatePtr
state_;
152 std::unique_ptr<KDL::ChainFkSolverPos>
fk_solver_;
153 std::vector<const moveit::core::JointModel*>
joints_;