42 template <
class KinematicsPlugin>
47 template <
class KinematicsPlugin>
52 template <
class KinematicsPlugin>
54 const std::string& cache_name)
62 kinematics::KinematicsBase::lookupParam<std::string>(
"cached_ik_path", opts.
cached_ik_path,
"");
64 cache_.initializeCache(robot_id, group_name, cache_name, KinematicsPlugin::getJointNames().size(), opts);
72 template <
class KinematicsPlugin>
74 const std::string& group_name,
75 const std::string& base_frame,
76 const std::vector<std::string>& tip_frames,
77 double search_discretization)
80 if (!KinematicsPlugin::initialize(robot_model, group_name, base_frame, tip_frames, search_discretization))
83 std::string cache_name = base_frame;
84 std::accumulate(tip_frames.begin(), tip_frames.end(), cache_name);
86 KinematicsPlugin::getJointNames().size());
90 template <
class KinematicsPlugin>
92 const std::string& group_name,
93 const std::string& base_frame,
94 const std::vector<std::string>& tip_frames,
95 double search_discretization)
98 if (!KinematicsPlugin::initialize(robot_description, group_name, base_frame, tip_frames, search_discretization))
101 std::string cache_name = base_frame;
102 std::accumulate(tip_frames.begin(), tip_frames.end(), cache_name);
104 KinematicsPlugin::getJointNames().size());
108 template <
class KinematicsPlugin>
110 const std::vector<double>& ik_seed_state,
111 std::vector<double>& solution,
112 moveit_msgs::MoveItErrorCodes& error_code,
113 const KinematicsQueryOptions& options)
const
116 const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose);
117 bool solution_found = KinematicsPlugin::getPositionIK(ik_pose, nearest.second, solution, error_code, options) ||
118 KinematicsPlugin::getPositionIK(ik_pose, ik_seed_state, solution, error_code, options);
120 cache_.updateCache(nearest, pose, solution);
121 return solution_found;
124 template <
class KinematicsPlugin>
126 const std::vector<double>& ik_seed_state,
127 double timeout, std::vector<double>& solution,
128 moveit_msgs::MoveItErrorCodes& error_code,
131 std::chrono::time_point<std::chrono::system_clock>
start(std::chrono::system_clock::now());
133 const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose);
134 bool solution_found =
135 KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, solution, error_code, options);
138 std::chrono::duration<double> diff = std::chrono::system_clock::now() -
start;
140 KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), solution, error_code, options);
143 cache_.updateCache(nearest, pose, solution);
144 return solution_found;
147 template <
class KinematicsPlugin>
149 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
150 const std::vector<double>& consistency_limits, std::vector<double>& solution,
151 moveit_msgs::MoveItErrorCodes& error_code,
const KinematicsQueryOptions& options)
const
153 std::chrono::time_point<std::chrono::system_clock>
start(std::chrono::system_clock::now());
155 const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose);
156 bool solution_found = KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, consistency_limits,
157 solution, error_code, options);
160 std::chrono::duration<double> diff = std::chrono::system_clock::now() -
start;
161 solution_found = KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), consistency_limits,
162 solution, error_code, options);
165 cache_.updateCache(nearest, pose, solution);
166 return solution_found;
169 template <
class KinematicsPlugin>
171 const std::vector<double>& ik_seed_state,
172 double timeout, std::vector<double>& solution,
173 const IKCallbackFn& solution_callback,
174 moveit_msgs::MoveItErrorCodes& error_code,
175 const KinematicsQueryOptions& options)
const
177 std::chrono::time_point<std::chrono::system_clock>
start(std::chrono::system_clock::now());
179 const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose);
180 bool solution_found = KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, solution,
181 solution_callback, error_code, options);
184 std::chrono::duration<double> diff = std::chrono::system_clock::now() -
start;
185 solution_found = KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), solution,
186 solution_callback, error_code, options);
189 cache_.updateCache(nearest, pose, solution);
190 return solution_found;
193 template <
class KinematicsPlugin>
195 const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
double timeout,
196 const std::vector<double>& consistency_limits, std::vector<double>& solution,
const IKCallbackFn& solution_callback,
197 moveit_msgs::MoveItErrorCodes& error_code,
const KinematicsQueryOptions& options)
const
199 std::chrono::time_point<std::chrono::system_clock>
start(std::chrono::system_clock::now());
201 const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose);
202 bool solution_found = KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, consistency_limits,
203 solution, solution_callback, error_code, options);
206 std::chrono::duration<double> diff = std::chrono::system_clock::now() -
start;
207 solution_found = KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), consistency_limits,
208 solution, solution_callback, error_code, options);
211 cache_.updateCache(nearest, pose, solution);
212 return solution_found;
215 template <
class KinematicsPlugin>
217 const std::vector<geometry_msgs::Pose>& ik_poses,
const std::vector<double>& ik_seed_state,
double timeout,
218 const std::vector<double>& consistency_limits, std::vector<double>& solution,
const IKCallbackFn& solution_callback,
219 moveit_msgs::MoveItErrorCodes& error_code,
const KinematicsQueryOptions& options,
222 std::chrono::time_point<std::chrono::system_clock>
start(std::chrono::system_clock::now());
223 std::vector<Pose> poses(ik_poses.size());
224 for (
unsigned int i = 0; i < poses.size(); ++i)
225 poses[i] = Pose(ik_poses[i]);
227 bool solution_found =
228 KinematicsPlugin::searchPositionIK(ik_poses, nearest.second, timeout, consistency_limits, solution,
229 solution_callback, error_code, options, context_state);
232 std::chrono::duration<double> diff = std::chrono::system_clock::now() -
start;
234 KinematicsPlugin::searchPositionIK(ik_poses, ik_seed_state, diff.count(), consistency_limits, solution,
235 solution_callback, error_code, options, context_state);
240 return solution_found;