37 #ifndef MOVEIT_ROS_PLANNING_LMA_KINEMATICS_PLUGIN_H 38 #define MOVEIT_ROS_PLANNING_LMA_KINEMATICS_PLUGIN_H 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> 80 getPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
81 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
85 searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
86 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
90 searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
91 const std::vector<double>& consistency_limits, std::vector<double>& solution,
92 moveit_msgs::MoveItErrorCodes& error_code,
96 searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
97 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
98 moveit_msgs::MoveItErrorCodes& error_code,
102 searchPositionIK(
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 virtual bool getPositionFK(
const std::vector<std::string>& link_names,
const std::vector<double>& joint_angles,
108 std::vector<geometry_msgs::Pose>& poses)
const;
110 virtual bool initialize(
const std::string& robot_description,
const std::string& group_name,
111 const std::string& base_name,
const std::string& tip_name,
double search_discretization);
140 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
141 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
142 moveit_msgs::MoveItErrorCodes& error_code,
const std::vector<double>& consistency_limits,
145 virtual bool setRedundantJoints(
const std::vector<unsigned int>& redundant_joint_indices);
LMAKinematicsPlugin()
Default constructor.
bool timedOut(const ros::WallTime &start_time, double duration) const
int getJointIndex(const std::string &name) const
const std::vector< std::string > & getJointNames() const
Return all the joint names in the order they are used internally.
double max_solver_iterations_
moveit_msgs::KinematicSolverInfo fk_chain_info_
int getKDLSegmentIndex(const std::string &name) const
virtual bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
int num_possible_redundant_joints_
robot_state::RobotStatePtr state_
robot_state::RobotStatePtr state_2_
Specific implementation of kinematics using Levenberg-Marquardt method available at KDL...
virtual bool setRedundantJoints(const std::vector< unsigned int > &redundant_joint_indices)
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
const std::vector< std::string > & getLinkNames() const
Return all the link names in the order they are represented internally.
std::vector< JointMimic > mimic_joints_
void getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const
random_numbers::RandomNumberGenerator random_number_generator_
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
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.
moveit_msgs::KinematicSolverInfo ik_chain_info_
robot_model::JointModelGroup * joint_model_group_
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
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)
bool isRedundantJoint(unsigned int index) const
robot_model::RobotModelPtr robot_model_
std::vector< unsigned int > redundant_joints_map_index_