42 template <
class KinematicsPlugin>
47 template <
class KinematicsPlugin>
52 template <
class KinematicsPlugin>
54 const std::string& group_name,
55 const std::string& base_frame,
const std::string& tip_frame,
56 double search_discretization)
59 if (!KinematicsPlugin::initialize(robot_description, group_name, base_frame, tip_frame, search_discretization))
71 kinematics::KinematicsBase::lookupParam<std::string>(
"cached_ik_path", opts.
cached_ik_path,
"");
73 cache_.initializeCache(robot_description, group_name, base_frame + tip_frame,
74 KinematicsPlugin::getJointNames().size(), opts);
84 template <
class KinematicsPlugin>
86 const std::string& group_name,
87 const std::string& base_frame,
88 const std::vector<std::string>& tip_frames,
89 double search_discretization)
92 if (!KinematicsPlugin::initialize(robot_description, group_name, base_frame, tip_frames, search_discretization))
98 std::string cache_name = base_frame;
99 std::accumulate(tip_frames.begin(), tip_frames.end(), cache_name);
101 KinematicsPlugin::getJointNames().size());
105 template <
class KinematicsPlugin>
107 const std::vector<double>& ik_seed_state,
108 std::vector<double>& solution,
109 moveit_msgs::MoveItErrorCodes& error_code,
113 const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose);
114 bool solution_found = KinematicsPlugin::getPositionIK(ik_pose, nearest.second, solution, error_code, options) ||
115 KinematicsPlugin::getPositionIK(ik_pose, ik_seed_state, solution, error_code, options);
117 cache_.updateCache(nearest, pose, solution);
118 return solution_found;
121 template <
class KinematicsPlugin>
123 const std::vector<double>& ik_seed_state,
124 double timeout, std::vector<double>& solution,
125 moveit_msgs::MoveItErrorCodes& error_code,
128 std::chrono::time_point<std::chrono::system_clock> start(std::chrono::system_clock::now());
130 const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose);
131 bool solution_found =
132 KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, solution, error_code, options);
135 std::chrono::duration<double>
diff = std::chrono::system_clock::now() - start;
137 KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), solution, error_code, options);
140 cache_.updateCache(nearest, pose, solution);
141 return solution_found;
144 template <
class KinematicsPlugin>
146 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
147 const std::vector<double>& consistency_limits, std::vector<double>& solution,
150 std::chrono::time_point<std::chrono::system_clock> start(std::chrono::system_clock::now());
152 const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose);
153 bool solution_found = KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, consistency_limits,
154 solution, error_code, options);
157 std::chrono::duration<double>
diff = std::chrono::system_clock::now() - start;
158 solution_found = KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), consistency_limits,
159 solution, error_code, options);
162 cache_.updateCache(nearest, pose, solution);
163 return solution_found;
166 template <
class KinematicsPlugin>
168 const std::vector<double>& ik_seed_state,
169 double timeout, std::vector<double>& solution,
171 moveit_msgs::MoveItErrorCodes& error_code,
174 std::chrono::time_point<std::chrono::system_clock> start(std::chrono::system_clock::now());
176 const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose);
177 bool solution_found = KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, solution,
178 solution_callback, error_code, options);
181 std::chrono::duration<double>
diff = std::chrono::system_clock::now() - start;
182 solution_found = KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), solution,
183 solution_callback, error_code, options);
186 cache_.updateCache(nearest, pose, solution);
187 return solution_found;
190 template <
class KinematicsPlugin>
192 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
193 const std::vector<double>& consistency_limits, std::vector<double>& solution,
const IKCallbackFn& solution_callback,
196 std::chrono::time_point<std::chrono::system_clock> start(std::chrono::system_clock::now());
198 const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose);
199 bool solution_found = KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, consistency_limits,
200 solution, solution_callback, error_code, options);
203 std::chrono::duration<double>
diff = std::chrono::system_clock::now() - start;
204 solution_found = KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), consistency_limits,
205 solution, solution_callback, error_code, options);
208 cache_.updateCache(nearest, pose, solution);
209 return solution_found;
212 template <
class KinematicsPlugin>
214 const std::vector<geometry_msgs::Pose>& ik_poses,
const std::vector<double>& ik_seed_state,
double timeout,
215 const std::vector<double>& consistency_limits, std::vector<double>& solution,
const IKCallbackFn& solution_callback,
219 std::chrono::time_point<std::chrono::system_clock> start(std::chrono::system_clock::now());
220 std::vector<Pose> poses(ik_poses.size());
221 for (
unsigned int i = 0; i < poses.size(); ++i)
222 poses[i] =
Pose(ik_poses[i]);
224 bool solution_found =
225 KinematicsPlugin::searchPositionIK(ik_poses, nearest.second, timeout, consistency_limits, solution,
226 solution_callback, error_code, options, context_state);
229 std::chrono::duration<double>
diff = std::chrono::system_clock::now() - start;
231 KinematicsPlugin::searchPositionIK(ik_poses, ik_seed_state, diff.count(), consistency_limits, solution,
232 solution_callback, error_code, options, context_state);
237 return solution_found;
CachedIKKinematicsPlugin()
kinematics::KinematicsBase::IKCallbackFn IKCallbackFn
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
bool initialize(const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::string &tip_frame, double search_discretization) override
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 KinematicsQueryOptions &options=KinematicsQueryOptions()) const override
unsigned int max_cache_size
bool initialize(const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization) override
std::string cached_ik_path
~CachedIKKinematicsPlugin()
class to represent end effector pose
bool lookupParam(const std::string ¶m, T &val, const T &default_val) const
bool searchPositionIK(const std::vector< geometry_msgs::Pose > &ik_poses, const std::vector< double > &ik_seed_state, double timeout, const std::vector< double > &consistency_limits, std::vector< double > &solution, const IKCallbackFn &solution_callback, moveit_msgs::MoveItErrorCodes &error_code, const KinematicsQueryOptions &options=KinematicsQueryOptions(), const moveit::core::RobotState *context_state=nullptr) const override
double min_joint_config_distance
#define ROS_ERROR_NAMED(name,...)
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 KinematicsQueryOptions &options=KinematicsQueryOptions()) const override