37 #ifndef MOVEIT_ROS_PLANNING_KDL_KINEMATICS_PLUGIN_ 38 #define MOVEIT_ROS_PLANNING_KDL_KINEMATICS_PLUGIN_ 45 #include <geometry_msgs/PoseStamped.h> 46 #include <moveit_msgs/GetPositionFK.h> 47 #include <moveit_msgs/GetPositionIK.h> 48 #include <moveit_msgs/KinematicSolverInfo.h> 49 #include <moveit_msgs/MoveItErrorCodes.h> 52 #include <kdl/jntarray.hpp> 53 #include <kdl/chainiksolvervel_pinv.hpp> 54 #include <kdl/chainiksolverpos_nr_jl.hpp> 55 #include <kdl/chainfksolverpos_recursive.hpp> 79 getPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
80 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
84 searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
85 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
89 searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
90 const std::vector<double>& consistency_limits, std::vector<double>& solution,
91 moveit_msgs::MoveItErrorCodes& error_code,
95 searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
96 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
97 moveit_msgs::MoveItErrorCodes& error_code,
101 searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
102 const std::vector<double>& consistency_limits, std::vector<double>& solution,
103 const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
106 virtual bool getPositionFK(
const std::vector<std::string>& link_names,
const std::vector<double>& joint_angles,
107 std::vector<geometry_msgs::Pose>& poses)
const;
109 virtual bool initialize(
const std::string& robot_description,
const std::string& group_name,
110 const std::string& base_name,
const std::string& tip_name,
double search_discretization);
139 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
140 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
141 moveit_msgs::MoveItErrorCodes& error_code,
const std::vector<double>& consistency_limits,
144 virtual bool setRedundantJoints(
const std::vector<unsigned int>& redundant_joint_indices);
robot_state::RobotStatePtr state_
int getKDLSegmentIndex(const std::string &name) const
double max_solver_iterations_
virtual bool initialize(const std::string &robot_description, const std::string &group_name, const std::string &base_name, const std::string &tip_name, double search_discretization)
robot_model::JointModelGroup * joint_model_group_
robot_state::RobotStatePtr state_2_
virtual bool setRedundantJoints(const std::vector< unsigned int > &redundant_joint_indices)
void getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const
bool timedOut(const ros::WallTime &start_time, double duration) const
int getJointIndex(const std::string &name) const
random_numbers::RandomNumberGenerator random_number_generator_
robot_model::RobotModelPtr robot_model_
virtual bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
bool checkConsistency(const KDL::JntArray &seed_state, const std::vector< double > &consistency_limit, const KDL::JntArray &solution) const
Check whether the solution lies within the consistency limit of the seed state.
Specific implementation of kinematics using KDL. This version can be used with any robot...
moveit_msgs::KinematicSolverInfo fk_chain_info_
const std::vector< std::string > & getJointNames() const
Return all the joint names in the order they are used internally.
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
std::vector< JointMimic > mimic_joints_
virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const
KDLKinematicsPlugin()
Default constructor.
moveit_msgs::KinematicSolverInfo ik_chain_info_
bool isRedundantJoint(unsigned int index) const
std::vector< unsigned int > redundant_joints_map_index_
const std::vector< std::string > & getLinkNames() const
Return all the link names in the order they are represented internally.
int num_possible_redundant_joints_
virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const