44 #include <geometry_msgs/PoseStamped.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>
63 class ChainIkSolverVelMimicSVD;
81 getPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
82 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
86 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
87 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
91 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
92 const std::vector<double>& consistency_limits, std::vector<double>& solution,
93 moveit_msgs::MoveItErrorCodes& error_code,
97 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
98 std::vector<double>& solution,
const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
102 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
103 const std::vector<double>& consistency_limits, std::vector<double>& solution,
104 const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
107 bool getPositionFK(
const std::vector<std::string>& link_names,
const std::vector<double>& joint_angles,
108 std::vector<geometry_msgs::Pose>& poses)
const override;
111 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
112 double search_discretization)
override;
117 const std::vector<std::string>&
getJointNames()
const override;
122 const std::vector<std::string>&
getLinkNames()
const override;
125 typedef Eigen::Matrix<double, 6, 1>
Twist;
130 KDL::JntArray& q_out,
const unsigned int max_iter,
const Eigen::VectorXd& joint_weights,
131 const Twist& cartesian_weights)
const;
143 bool checkConsistency(
const Eigen::VectorXd& seed_state,
const std::vector<double>& consistency_limits,
144 const Eigen::VectorXd& solution)
const;
153 void getRandomConfiguration(
const Eigen::VectorXd& seed_state,
const std::vector<double>& consistency_limits,
154 Eigen::VectorXd& jnt_array)
const;
157 void clipToJointLimits(
const KDL::JntArray& q, KDL::JntArray& q_delta, Eigen::ArrayXd& weighting)
const;