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_id,
const std::string& group_name,
const std::string& cache_name,
101 const unsigned int num_joints,
const Options& opts = Options());
106 void updateCache(
const IKEntry& nearest,
const Pose& pose,
const std::vector<double>& config)
const;
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;
145 mutable std::mutex
lock_;
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;
182 template <
typename KinematicsPlugin,
typename =
bool>
187 template <
typename KinematicsPlugin>
189 std::declval<const moveit::core::RobotModel&>(), std::string(),
190 std::string(), std::vector<std::string>(), 0.0))> : std::true_type
194 template <
typename KinematicsPlugin,
typename =
bool>
199 template <
typename KinematicsPlugin>
201 decltype(
std::declval<KinematicsPlugin&>().KinematicsPlugin::initialize(
202 std::string(), std::string(), std::string(), std::vector<std::string>(), 0.0))>
208 template <
class KinematicsPlugin>
224 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
225 double search_discretization)
override
227 return initializeImpl(robot_model, group_name, base_frame, tip_frames, search_discretization);
230 bool initialize(
const std::string& robot_description,
const std::string& group_name,
const std::string& base_frame,
231 const std::string& tip_frame,
double search_discretization)
override
233 return initializeImpl(robot_description, group_name, base_frame, tip_frame, search_discretization);
236 bool getPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
237 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
240 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
241 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
244 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
245 const std::vector<double>& consistency_limits, std::vector<double>& solution,
246 moveit_msgs::MoveItErrorCodes& error_code,
249 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
250 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
251 moveit_msgs::MoveItErrorCodes& error_code,
254 bool searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
255 const std::vector<double>& consistency_limits, std::vector<double>& solution,
256 const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
262 void initCache(
const std::string& robot_id,
const std::string& group_name,
const std::string& cache_name);
267 template <
class T = KinematicsPlugin>
268 typename std::enable_if<HasRobotModelApi<T>::value,
bool>::type
270 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
271 double search_discretization)
273 if (tip_frames.size() != 1)
275 ROS_ERROR_NAMED(
"cached_ik",
"This solver does not support multiple tip frames");
280 if (!KinematicsPlugin::initialize(robot_model, group_name, base_frame, tip_frames, search_discretization))
286 template <
class T = KinematicsPlugin>
287 typename std::enable_if<!HasRobotModelApi<T>::value,
bool>::type
289 const std::string& ,
const std::vector<std::string>& ,
double )
294 template <
class T = KinematicsPlugin>
295 typename std::enable_if<HasRobotDescApi<T>::value,
bool>::type
296 initializeImpl(
const std::string& robot_description,
const std::string& group_name,
const std::string& base_frame,
297 const std::string& tip_frame,
double search_discretization)
300 if (!KinematicsPlugin::initialize(robot_description, group_name, base_frame, tip_frame, search_discretization))
302 initCache(robot_description, group_name, base_frame + tip_frame);
306 template <
class T = KinematicsPlugin>
307 typename std::enable_if<!HasRobotDescApi<T>::value,
bool>::type
308 initializeImpl(
const std::string& ,
const std::string& ,
const std::string& ,
309 const std::string& ,
double )
326 template <
class KinematicsPlugin>
327 class CachedMultiTipIKKinematicsPlugin :
public CachedIKKinematicsPlugin<KinematicsPlugin>
336 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
337 double search_discretization)
override;
339 bool initialize(
const std::string& robot_description,
const std::string& group_name,
const std::string& base_frame,
340 const std::vector<std::string>& tip_frames,
double search_discretization)
override;
342 bool searchPositionIK(
const std::vector<geometry_msgs::Pose>& ik_poses,
const std::vector<double>& ik_seed_state,
343 double timeout,
const std::vector<double>& consistency_limits, std::vector<double>& solution,
344 const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,