37 #ifndef MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_PLUGIN_ 38 #define MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_PLUGIN_ 45 #include <boost/filesystem.hpp> 46 #include <unordered_map> 77 Pose(
const geometry_msgs::Pose& pose);
89 using IKEntry = std::pair<std::vector<Pose>, std::vector<double>>;
100 void initializeCache(
const std::string& robot_description,
const std::string& group_name,
101 const std::string& cache_name,
const unsigned int num_joints,
Options opts =
Options());
111 void updateCache(
const IKEntry& nearest,
const std::vector<Pose>& poses,
const std::vector<double>& config)
const;
117 double configDistance2(
const std::vector<double>& config1,
const std::vector<double>& config2)
const;
149 class IKCacheMap :
public std::unordered_map<std::string, IKCache*>
155 IKCacheMap(
const std::string& robot_description,
const std::string& group_name,
unsigned int num_joints);
162 const std::vector<std::string>& active,
163 const std::vector<Pose>& poses)
const;
169 const std::vector<std::string>& active,
const std::vector<Pose>& poses,
170 const std::vector<double>& config);
173 std::string getKey(
const std::vector<std::string>& fixed,
const std::vector<std::string>& active)
const;
180 template <
class KinematicsPlugin>
195 bool getPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
196 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
199 bool initialize(
const std::string& robot_description,
const std::string& group_name,
const std::string& base_frame,
200 const std::string& tip_frame,
double search_discretization)
override;
202 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
203 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
206 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
207 const std::vector<double>& consistency_limits, std::vector<double>& solution,
208 moveit_msgs::MoveItErrorCodes& error_code,
211 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
212 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
213 moveit_msgs::MoveItErrorCodes& error_code,
216 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
217 const std::vector<double>& consistency_limits, std::vector<double>& solution,
218 const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
236 template <
class KinematicsPlugin>
245 bool initialize(
const std::string& robot_description,
const std::string& group_name,
const std::string& base_frame,
246 const std::vector<std::string>& tip_frames,
double search_discretization)
override;
248 bool searchPositionIK(
const std::vector<geometry_msgs::Pose>& ik_poses,
const std::vector<double>& ik_seed_state,
249 double timeout,
const std::vector<double>& consistency_limits, std::vector<double>& solution,
250 const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
std::pair< std::vector< Pose >, std::vector< double >> IKEntry
const IKEntry & getBestApproximateIKSolution(const Pose &pose) const
double configDistance2(const std::vector< double > &config1, const std::vector< double > &config2) const
NearestNeighborsGNAT< IKEntry * > ik_nn_
ROSCONSOLE_DECL void initialize()
kinematics::KinematicsBase::IKCallbackFn IKCallbackFn
double min_config_distance2_
unsigned int max_cache_size_
unsigned int max_cache_size
A cache of inverse kinematic solutions.
std::string cached_ik_path
Specific implementation of kinematics using KDL. This version can be used with any robot...
class to represent end effector pose
void updateCache(const IKEntry &nearest, const Pose &pose, const std::vector< double > &config) const
unsigned int last_saved_cache_size_
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
std::string robot_description_
void verifyCache(kdl_kinematics_plugin::KDLKinematicsPlugin &fk) const
double min_joint_config_distance
boost::filesystem::path cache_file_name_
std::vector< IKEntry > ik_cache_
double min_pose_distance_
tf2::Quaternion orientation
double distance(const urdf::Pose &transform)
void initializeCache(const std::string &robot_description, const std::string &group_name, const std::string &cache_name, const unsigned int num_joints, Options opts=Options())